//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 []);
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 = 0.0; t < 50.0; t += h){
		step(t, h, X);
		if(j == 0) 
			printf("%9.3f %9.3f %9.3f %9.3f %9.3f\n", t, X[0], X[1], X[2], X[3]);
		j++;
		if(j >= 10)
			j = 0;
	}
	return 0;
}
//First order integration done here (could be replaced with rung kutta)
void step(double t, double h, double X[]){
	
	double 	dX[SIZE], F1[SIZE];
	
	//calculate F1
	derivative(t, X, dX);
	multiply(h, dX, F1);
	add(X, F1);
}
//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 X[], double X1[]){
	int i;
	for( i = 0; i < SIZE; i++) 
		X[i] = X[i] + X1[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];
}