-- LSPieceImpl.mesa
-- Maureen Stone April 29, 1983 2:37 pm  took out an INLINE for spying
-- Michael Plass  August 26, 1982 1:41 pm

DIRECTORY
  Complex,
  Cubic,
  LSPiece,
  Real USING [RealException],
  RealFns USING[SqRt],
  Seq,
  Vector;

LSPieceImpl: PROGRAM
IMPORTS
  Complex, Cubic, Real, Vector, RealFns
EXPORTS LSPiece =
BEGIN OPEN Seq;

SingularSystem: SIGNAL = CODE;

FitPiece: PUBLIC PROCEDURE
    [z: ComplexSequence, t: RealSequence,
    from, thru: NAT,
    eps: REAL _ .00001,
    maxd: REAL _ .00001,
    maxit: NAT _ 500,
    deltat: REAL _ 0.000005,
    initFree, finalFree: BOOLEAN _ FALSE,
    initTangent, finalTangent: Complex.Vec _ [0,0],
    useOldTValues: BOOLEAN _ FALSE]
    RETURNS [b: Cubic.Bezier, err: REAL, iterations: NAT, maxDev: REAL] = {
    f: Cubic.Coeffs; -- the current approximation
    l: NAT _ z.length;
    Wrap: PROC [i: NAT] RETURNS [NAT] = INLINE {RETURN[IF i>=l THEN i-l ELSE i]};
    ArcLengthInitialTValues: PROCEDURE = {
        arcLen: REAL _ 0;
        t[from] _ 0;
        FOR i:NAT _ Wrap[from+1], Wrap[i+1] UNTIL i=Wrap[thru+1] DO
            arcLen _ arcLen + Complex.Abs[Complex.Sub[z[i],z[IF i=0 THEN l-1 ELSE i-1]]];
            t[i] _ arcLen;
            ENDLOOP;
        FOR i:NAT _ Wrap[from+1], Wrap[i+1] UNTIL i=Wrap[thru+1] DO
            t[i] _ t[i]/arcLen
            ENDLOOP;
        };
    Error: PROCEDURE RETURNS [s: REAL] = {
        OPEN f;
        maxDevSqr: REAL _ 0;
        s _ 0;
        FOR i:NAT _ from, Wrap[i+1] DO
            ti:REAL _ t[i];
            x:REAL _ ((c3.x*ti+c2.x)*ti+c1.x)*ti+c0.x;
            y:REAL _ ((c3.y*ti+c2.y)*ti+c1.y)*ti+c0.y;
            zi: Complex.Vec _ z[i];
            d:REAL _ (zi.x-x)*(zi.x-x) + (zi.y-y)*(zi.y-y);
            s _ s+d;
            IF d>maxDevSqr THEN maxDevSqr _ d;
            IF i=thru THEN EXIT
            ENDLOOP;
        maxDev _ RealFns.SqRt[maxDevSqr];
        };
    Adjustment: PROCEDURE [z: Complex.Vec, t: REAL]
        RETURNS [delta:REAL] = INLINE {
        OPEN f;
        x,dx,ddx: REAL;
        y,dy,ddy: REAL;
        derivSqrDist: REAL;
        derivDerivSqrDist: REAL;
        x _ ((c3.x*t+c2.x)*t+c1.x)*t+c0.x;
        dx _ (3*c3.x*t+2*c2.x)*t+c1.x;
        ddx _ 6*c3.x*t+2*c2.x;
        y _ ((c3.y*t+c2.y)*t+c1.y)*t+c0.y;
        dy _ (3*c3.y*t+2*c2.y)*t+c1.y;
        ddy _ 6*c3.y*t+2*c2.y;
        derivSqrDist _ (x-z.x)*dx + (y-z.y)*dy;
        derivDerivSqrDist _ dx*dx + dy*dy + (x-z.x)*ddx + (y-z.y)*ddy;
        delta _ IF derivDerivSqrDist=0 THEN 0 ELSE -derivSqrDist/derivDerivSqrDist;
        };
    AdjustTValues: PROCEDURE RETURNS [maxChange: REAL] =  {
        delta: REAL;
        maxChange _ 0;
        FOR i:NAT _ from, Wrap[i+1] DO
            delta _ Adjustment[z[i],t[i]];
            IF ABS[delta] > maxChange THEN maxChange _ ABS[delta];
            t[i] _ t[i]+delta;
            IF i=thru THEN EXIT
            ENDLOOP;
        };
    Rescale: PROCEDURE = INLINE {
        t0: REAL _ t[from];
        interval: REAL _ t[thru]-t0;
        FOR i:NAT _ from, Wrap[i+1] DO
            t[i] _ (t[i]-t0)/interval;
            IF i=thru THEN EXIT
            ENDLOOP;
        };
    basis: ARRAY [0..8) OF Cubic.Coeffs;
    free: ARRAY [0..8) OF BOOLEAN;
    a: ARRAY [0..8) OF REAL;
    nFree: NAT;
    SetupBasis: PROCEDURE = {
        FOR i:NAT IN [0..8) DO free[i] _ FALSE ENDLOOP;
        basis[0] _ Cubic.BezierToCoeffs[[[1,0],[1,0],[0,0],[0,0]]]; a[0] _ z[from].x;
        basis[1] _ Cubic.BezierToCoeffs[[[0,1],[0,1],[0,0],[0,0]]]; a[1] _ z[from].y;
        IF initFree THEN free[0] _ free[1] _ TRUE;
        IF initTangent=[0,0] THEN {
            basis[2] _ Cubic.BezierToCoeffs[[[0,0],[1,0],[0,0],[0,0]]]; free[2] _ TRUE;
            basis[3] _ Cubic.BezierToCoeffs[[[0,0],[0,1],[0,0],[0,0]]]; free[3] _ TRUE;
            }
        ELSE {
            basis[2] _ Cubic.BezierToCoeffs[[[0,0],initTangent,[0,0],[0,0]]]; free[2] _ TRUE;
            basis[3] _ Cubic.BezierToCoeffs[[[0,0],Complex.Mul[initTangent,[0,1]],[0,0],[0,0]]]; a[3] _ 0; 
            };
        IF finalTangent=[0,0] THEN {
            basis[4] _ Cubic.BezierToCoeffs[[[0,0],[0,0],[1,0],[0,0]]]; free[4] _ TRUE;
            basis[5] _ Cubic.BezierToCoeffs[[[0,0],[0,0],[0,1],[0,0]]]; free[5] _ TRUE;
            }
        ELSE {
            basis[4] _ Cubic.BezierToCoeffs[[[0,0],[0,0],finalTangent,[0,0]]]; free[4] _ TRUE;
            basis[5] _ Cubic.BezierToCoeffs[[[0,0],[0,0],Complex.Mul[finalTangent,[0,1]],[0,0]]]; a[5] _ 0; 
            };
        basis[6] _ Cubic.BezierToCoeffs[[[0,0],[0,0],[1,0],[1,0]]]; a[6] _ z[thru].x;
        basis[7] _ Cubic.BezierToCoeffs[[[0,0],[0,0],[0,1],[0,1]]]; a[7] _ z[thru].y;
        IF finalFree THEN free[6] _ free[7] _ TRUE;
        nFree _ 0; FOR i:NAT IN [0..8) DO IF free[i] THEN nFree _ nFree + 1 ENDLOOP;
        };
    EvalBasis: PROCEDURE [i: NAT, t: REAL] RETURNS [Complex.Vec] = {
        b: Cubic.Coeffs _ basis[i];
        RETURN[[
            ((b.c3.x*t+b.c2.x)*t+b.c1.x)*t+b.c0.x,
            ((b.c3.y*t+b.c2.y)*t+b.c1.y)*t+b.c0.y
            ]]
        };
    rightSide: Column;
    leftSide: Matrix;
    unknown: Column;
    arrayStorage: ARRAY [0..8*(8+1+1)) OF REAL;
    descriptorStorage: ARRAY [0..8) OF Row;
    AllocateArrayDescriptors: PROCEDURE[n: NAT] = {
        p: LONG POINTER;
        IF n>8 THEN ERROR;
        p _ @descriptorStorage;
        leftSide _ DESCRIPTOR[p, n];
        p _ @arrayStorage;
        FOR i: NAT IN [0..n) DO
            leftSide[i] _ DESCRIPTOR[p, n];
            p _ p + n*SIZE[REAL];
            ENDLOOP;
        rightSide _ DESCRIPTOR[p, n];
        p _ p + n*SIZE[REAL];
        unknown _ DESCRIPTOR[p, n];
        p _ p + n*SIZE[REAL];
        };
    SolveCurve: PROCEDURE = {
        singular: BOOLEAN _ FALSE;
        FOR i: NAT IN [0..nFree) DO
            FOR j: NAT IN [0..nFree) DO leftSide[i][j] _ 0 ENDLOOP;
            rightSide[i] _ 0;
            ENDLOOP;
        FOR k: NAT _ from, Wrap[k+1] DO
            tk: REAL = t[k];
            row: NAT _ 0;
            FOR i: NAT IN [0..8) DO IF free[i] THEN {
                gi: Complex.Vec _ EvalBasis[i,tk];
                column: NAT _ 0;
                FOR j: NAT IN [0..8) DO
                    gj: Complex.Vec _ EvalBasis[j,tk];
                    IF free[j] THEN
                        leftSide[row][column] _ leftSide[row][column] + Vector.Dot[gi,gj]
                    ELSE
                        rightSide[row] _ rightSide[row] - a[j]*Vector.Dot[gi,gj];
                    IF free[j] THEN column _ column + 1;
                    ENDLOOP;
                rightSide[row] _ rightSide[row] + Vector.Dot[z[k],gi];
                row _ row + 1;
                };
                ENDLOOP;
            IF k=thru THEN EXIT
            ENDLOOP;
        SolveLinearSystem[leftSide, rightSide, nFree, unknown!
            Real.RealException => CHECKED {singular _ TRUE; CONTINUE}];
        IF singular THEN {
            IF paranoidAboutSingularSystems THEN SIGNAL SingularSystem;
            singularSystemCount _ singularSystemCount + 1;
            FOR i:NAT IN [0..8) DO
                IF free[i] THEN a[i] _ 0;
                ENDLOOP; 
            }
        ELSE {
            row: NAT _ 0;
            FOR i:NAT IN [0..8) DO
                IF free[i] THEN {a[i] _ unknown[row]; row _ row + 1};
                ENDLOOP;
            };
        };
    FindCoeffs: PROCEDURE = {
        f _ [[0,0],[0,0],[0,0],[0,0]];
        FOR i:NAT IN [0..8) DO
            f _ [Complex.Add[f.c0,Vector.Mul[basis[i].c0,a[i]]],
                 Complex.Add[f.c1,Vector.Mul[basis[i].c1,a[i]]],
                 Complex.Add[f.c2,Vector.Mul[basis[i].c2,a[i]]],
                 Complex.Add[f.c3,Vector.Mul[basis[i].c3,a[i]]]];
            ENDLOOP;
        };

    olderr: REAL _ 1.0E+20;
    IF NOT useOldTValues THEN ArcLengthInitialTValues;
    SetupBasis;
    AllocateArrayDescriptors[nFree];
    FOR iterations IN [0..maxit) DO
        IF initFree OR finalFree THEN Rescale;
        SolveCurve;
        FindCoeffs;
        err _ Error[];
        IF err>olderr OR err<eps OR maxDev<maxd THEN EXIT;
        olderr _ err;
        IF AdjustTValues[]<deltat THEN EXIT;
        ENDLOOP;
    b _ Cubic.CoeffsToBezier[f];
    };

Matrix: TYPE = LONG DESCRIPTOR FOR ARRAY OF Row;
Row: TYPE = LONG DESCRIPTOR FOR ARRAY OF REAL;
Column: TYPE = LONG DESCRIPTOR FOR ARRAY OF REAL;

SolveLinearSystem: PROCEDURE [A: Matrix, b: Column, n: NAT, x: Column] = {
    -- solve Ax=b by Gaussian Elimination
    FOR i: NAT IN [0..n) DO
        bestk: NAT _ i;
        FOR k: NAT IN [i..n) DO
            IF ABS[A[k][i]] > ABS[A[bestk][i]] THEN bestk _ k;
            ENDLOOP;
        {t: Row _ A[i]; A[i] _ A[bestk]; A[bestk] _ t};
        {t: REAL _ b[i]; b[i] _ b[bestk]; b[bestk] _ t};
        FOR k: NAT IN (i..n) DO
            IF A[k][i]#0 THEN {
                r: REAL = A[k][i]/A[i][i]; -- Singular A causes Real.RealException = divide by zero
                FOR j: NAT IN [i..n) DO
                    A[k][j] _ A[k][j] - A[i][j]*r
                    ENDLOOP;
                b[k] _ b[k] - b[i]*r
                };
            ENDLOOP;
        ENDLOOP;
    -- Now A is upper-triangular, so back substitute
    FOR i: NAT IN [0..n) DO x[i] _ 0; ENDLOOP;
    FOR i: NAT DECREASING IN [0..n) DO
        xi: REAL _ b[i];
        FOR j: NAT IN (i..n) DO
            xi _ xi - A[i][j]*x[j];
            ENDLOOP;
        x[i] _ xi / A[i][i];
        ENDLOOP;   
    };

paranoidAboutSingularSystems: BOOLEAN _ FALSE;
singularSystemCount: INT _ 0;

END.

Michael Plass  August 18, 1982 10:03 am: Switched to array descriptors to eliminate excess allocations

Michael Plass  August 19, 1982 1:17 pm: Cleaned up error calculation.

Michael Plass  August 26, 1982 1:41 pm: Allowed the SingularSystem signal to be disabled.