-- PiecewiseCubicImpl.mesa
-- Michael Plass 15-Dec-81  8:38:56

DIRECTORY PiecewiseCubic;

PiecewiseCubicImpl: PROGRAM EXPORTS PiecewiseCubic =
BEGIN

Handle:  TYPE = PiecewiseCubic.Handle;
PieceRec: TYPE = PiecewiseCubic.PieceRec;

Zero: PUBLIC PROCEDURE RETURNS [Handle] = {RETURN [NIL]};

Piece: PUBLIC PROCEDURE
    [domainStart, domainEnd: REAL,
     initValue, initSlope, finalSlope, finalValue: REAL]
    RETURNS [Handle] =
    BEGIN
    IF domainStart<domainEnd AND
      (initValue#0 OR initSlope#0 OR finalSlope#0 OR finalValue#0) THEN
        BEGIN
	piece: PieceRec ← [domainStart:domainStart, domainEnd:domainEnd, initValue:initValue, initSlope:initSlope,finalSlope:finalSlope, finalValue:finalValue];
	RETURN[LIST[piece]]
	END
    ELSE RETURN[NIL]
    END;

ScalePiece: PROCEDURE [p: PieceRec, a: REAL] RETURNS [PieceRec] =
    BEGIN OPEN p;
    RETURN [[domainStart:domainStart, domainEnd:domainEnd,
    	     initValue:initValue*a,
	     initSlope:initSlope*a,
	     finalSlope:finalSlope*a,
	     finalValue:finalValue*a]]
    END;

BreakPiece: PROCEDURE [f: Handle, d: REAL] RETURNS [Handle] =
    BEGIN
    IF d<=f.first.domainStart OR d>=f.first.domainEnd THEN RETURN [f]
    ELSE BEGIN OPEN f.first;
    	 jointValue,jointSlope: REAL;
	 [value:jointValue, deriv:jointSlope] ← EvalAll[f,d];
	 RETURN [CONS[[domainStart:domainStart, domainEnd:d,
    	     		initValue:initValue,
	     		initSlope:initSlope,
	     		finalSlope:jointSlope,
	     		finalValue:jointValue],
		      CONS[[domainStart:d, domainEnd:domainEnd,
		     	    initValue:jointValue,
			    initSlope:jointSlope,
			    finalSlope:finalSlope,
			    finalValue:finalValue],f.rest]]] 
	 END
    END;

CombinePieces: PROCEDURE [a: REAL, p: PieceRec, b: REAL, q: PieceRec]
    RETURNS [PieceRec] =
    BEGIN -- p and q should have the same domain
    RETURN [[domainStart:p.domainStart, domainEnd:p.domainEnd,
    	     initValue:a*p.initValue+b*q.initValue,
	     initSlope:a*p.initSlope+b*q.initSlope,
	     finalSlope:a*p.finalSlope+b*q.finalSlope,
	     finalValue:a*p.finalValue+b*q.finalValue]]
    END;

Scale: PUBLIC PROCEDURE [f: Handle, a: REAL] RETURNS [Handle] =
    BEGIN
    IF a=0 OR f=NIL THEN RETURN [NIL]
    ELSE IF a=1 THEN RETURN [f]
    ELSE RETURN [CONS[ScalePiece[f.first,a],Scale[f.rest,a]]]
    END;

EnumerateCommonPieces: PUBLIC PROCEDURE [f: Handle, g: Handle, P: PiecewiseCubic.PieceProc] =
    BEGIN
    WHILE f#NIL OR g#NIL DO
        WHILE f#NIL AND (g=NIL OR f.first.domainStart<g.first.domainStart) DO
	    f ← BreakPiece[f,f.first.domainStart];
	    P[f.first, [f.first.domainStart, f.first.domainEnd, 0, 0, 0, 0]];
	    f ← f.rest;
	    ENDLOOP;
	WHILE g#NIL AND (f=NIL OR g.first.domainStart<f.first.domainStart) DO
	    g ← BreakPiece[g,g.first.domainStart];
	    P[[g.first.domainStart, g.first.domainEnd, 0, 0, 0, 0], g.first];
	    g ← g.rest;
	    ENDLOOP;
	WHILE f#NIL AND g#NIL AND (f.first.domainStart=g.first.domainStart) DO
	    f ← BreakPiece[f,g.first.domainEnd];
	    g ← BreakPiece[g,f.first.domainEnd];
	    P[f.first,g.first];
	    f ← f.rest;
	    g ← g.rest;
	    ENDLOOP;
	ENDLOOP;
    END;

Combine: PUBLIC PROCEDURE [a: REAL, f: Handle, b: REAL, g: Handle]
    RETURNS [Handle] =
    BEGIN
    IF f=NIL THEN RETURN[Scale[g,b]]
    ELSE IF g=NIL THEN RETURN[Scale[f,a]]
    ELSE IF g.first.domainStart>f.first.domainStart THEN
        RETURN[Combine[b,g,a,f]]
    ELSE IF g.first.domainStart=f.first.domainStart THEN
        BEGIN
	IF g.first.domainEnd=f.first.domainEnd THEN
	    RETURN[CONS[CombinePieces[a,f.first,b,g.first],
	    		Combine[a,f.rest,b,g.rest]]]
	ELSE IF g.first.domainEnd<f.first.domainEnd THEN
	    RETURN[Combine[a,BreakPiece[f,g.first.domainEnd],b,g]]
	ELSE RETURN[Combine[a,f,b,BreakPiece[g,f.first.domainEnd]]]
	END
    ELSE IF g.first.domainStart<f.first.domainStart THEN
        BEGIN
	IF g.first.domainEnd<=f.first.domainStart THEN
	    RETURN[CONS[ScalePiece[g.first,b],Combine[a,f,b,g.rest]]]
	ELSE RETURN[Combine[a,f,b,BreakPiece[g,f.first.domainStart]]] 
	END
    ELSE ERROR
    END;

Eval: PUBLIC PROCEDURE [f: Handle, t: REAL] RETURNS [REAL] =
     BEGIN
     WHILE f#NIL AND t>f.first.domainEnd DO f ← f.rest ENDLOOP;
     IF f=NIL OR t<f.first.domainStart THEN RETURN [0]
     ELSE
         BEGIN OPEN f.first;
	 interval: REAL ← domainEnd-domainStart;
	 c0: REAL ← initValue;
	 c1: REAL ← initSlope;
	 c2: REAL ← ((-2*initSlope-finalSlope)*interval+3*(finalValue-initValue))/(interval*interval);
	 c3: REAL ← ((initSlope+finalSlope)*interval+2*(initValue-finalValue))/(interval*interval*interval);
         t ← (t-domainStart);
	 RETURN[((c3*t+c2)*t+c1)*t+c0]
         END
     END;

EvalAll: PUBLIC PROCEDURE [f: Handle, t: REAL] RETURNS [value,deriv,derivDeriv,derivDerivDeriv,domainStart,domainEnd: REAL] =
     BEGIN
     WHILE f#NIL AND t>f.first.domainEnd DO f ← f.rest ENDLOOP;
     IF f=NIL OR t<f.first.domainStart THEN RETURN [0,0,0,0,0,0]
     ELSE
         BEGIN OPEN f.first;
	 interval: REAL ← domainEnd-domainStart;
	 c0: REAL ← initValue;
	 c1: REAL ← initSlope;
	 c2: REAL ← ((-2*initSlope-finalSlope)*interval+3*(finalValue-initValue))/(interval*interval);
	 c3: REAL ← ((initSlope+finalSlope)*interval+2*(initValue-finalValue))/(interval*interval*interval);
         t ← (t-domainStart);
	 RETURN[((c3*t+c2)*t+c1)*t+c0, (3*c3*t+2*c2)*t+c1, 6*c3*t+2*c2, 6*c3, f.first.domainStart, f.first.domainEnd]
         END
     END;

END.