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

void OneRun(int stages, double Weight[]); /* Precondition: values have been read in from a file. Postcondition: computes traject-
ory of 2-stage missile in increments. */

void RK4_System1(int n, double h, double t, double X[], int nsteps); /* Precondition: increment value and number of steps have been entered by the user 
and initial values and time have been initialized. Postcondition: computes a series of x values based on time and functions. */

void XP_System(int Neq, double T, double X[], double DY[]); /* Precondition: X and time values have been computed. Postcondition: computes 
derivatives of X values. */

double Celerity08(double temperature, double altitude); /* Precondition: temperature has been computed. Postcondition: returns speed of sound
for given temperature. */

double Temperature08(double altitude); /* Precondition: user has entered altitude at the console. Postcondition: returns temperature using 
interpolation. */

double Lagrangian(double x[], double y[], int K, double to_find, int n_pairs); /* Precondition: tables of x and y values,order of interpola-
tion K, and x value to be found have been entered from the console. Postcondition: returns y value using Lagrangian interpolation */

double Pressure08(double altitude, double A[]); /* Precondition: user has entered altitude at the console and curve has been 
fitted to atmospheric pressure data. Postcondition: returns atmospheric pressure at given altitude. */

void output(int j, double T, double Y[]); /* Precondition: dependent variables for each of the first-order equations have been computed. 
Postcondition: outputs the iteration, time value, and dependent variables for the first-order equations. */

double Gravity08(double altitude); /* Precondition: user has entered altitude in meters at the console. Postcondition: returns acceleration
due to gravity in meters/second^2 for given altitude. */

int print_control, n_stage;		// global variables

double Vrt, Pang, Pend, Az, time, Pressure, Temperature, Mach, Celerity, Q, Fa, start_lat, start_long, start_ht, velocity, acceleration;

double Thrust[5], FlowRate[5], XSect[5], CD[5], COT[5], TI[5], F[3], G[3];

int main()
{
	int closed, i, stages;
	double Weight[5];
	FILE *Fp;
	
	Fp = fopen("Binput.txt", "r");		// read input data from file
	fscanf(Fp, "%d %d %lf %lf %lf", &stages, &print_control, &start_lat, &start_long, &start_ht);
	
	for (i = 0; i < stages; i++)
		fscanf(Fp, "%lf %lf %lf %lf %lf %lf %lf", &Thrust[i], &Weight[i], &FlowRate[i], &XSect[i], &CD[i], &COT[i], &TI[i]);
	fscanf(Fp, "%lf %lf %lf %lf", &Vrt, &Pang, &Pend, &Az);
	closed = fclose(Fp);

	for (i = 0; i < stages; i++)
	{
		Thrust[i] *= 0.4535924;
		Weight[i] *= 0.04625455;
		FlowRate[i] *= 0.04625455;
		XSect[i] *= pow(0.3048, 2.0);
	}
	Az *= 0.01745329;
	Pang *= 0.01745329;

	OneRun(stages, Weight);		// run simulation
	return 0;
}

void OneRun(int stages, double Weight[])
{
	int nsteps, i;
	double X[7];

	time = 0.0;
	for (i = 0; i < 6; i++)				// initialize position and velocity
		X[i] = 0.0;
	X[2] = start_ht;				// initialize altitude
	for (i = 0; i < stages; i++)			// run one simulation for each stage
	{
		nsteps = (int) COT[i] / TI[i];		// compute number of subintervals for integration
		n_stage = i;				// set n_stage equal to the number of the current stage
		X[6] = Weight[i];			// initialize weight of missile for this stage
		RK4_System1(7, TI[i], time, X, nsteps);		
	}	
}

void RK4_System1(int n, double h, double t, double X[], int nsteps)
{
	
	double Y[n];
	double K[4][n];
	int i, j;
	
	output(0, 0.0, X);

	for (j = 0; j < nsteps; j++)
	{
		XP_System(n, t, X, K[0]);
		for (i = 0; i < n; i++)
			Y[i] = X[i] + 0.5 * h * K[0][i];
		XP_System(n, t + h/2, Y, K[1]);
		for (i = 0; i < n; i++)
			Y[i] = X[i] + 0.5 * h * K[1][i];
		XP_System(n, t + h/2, Y, K[2]);
		for (i = 0; i < n; i++)
			Y[i] = X[i] + h * K[2][i];
		XP_System(n, t + h, Y, K[3]);
		for (i = 0; i < n; i++)
			X[i] = X[i] + (1.0/6.0) * h * (K[0][i] + (2 *  K[1][i]) + (2 * K[2][i]) + K[3][i]);
		t = t + h;
		time = time + h;			// increment time
		output(j + 1, t, X);
	}
}

void XP_System(int Neq, double T, double X[], double DY[])
{
	int i;
	double drag[3] = {0.0, 0.0, 0.0};						// initialize drag force to zero for starting condition
	double C[3];
	double A0[6] = {-9.8572566e-18, 7.9877769e-13, -2.4260748e-08, 3.4967768e-04, -2.5611872e+00, 1.0197162e+04}; // coefficients for pressure curve
	double A1[6] = {-2.1244088e-20, 5.3110219e-15, -5.2048015e-10, 2.5216732e-05, -6.2330153e-01, 6.7811128e+03};
	
	/*for (i = 0; i < 3; i++)								// set C vector equal to current velocity
		C[i] = X[i + 3]; */
	PITCH(T, Az, Pang, Vrt, Pend, F, C);
	Temperature = Temperature08(X[2]);						// compute temperature based on current altitude	
	Celerity = Celerity08(Temperature, X[2]);					// compute celerity based on temperature
	velocity = sqrt(pow(X[3], 2.0) + pow(X[4], 2.0) + pow(X[5], 2.0));		// compute velocity
	Mach =  velocity / Celerity;							// compute Mach number
	if(X[2] < 25000)								// compute pressure given altitude
		Pressure = Pressure08(X[2], A0);
	else
		Pressure = Pressure08(X[2], A1);
	Q = 0.7 * Pressure * pow(Mach, 2.0);						// compute dynamic pressure
	Fa = CD[n_stage] * Q * XSect[n_stage];						// compute drag force
	G[0] = 0.0;									// compute acceleration due to gravity
	G[1] = 0.0;
	G[2] = Gravity08(X[2]);	
	// printf("Pressure = %lf, Temperature = %lf, Celerity = %lf, Gravity = %lf\n", Pressure, Temperature, Celerity, G[2]);
	if (velocity != 0.0)								// test if linear velocity is equal to zero as at initial condition
	{							
		drag[0] = C[0] / velocity;						// compute drag force in the i unit vector
		drag[1] = C[1] / velocity;						// compute drag force in the j unit vector
		drag[2] = C[2] / velocity;						// compute drag force in the k unit vector
	}							
	DY[0] = C[0];									// velocity in the i unit vector
	DY[1] = C[1];									// velocity in the j unit vector
	DY[2] = C[2];									// velocity in the k unit vector
	DY[3] = ((Thrust[n_stage] * F[0]) - (Fa * drag[0])) / X[6];			// compute acceleration in the i unit vector
	DY[4] = ((Thrust[n_stage] * F[1]) - (Fa * drag[1])) / X[6];			// compute acceleration in the j unit vector
	DY[5] = ((Thrust[n_stage] * F[2]) - G[2] - (Fa * drag[2])) / X[6];		// compute acceleration in the k unit vector
	DY[6] = -FlowRate[n_stage];							// compute flow rate
	acceleration = sqrt(pow(DY[3], 2.0) + pow(DY[4], 2.0) + pow(DY[5], 2.0));	// compute straight-line acceleration	
}

double Celerity08(double temperature, double altitude)
{
	double speed;
	
	if (altitude >= 65000.0)
		speed = 1000.0;
	speed = sqrt(((1.4 * 8314.32) / 28.9644) * temperature);
	return speed;
}

double Temperature08(double altitude)
{
	double temperature;
	double height[15] = {0.0, 5000.0, 10000.0, 15000.0, 20000.0, 25000.0, 30000.0, 35000.0, 40000.0, 45000.0, 50000.0, 
		55000.0, 60000.0, 65000.0, 70000.0};
	double degrees[15] = {290.0, 240.0, 220.0, 225.0, 230.0, 245.0, 255.0, 265.0, 270.0, 275.0, 270.0, 255.0, 235.0, 
		215.0, 200.0};
	if (altitude >= 65000.0)
		return 180.0;
	temperature = Lagrangian(height, degrees, 4, altitude, 15);
	return temperature;
}

double Lagrangian(double x[], double y[], int K, double to_find, int n_pairs)
{
	int i, j, m, stop;
	double result = 0.0;
	double partial_product;

	for (i=0; i < n_pairs; i++)
	{
		if (to_find <= x[i])
		{
			i--;
			break;
		}
	}
	if (K==4 && i !=0)
		i--;
	if (K==4 && i == n_pairs - 2)
		i -= 2;
	if (K==3 && i == n_pairs - 1)
		i -= 2;
	if (K == 3 && i == n_pairs - 2)
		i--;
	stop = i + K;
	for (j=i; j < stop; j++)
	{
		partial_product = 1.0;
		for (m=i; m < stop; m++)
		{
			if (m != j)
				partial_product *= ((to_find - x[m]) / (x[j] - x[m]));
		}
		partial_product = partial_product * y[j];
		result += partial_product;
	}
	return result;
}

double Pressure08(double altitude, double A[])
{
	int i;
	double pressure = 0.0;
	double j = 5.0;

	if (altitude > 70000)
		return 0;
	for (i = 0; i <= 5; i++)
	{
		pressure += (A[i] * pow(altitude, j));
		j -= 1.0;
	}
	return pressure;
}		  

void output(int j, double T, double Y[])
{
	int i;
	double met_long, met_lat;
	double y[3];
	double C[3];

	met_lat = start_lat * 1852.0 * 60.0;			// convert starting latitude to meters
	met_long = start_long * 1852.0 * 60.0;			// convert starting longitude to meters
	for (i = 0; i < 3; i++)
	{
		y[i] = Y[i];					// initialize position 
		C[i] = Y[i + 3];				// initialize velocity vector
	}	
	BLKOUT(1, print_control, time, y, C, F, G, Pang, Vrt, Pend, Az, FlowRate[n_stage], XSect[n_stage], Thrust[n_stage], G[2], Pressure, 
		Temperature, Mach, Celerity, Q, CD[n_stage], Fa, (met_lat + Y[0]) / (1852.0 * 60.0), (met_long + Y[1]) / (1852.0 * 60.0),
		sqrt(pow(Y[0], 2.0) + pow(Y[1], 2.0)), velocity, acceleration, COT[n_stage], TI[n_stage]);
}
 
double Gravity08(double altitude)
{
	double GM = 3.98622e14;
	double radius, acceleration;
	
	radius = 6378178 + altitude;
	acceleration = GM / pow(radius, 2.0);
	return acceleration;
}	




/********************CODE 2**************************************/
#include <stdio.h>
#include <math.h>
#include "PitchBlkOut.h"

double Pressure07(double alt);
double Temperature07(double alt);
double Celerity07(double temp);
double Gravity07(double alt);
double do_lagrange(double pairs[][2], double x, int k, int num_pairs);
void RK4_System(int n, double h, double t, double x[], int nsteps);
void XP_System(int Neq, double T, double Y[], double DY[]);
void output(int j, double T, double Y[]);
void OneRun07();

int NumStages;
int CONTROL;
int stage;

double Time,
                         COT[2],Thrust[2],Weight[2],Flow[2],CrossSection[2],CD[2],
                         TI[2],AZ,PANG,VEND,PEND;
double PLAT,PLONG,HEIGHT;

double Velocity,C[3],F[3],Gravity,G[3],Pressure,Temperature,Celerity,
                         Mach,Q,AF,Latitude,Longitude,Altitude,Acceleration,Range;

int main()
{
        double iLat,iLong,iHeight;
        double iThrust[2],iWeight[2],iFlow[2],iCrossSection[2],iDrag[2],
                                 iCutOff[2],iStepSize[2];
        double iVertRise,iPitchAngle,iPitchEnd,iLaunchAzimuth;

        int i;

        printf("Michael Eads NN:07 Numerical Analysis\n");

        printf("Control values:\n");
        printf("  Number of stages:\n");
        scanf("%d",&NumStages);
        printf("  Print control:\n");
        scanf("%d",&CONTROL);
        printf("Launch Point\n");
        printf("  Latitude:\n");
        scanf("%lf",&iLat);
        printf("  Longitude:\n");
        scanf("%lf",&iLong);
        printf("  Height:\n");
        scanf("%lf",&iHeight);
        for(i = 0; i < NumStages; i++) {
                printf("The vehicle (%d stage):\n",i);
                printf("  Thrust(lbs):\n");
                scanf("%lf",&iThrust[i]);
                printf("  Weight(lbs):\n");
                scanf("%lf",&iWeight[i]);
 printf("  Flow(lbs/sec):\n");
                scanf("%lf",&iFlow[i]);
                printf("  Cross-sectional area(ft^2):\n");
                scanf("%lf",&iCrossSection[i]);
                printf("  Drag coefficient:\n");
                scanf("%lf",&iDrag[i]);
                printf("  Cut-off time(secs):\n");
                scanf("%lf",&iCutOff[i]);
                printf("  Step size(secs):\n");
                scanf("%lf",&iStepSize[i]);

        }
        printf("Pitch program:\n");
        printf("  Vertical rise time(secs):\n");
        scanf("%lf",&iVertRise);
        printf("  Pitch angle(degrees from vert):\n");
        scanf("%lf",&iPitchAngle);
        printf("  Pitch Program end time(secs):\n");
        scanf("%lf",&iPitchEnd);
        printf("  Launch azimuth(degrees from north):\n");
        scanf("%lf",&iLaunchAzimuth);

        // Echo input
        printf("Control values:\n");
        printf("  Number of stages:%d\n",NumStages);
        printf("  Print control:%d\n",&CONTROL);
        printf("Launch Point\n");
        printf("  Latitude:%lf\n",iLat);
        printf("  Longitude:%lf\n",iLong);
        printf("  Height:%lf\n",iHeight);
        for(i = 0; i < NumStages; i++) {
                printf("The vehicle (%d stage):\n",i);
                printf("  Thrust(lbs):%lf\n",iThrust[i]);
                printf("  Weight(lbs):%lf\n",iWeight[i]);
                printf("  Flow(lbs/sec):%lf\n",iFlow[i]);
                printf("  Cross-sectional area(ft^2):%lf\n",iCrossSection[i]);
                printf("  Drag coefficient:%lf\n",iDrag[i]);
                printf("  Cut-off time(secs):%lf\n",iCutOff[i]);
                printf("  Step size(secs):%lf\n",iStepSize[i]);

        }
        printf("Pitch program:\n");
        printf("  Vertical rise time(secs):%lf\n",iVertRise);
        printf("  Pitch angle(degrees from vert):%lf\n",iPitchAngle);
        printf("  Pitch Program end time(secs):%lf\n",iPitchEnd);
        printf("  Launch azimuth(degrees from north):%lf\n",iLaunchAzimuth);

        // Input translation
        PLAT = iLat;
        PLONG = iLong;
        HEIGHT = iHeight;
        for(i = 0; i < NumStages; i++) {
           COT[i] = iCutOff[i];
                Thrust[i] = iThrust[i] * 0.4535924;
                Flow[i] = -iFlow[i] * 0.04625455;
                Weight[i] = iWeight[i] * 0.04625455;
                CrossSection[i] = iCrossSection[i] * pow(0.3048,2.0);
                CD[i] = iDrag[i];
                TI[i] = iStepSize[i];
        }
        AZ = iLaunchAzimuth;
        PANG = iPitchAngle;
        VEND = iVertRise;
        PEND = iPitchEnd;

        Latitude = PLAT;
        Longitude = PLONG;

        OneRun07();

}

void OneRun07()
{
        double X[7];
        double Ft,Fa,Fg;
        double alt = HEIGHT;
        G[0] = 0.0; G[1] = 0.0; G[2] = -1.0;
        F[0] = 0.0; F[1] = 0.0; F[2] = 1.0;
        C[0] = 0.0; C[1] = 0.0; C[2] = 0.0;

        stage = 0;

        X[0] = 0.0;
        X[1] = 0.0;
        X[2] = HEIGHT;
        X[3] = 0.0;
        X[4] = 0.0;
        X[5] = 0.0;
        X[6] = Weight[stage];

        // Initialize other values
        Pressure = Pressure07(alt);
        Temperature = Temperature07(alt);
        Celerity = Celerity07(Temperature);
        Gravity = Gravity07(alt);

        Velocity = 1.0;
        Mach =  Velocity / Celerity;
        Q = 0.7 * Pressure * pow(Mach,2.0);

        // Calculate forces
        Ft = Thrust[stage];
        Fg = alt * Gravity;
        Fa = CD[stage] * Q * CrossSection[stage];

        Acceleration = (Ft - Fa) / (9.80665 * Weight[stage]);


        RK4_System(7,TI[stage],0.0,X,2);
}

void RK4_System(int n, double h, double t, double x[], int nsteps)
{
        int i,j,not_k;
        double y[n],dy[n];
        double K[4][n];

        output(-1,t,x);
        for(j = 0; j < nsteps; j++) {
                // K1
                XP_System(n,t,x,K[0]);
                for(i = 0; i < n; i++) {
                        y[i] = x[i] + 0.5 *     h * K[0][i];
                }

                // K2
                XP_System(n,t+(h/2.0),y,K[1]);
                for(i = 0; i < n; i++) {
                        y[i] = x[i] + 0.5 *     h * K[1][i];
                }

                // K3
                XP_System(n,t+(h/2.0),y,K[2]);
                for(i = 0; i < n; i++) {
                        y[i] = x[i] + h * K[2][i];
                }

                // K4
                XP_System(n,t+h,y,K[3]);
                for(i = 0; i < n; i++) {
                        x[i] = x[i] + (h/6.0) * (K[0][i] + 2.0*K[1][i] + 2.0*K[2][i] + K[3][i]);
                }
                t = t+h;
                output(j,t,x);
        }
}

void XP_System(int Neq, double T, double Y[], double DY[])
{
        double alt;
        double Fg,Ft,Fa;

        alt = Y[2];


        // Caclulate constants
        Pressure = Pressure07(alt);
        Temperature = Temperature07(alt);
        Celerity = Celerity07(Temperature);
        Gravity = Gravity07(alt);

        Velocity = sqrt(pow(Y[3],2.0) + pow(Y[4],2.0) + pow(Y[5],2.0));
        Mach =  Velocity / Celerity;
        Q = 0.7 * Pressure * pow(Mach,2.0);

        // Calculate forces
        Ft = Thrust[stage];
        Fg = (Y[6]) * Gravity;
        Fa = CD[stage] * Q * CrossSection[stage];

        // Setup velocity vector (I think?)
        if(T != 0.0) {
        C[0] = Y[3] / Velocity;
        C[1] = Y[4] / Velocity;
        C[2] = Y[5] / Velocity;
        } else {C[0] = 0.0; C[1] = 0.0; C[2] = 0.0;}

        Acceleration = (Ft - Fa) / (9.80665 * (Y[6]));
        PITCH(T,AZ,PANG,VEND,PEND,F,C);

        //Calculate derivatives
        DY[0] = Y[3];
        DY[1] = Y[4];
        DY[2] = Y[5];
        DY[3] = (Ft * F[0] + Fg * G[0] - Fa * C[0]) / (Y[6]);
        DY[4] = (Ft * F[1] + Fg * G[1] - Fa * C[1]) / (Y[6]);
        DY[5] = (Ft * F[2] + Fg * G[2] - Fa * C[2]) / (Y[6]);
        DY[6] = Flow[stage] * T;

        //printf("DY[6] = %lf, Flow=%lf, T=%lf\n",DY[6],Flow[stage],T);
}

void output(int j, double T, double Y[])
{
        int i;
        for( i = 0; i < 7; i++)
                printf("Y[%d] = %lf\n",i,Y[i]);
        BLKOUT(1, CONTROL,
                        T, Y, C,
                        F, G,
                        PANG, VEND, PEND, AZ,
                        Flow[stage],CrossSection[stage],Thrust[stage],
                        Gravity,Pressure,Temperature,Mach,
                        Celerity,Q,CD[stage],AF,
                        Latitude,Longitude,Range, Velocity,
                        Acceleration,COT[stage],TI[stage]);

}

double Pressure07(double alt)
{
        double a1[6] = {
                -9.666667e-04,
                 7.833333e-02,
    -2.379167e+00,
           3.429167e+01,
                -2.511667e+02,
                 1.000000e+03
        };

        double a2[6] = {
                -2.083333e-06,
                 5.208333e-04,
                -5.104167e-02,
                 2.472917e+00,
                -6.112500e+01,
                 6.650000e+02
        };

        double pressure;
        int i;

        alt /= 1000.0;
        pressure = 0.0;
        if(alt > 65.0) pressure = 0.0;
        else if(alt > 25.0) {
                for(i = 0; i < 6; i++) {
                        pressure += pow(alt,5-i) * a2[i];
                }
        } else {
                for(i = 0; i < 6; i++) {
                        pressure += pow(alt,5-i) * a1[i];
                }
        }

        return pressure * 1000.0 * 0.010197162;
}

double Temperature07(double alt)
{
        double pairs[15][2];
        double result;

        alt /= 1000.0;

        pairs[0][0] =  0.0;  pairs[0][1] = 290.0;
        pairs[1][0] =  5.0;  pairs[1][1] = 240.0;
        pairs[2][0] = 10.0;  pairs[2][1] = 220.0;
        pairs[3][0] = 15.0;  pairs[3][1] = 225.0;
        pairs[4][0] = 20.0;  pairs[4][1] = 230.0;
        pairs[5][0] = 25.0;  pairs[5][1] = 245.0;
        pairs[6][0] = 30.0;  pairs[6][1] = 255.0;
        pairs[7][0] = 35.0;  pairs[7][1] = 265.0;
        pairs[8][0] = 40.0;  pairs[8][1] = 270.0;
        pairs[9][0] = 45.0;  pairs[9][1] = 275.0;
        pairs[10][0] = 50.0; pairs[10][1] = 270.0;
        pairs[11][0] = 55.0; pairs[11][1] = 255.0;
        pairs[12][0] = 60.0; pairs[12][1] = 235.0;
        pairs[13][0] = 65.0; pairs[13][1] = 215.0;
        pairs[14][0] = 70.0; pairs[14][1] = 200.0;

        if(alt > 65.0) result = 180.0;
        else result = do_lagrange(pairs,alt,4,15);

        return result;
}

double Celerity07(double temp)
{
        double result;

        if(temp == 180.0) result = 1000.0;
        else result = pow((1.4 * (8314.32 / 28.9644)) * temp,0.5);
        return result;
}

double Gravity07(double alt)
{
        double GM = 3.98622 * pow(10.0,14.0);
        double r0 = 6.378178 * pow(10.0,6.0);

        return GM/pow(r0 + alt,2.0);
}

double do_lagrange(double pairs[][2], double x, int k, int num_pairs)
{
        int i,j,x_begin,x_end,x_middle;
        int x_half, k_count;
        double partial,result;

        partial = result = 0.0;

        x_half = k / 2;
        k_count = k;

        for(i = 0; i < num_pairs; i++)          /*Find the middle point*/
                if(pairs[i][0] >= x) break;

        x_middle = i;
        if(x_middle < 0) x_middle = 0;

        x_begin = x_end = 0;
        x_begin = x_middle - x_half;
        x_end = x_middle + x_half;
        if(x_begin < 0) {                                       /*Too close to the beginning*/
                x_begin = 0;
                k_count -= (x_end - x_middle);
                k_count -= (x_middle - x_begin);
                x_end+=k_count;
        }
        else if(x_end >= num_pairs) {           /*Too close to the end*/
                x_end = num_pairs - 1;
                k_count -= (x_end - x_middle);
                k_count -= (x_middle - x_begin);
                x_end+=k_count;
        } else {                                                        /*Middle case*/
                k_count -= (x_end - x_middle);
                k_count -= (x_middle - x_begin);
                x_end+=k_count;
                if(x_end >= num_pairs) {                /*This case can happen again*/
                        x_end -= k_count;
                        x_begin -= k_count;
                }
        }


        result = 0.0;
        for(i = x_begin; i < x_end; i++) {
                partial = pairs[i][1];
                for(j = x_begin; j < x_end; j++) {
                        if(i != j) {
                                partial *= (x - pairs[j][0]) / (pairs[i][0] - pairs[j][0]);
                        }
                }
                result += partial;
        }
        return result;
}