-- 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.