/**
 * <p>
 * Naive implementation of RK4 (4th order Runge Kutta integration)
 * for handling continuous-time components of distributed control
 * simulation.
 * </p>
 * @author     Michael Schuresko
 * @version    %I%, %G%
 * @since      1.0
*/

public class Rk4Naive implements INumericalIntegrator
{

    double m_lfStepSize;

    static final double lfOneThird = 1.0/3.0;

    protected Rk4Naive()
    {
	m_lfStepSize = 0.0001;
    }

    public Rk4Naive(double lfStepSize)
    {
	m_lfStepSize = lfStepSize;
    }

    /**
     * Assumes that time does not step over an agent 
     * state change event.
     * Other classes should handle this constraint.
     * @param lfTimeStop integrate until this time.
     * @param lfTCurr current (start) time
     * @param arrLfState state vector (initially initial state,
     * overwritten with final state)
     * @param arrLfDerivScratch state derivative vector scratch space 
     * (should be same dimensionality as state vector)
     * @param agent agent mapping (currently only support for homogenous swarms
     * )
     * @param arrStates state of each agent (assumed
     * not to change over integration)
     * @param arrSensors array of sensor interfaces.
     * @param drawCallback callback for drawing routines (if any)
     * (can be null)
     * @param haltCallback callback to check if we should halt
     * (can be null)
     * @return actual absolute (simulation) 
     * time at which we stopped integration, whether due to an
     * interruption, or concluding the computation (lfTimeStop).
     */
    public double integrateUntil(double lfTimeStop,
				 double lfTCurr,
				 double [] arrLfState,
				 double [] arrLfDerivScratch,
				 IAgent agent,
				 StateBundle [] arrStates,
				 ISensor [] arrSensors,
				 IEnvironment env,
				 ISimUICallback drawCallback,
				 ISimUICallback haltCallback)
    {
	double [] k1 = new double[arrLfState.length];
	double [] k2 = new double[arrLfState.length];
	double [] k3 = new double[arrLfState.length];
	double [] k4 = new double[arrLfState.length];
	double lfT = lfTCurr;

	while(lfT+m_lfStepSize <= lfTimeStop) {
	    lfT = stepOnce(lfT, m_lfStepSize, arrLfState,
			   k1, k2, k3, k4, arrLfDerivScratch,
			   agent, arrStates, arrSensors, env);
	    if(drawCallback != null) {
		drawCallback.doCallback(arrLfState, agent,
					arrStates,
					arrSensors, env, null, lfT);
	    }
	    if(haltCallback != null) {
		boolean bHalt = 
		    haltCallback.doCallback(arrLfState, agent,
					    arrStates,
					    arrSensors, env, null,
					    lfT);
		if(bHalt == true) {
		    return lfT;
		}
	    }
	}

	double lfTRemaining = lfTimeStop-lfT;

	lfT = stepOnce(lfT, lfTRemaining, arrLfState,
		       k1, k2, k3, k4, arrLfDerivScratch,
		       agent, arrStates, arrSensors, env);
	return lfT;
    }

    double stepOnce(double t, double h, 
		    double[] y, 
		    double[] k1, double [] k2, double [] k3, double [] k4,
		    double [] scratch,
		    IAgent agent,
		    StateBundle [] arrStates,
		    ISensor [] arrSensors,
		    IEnvironment env)
    {
	int nOffset = 0;
      	for(int i = 0; i < arrStates.length; ++i) {
	    arrStates[i].getControlFunc().getDerivs(t, y, arrSensors[i], 
						    env,
						    arrStates[i].getVars(), 
						    k1, 
						    nOffset);
	    nOffset += arrStates[i].getControlFunc().getNumStateVars();
	}

	for(int i = 0; i < y.length; ++i) {
	    scratch[i] = y[i]+0.5*h*k1[i];
	}

	nOffset = 0;
      	for(int i = 0; i < arrStates.length; ++i) {
	    //	    agent.getDerivs(t+0.5*h, scratch, arrSensors[i], env,
	    //	    arrStates[i], k2, 
	    //	    i*agent.getNumStateVars());
	    arrStates[i].getControlFunc().getDerivs(t+0.5*h, 
						    scratch, arrSensors[i], 
						    env,
						    arrStates[i].getVars(), 
						    k2, 
						    nOffset);
	    nOffset += arrStates[i].getControlFunc().getNumStateVars();
	}

	for(int i = 0; i < y.length; ++i) {
	    scratch[i] = y[i]+0.5*h*k2[i];
	}

	nOffset = 0;
      	for(int i = 0; i < arrStates.length; ++i) {
	    //	    agent.getDerivs(t+0.5*h, scratch, arrSensors[i], env,
	    //	    arrStates[i], k3, 
	    //	    i*agent.getNumStateVars());
	    arrStates[i].getControlFunc().getDerivs(t+0.5*h, 
						    scratch, arrSensors[i], 
						    env,
						    arrStates[i].getVars(), 
						    k3, 
						    nOffset);
	    nOffset += arrStates[i].getControlFunc().getNumStateVars();
	}

	for(int i = 0; i < y.length; ++i) {
	    scratch[i] = y[i]+h*k3[i];
	}

	nOffset = 0;
      	for(int i = 0; i < arrStates.length; ++i) {
	    //	    agent.getDerivs(t+h, scratch, arrSensors[i], env,
	    //	    arrStates[i], k4, 
	    //	    i*agent.getNumStateVars());
	    arrStates[i].getControlFunc().getDerivs(t+h, 
						    scratch, arrSensors[i], 
						    env,
						    arrStates[i].getVars(), 
						    k4, 
						    nOffset);
	    nOffset += arrStates[i].getControlFunc().getNumStateVars();
	}

	for(int i = 0; i < y.length; ++i) {
	    y[i] = y[i]+lfOneThird*h*(k2[i]+k3[i]+
				    0.5*(k1[i]+k4[i]));
	    
	}

	return t+h;
    }
}