-- LSFitImpl.mesa
-- Last edited by Michael Plass 10-Mar-82 15:23:07
-- Michael Plass and Maureen Stone Oct-81

DIRECTORY
  Complex,
  LinearSystem,
  Real USING [RealException],
  LSFit;

LSFitImpl: CEDAR PROGRAM IMPORTS Complex, LinearSystem, Real EXPORTS LSFit =
BEGIN

RealProc: TYPE = PROCEDURE[i:NAT] RETURNS [REAL];
ComplexSequence: TYPE = LSFit.ComplexSequence;
ComplexSequenceRec: TYPE = LSFit.ComplexSequenceRec;
RealSequence: TYPE = LSFit.RealSequence;
RealSequenceRec: TYPE = LSFit.RealSequenceRec;
PointNumber: TYPE = NAT;
Patch: TYPE = LSFit.Patch;
PatchSequence: TYPE = LSFit.PatchSequence;
PatchSequenceRec: TYPE = LSFit.PatchSequenceRec;

Handle: TYPE = LSFit.Handle;
StateRec: TYPE = LSFit.StateRec;

Create: PUBLIC PROCEDURE [sa: ComplexSequence] RETURNS [h: Handle] =
    BEGIN
    h ← SamplesToHandle[sa];
    h.weight ← NEW[RealSequenceRec[h.n]];
    FOR i:NAT IN [0..h.n) DO h.weight[i] ← 1.0 ENDLOOP;
    h.t ← NEW[RealSequenceRec[h.n]];
    END;

InitialKnots: PUBLIC PROCEDURE [h: Handle, nknots: NAT ← 2] =
    BEGIN
    h.knots ← NEW[RealSequenceRec[nknots]];
    FOR i: NAT IN [0..nknots) DO
        h.knots[i] ← i;
	ENDLOOP;
    END;

XYat: PUBLIC PROCEDURE [h: Handle, t: REAL] RETURNS [f: Complex.Vec] =
    BEGIN
    patchNumber: NAT ← 0;
    interval: REAL;
    IF h=NIL THEN RETURN[[-1,-1]];
    WHILE patchNumber < h.knots.length-2 AND h.knots[patchNumber+1] < t DO
        patchNumber ← patchNumber + 1;
	ENDLOOP;
    interval ← h.knots[patchNumber+1]-h.knots[patchNumber];
    IF interval=0 THEN t←0
    ELSE t←(t-h.knots[patchNumber])/interval;
    {OPEN h.xPatches[patchNumber]; f.x ← ((c3*t+c2)*t+c1)*t+c0};
    {OPEN h.yPatches[patchNumber]; f.y ← ((c3*t+c2)*t+c1)*t+c0};
    END;

ClosestKnot: PUBLIC PROCEDURE [h: Handle, z: Complex.Vec]
    RETURNS [NAT] =
    BEGIN
    dd: REAL ← 10E+20;
    bestk:NAT ← 0;
    FOR i:NAT IN (0..h.knots.length-1) DO
        d: REAL ← Complex.SqrAbs[Complex.Sub[z,[h.xPatches[i].c0,h.yPatches[i].c0]]];
	IF d<dd THEN {bestk←i; dd ← d}
	ENDLOOP;
    RETURN[bestk]
    END;

SamplesToHandle: PROCEDURE [sa: ComplexSequence]
    RETURNS [h: Handle] =
    {h ← NEW[StateRec]; BEGIN OPEN h↑;
    z ← sa;
    n ← sa.length;
    END};

BasisProc: TYPE = PROC [basisIndex: NAT, x: REAL] RETURNS [REAL];

FitOneDimensionalSpline: PROCEDURE
   [X,Y,W: RealProc, l,n: NAT, -- points are numbered [l..n] (that's l, not 1)
    Basis: BasisProc, nbasis: NAT] -- basis functions are numbered [0..nbasis) 
    RETURNS [LinearSystem.ColumnN] =
    BEGIN OPEN LinearSystem;
    b: ColumnN ← NEW[VecSeq[nbasis]];
    c: ColumnN ← NIL;
    A: MatrixN ← NEW[MatrixSeq[nbasis]];
    FOR i: NAT IN [0..nbasis) DO 
      A[i] ← NEW[VecSeq[nbasis]];
      FOR j: NAT IN [0..nbasis) DO A[i][j] ← 0; ENDLOOP;
      b[i] ← 0;
      ENDLOOP;
    FOR k:NAT IN [l..n] DO
        xk:REAL = X[k];
	FOR i:NAT IN [0..nbasis) DO
	    gi:REAL ← Basis[i,xk];
	    IF gi#0 THEN	
	        BEGIN
		FOR j:NAT IN [0..nbasis) DO
	            gj: REAL ← Basis[j,xk];
		    A[i][j] ← A[i][j] + W[k]*gi*gj;
		    ENDLOOP;
		b[i] ← b[i] + W[k]*Y[k]*gi;
		END;
	    ENDLOOP;
	ENDLOOP;
    c ← SolveN[A,b,nbasis!
	Real.RealException => CHECKED {CONTINUE}];
    RETURN[c];
    END;

FindPatches: PROCEDURE [h: Handle, c: LinearSystem.ColumnN]
    RETURNS [C: PatchSequence]=
-- convert the coefficients for the basis functions to cubic patches
    BEGIN OPEN h;
    npatches: NAT ← knots.length-1;
    C ← NEW[PatchSequenceRec[npatches]];
    FOR n: NAT IN [0..npatches) DO
    	C[n] ← [0,0,0,0];
	FOR i: NAT IN [0..c.ncols) DO
--most of these are all 0.
	    temp: Patch ← BasisCoeffs[h,i,n];	
	    IF temp = [0,0,0,0] THEN LOOP;	--for efficiency
	    C[n].c0 ← C[n].c0 + c[i]*temp.c0;
	    C[n].c1 ← C[n].c1 + c[i]*temp.c1;
	    C[n].c2 ← C[n].c2 + c[i]*temp.c2;
	    C[n].c3 ← C[n].c3 + c[i]*temp.c3;
	    ENDLOOP;
	ENDLOOP;
    END; 

EvenL: Patch ← [c0: 0, c1: 0, c2: 3, c3: -2];
EvenR: Patch ← [c0: 1, c1: 0, c2: -3, c3: 2];
OddL: Patch ← [c0: 0, c1: 0, c2: -1, c3: 1];
OddR: Patch ← [c0: 0, c1: 1, c2: -2, c3: 1];

ScalePatch: PROC [p: Patch, r: REAL] RETURNS [Patch] =
    BEGIN OPEN p;
    RETURN[[c0:c0*r, c1:c1*r, c2:c2*r, c3:c3*r]]
    END;

-- compute the value of the basis function i for the variable x
EvalBasis: PROC [h: Handle, i: NAT, x: REAL]
    RETURNS [v: REAL] =
    BEGIN OPEN h;
    basis: Patch;
    k0: NAT ← 0;
    WHILE k0<knots.length-2 AND x>=knots[k0+1] DO k0 ← k0 + 1 ENDLOOP;
    basis ← BasisCoeffs[h,i,k0];	--get the coefficients of the basis in this patch
    {interval: REAL ← knots[k0+1]-knots[k0];
     t: REAL ← IF interval=0 THEN 0 ELSE (x-knots[k0])/interval;
     RETURN[t*(t*(t*basis.c3+basis.c2)+basis.c1)+basis.c0]};
    END;
    
-- return the coefficients of the basis function i inside the specified patch
BasisCoeffs: PROCEDURE [h: Handle, i, patch: NAT]
    RETURNS [p: Patch] =
    BEGIN OPEN h;
    leftHalf: NAT ← patch+1;
    rightHalf: NAT ← patch;
    IF closedCurve AND patch = knots.length-2 THEN leftHalf ← 0;
    p ← SELECT i FROM
	2*rightHalf => EvenR,
	2*rightHalf+1 => ScalePatch[OddR,knots[patch+1]-knots[patch]],
        2*leftHalf => EvenL,
	2*leftHalf+1 => ScalePatch[OddL,knots[patch+1]-knots[patch]],
	ENDCASE => [0,0,0,0];
    END;

ImproveParametricSpline: PUBLIC PROCEDURE[h: Handle] =
    BEGIN
    AdjustTValues[h];
    FitXAndY[h];
    END;

InitialTValues: PUBLIC PROCEDURE [h: Handle] =
    BEGIN OPEN h;
    arcLen: REAL ← 0;
    t[0] ← 0;
    FOR i:NAT IN [1..n) DO
        arcLen ← arcLen + Complex.Abs[Complex.Sub[z[i],z[i-1]]];
	t[i] ← arcLen;
	ENDLOOP;
    FOR i:NAT IN [0..n) DO
        t[i] ← knots[0] + t[i]*(knots[knots.length-1]-knots[0])/arcLen
	ENDLOOP;
    END;

AdjustTValues: PUBLIC PROCEDURE [h: Handle] =
    BEGIN OPEN h↑;
    k0: INTEGER ← 0;
    delta,interval: REAL ← 0;
    npatches: INTEGER ← knots.length-1;
    IF knots=NIL OR t=NIL OR xPatches=NIL OR yPatches=NIL THEN RETURN;
    FOR i: NAT IN [0..n) DO
        WHILE t[i] > knots[k0+1] AND k0 < npatches-1 DO k0 ← k0+1 ENDLOOP;
	WHILE k0>0 AND t[i] <= knots[k0] DO k0 ← k0-1 ENDLOOP;
	interval ← knots[k0+1]-knots[k0];
	IF interval # 0 THEN delta ← ImproveT[(t[i]-knots[k0])/interval,
	    			xPatches[k0],yPatches[k0],z[i].x,z[i].y];
	t[i] ← t[i]+delta*interval;
	IF closedCurve THEN
	    BEGIN
	    WHILE t[i] < knots[0] DO
	        t[i] ← t[i] + (knots[npatches]-knots[0]) ENDLOOP;
	    WHILE t[i] >= knots[npatches] DO
	    	t[i] ← t[i] - (knots[npatches]-knots[0]) ENDLOOP;
	    END;
	ENDLOOP;
--    Sort[t];
    IF NOT closedCurve THEN
        BEGIN
	knots[0] ← t[0];
        knots[npatches] ← t[n-1]
	END;
--    Sort[knots];
    END;

FitXAndY: PUBLIC PROCEDURE [h: Handle] =
    BEGIN OPEN h;
    X:RealProc = {RETURN[z[i].x]};
    Y:RealProc = {RETURN[z[i].y]};
    W:RealProc = {RETURN[IF weight=NIL THEN 1.0 ELSE weight[i]]};
    T:RealProc = {RETURN[t[i]]};
    nbasis: NAT ← 2*knots.length;
    Phi: BasisProc = {RETURN[EvalBasis[h, basisIndex, x]]};
    IF closedCurve THEN nbasis ← nbasis - 2;
    xPatches ← FindPatches
	    [h, FitOneDimensionalSpline[T,X,W,0,n-1,Phi,nbasis]];
    yPatches ← FindPatches
	    [h, FitOneDimensionalSpline[T,Y,W,0,n-1,Phi,nbasis]];
    END;

ImproveT: PROCEDURE [ti: REAL, X, Y: Patch, XI,YI: REAL] RETURNS [delta: REAL] =
    BEGIN
    FX: PROC [t:REAL] RETURNS [REAL] = INLINE
      {RETURN[X.c0 + t*(X.c1 + t*(X.c2 + t*(X.c3)))]};
    DFX: PROC [t:REAL] RETURNS [REAL] = INLINE
      {RETURN[X.c1 + t*(2*X.c2 + t*(3*X.c3))]};
    DDFX: PROC [t:REAL] RETURNS [REAL] = INLINE
      {RETURN[2*X.c2+ t*(2*3*X.c3)]};
    FY: PROC [t:REAL] RETURNS [REAL] = INLINE
      {RETURN[Y.c0 + t*(Y.c1 + t*(Y.c2 + t*(Y.c3)))]};
    DFY: PROC [t:REAL] RETURNS [REAL] = INLINE
      {RETURN[Y.c1 + t*(2*Y.c2 + t*(3*Y.c3))]};
    DDFY: PROC [t:REAL] RETURNS [REAL] = INLINE
      {RETURN[2*Y.c2 + t*(2*3*Y.c3)]};
    derivSqrDist:REAL = (FX[ti]-XI)*DFX[ti] + (FY[ti]-YI)*DFY[ti];
    derivDerivSqrDist:REAL =
        (DFX[ti])*(DFX[ti]) + (FX[ti]-XI)*DDFX[ti] +
        (DFY[ti])*(DFY[ti]) + (FY[ti]-YI)*DDFY[ti];
    IF derivDerivSqrDist # 0 THEN delta ←  -derivSqrDist / derivDerivSqrDist
    ELSE delta ← 0;
    END; 

Sort: PUBLIC PROCEDURE [v: RealSequence] = {
--sorts v sequence in place.  Would be awful except that sequence is mostly ordered already
    l: NAT ← v.length-1;
    nswitches: NAT ← 1;
    UNTIL nswitches=0 DO
        nswitches ← 0;
	FOR i: NAT IN [0..l) DO
	    IF v[i+1] < v[i] THEN {
	         t: REAL ← v[i+1];
	         v[i+1] ← v[i];
	         v[i] ← t;
	         nswitches ← nswitches+1;
	         }
	    ENDLOOP;
	ENDLOOP;
    };

ReParameterize: PUBLIC PROC [p: Patch, t0,t1: REAL, nt0,nt1: REAL] RETURNS[np: Patch] = { OPEN LinearSystem;
    A: Matrix4;
    B: Column4;
    C: Column4;
    F: PROC [t: REAL] RETURNS[REAL] = {RETURN[p.c0 + t*(p.c1 + t*(p.c2 + t*(p.c3)))]};
    DF: PROC [t: REAL] RETURNS[REAL] = {RETURN[p.c1 + t*(2*p.c2 + t*(3*p.c3))]};
    r: REAL ← (t1-t0)/(nt1-nt0);
    A[1] ← [nt1*nt1*nt1, nt1*nt1, nt1, 1];
    A[2] ← [nt0*nt0*nt0, nt0*nt0, nt0, 1];
    A[3] ← [3*nt1*nt1, 2*nt1, 1, 0];
    A[4] ← [3*nt0*nt0, 2*nt0, 1, 0];
    B[1] ← F[t1];
    B[2] ← F[t0];
    B[3] ← r*DF[t1];
    B[4] ← r*DF[t0];
    C ← Solve4[A,B];
    np ← [c3: C[1],c2: C[2],c1: C[3],c0: C[4]];
    };
    
FitOneDimensionalCubic: PUBLIC PROCEDURE [X,Y,W: RealProc, l,n:NAT]
			RETURNS [Patch] =
    BEGIN -- finds cubic f(X) to minimize SUM (f(X[i])-Y[i])↑2 OVER l <= i <= n
    singular:BOOLEAN ← FALSE;
    b,c:LinearSystem.Column4 ← [0,0,0,0];
    A:LinearSystem.Matrix4 ← [[0,0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,0]];
    FOR i:NAT IN [l..n] DO
        xi:REAL = X[i];
        r:REAL ← 1;
	FOR k:NAT IN [1..4] DO
	    s:REAL ← 1;	
	    FOR j:NAT IN [1..4] DO
	        A[k][j] ← A[k][j] + W[i]*r*s;
		s ← s*xi
		ENDLOOP;
	    b[k] ← b[k] + W[i]*Y[i]*r;
	    r ← r*xi
	    ENDLOOP;
	ENDLOOP;
    c ← LinearSystem.Solve4[A,b!
	Real.RealException => CHECKED {singular←TRUE;CONTINUE}];
    IF singular THEN RETURN [[0,1,0,0]]
    ELSE RETURN [[c3:c[4],c2:c[3],c1:c[2],c0:c[1]]]
    END; 

InitialParametricCubic: PUBLIC PROCEDURE[sa: ComplexSequence]
    RETURNS [h: Handle] =
    {h ← SamplesToHandle[sa]; BEGIN OPEN h↑;
    X:RealProc = {RETURN[z[i].x]};
    Y:RealProc = {RETURN[z[i].y]};
    W:RealProc = {RETURN[IF weight=NIL THEN 1.0 ELSE weight[i]]};
    T:RealProc = {RETURN[t[i]]};
    arcLen:REAL ← 0;
    t ← NEW[RealSequenceRec[n]];
    t[0] ← 0;
    FOR i:NAT IN [1..n) DO
        arcLen ← arcLen + Complex.Abs[Complex.Sub[z[i],z[i-1]]];
	t[i] ← arcLen;
	ENDLOOP;
    FOR i:NAT IN [0..n) DO
        t[i] ← t[i] / arcLen
	ENDLOOP;
    knots ← NEW[RealSequenceRec[2]];
    knots[0] ← 0;
    knots[1] ← 1;
    xPatches ← NEW[PatchSequenceRec[1]];
    yPatches ← NEW[PatchSequenceRec[1]];
    xPatches[0] ← FitOneDimensionalCubic[T,X,W,0,n-1];
    yPatches[0] ← FitOneDimensionalCubic[T,Y,W,0,n-1];
    END};

ImproveParametricCubic: PUBLIC PROCEDURE [h: Handle, first: NAT ← 0, last: NAT ← LAST[NAT]] =
    BEGIN OPEN h↑;
    X:RealProc = {RETURN[z[i].x]};
    Y:RealProc = {RETURN[z[i].y]};
    W:RealProc = {RETURN[IF weight=NIL THEN 1.0 ELSE weight[i]]};
    T:RealProc = {RETURN[t[i]]};
    range: REAL;
    IF last>=n THEN last ← n-1;
    range ← t[last]-t[first];
    FOR i:NAT IN [first..last] DO
        t[i] ← t[i] + ImproveT[t[i],xPatches[0],yPatches[0],z[i].x,z[i].y];
	ENDLOOP;
    {t0,tn1:REAL;
     t0 ← t[first];
     tn1 ← t[last];
     IF tn1 # t0 THEN FOR i:NAT IN [first..last] DO
	 t[i] ← (t[i]-t0) * range / (tn1 - t0)
	 ENDLOOP}; 
    xPatches[0] ← FitOneDimensionalCubic[T,X,W,first,last];
    yPatches[0] ← FitOneDimensionalCubic[T,Y,W,first,last];
    END; 

FitParametricCubic: PUBLIC PROCEDURE [h: Handle, first, last: NAT, epsilon: REAL] =
    BEGIN OPEN h;
    X:RealProc = {RETURN[z[i].x]};
    Y:RealProc = {RETURN[z[i].y]};
    W:RealProc = {RETURN[IF weight=NIL THEN 1.0 ELSE weight[i]]};
    T:RealProc = {RETURN[t[i]]};
    arcLen:REAL ← 0;
    error: REAL ← epsilon+1;
    t ← NEW[RealSequenceRec[n]];
    t[0] ← 0;
    FOR i:NAT IN [1..n) DO
        arcLen ← arcLen + Complex.Abs[Complex.Sub[z[i],z[i-1]]];
	t[i] ← arcLen;
	ENDLOOP;
    {t0,tn1:REAL;
     t0 ← t[first];
     tn1 ← t[last];
     IF tn1 # t0 THEN FOR i:NAT IN [first..last] DO
	 t[i] ← (t[i]-t0) / (tn1 - t0)
	 ENDLOOP}; 
    knots ← NEW[RealSequenceRec[2]];
    knots[0] ← 0;
    knots[1] ← 1;
    xPatches ← NEW[PatchSequenceRec[1]];
    yPatches ← NEW[PatchSequenceRec[1]];
    xPatches[0] ← FitOneDimensionalCubic[T,X,W,first,last];
    yPatches[0] ← FitOneDimensionalCubic[T,Y,W,first,last];
    THROUGH [0..400) WHILE error>=epsilon DO
    	    error ← 0;
            FOR i:NAT IN [first..last] DO
	        delta: REAL ← ImproveT[t[i],xPatches[0],yPatches[0],z[i].x,z[i].y];
        	t[i] ← t[i] + delta;
		error ← error + ABS[delta];
		ENDLOOP;
	    {t0,tn1:REAL;
	     t0 ← t[first];
	     tn1 ← t[last];
	     IF tn1 # t0 THEN FOR i:NAT IN [first..last] DO
		 t[i] ← (t[i]-t0) / (tn1 - t0)
		 ENDLOOP}; 
	    xPatches[0] ← FitOneDimensionalCubic[T,X,W,first,last];
	    yPatches[0] ← FitOneDimensionalCubic[T,Y,W,first,last];
	    ENDLOOP;
    END;    
END.