
    <<LSPieceImpl.mesa>>
        <<Copyright © 1985 by Xerox Corporation.  All rights reserved.>>
        <<Wyatt, September 5, 1985 1:30:37 pm PDT>>
        <<Stone, October 2, 1987 4:33:48 pm PDT>>
        <<Plass, October 12, 1987 5:29:39 pm PDT>>
        <<Bier, February 6, 1989 5:44:05 pm PST>>

DIRECTORY
CedarProcess, Complex, Cubic2, Cubic2Extras, CubicPathsExtras, Lines2d, LSPiece, LSPieceExtras, Real, RealFns, Seq, Vector, 
Vectors2d;

    LSPieceImpl: CEDAR PROGRAM
IMPORTS CedarProcess, Complex, Cubic2, Cubic2Extras, CubicPathsExtras, Lines2d, Real, RealFns, Vector, Vectors2d
    EXPORTS LSPiece, LSPieceExtras
    = BEGIN

    ComplexSequence: TYPE = Seq.ComplexSequence;
    Metrics: TYPE = LSPiece.Metrics;
    RealSequence: TYPE = Seq.RealSequence;
    SingularSystem: SIGNAL = CODE;
    useAve: BOOLEAN _ FALSE;
    errInc: BOOLEAN _ TRUE;    --make the error exit on increasing error conditional
    onlyOnMaxItr: BOOLEAN _ FALSE;    --an extreme case, for instrumentation
    atLeastOnce: BOOLEAN _ TRUE;    --initial approximation is often bad

    FitPiece: PUBLIC PROC
        [z: ComplexSequence, t: RealSequence,
        metrics: LSPiece.Metrics,
        from, thru: NAT,
        initFree, finalFree: BOOLEAN _ FALSE,
        initTangent, finalTangent: Complex.VEC _ [0,0],
        useOldTValues: BOOLEAN _ FALSE]
        RETURNS [b: Cubic2.Bezier, err: REAL, iterations: INT, maxDev: REAL, indexOfMaxDev: NAT] = TRUSTED {
        f: Cubic2.Coeffs; -- the current approximation
        l: NAT _ z.length;
        nSamples: NAT _ (IF from>thru THEN from-thru ELSE z.length-from+thru)+1;
        Wrap: PROC [i: NAT] RETURNS [NAT] = TRUSTED INLINE {RETURN[IF i>=l THEN i-l ELSE i]};
        ArcLengthInitialTValues: PROC = TRUSTED {
            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: PROC RETURNS [s: REAL] = TRUSTED {
            OPEN f;
            maxDevSqr: REAL _ 0;
            s _ 0;
            indexOfMaxDev _ from;
            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; indexOfMaxDev _ i};
                IF i=thru THEN EXIT
                ENDLOOP;
            maxDev _ RealFns.SqRt[maxDevSqr];
            IF useAve THEN s _ s/nSamples;
            };
        Adjustment: PROC [z: Complex.VEC, t: REAL]
            RETURNS [delta:REAL] = TRUSTED 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: PROC RETURNS [maxChange: REAL] = TRUSTED {
            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: PROC = TRUSTED 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 Cubic2.Coeffs;
        free: ARRAY [0..8) OF BOOLEAN;
        a: ARRAY [0..8) OF REAL;
        nFree: NAT;
        SetupBasis: PROC = TRUSTED {
            FOR i:NAT IN [0..8) DO free[i] _ FALSE ENDLOOP;
            basis[0] _ Cubic2.BezierToCoeffs[[[1,0],[1,0],[0,0],[0,0]]]; a[0] _ z[from].x;
            basis[1] _ Cubic2.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] _ Cubic2.BezierToCoeffs[[[0,0],[1,0],[0,0],[0,0]]]; free[2] _ TRUE;
                basis[3] _ Cubic2.BezierToCoeffs[[[0,0],[0,1],[0,0],[0,0]]]; free[3] _ TRUE;
                }
            ELSE {
                basis[2] _ Cubic2.BezierToCoeffs[[[0,0],initTangent,[0,0],[0,0]]]; free[2] _ TRUE;
                basis[3] _ Cubic2.BezierToCoeffs[[[0,0],Complex.Mul[initTangent,[0,1]],[0,0],[0,0]]]; a[3] _ 0; 
                };
            IF finalTangent=[0,0] THEN {
                basis[4] _ Cubic2.BezierToCoeffs[[[0,0],[0,0],[1,0],[0,0]]]; free[4] _ TRUE;
                basis[5] _ Cubic2.BezierToCoeffs[[[0,0],[0,0],[0,1],[0,0]]]; free[5] _ TRUE;
                }
            ELSE {
                basis[4] _ Cubic2.BezierToCoeffs[[[0,0],[0,0],finalTangent,[0,0]]]; free[4] _ TRUE;
                basis[5] _ Cubic2.BezierToCoeffs[[[0,0],[0,0],Complex.Mul[finalTangent,[0,1]],[0,0]]]; a[5] _ 0; 
                };
            basis[6] _ Cubic2.BezierToCoeffs[[[0,0],[0,0],[1,0],[1,0]]]; a[6] _ z[thru].x;
            basis[7] _ Cubic2.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: PROC [i: NAT, t: REAL] RETURNS [Complex.VEC] = TRUSTED {
            b: Cubic2.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: PROC[n: NAT] = TRUSTED {
            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: PROC = TRUSTED {
            singular: BOOLEAN _ FALSE;
            SetupSystem: PROC = TRUSTED {
                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;
                };
            SetupSystem[ ! Real.RealException => CHECKED {singular _ TRUE; CONTINUE}];
            IF NOT singular THEN 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: PROC = CHECKED {
            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..metrics.maxItr) DO
            IF initFree OR finalFree THEN Rescale;
            SolveCurve;
            CedarProcess.CheckAbort[];    --once per iteration
            FindCoeffs;
            err _ Error[];
            <<make the error exit on increasing error conditional>>
            IF NOT onlyOnMaxItr AND (atLeastOnce AND iterations > 0)  AND
                ((errInc AND err>olderr) OR err<metrics.sumErr OR maxDev<metrics.maxDev) THEN EXIT;
            olderr _ err;
            IF AdjustTValues[] NOT IN [metrics.deltaT..50] AND NOT onlyOnMaxItr THEN EXIT;
                <<Bail out early if the t values change radically>>
            ENDLOOP;
        b _ Cubic2.CoeffsToBezier[f];
        };

    FitPiece2: PUBLIC PROC
        [z: ComplexSequence, t: RealSequence,
        metrics: Metrics,
        from, thru: NAT,
        initFree, finalFree: BOOLEAN _ FALSE,
        initTangent, finalTangent: Complex.VEC _ [0,0],
        useOldTValues: BOOLEAN _ FALSE,
        someInterp: BOOLEAN _ FALSE,
        realSample: LSPieceExtras.RealSample _ NIL]
        RETURNS [b: Cubic2.Bezier, err: REAL, iterations: INT, maxDev: REAL, indexOfMaxDev: NAT] = TRUSTED {
        f: Cubic2.Coeffs; -- the current approximation
        l: NAT _ z.length;
        nSamples: NAT _ (IF from>thru THEN from-thru ELSE z.length-from+thru)+1;
        Wrap: PROC [i: NAT] RETURNS [NAT] = CHECKED INLINE {RETURN[IF i>=l THEN i-l ELSE i]};
        ArcLengthInitialTValues: PROC = CHECKED {
            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: PROC RETURNS [s: REAL] = CHECKED {
            OPEN f;
            maxDevSqr: REAL _ 0;
            s _ 0;
            indexOfMaxDev _ from;
            FOR i:NAT _ from, Wrap[i+1] DO
                IF NOT someInterp OR realSample[i] THEN {  -- if not a realSample then error does not matter.
                    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; indexOfMaxDev _ i};
                    };
                IF i=thru THEN EXIT
                ENDLOOP;
            maxDev _ RealFns.SqRt[maxDevSqr];
            IF useAve THEN s _ s/nSamples; -- nSamples may be wrong if interp.
            };
        Adjustment: PROC [z: Complex.VEC, t: REAL]
            RETURNS [delta:REAL] = CHECKED 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: PROC RETURNS [maxChange: REAL] = CHECKED {
            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: PROC = CHECKED 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 Cubic2.Coeffs;
        free: ARRAY [0..8) OF BOOLEAN;
        a: ARRAY [0..8) OF REAL;
        nFree: NAT;
        SetupBasis: PROC = CHECKED {
            FOR i:NAT IN [0..8) DO free[i] _ FALSE ENDLOOP;
            basis[0] _ Cubic2.BezierToCoeffs[[[1,0],[1,0],[0,0],[0,0]]]; a[0] _ z[from].x;
            basis[1] _ Cubic2.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] _ Cubic2.BezierToCoeffs[[[0,0],[1,0],[0,0],[0,0]]]; free[2] _ TRUE;
                basis[3] _ Cubic2.BezierToCoeffs[[[0,0],[0,1],[0,0],[0,0]]]; free[3] _ TRUE;
                }
            ELSE {
                basis[2] _ Cubic2.BezierToCoeffs[[[0,0],initTangent,[0,0],[0,0]]]; free[2] _ TRUE;
                basis[3] _ Cubic2.BezierToCoeffs[[[0,0],Complex.Mul[initTangent,[0,1]],[0,0],[0,0]]]; a[3] _ 0; 
                };
            IF finalTangent=[0,0] THEN {
                basis[4] _ Cubic2.BezierToCoeffs[[[0,0],[0,0],[1,0],[0,0]]]; free[4] _ TRUE;
                basis[5] _ Cubic2.BezierToCoeffs[[[0,0],[0,0],[0,1],[0,0]]]; free[5] _ TRUE;
                }
            ELSE {
                basis[4] _ Cubic2.BezierToCoeffs[[[0,0],[0,0],finalTangent,[0,0]]]; free[4] _ TRUE;
                basis[5] _ Cubic2.BezierToCoeffs[[[0,0],[0,0],Complex.Mul[finalTangent,[0,1]],[0,0]]]; a[5] _ 0; 
                };
            basis[6] _ Cubic2.BezierToCoeffs[[[0,0],[0,0],[1,0],[1,0]]]; a[6] _ z[thru].x;
            basis[7] _ Cubic2.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: PROC [i: NAT, t: REAL] RETURNS [Complex.VEC] = CHECKED {
            b: Cubic2.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: PROC[n: NAT] = TRUSTED {
            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: PROC = TRUSTED {
            singular: BOOLEAN _ FALSE;
            SetupSystem: PROC = TRUSTED {
                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;
                };
            SetupSystem[ ! Real.RealException => CHECKED {singular _ TRUE; CONTINUE}];
            IF NOT singular THEN 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: PROC = CHECKED {
            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..metrics.maxItr) DO
            IF initFree OR finalFree THEN Rescale;
            SolveCurve;
            CedarProcess.CheckAbort[];    --once per iteration
            FindCoeffs;
            err _ Error[];
            <<make the error exit on increasing error conditional>>
            IF NOT onlyOnMaxItr AND (atLeastOnce AND iterations > 0)  AND
                ((errInc AND err>olderr) OR err<metrics.sumErr OR maxDev<metrics.maxDev) THEN EXIT;
            olderr _ err;
            IF AdjustTValues[] NOT IN [metrics.deltaT..50] AND NOT onlyOnMaxItr THEN EXIT;
                <<Bail out early if the t values change radically>>
            ENDLOOP;
        b _ Cubic2.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: PROC [A: Matrix, b: Column, n: NAT, x: Column] = TRUSTED {
        <<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;

    ComputeCurvature: PUBLIC SAFE PROC [farCP, nearCP, joint: Complex.VEC, jointFirst: BOOL] RETURNS [REAL] ~ {
        jointLine: Lines2d.Line;
        aDistSq, hDist: REAL;
        IF jointFirst THEN jointLine _ Lines2d.LineFromPoints[joint, nearCP]
        ELSE jointLine _ Lines2d.LineFromPoints[nearCP, joint];
        hDist _ Lines2d.SignedLineDistance[farCP, jointLine];
        aDistSq _ Vectors2d.DistanceSquared[nearCP, joint];
        RETURN[ 4/3.0 * hDist/aDistSq];
        };

    CurvatureAtPt: PUBLIC SAFE PROC [bezier: Cubic2.Bezier, pt: Complex.VEC] RETURNS [REAL] ~ TRUSTED{
        tBezier: Cubic2.Bezier;
        IF pt = bezier.b0 THEN RETURN[ComputeCurvature[bezier.b2, bezier.b1, bezier.b0, TRUE]];
        IF pt = bezier.b3 THEN RETURN[ComputeCurvature[bezier.b1, bezier.b2, bezier.b3, FALSE]];
        [tBezier, ----] _ Cubic2Extras.AlphaSplit[bezier, CubicPathsExtras.GetParam[bezier, pt]];
        RETURN[ComputeCurvature[tBezier.b1, tBezier.b2, tBezier.b3, FALSE]];
        };

    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.