CheckStateProc = PROCEDURE (
      t: Time; 
      VAR p: Position; 
      VAR v: Velocity; 
      VAR a:Acceleration;
    ): Time;
    (*
      Called by the simulator to process a state of the path,
      and handle any discrete events happening there.
      In particular, "checkState" will be applied to the 
      first and last states of the path.

      The "checkState" procedure is expected TO 
      
        (1) ``use up'' the state (print it, plot it, whatever);
        (2) adjust the position and velocity, if necessary;
        (3) compute the corresponding acceleration; and 
        (4) return an ending time "tf" for the next step. 
        
      If "tf" is greater than "t", the next step will stretch from "t"
      to some time not later than "tf".  If "tf" is equal to "t" (or
      earlier), the "integrate" method will stop, and return "t,p,v"
      as the final state of the path.



PROCEDURE Init(g: T; n: CARDINAL) =
  BEGIN
    g.n := n;
    g.pf := NEW(Vector, n);
    g.vf := NEW(Vector, n);
    g.af := NEW(Vector, n);
  END Init;

PROCEDURE StartStep(
    t0: LONGREAL;             (* Step starting time *)
    READONLY p0, v0: Vector;  (* Step starting state *)
    READONLY a0: Vector;      (* Acceleration at starting state *)
    t6: LONGREAL;             (* Step finish time *)
    VAR pw, vw: Vector;       (* Work areas *)
    evalRHS: EvalRHS          (* Computes the right-hand side *)
  ): LONGREAL =
  (*
    Starts a new integration step.
    
    The procedures "StartStep" and "FinishStep" together perform the 
    next step of the integration, from time "t0" and state 
    "(p0, v0)" to time "t0 + dt". 

    "StartStep" only computes enough information about the system's
    path to obtain a good estimate of the integration error (which is
    returned as a result of the call).  This information is saved in
    the integrator's work vectors, "g.v[1..5], g.a[1..5]".  If the
    client decides that the error is acceptable, it should call
    "FinishStep" to complete the computation; otherwise it should
    adjust the step length "dt" and call "StartStep" again.
    
    "StartStep" expect to be given in "a0" the right-hand side of
    the differential equation for time "t0" and state "(p0, v0)".
    It will evaluate the right-hand side five times on its own,
    for five tentative extrapolated states between "t0" and "t0 + dt".
    
    "StartStep" uses the vectors "pw" and "vw" as temporary working areas.
  *)
  VAR error: LONGREAL := 0.0D0;
  BEGIN
    WITH
      n = NUMBER(p0),
      dt = t6 - t0,
      v1 = g.v[1]^, a1 = g.a[1]^,
      v2 = g.v[2]^, a2 = g.a[2]^,
      v3 = g.v[3]^, a3 = g.a[3]^,
      v4 = g.v[4]^, a4 = g.a[4]^,
      v5 = g.v[5]^, a5 = g.a[5]^
    DO
      FOR i := 0 TO n-1 DO
        pw[i] := p0[i] + dt*C01*v0[i];
        vw[i] := v0[i] + dt*C01*a0[i];
      END;
      v1 := vw;
      evalRHS(t + Ct1*dt, pw, vw, a1);

      FOR i := 0 TO n-1 DO
        pw[i] := p0[i] + dt*(C02*v0[i] + C12*v1[i]);
        vw[i] := v0[i] + dt*(C02*a0[i] + C12*a1[i]);
      END;
      v2 := vw;
      evalRHS(t + Ct2*dt, pw, vw, a2);

      FOR i := 0 TO n-1 DO
        pw[i] := p0[i] + dt*(C03*v0[i] + C13*v1[i] + C23*v2[i]);
        vw[i] := v0[i] + dt*(C03*a0[i] + C13*a1[i] + C23*a2[i]);
      END;
      v3 := vw;
      evalRHS(t + Ct3*dt, pw, vw, a3);

      FOR i := 0 TO n-1 DO
        pw[i] := p0[i] + dt*(C04*v0[i] + C14*v1[i] + C24*v2[i] + C34*v3[i]);
        vw[i] := v0[i] + dt*(C04*a0[i] + C14*a1[i] + C24*a2[i] + C34*a3[i]);
      END;
      v4 := vw;
      evalRHS(t + Ct4*dt, pw, vw, a4);

      FOR i := 0 TO n-1 DO
        pw[i] := p0[i] + dt*(C05*v0[i] + C15*v1[i] + C25*v2[i] + C35*v3[i] + C45*v4[i]);
     
        vw[i] := v0[i] + dt*(C05*a0[i] + C15*a1[i] + C25*a2[i] + C35*a3[i] + C45*a4[i]);
      END;
      v5 := vw;
      evalRHS(t + Ct5*dt, pw, vw, a5);

      FOR i := 0 TO n-1 DO
        WITH
          verr = ABS(Ce0*v0[i] + Ce2*v2[i] + Ce3*v3[i] + Ce4*v4[i] + Ce5*v5[i])
        DO
          IF verr > error THEN error := verr END;
        END;
        WITH
          aerr = ABS(Ce0*a0[i] + Ce2*a2[i] + Ce3*a3[i] + Ce4*a4[i] + Ce5*a5[i])
        DO
          IF aerr > error THEN error := aerr END;
        END
      END;
    END;
    RETURN error;
  END StartStep;
 
PROCEDURE FinishStep(
    t0: LONGREAL;             (* Step starting time *)
    READONLY p0, v0: Vector;  (* Step starting state *)
    READONLY a0: Vector;      (* Acceleration at starting state *)
    t6: LONGREAL;             (* Step finish time *)
    VAR p6, v6: Vector;       (* Step finish state *)
  ) =
  (*
    Completes the integration step.
    
    This procedure should be called after "StartStep" to finish
    the integration from time "t0" and state "(p0, v0)"
    to time "t6"; the final state is stored in "(p6, v6").
  *)
  BEGIN
    WITH
      n = NUMBER(p0),
      dt = t6 - t0
    DO
      FOR i := 0 TO n-1 DO
        p6[i] := p0[i] + dt*(Cv0*v0[i] + Cv2*v2[i] + Cv3*v3[i] + Cv4*v4[i]);
        v6[i] := v0[i] + dt*(Cv0*a0[i] + Cv2*a2[i] + Cv3*a3[i] + Cv4*a4[i]);
      END
    END
  END FinishStep;

PROCEDURE AdjustStepSize(
    dt: LONGREAL; 
    error, tol: LONGREAL; 
    dtMin, dtMax: LONGREAL;
  ): LONGREAL =
  (*
    Adjusts the step length "dt", considering the
    integration error "error" estimated for the last
    integration step, and the maximum allowed error "tol".  
  *)
  BEGIN
    IF error > 0.0D0 THEN
      WITH
        delta = 0.840896415D0*Math.sqrt(Math.sqrt(tol/error))
      DO
        IF delta <= 0.1D0 THEN dt := 0.1D0*dt
        ELSIF delta >= 4.0D0 THEN dt := 4.0D0*dt
        ELSE dt := delta*dt END
      END
    ELSE
      dt := 4.0D0*dt
    END;
    RETURN MIN(dtMax, MAX(dtMin, dt));
  END AdjustStepSize;

PROCEDURE HermiteInterpolate(
    te:LONGREAL;             (* Time of interpolation *)
    VAR t0: LONGREAL;        (* IN: initial time, OUT: interpolated time ( = "te"). *)
    VAR p0, v0: Vector;      (* IN: initial state, OUT: interpolated state. *)
    t1: LONGREAL;            (* Final time *)
    READONLY p1, v1: Vector; (* Final state *)
  ) =
  (*
    Computes a system's state for time "te", by Hermite interpolation
    of the state "(p0, v0)" at "t0" and "(p1, v1)" at "t1".  The time
    "t0" is set to "te", and the intepolated state is returned in
    "(t0, v0)".
  *)
  CONST Three = 3.0D0;
        OneThird = 1.0D0/3.0D0;
  BEGIN
    IF te = t0 THEN
      (* OK *)
    ELSIF te = t1 THEN
      t0 := t1; p0 := p1; v0 := v1
    ELSE
      <* ASSERT t0 # t1 *>
      WITH
        n = NUMBER(p0),
        dt = t1 - t0,
        r = (te - t0)/dt,
        s = (t1 - te)/dt
      DO
        FOR i := 0 TO LAST(p0) DO 
          WITH
            pa = p0[i],
            pb = p1[i],
            va = v0[i],
            vb = v1[i],
            
            px = pa + OneThird * va,
            py = pb - OneThird * vb,
            
            pax = s * pa + r * px,
            pxy = s * px + r * py,
            pyb = s * py + r * pb,
            
            paxy = s * pax + r * pxy,
            pxyb = s * pxy + r * pyb,
            
            paxyb = s * paxy + r * pxyb,

            vm = Three * (pb - pa) - (va + vb),
            
            vam = s * va + r * vm,
            vmb = s * vm + r * vb,
            
            vamb = s * vam + r * vmb
          DO
            p0[i] := paxyb; v0[i] := vamb
          END
        END
      END
    END;
  END HermiteInterpolate;