package integrators;

import main.*;

/** Fifth-order adaptive step-size Runga-Kutta.
	<p>see Numerical Recipes in C; pg 717 */
public class CashKarpIntegrator extends Integrator	{
	
	final String				type = "CashKarp";
	
	// Coefficients for the Cash-Karp integrator
	static final float		a2 = 1.0f/5.0f,a3 = 3.0f/10.0f,a4 = 3.0f/5.0f,a5 = 1.0f,a6 = 7.0f/8.0f;		
	static final float		b21 = 1.0f/5.0f; 
	static final float		b31 = 3.0f/40.0f,b32 = 9.0f/40.0f;
	static final float		b41 = 3.0f/10.0f,b42 = -9.0f/10.0f,b43 = 6.0f/5.0f;
	static final float		b51 = -11.0f/54.0f,b52 = 5.0f/2.0f,b53 = -70.0f/27.0f,b54 = 35.0f/27.0f;
	static final float		b61 = 1631.0f/55296.0f,b62 = 175.0f/512.0f,b63 = 575.0f/13824.0f,b64 = 44275.0f/110592.0f,b65 = 253.0f/4096.0f;
	static final float		c1 = 37.0f/378.0f,c2 = 0.0f,c3 = 250.0f/621.0f,c4 = 125.0f/594.0f,c5 = 0.0f,c6 = 512.0f/1771.0f;
	static final float		dc1 = c1 - 2825.0f/27648.0f,dc2 = c2 - 0.0f,dc3 = c3 - 18575.0f/48384.0f,dc4 = c4 - 13525.0f/55296.0f,dc5 = c5 - 277.0f/14336.0f,dc6 = c6 - 1.0f/4.0f;



	NodeValues			[]	derivs1, derivs2, derivs3, derivs4, derivs5, derivs6;
	NodeValues			[]	values, values2;

	private	int				stepNum = 0;
	private float 			defaultTimestep = 0.01f;									
	private float 			ERRCON = 1.89e-4f;								// used in ckBoostTimestep
	private float		 	SAFETY = 0.9f;								// keeps timestep boosts and cuts safe
	private float			pShrink = -0.25f;		
	private float 			pBoost = -0.2f;
	private float 			errTol = 0.0001f;
	
	float					timestep = defaultTimestep;
	
	
	public CashKarpIntegrator()	{	}
	
	public void init(Model model)	{
		super.init(model);
		errorTolerance = 0.0001f;
		
		derivs1 = new NodeValues[num_cells * num_nodes];
		derivs2 = new NodeValues[num_cells * num_nodes];
		derivs3 = new NodeValues[num_cells * num_nodes];
		derivs4 = new NodeValues[num_cells * num_nodes];
		derivs5 = new NodeValues[num_cells * num_nodes];
		derivs6 = new NodeValues[num_cells * num_nodes];
		
		values = new NodeValues[num_cells * num_nodes];
		values2 = new NodeValues[num_cells * num_nodes];
		
		for(int i = 0; i < num_cells; i++) {
			for(int j = 0; j < num_nodes; j++) {
				int k = (i * num_nodes) + j;
				int	num_sides = model.cells[0].nodes[j].numSides;
				derivs1[k] = new NodeValues(num_sides);
				derivs2[k] = new NodeValues(num_sides);
				derivs3[k] = new NodeValues(num_sides);
				derivs4[k] = new NodeValues(num_sides);
				derivs5[k] = new NodeValues(num_sides);
				derivs6[k] = new NodeValues(num_sides);

				values[k] = new NodeValues(num_sides);
				values2[k] = new NodeValues(num_sides);
			}
		}
		reset();
	}
	
	public void reset()	
	{
		super.reset();
		timestep = defaultTimestep;
	}
	
	public float IntegrateOneStep()	{
		int	i,j;
		float	ret_timestep = 0;
		float maxerr = 0.0f;
		boolean toobig = true;
		int		cell_num, node_num, side, index;
//System.out.println("integrate one step " + Globals.time);
		
		// Fill up the values array with current values
		getCurrentValues(values);
		setIntegrationValues(values);

		while(toobig == true)  {
			for(int step_num = 0; step_num < 6; step_num++) {

				switch(step_num)
				{
					case 0: getDerivatives(derivs1); break;
					case 1: getDerivatives(derivs2); break;
					case 2: getDerivatives(derivs3); break;
					case 3: getDerivatives(derivs4); break;
					case 4: getDerivatives(derivs5); break;
					case 5: getDerivatives(derivs6); break;
				}
				
				for(cell_num = 0; cell_num < model.numCells; cell_num++)
				{
					for(node_num = 0; node_num < num_nodes; node_num++)
					{
						index = cell_num * num_nodes + node_num;
						Node node = model.cells[cell_num].nodes[node_num];
						for(side = 0; side < values2[index].numSides; side++)
						{
							switch(step_num)
							{
								case 0:	values2[index].values[side] = values[index].values[side] + 
																			b21 * derivs1[index].values[side]
																			 * timestep;
								break;
								case 1: values2[index].values[side] = values[index].values[side] + 
																			(b31 * derivs1[index].values[side] + 
																			 b32 * derivs2[index].values[side]
																			) * timestep;
								break;
								case 2: values2[index].values[side] = values[index].values[side] + 
																			(b41 * derivs1[index].values[side] + 
																			 b42 * derivs2[index].values[side] + 
																			 b43 * derivs3[index].values[side]
																			) * timestep;
								break;
								case 3: values2[index].values[side] = values[index].values[side] + 
																			(b51 * derivs1[index].values[side] + 
																			 b52 * derivs2[index].values[side] +
																			 b53 * derivs3[index].values[side] + 
																			 b54 * derivs4[index].values[side]
																			) * timestep;
								break;
								case 4:  values2[index].values[side] = values[index].values[side] + 
																			(b61 * derivs1[index].values[side] + 
																			 b62 * derivs2[index].values[side] + 
																			 b63 * derivs3[index].values[side] + 
																			 b64 * derivs4[index].values[side] + 
																			 b65 * derivs5[index].values[side]
																			) * timestep;
								break;
								case 5:  values2[index].values[side] = values[index].values[side] +
															(c1 * derivs1[index].values[side] +    	//  this is the fifth order step
									  						 c2 * derivs2[index].values[side] + 
									  						 c3 * derivs3[index].values[side] + 
									  						 c4 * derivs4[index].values[side] + 
									   						 c5 * derivs5[index].values[side] + 
															 c6 * derivs6[index].values[side]
															) * timestep; 
								break;
							}
						}
					}
				}

				// Fill model with new values from this step number in the integration
				setIntegrationValues(values2);
			}

int	largest_index = -1;
			for(cell_num = 0; cell_num < model.numCells; cell_num++)		// find maximum nodal (i.e. per/equation) error
			{
				for(node_num = 0; node_num < num_nodes; node_num++)
				{
					index = cell_num * num_nodes + node_num;
float temp = ckGetErrorEstimate(index);
if(temp > maxerr) largest_index = index;
					maxerr = Math.max(ckGetErrorEstimate(index),maxerr);
				}
			}
//System.out.println("maxerr = " + maxerr);

			maxerr = maxerr/errTol;  // this is now current error/desired error

			if(maxerr > 1)  {     	// max error too large.  Cut step size and try again
				ckCutTimestep(maxerr);
				setIntegrationValues(values);	// Go back to original values and start again
				maxerr = 0.0f;
			}
			else	{				// max error acceptable
				ret_timestep = timestep;
				
				ckBoostTimestep(maxerr);
				toobig = false;		// Flag that we're done
			}

largest_index %= num_nodes;
//if(timestep < 0.01) System.out.println("Largest index = " + largest_index + ": " + model.cells[0].nodes[largest_index]);
		}
	
		setFinalValues(values2);
		return ret_timestep;
	}
	

	void ckCutTimestep(float err)  {
		float htemp = timestep * SAFETY * ((float) Math.pow(err,pShrink));
		timestep = Math.max(htemp,0.1f * timestep);    // cut by at most a factor of 10
//if(timestep < 0.00001) 
//System.out.println("T = " + Globals.time + " : Tstep = " + timestep);
	}
	
	void ckBoostTimestep(float err) {
		if(err > ERRCON)  {   //   boost by at most a factor of 5
			timestep *= (SAFETY * ((float) Math.pow(err,pBoost)));
		}
		else  {
			timestep *= 5.0f;
		}
//System.out.println("T = " + Globals.time + " : Tstep = " + timestep);
	}
	
	public float ckGetErrorEstimate(int index) {
		float	err, maxerr = 0.0f;
		float	sfac = 1f;
		for(int side = 0; side < derivs1[index].numSides; side++) {
			
			err =   (dc1 * derivs1[index].values[side] +    //  yn+1 - y*n+1
					 dc2 * derivs2[index].values[side] + 
					 dc3 * derivs3[index].values[side] + 
					 dc4 * derivs4[index].values[side] + 
					 dc5 * derivs5[index].values[side] + 
					 dc6 * derivs6[index].values[side]
					) * timestep;
			if(Float.isNaN(err) || Float.isInfinite(err))
				maxerr = 1000000.0f;
			else
				maxerr = Math.max((Math.abs(err)/sfac),maxerr);
		}
/*if(Globals.time > 4 && index == 1) System.out.println("err for index " + index + "  "  +  maxerr
											+ "  " + derivs1[index].values[0]
											+ "  " + derivs2[index].values[0]
											+ "  " + derivs3[index].values[0]
											+ "  " + derivs4[index].values[0]
											+ "  " + derivs5[index].values[0]
											+ "  " + derivs6[index].values[0]);
*/		eEst = maxerr;
		return(maxerr);
	}

	public float getTimestep() { return timestep; }
	public void setTimestep(float t) { timestep = t; }
	
}