-- LSCurveImpl.mesa
-- Last edited by Michael Plass November 29, 1982 9:52 am
-- Michael Plass and Maureen Stone Oct-81

DIRECTORY
  Cubic,
  Complex,
  LinearSystem,
  LSCurve,
  PiecewiseCubic,
  Real USING [RealException],
  Seq,
  Vector;

LSCurveImpl: PROGRAM IMPORTS Complex, Cubic, LinearSystem, PiecewiseCubic, Real, Vector EXPORTS LSCurve =
BEGIN OPEN Seq;

RealProc: TYPE = PROCEDURE[i:NAT] RETURNS [REAL];
PointNumber: TYPE = NAT;
Patch: TYPE = LSCurve.Patch;
PatchSequence: TYPE = LSCurve.PatchSequence;
PatchSequenceRec: TYPE = LSCurve.PatchSequenceRec;
SplineBasis: TYPE = LSCurve.SplineBasis;
SplineBasisRec: TYPE = LSCurve.SplineBasisRec;
SplineFunction: TYPE = LSCurve.SplineFunction;

Handle: TYPE = LSCurve.Handle;
StateRec: TYPE = LSCurve.StateRec;

Create: PUBLIC PROCEDURE [sa: Seq.ComplexSequence] RETURNS [h: Handle] =
    BEGIN
    h _ SamplesToHandle[sa];
    h.weight _ NEW[RealSequenceRec[h.z.length]];
    FOR i:NAT IN [0..h.z.length) DO h.weight[i] _ 1.0 ENDLOOP;
    h.t _ NEW[RealSequenceRec[h.z.length]];
    h.oldt _ NEW[RealSequenceRec[h.z.length]];
    h.oldoldt _ NEW[RealSequenceRec[h.z.length]];
    END;

XYat: PUBLIC PROCEDURE [h: Handle, t: REAL] RETURNS [f: Complex.Vec] =
    BEGIN
    patchNumber: NAT _ 0;
    ValidateCache[h];
    f.x _ PiecewiseCubic.Eval[h.patchCache.x,t];
    f.y _ PiecewiseCubic.Eval[h.patchCache.y,t];
    END;

SamplesToHandle: PROCEDURE [s: Seq.ComplexSequence]
    RETURNS [h: Handle] =
    {h _ NEW[StateRec]; BEGIN OPEN h^;
    z _ s;
    l _ 0;
    n _ s.length-1;
    END};

CountTrues: PROCEDURE [b: BooleanSequence] RETURNS [count:NAT] =
    BEGIN
    count _ 0;
    FOR i:NAT IN [0..b.length) DO IF b[i] THEN count _ count + 1 ENDLOOP;
    END;

EvalBasis: PUBLIC PROCEDURE [h: Handle, i: NAT, t: REAL]
    RETURNS [z: Complex.Vec] = INLINE
    {OPEN h.splineBasis[i]; RETURN [[PiecewiseCubic.Eval[x,t],PiecewiseCubic.Eval[y,t]]]};

SolveCurve: PUBLIC PROCEDURE [h: Handle] =
    BEGIN OPEN h,LinearSystem;
    nFree: NAT _ CountTrues[free];
    b: ColumnN _ NEW[VecSeq[nFree]];
    c: ColumnN _ NIL;
    A: MatrixN _ NEW[MatrixSeq[nFree]];
    FOR i: NAT IN [0..nFree) DO 
      A[i] _ NEW[VecSeq[nFree]];
      FOR j: NAT IN [0..nFree) DO A[i][j] _ 0 ENDLOOP;
      b[i] _ 0;
      ENDLOOP;
    FOR k:NAT IN [l..n] DO
        tk:REAL = t[k];
        row: NAT _ 0;
        FOR i:NAT IN [0..a.length) DO IF free[i] THEN
            BEGIN
            gi:Complex.Vec _ EvalBasis[h,i,tk];
            IF gi#[0,0] THEN        
                BEGIN
                column: NAT _ 0;
                FOR j:NAT IN [0..a.length) DO
                    gj: Complex.Vec _ EvalBasis[h,j,tk];
                    IF free[j] THEN
                        A[row][column] _ A[row][column] + weight[k]*Vector.Dot[gi,gj]
                    ELSE
                        b[row] _ b[row] - weight[k]*a[j]*Vector.Dot[gi,gj];
                    IF free[j] THEN column _ column + 1;
                    ENDLOOP;
                END;
            b[row] _ b[row] + weight[k]*Vector.Dot[z[k],gi];
            row _ row + 1;
            END ENDLOOP;
        ENDLOOP;
    c _ SolveN[A,b,nFree!
        Real.RealException => CHECKED {CONTINUE}];
    IF c#NIL THEN
        BEGIN
        row: NAT _ 0;
        FOR i:NAT IN [0..a.length) DO
            IF free[i] THEN a[i] _ c[row];
            IF free[i] THEN row _ row + 1;
            ENDLOOP;
        patchCacheValid _ FALSE;
        END;
    END;

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

UnitInitialTValues: PUBLIC PROCEDURE [h: Handle] =
    BEGIN OPEN h;
    IF z.length < 2 THEN RETURN;
    FOR i:NAT IN [0..z.length) DO
        t[i] _ i*1.0/(z.length-1);
        ENDLOOP;
    END;

AngleInitialTValues: PUBLIC PROCEDURE [h: Handle] =
    BEGIN OPEN h;
    twopi: REAL = 2*3.1415926;
    m: Complex.Vec _ [0,0];
    FOR i:NAT IN [0..z.length) DO
        m _ Complex.Add[m,z[i]];
        ENDLOOP;
    m _ Vector.Div[m,z.length];
    FOR i:NAT IN [0..z.length) DO
        t[i] _ Complex.Arg[Complex.Sub[z[i],m]]/twopi;
        WHILE t[i] < 0 DO t[i] _ t[i] + 1 ENDLOOP;
        WHILE t[i] > 1 DO t[i] _ t[i] - 1 ENDLOOP;
        ENDLOOP;
    END;

AdjustTValues: PUBLIC PROCEDURE [h: Handle] RETURNS [maxChange: REAL] =
    BEGIN OPEN h;
    delta: REAL;
    {newt: RealSequence _ oldoldt; oldoldt _ oldt; oldt _ t; t _ newt};
    ValidateCache[h];
    maxChange _ 0;
    FOR i: NAT IN [l..n] DO
        delta _ Adjustment[z[i],patchCache,oldt[i]];
        IF ABS[delta] > maxChange THEN maxChange _ ABS[delta];
        t[i] _ oldt[i]+delta;
        IF closedCurve THEN
            BEGIN
            WHILE t[i] < 0 DO t[i] _ t[i] + 1 ENDLOOP;
            WHILE t[i] > 1 DO t[i] _ t[i] - 1 ENDLOOP;
            END
        ENDLOOP;
    IF initialEndFree THEN
        BEGIN
        t0: REAL _ t[l];
        FOR i: NAT IN [l..n] DO
            t[i] _ t[i] - t0;
            ENDLOOP;
        END;
    IF finalEndFree THEN
        BEGIN
        interval: REAL _ t[n];
        FOR i: NAT IN [l..n] DO
            t[i] _ t[i]/interval;
            ENDLOOP;
        END;
    END;

Adjustment: PROCEDURE [z: Complex.Vec, f: SplineFunction, t: REAL]
    RETURNS [delta:REAL] =
    BEGIN
--    epsilon: REAL _ 1.0/8388608;
    x,dx,ddx,dddx: REAL;
    y,dy,ddy,dddy: REAL;
    lo,hi: REAL;
    derivSqrDist: REAL;
    derivDerivSqrDist: REAL;
    [x,dx,ddx,dddx,lo,hi] _ PiecewiseCubic.EvalAll[f.x, t];
    [y,dy,ddy,dddy,lo,hi] _ PiecewiseCubic.EvalAll[f.y, t];
    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;
--    IF t+delta>hi THEN delta _ hi+epsilon-t;
--    IF t+delta<lo THEN delta _ lo-epsilon-t;
    END;

Sqr: PROC [x:REAL] RETURNS [REAL] = INLINE{RETURN[x*x]};

AccelTValues: PUBLIC PROCEDURE [h: Handle] RETURNS [r, maxChange: REAL] =
    BEGIN OPEN h;
    sqrOldDelta, deltaDotOldDelta: REAL _ 0;
    r _ 0;
    maxChange _ AdjustTValues[h];
    SolveCurve[h]; maxChange _ AdjustTValues[h];
    FOR i: NAT IN [l..n] DO
        sqrOldDelta _ sqrOldDelta + Sqr[oldt[i]-oldoldt[i]];
        deltaDotOldDelta _ deltaDotOldDelta + (t[i]-oldt[i])*(oldt[i]-oldoldt[i]);
        ENDLOOP;
    IF sqrOldDelta=0 THEN RETURN;
    r _ deltaDotOldDelta/sqrOldDelta;
    IF ABS[r]>=1 THEN RETURN;
    maxChange _ 0;
    FOR i: NAT IN [l..n] DO
        delta: REAL _ (t[i] - oldt[i])/(1-r);
        t[i] _ oldt[i] + delta;
        IF ABS[delta] > maxChange THEN maxChange _ ABS[delta];
        ENDLOOP;
    END;

PatchesOf: PUBLIC PROCEDURE [h: Handle] RETURNS [x, y: PatchSequence, knots: RealSequence] =
    BEGIN
    i,nPatches: NAT _ 0;
    CountPatches: PiecewiseCubic.PieceProc = {nPatches _ nPatches+1};
    StorePatch: PiecewiseCubic.PieceProc =
        BEGIN
        interval: REAL _ p.domainEnd-p.domainStart;
        b: Cubic.Bezier _
           [b0: [p.initValue, q.initValue],
            b1: [p.initValue + p.initSlope*interval/3, q.initValue + q.initSlope*interval/3],
            b2: [p.finalValue - p.finalSlope*interval/3, q.finalValue - q.finalSlope*interval/3],
            b3: [p.finalValue, q.finalValue]];
        c: Cubic.Coeffs _ Cubic.BezierToCoeffs[b];
        x[i] _ [c0:c.c0.x, c1:c.c1.x, c2:c.c2.x, c3:c.c3.x];
        y[i] _ [c0:c.c0.y, c1:c.c1.y, c2:c.c2.y, c3:c.c3.y];
        knots[i] _ p.domainStart;
        knots[i+1] _ p.domainEnd;
        i _ i + 1;
        END;
    ValidateCache[h];
    PiecewiseCubic.EnumerateCommonPieces[h.patchCache.x,h.patchCache.y,CountPatches];
    x _ NEW[PatchSequenceRec[nPatches]];
    y _ NEW[PatchSequenceRec[nPatches]];
    knots _ NEW[RealSequenceRec[nPatches+1]];
    PiecewiseCubic.EnumerateCommonPieces[h.patchCache.x,h.patchCache.y,StorePatch];
    END;

ValidateCache: PROCEDURE [h: Handle]  =
    BEGIN OPEN h;
    IF patchCacheValid THEN RETURN;
    patchCache _ [PiecewiseCubic.Zero[],PiecewiseCubic.Zero[]];
    FOR i:NAT DECREASING IN [0..a.length) DO
            patchCache _ [PiecewiseCubic.Combine[1,patchCache.x,a[i],splineBasis[i].x],
                      PiecewiseCubic.Combine[1,patchCache.y,a[i],splineBasis[i].y]]
        ENDLOOP;
    patchCacheValid_TRUE;
    END;

PointSlopeBasis: PUBLIC PROCEDURE [h: Handle, b: Cubic.Bezier] =
    BEGIN OPEN h,PiecewiseCubic;
    vx,vy: REAL;
    closedCurve _ FALSE;
    initialEndFree _ FALSE;
    finalEndFree _ FALSE;
    patchCacheValid _ FALSE;
    splineBasis _ NEW[SplineBasisRec[8]];
    a _ NEW[RealSequenceRec[8]];
    free _ NEW[BooleanSequenceRec[8]];
    -- (a0,a1) and (a6,a7) are the positions of the endpoints
    -- a2 is the velocity component parallel to b1-b0
    -- a3 is the velocity component perpendicular to b1-b0
    -- a4 is the velocity component parallel to b2-b3
    -- a5 is the velocity component perpendicular to b2-b3
    splineBasis[0] _ [Piece[0,1,1,0,0,0],Zero[]]; a[0] _ b.b0.x;
    splineBasis[1] _ [Zero[],Piece[0,1,1,0,0,0]]; a[1] _ b.b0.y;
    [[vx,vy]] _ Complex.Sub[b.b1,b.b0];
    splineBasis[2] _ [Combine[3*vx,Piece[0,1,0,1,0,0],0,Zero[]],
                          Combine[3*vy,Piece[0,1,0,1,0,0],0,Zero[]]]; a[2] _ 1;
    splineBasis[3] _ [Combine[-3*vy,Piece[0,1,0,1,0,0],0,Zero[]],
                          Combine[3*vx,Piece[0,1,0,1,0,0],0,Zero[]]];  a[3] _ 0;
    [[vx,vy]] _ Complex.Sub[b.b3,b.b2];
    splineBasis[4] _ [Combine[3*vx,Piece[0,1,0,0,1,0],0,Zero[]],
                          Combine[3*vy,Piece[0,1,0,0,1,0],0,Zero[]]]; a[4] _ 1;
    splineBasis[5] _ [Combine[-3*vy,Piece[0,1,0,0,1,0],0,Zero[]],
                          Combine[3*vx,Piece[0,1,0,0,1,0],0,Zero[]]];  a[5] _ 0;
    splineBasis[6] _ [Piece[0,1,0,0,0,1],Zero[]]; a[6] _ b.b3.x;
    splineBasis[7] _ [Zero[],Piece[0,1,0,0,0,1]]; a[7] _ b.b3.y;
    FOR i:NAT IN [0..8) DO
        free[i] _ FALSE;
        ENDLOOP;
    END;

SmoothBasis: PUBLIC PROCEDURE [h: Handle, nKnots: NAT] =
    BEGIN OPEN h;
    closedCurve _ TRUE;
    initialEndFree _ FALSE;
    finalEndFree _ FALSE;
    patchCacheValid _ FALSE;
    splineBasis _ NEW[SplineBasisRec[4*nKnots]];
    a _ NEW[RealSequenceRec[4*nKnots]];
    free _ NEW[BooleanSequenceRec[4*nKnots]];
    FOR i:NAT IN [0..nKnots) DO
        even: PiecewiseCubic.Handle _ EvenBasis[i*1.0/nKnots,1.0/nKnots];
        odd: PiecewiseCubic.Handle _ OddBasis[i*1.0/nKnots,1.0/nKnots];
        splineBasis[4*i] _ [even,PiecewiseCubic.Zero[]];
        splineBasis[4*i+1] _ [PiecewiseCubic.Zero[],even];
        splineBasis[4*i+2] _ [odd,PiecewiseCubic.Zero[]];
        splineBasis[4*i+3] _ [PiecewiseCubic.Zero[],odd];
        ENDLOOP;
    FOR i:NAT IN [0..4*nKnots) DO
        a[i] _ 0;
        free[i] _ TRUE;
        ENDLOOP;
    END;

EvenBasis: PROCEDURE [center, radius: REAL] RETURNS [PiecewiseCubic.Handle] =
    BEGIN OPEN PiecewiseCubic;
    l: REAL _ center-radius;
    IF l<0 THEN l_l+1;
    RETURN [Combine[1,Piece[l,l+radius,0,0,0,1],
                        1,Piece[center,center+radius,1,0,0,0]]]
    END;

OddBasis: PROCEDURE [center, radius: REAL] RETURNS [PiecewiseCubic.Handle] =
    BEGIN OPEN PiecewiseCubic;
    l: REAL _ center-radius;
    IF l<0 THEN l_l+1;
    RETURN [Combine[1,Piece[l,l+radius,0,0,1,0],
                        1,Piece[center,center+radius,0,1,0,0]]]
    END;

BSplineBasis: PUBLIC PROCEDURE [h: Handle, nKnots: NAT] =
    BEGIN OPEN h;
    closedCurve _ TRUE;
    initialEndFree _ FALSE;
    finalEndFree _ FALSE;
    patchCacheValid _ FALSE;
    splineBasis _ NEW[SplineBasisRec[2*nKnots]];
    a _ NEW[RealSequenceRec[2*nKnots]];
    free _ NEW[BooleanSequenceRec[2*nKnots]];
    FOR i:NAT IN [0..nKnots) DO
        elem: PiecewiseCubic.Handle _ BSplineBasisElement[i,nKnots];
        splineBasis[2*i] _ [elem,PiecewiseCubic.Zero[]];
        splineBasis[2*i+1] _ [PiecewiseCubic.Zero[],elem];
        ENDLOOP;
    FOR i:NAT IN [0..2*nKnots) DO
        a[i] _ 0;
        free[i] _ TRUE;
        ENDLOOP;
    END;

BSplineBasisElement: PROCEDURE [i, n: NAT] RETURNS [f: PiecewiseCubic.Handle] =
    BEGIN
    k: REAL _ i-2;
    IF k<0 THEN k_k+n;
    f _ PiecewiseCubic.Piece[k/n,(k+1)/n,0,0,0.75*n,0.25];
    k_k+1; IF k>n-1 THEN k_k-n;
    f _ PiecewiseCubic.Combine[1,f,1,PiecewiseCubic.Piece[k/n,(k+1)/n,0.25,0.75*n,0,1]];
    k_k+1; IF k>n-1 THEN k_k-n;
    f _ PiecewiseCubic.Combine[1,f,1,PiecewiseCubic.Piece[k/n,(k+1)/n,1,0,-0.75*n,0.25]];
    k_k+1; IF k>n-1 THEN k_k-n;
    f _ PiecewiseCubic.Combine[1,f,1,PiecewiseCubic.Piece[k/n,(k+1)/n,0.25,-0.75*n,0,0]];
    END;

CubicBasis: PUBLIC PROCEDURE [h: Handle] =
    BEGIN OPEN h;
    zero: PiecewiseCubic.Handle _ PiecewiseCubic.Zero[];
    one: PiecewiseCubic.Handle _ PiecewiseCubic.Piece[-10,10,1,0,0,1];
    iden: PiecewiseCubic.Handle _ PiecewiseCubic.Piece[-10,10,-10,1,1,10];
    sqr: PiecewiseCubic.Handle _ PiecewiseCubic.Piece[-10,10,100,-20,20,100];
    cube: PiecewiseCubic.Handle _ PiecewiseCubic.Piece[-10,10,-1000,300,300,1000];
    closedCurve _ FALSE;
    initialEndFree _ TRUE;
    finalEndFree _ TRUE;
    patchCacheValid _ FALSE;
    splineBasis _ NEW[SplineBasisRec[8]];
    a _ NEW[RealSequenceRec[8]];
    free _ NEW[BooleanSequenceRec[8]];
    splineBasis[0] _ [one,zero];
    splineBasis[1] _ [zero,one];
    splineBasis[2] _ [iden,zero];
    splineBasis[3] _ [zero,iden];
    splineBasis[4] _ [sqr,zero];
    splineBasis[5] _ [zero,sqr];
    splineBasis[6] _ [cube,zero];
    splineBasis[7] _ [zero,cube];
    FOR i:NAT IN [0..8) DO
        a[i] _ 0;
        free[i] _ TRUE;
        ENDLOOP;
    END;

CubicPieceBasis: PUBLIC PROCEDURE [h: Handle] =
    BEGIN OPEN h;
    zero: PiecewiseCubic.Handle _ PiecewiseCubic.Zero[];
    one: PiecewiseCubic.Handle _  PiecewiseCubic.Piece[0,1,1,0,0,1];
    iden: PiecewiseCubic.Handle _ PiecewiseCubic.Piece[0,1,0,1,1,1];
    sqr: PiecewiseCubic.Handle _   PiecewiseCubic.Piece[0,1,0,0,2,1];
    cube: PiecewiseCubic.Handle _ PiecewiseCubic.Piece[0,1,0,0,3,1];
    closedCurve _ FALSE;
    initialEndFree _ TRUE;
    finalEndFree _ TRUE;
    patchCacheValid _ FALSE;
    splineBasis _ NEW[SplineBasisRec[8]];
    a _ NEW[RealSequenceRec[8]];
    free _ NEW[BooleanSequenceRec[8]];
    splineBasis[0] _ [one,zero];
    splineBasis[1] _ [zero,one];
    splineBasis[2] _ [iden,zero];
    splineBasis[3] _ [zero,iden];
    splineBasis[4] _ [sqr,zero];
    splineBasis[5] _ [zero,sqr];
    splineBasis[6] _ [cube,zero];
    splineBasis[7] _ [zero,cube];
    FOR i:NAT IN [0..8) DO
        a[i] _ 0;
        free[i] _ TRUE;
        ENDLOOP;
    END;

ParabolaBasis: PUBLIC PROCEDURE [h: Handle] =
    BEGIN OPEN h;
    zero: PiecewiseCubic.Handle _ PiecewiseCubic.Zero[];
    one: PiecewiseCubic.Handle _ PiecewiseCubic.Piece[-10,10,1,0,0,1];
    iden: PiecewiseCubic.Handle _ PiecewiseCubic.Piece[-10,10,-10,1,1,10];
    sqr: PiecewiseCubic.Handle _ PiecewiseCubic.Piece[-10,10,100,-20,20,100];
    closedCurve _ FALSE;
    initialEndFree _ TRUE;
    finalEndFree _ TRUE;
    patchCacheValid _ FALSE;
    splineBasis _ NEW[SplineBasisRec[6]];
    a _ NEW[RealSequenceRec[6]];
    free _ NEW[BooleanSequenceRec[6]];
    splineBasis[0] _ [one,zero];
    splineBasis[1] _ [zero,one];
    splineBasis[2] _ [iden,zero];
    splineBasis[3] _ [zero,iden];
    splineBasis[4] _ [sqr,zero];
    splineBasis[5] _ [zero,sqr];
    FOR i:NAT IN [0..6) DO
        a[i] _ 0;
        free[i] _ TRUE;
        ENDLOOP;
    END;

CircleBasis: PUBLIC PROCEDURE [h: Handle] =
    BEGIN OPEN h;
    twopi: REAL _ 2*3.1415926;
    zero: PiecewiseCubic.Handle _ PiecewiseCubic.Zero[];
    one: PiecewiseCubic.Handle _ PiecewiseCubic.Piece[0,1,1,0,0,1];
    cos: PiecewiseCubic.Handle _ CosBasis[];
    sin: PiecewiseCubic.Handle _ SinBasis[];
    closedCurve _ TRUE;
    initialEndFree _ FALSE;
    finalEndFree _ FALSE;
    patchCacheValid _ FALSE;
    splineBasis _ NEW[SplineBasisRec[3]];
    a _ NEW[RealSequenceRec[3]];
    free _ NEW[BooleanSequenceRec[3]];
    splineBasis[0] _ [one,zero];
    splineBasis[1] _ [zero,one];
    splineBasis[2] _ [cos,sin];
    FOR i:NAT IN [0..3) DO
        a[i] _ 0;
        free[i] _ TRUE;
        ENDLOOP;
    END;

EllipseBasis: PUBLIC PROCEDURE [h: Handle] =
    BEGIN OPEN h;
    twopi: REAL _ 2*3.1415926;
    zero: PiecewiseCubic.Handle _ PiecewiseCubic.Zero[];
    one: PiecewiseCubic.Handle _ PiecewiseCubic.Piece[0,1,1,0,0,1];
    cos: PiecewiseCubic.Handle _ CosBasis[];
    sin: PiecewiseCubic.Handle _ SinBasis[];
    closedCurve _ TRUE;
    initialEndFree _ FALSE;
    finalEndFree _ FALSE;
    patchCacheValid _ FALSE;
    splineBasis _ NEW[SplineBasisRec[5]];
    a _ NEW[RealSequenceRec[5]];
    free _ NEW[BooleanSequenceRec[5]];
    splineBasis[0] _ [one,zero];
    splineBasis[1] _ [zero,one];
    splineBasis[2] _ [cos,sin];
    splineBasis[3] _ [sin,zero];
    splineBasis[4] _ [zero,cos];
    FOR i:NAT IN [0..5) DO
        a[i] _ 0;
        free[i] _ TRUE;
        ENDLOOP;
    END;

CosBasis: PROC RETURNS[PiecewiseCubic.Handle] =
    {RETURN[LIST[[domainStart: 0, domainEnd: 0.25, initValue: 99.98286, initSlope: 1.302177e-2, finalSlope: -662.7453, finalValue: -1.397745e-4], [domainStart: 0.25, domainEnd: 0.5, initValue: -1.397745e-4, initSlope: -662.7453, finalSlope: 2.099349e-3, finalValue: -99.9828], [domainStart: 0.5, domainEnd: 0.75, initValue: -99.9828, initSlope: 2.099349e-3, finalSlope: 662.7528, finalValue: 6.351456e-6], [domainStart: 0.75, domainEnd: 1, initValue: 6.351456e-6, initSlope: 662.7528, finalSlope: 1.302177e-2, finalValue: 99.98286]]]};

SinBasis: PROC RETURNS[PiecewiseCubic.Handle] =
    {RETURN[LIST[[domainStart: 0, domainEnd: 0.25, initValue: 1.728265e-4, initSlope: 662.7498, finalSlope: -4.030657e-4, finalValue: 99.98302], [domainStart: 0.25, domainEnd: 0.5, initValue: 99.98302, initSlope: -4.030657e-4, finalSlope: -662.7458, finalValue: 1.005444e-4], [domainStart: 0.5, domainEnd: 0.75, initValue: 1.005444e-4, initSlope: -662.7458, finalSlope: 1.04512e-3, finalValue: -99.98254], [domainStart: 0.75, domainEnd: 1, initValue: -99.98254, initSlope: 1.04512e-3, finalSlope: 662.7498, finalValue: 1.728265e-4]]]};

END.

-- Quick approximations
    cos _ PiecewiseCubic.Combine[1,PiecewiseCubic.Combine[1,PiecewiseCubic.Combine[1,
                PiecewiseCubic.Piece[0,0.25,1,0,-twopi,0],1,
            PiecewiseCubic.Piece[0.25,0.5,0,-twopi,0,-1]],1,
            PiecewiseCubic.Piece[0.5,0.75,-1,0,twopi,0]],1,
            PiecewiseCubic.Piece[0.75,1,0,twopi,0,1]];
    sin _ PiecewiseCubic.Combine[1,PiecewiseCubic.Combine[1,PiecewiseCubic.Combine[1,
                PiecewiseCubic.Piece[0,0.25,0,twopi,0,1],1,
            PiecewiseCubic.Piece[0.25,0.5,1,0,-twopi,0]],1,
            PiecewiseCubic.Piece[0.5,0.75,0,-twopi,0,-1]],1,
            PiecewiseCubic.Piece[0.75,1,-1,0,twopi,0]];