//
// A program to do Runge Kutta integration of a mass spring damper system
//
#include <stdio.h>

void	multiply(double, double[], double[]);
void	add(double[], double[], double[]);
void	step(double, double, double[]);
void	derivative(double, double[], double[]);

#define	SIZE		2		// the length of the state vector
#define	Ks		1000		// the spring coefficient
#define	Kd		10000		// the damping coefficient
#define	Mass		10		// the mass coefficient
#define	Force		100		// the applied force

int main(){
	FILE	*fp;

	double	h = 0.001;
	double	t;
	int		j = 0;

	double	X[SIZE];	// create state variable list
	X[0] = 0;	// set initial condition for x
	X[1] = 0;	// set initial condition for v

	if( ( fp = fopen("out.txt", "w")) != NULL){
		fprintf(fp, "   t(s)       x          v \n\n"); 
		for( t = 0.0; t < 50.0; t += h ){
			step(t, h, X);
			if(j == 0) fprintf(fp, "%9.5f %9.5f %9.5f\n", t, X[0], X[1]);
			j++; if(j >= 10) j = 0;
		}
	}
	fclose(fp);
}

//
// First order integration done here (could be replaced with runge kutta)
//
void step(double t, double h, double X[]){
	double 	tmp[SIZE],
			dX[SIZE],
			F1[SIZE],
			F2[SIZE],
			F3[SIZE],
			F4[SIZE];
	// Calculate F1
	derivative(t, X, dX);
	multiply(h, dX, F1);

	// Calculate F2
	multiply(0.5, F1, tmp);
	add(X, tmp, tmp);
	derivative(t+h/2.0, tmp, dX);
	multiply(h, dX, F2);

	// Calculate F3
	multiply(0.5, F2, tmp);
	add(X, tmp, tmp);
	derivative(t+h/2.0, tmp, dX);
	multiply(h, dX, F3);

	// Calculate F4
	add(X, F3, tmp);
	derivative(t+h, tmp, dX);
	multiply(h, dX, F4);

	// Calculate the weighted sum
	add(F2, F3, tmp);
	multiply(2.0, tmp, tmp);
	add(F1, tmp, tmp);
	add(F4, tmp, tmp);
	multiply(1.0/6.0, tmp, tmp);
	add(tmp, X, X);
}

//
// State Equations Calculated Here
//
void derivative(double t, double X[], double dX[]){
	dX[0] = X[1];
	dX[1] = (-Ks/Mass)*X[0] + (-Kd/Mass)*X[1] + (Force/Mass);
}

//
// A subroutine to add vectors to simplify other equations
//
void add(double X1[], double X2[], double R[]){
	for(int i = 0; i < SIZE; i++) R[i] = X1[i] + X2[i];
}

//
// A subroutine to multiply a vector by a scalar to simplify other equations
//
void multiply(double X, double V[], double R[]){
	for(int i = 0; i < SIZE; i++) R[i] = X*V[i];
}