//Solving State Equations using C
//By: Clayton Campagna, Spet 16, 2004

#include <stdio.h>
#include <math.h>

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

#define SIZE 4 // Length of the state vector

double m1, m2, kd1, ks1, kd2, ks2, kd3, ks3, force, t_start, t_end, steps, h, t;

int main(void){
	
	
	int j=0;
	
	printf("Enter the value of m1->\n");
	scanf("%lf", &m1);

	printf("Enter the value of m2->\n");
	scanf("%lf", &m2);

	printf("Enter the value of Kd1->\n");
	scanf("%lf", &kd1);

	printf("Enter the value of ks1->\n");
	scanf("%lf", &ks1);

	printf("Enter the value of kd2->\n");
	scanf("%lf", &kd2);

	printf("Enter the value of ks2->\n");
	scanf("%lf", &ks2);

	printf("Enter the value of kd3->\n");
	scanf("%lf", &kd3);

	printf("Enter the value of ks3->\n");
	scanf("%lf", &ks3);

	printf("Enter the value of force->\n");
	scanf("%lf", &force);
	
   	printf("Enter the lower integration limit->\n");
	scanf("%lf", &t_start);
	
	printf("Enter the upper integration limit->\n");
	scanf("%lf", &t_end);

	printf("Enter the number of steps->\n");
	scanf("%lf", &steps);

	h = (t_end - t_start) / steps; // compute step size

	double X[SIZE]; //Create state variable list
	X[0] = 0; //set initial conditions for x1
	X[1] = 0; //set initial condition for x2
	X[2] = 0; //set intial condition for v1
	X[3] = 0; //set initial condition for v2

	printf("    t(s)   	x1	x2	  v1	    v2\n\n");
	for( t = t_start; t < t_end; t += h ){
		if(j == 0) printf("%9.5f %9.5f %9.5f %9.5f %9.5f\n", t, X[0], X[1], X[2], X[3]);
		j++; if(j >= 10) j = 0;
		step(t, h, X);
	}
	
	return 0;
}
//First order integration done here (could be replaced with rung 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[2];
	dX[1] = X[3];
	dX[2] = ((-kd1-kd2)/m1)*X[2] + (kd2/m1)*X[3] + ((-ks1-ks2)/m1)*X[0] + (ks2/m1)*X[1];
	dX[3] = (kd2/m2)*X[2] + ((-kd2-kd3)/m2)*X[3] + (ks2/m2)*X[0] + ((-ks2-ks3)/m2)*X[1] + (force/m2);
}
// A subroutine to add vectors to simplify other equations
void add(double X1[], double X2[], double R[]){
	
	int i;
	for( 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[]){
	int i;
	for(i = 0; i < SIZE; i++)
		R[i] = X*V[i];
}