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


// #
// # Global Variables & Declarations
// #
int i = 0,j = 0,k = 0;
int NumStages;    
int CONTROL;      
int STAGE;       
int VEHICLE = 1;
double Latitude;
double Longitude;
double Altitude;
double Acc;
double TIME;
double COT[5];
double THRUST[5];
double WEIGHT[5];
double FLOW[5];
double XSEC[5];
double CD[5];
double TIN[5];
double AZ;
double PANG;
double VEND;
double PEND;
double PLAT;
double PLONG;
double HEIGHT;
double Velocity;
double Gravity;
double Pressure;
double Temperature;
double Celerity;
double Mach;
double Q;
double AF;





double C[3] = {0, 0, 0}; 
double G[3] = {0, 0, -1};
double F[3] = {0, 0, 0}; 

		double lfLatitude,lfLongitude,lfHeight;
        double lfThrust[5],lfWeight[5],lfFlow[5],lfCrossSection[5],lfDrag[5],lfCutOff[5],lfStepSize[5];
        double lfVertRise,lfPitchAngle,lfPitchEnd,lfLaunchAzimuth;
		double X[7];	


// Finds the product
double lgproduct( double x[], int n , double X, int i, int upperBound,int lowerBound) 
{ 
     
     double product = 1; // Initial Product Value 
	 int j = 0;
     for(j = lowerBound; j <= upperBound ; j++) 
     { 
          if (j != i)  
          { 
               product = (product) * ((X - x[j])/(x[i]- x[j])); // Calculate the product 
 
          } 
     } 
      
     return product; 
}// For calculating the sum of products
double lgsummation(double x[], double y[], int n, double X, int upperBound, int lowerBound)
{
	double summation = 0;
	int i = lowerBound;
	n = upperBound;

     for(i = lowerBound; i <= upperBound; i++) 
     { 
          summation = summation + ( y[i] * lgproduct(x,n,X,i,upperBound,lowerBound)); 
     } 


	return summation; 
}




double interpolate(double x[], double y[], int k, double X, int n)
{
	int i = 0;
	int lowerBound = 0;
	int upperBound = 0;
	int range = 0;
	double fX = 0;

	
	// Finding the upper and lower bounds according to K
	for (i = 0; i < n; i++)  
	{
		if (x[i] > X)
		{
			lowerBound = i-1;
			upperBound = i;
			break;

		}
	}
	range = k/2-1;
	upperBound = upperBound + range;
	lowerBound = lowerBound - range;

	if (lowerBound < 0) 
	{
		upperBound = abs(lowerBound)+upperBound;
		lowerBound = 0; 	
	}
	else if (upperBound > n)  
	{
		lowerBound = lowerBound - (abs(upperBound - n));
		upperBound = n;
	}

	fX = lgsummation(x,y,n,X,upperBound,lowerBound);

	return fX;
}



double Pressure15(double alt)
{
	double A[6];
	alt = alt/1000.0;
	if (alt <= 25)
	{
		A[0] = -9.6667e-04;
		A[1] = 7.8333e-02;
		A[2] = -2.3792;
		A[3] = 3.4292e+01;
		A[4] = -2.5117e+02;
		A[5] = 1000;
		return (A[0]*pow(alt,5) + A[1]*pow(alt,4) + A[2]*pow(alt,3) + A[3]*pow(alt,2) + A[4]*pow(alt,1) + A[5])*1000*.010197162;
	}
	else if(alt <= 65)
	{
		A[0] = -2.0833e-06;
		A[1] = 5.2083e-04;
		A[2] = -5.1042e-02;
		A[3] = 2.4729;
		A[4] = -6.1125e+01;
		A[5] = 6.6500e+02;
		return (A[0]*pow(alt,5) + A[1]*pow(alt,4) + A[2]*pow(alt,3) + A[3]*pow(alt,2) + A[4]*pow(alt,1) + A[5])*1000*.010197162;
	}
	else 
	{
		return 0;
	}

}


double Temperature15(double alt)
{
	double A[15];
	double T[15];
		alt = alt/1000.0;
		A[0] = 0.0;
		A[1] = 5.0;
		A[2] = 10.0;
		A[3] = 15;
		A[4] = 20;
		A[5] = 25;
		A[6] = 30;
		A[7] = 35;
		A[8] = 40;
		A[9] = 45;
		A[10] = 50;
		A[11] = 55;
		A[12] = 60;
		A[13] = 65;
		A[14] = 70;

		T[0] = 290;
		T[1] = 240;
		T[2] = 220;
		T[3] = 225;
		T[4] = 230;
		T[5] = 245;
		T[6] = 255;
		T[7] = 265;
		T[8] = 270;
		T[9] = 275;
		T[10] = 270;
		T[11] = 255;
		T[12] = 235;
		T[13] = 215;
		T[14] = 200;

	if (alt <= 65)
	{
		return interpolate(A,T,4,alt,15); //Computer Interpolation

	}
	else 
	{
		return 180;
	}	
		
}


double Celerity15(double temp)
{
	if (temp == 180)
	{
		return 1000;
	}

	return sqrt(((1.4)*((8.31432e+03)/(28.9644))*temp));
}

double Gravity15(double alt)
{
	return (3.98622e+14)/(pow(((6.378178e+06)+(alt)),2));
}
 
void output(int j, double T, double Y[])
{
    int i;
  	double tmpLong, tmpZ, tmplat, range, C[3];
 
	Latitude = -(Y[0] /(1852 * 60)) + Latitude;
	Longitude = -(Y[1] /(1852 * 60)) + Longitude;

	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;
	}

	range = sqrt(pow(Y[0], 2.0) + pow(Y[1], 2.0))/1852.0;

	BLKOUT(0, CONTROL, TIME, Y, C, F, G, PANG, VEND, PEND, AZ, 
		-FLOW[STAGE], XSEC[STAGE], THRUST[STAGE], 
		Gravity, Pressure, Temperature, Mach, 
		Celerity, Q, CD[STAGE], AF, 
		Latitude, Longitude, range, Y[5], 
		Acc, COT[STAGE], TIN[STAGE]);
  
}


void XP_System(double T, double Y[], double DY[])
{
		double Fg = 0;
		double Ft = 0;
		double Fa = 0; 
		double mass = 0;
        int i;
		Altitude = Y[2]; 
        Gravity = Gravity15(Y[2]); 
        Pressure = Pressure15(Y[2]);
        Temperature = Temperature15(Y[2]);
        Celerity = Celerity15(Temperature);
		Velocity = sqrt(Y[3]*Y[3] + Y[4]*Y[4] + Y[5]*Y[5]);  
        Mach =  Velocity / Celerity;   
		Q = 0.7 * Pressure * (Mach * Mach);
		Ft = THRUST[STAGE];
		mass = WEIGHT[STAGE] - FLOW[STAGE] * T;
        Fg = (mass) * Gravity;   
        Fa = CD[STAGE] * Q * XSEC[STAGE];  
        Acc = (Ft - Fa) / (9.80665 * mass);
		AF = Fa;   

        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;
		}
        PITCH(T,AZ,PANG,VEND,PEND,F,C);
		DY[0] = Y[3];      
		DY[1] = Y[4];
		DY[2] = Y[5];
		DY[3] = (Ft * F[0] + Fg * G[0] - Fa * C[0]) / (mass);
		DY[4] = (Ft * F[1] + Fg * G[1] - Fa * C[1]) / (mass);
		DY[5] = (Ft * F[2] + Fg * G[2] - Fa * C[2]) / (mass);
		DY[6] = -FLOW[STAGE];
}



void RK4_System1(int n, double h, double T, double X[], int nsteps)
{
	double Y[n];							
	double K[4][n];

	output(0,T,X);
	
		for (j = 0; j < nsteps; j++)   
	{
		XP_System(T, X, K[0]);   
				
		for (i=0; i<n; i++)		
		{
			Y[i] = X[i] + .5 * h * K[0][i];    
		}
		XP_System(T + h/2, Y, K[1]);  	
		for (i=0; i<n; i++)		
		{
			Y[i] = X[i] + .5 * h * K[1][i];    
		}
		XP_System(T + h/2, Y, K[2]);   
				
		for (i=0; i<n; i++)		
		{
			Y[i] = X[i] + h * K[2][i];         
		}
			
		XP_System(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 = T;
		output(j, T, X); 
	}
}

void OneRun15(double X[7])
{
       
	int nsteps, i;


	 
	TIME = 0.0;		
	X[0] = 0.0;
    X[1] = 0.0;
    X[2] = HEIGHT;  
    X[3] = 0.0;       
    X[4] = 0.0;
    X[5] = 1;
  	G[0] = 0.0;									 
	G[1] = 0.0;
	G[2] = -1;    	
	 
	for (i=0; i<NumStages; i++)			 
	{
		nsteps = (int) COT[i] / TIN[i];	 
		STAGE = i;				 
		X[6] = WEIGHT[i];			 
		RK4_System1(7, TIN[i], TIME, X, nsteps);		
	}	
     
}






int main () 
{

printf("Anthony Marzi::15 - Analysis B \n\n");

printf("Number of stages: \n");
scanf("%d",&NumStages);
printf("Print control: \n");
scanf("%d",&CONTROL);
printf("Latitude(in degrees): \n");
scanf("%lf",&lfLatitude);
printf("Longitude(in degrees): \n");
scanf("%lf",&lfLongitude);
printf("Height(in meters): \n\n  ");
scanf("%lf",&lfHeight);
for(i=0; i<NumStages; i++) 
{
	printf("Stage %d:\n",i);
	printf("Thrust: \n");
	scanf("%lf",&lfThrust[i]);
	printf("Weight: \n");
	scanf("%lf",&lfWeight[i]);
	printf("Flow Rate: \n");
	scanf("%lf",&lfFlow[i]);
	printf("Cross-sectional area: \n");
	scanf("%lf",&lfCrossSection[i]);
	printf("Drag coefficient: \n");
	scanf("%lf",&lfDrag[i]);
	printf("Cut-off time: \n");
	scanf("%lf",&lfCutOff[i]);
	printf("Step size(secs): \n");
	scanf("%lf",&lfStepSize[i]);
}
printf("Pitch program: \n");
printf("Vertical rise time: \n");
scanf("%lf",&lfVertRise);
printf("Pitch angle: \n");
scanf("%lf",&lfPitchAngle);
printf("Pitch Program end time:\n ");
scanf("%lf",&lfPitchEnd);
printf("Launch azimuth: \n");
scanf("%lf",&lfLaunchAzimuth);


// #
// # Echo Input
// #

printf("\n\n\nINPUT\nControl Values:\n");
printf("\t  Number of stages: %d\n",NumStages);
printf("\t  CONTROL: %d\n", CONTROL);
printf("\t  Latitude: %lf\n",lfLatitude);
printf("\t  Longitude: %lf\n",lfLongitude);
printf("\t  Height: %lf\n",lfHeight);
for(i=0; i<NumStages; i++) 
{
printf("\nStage: %d :\n",i+1);
printf("\t  Thrust(lbs): %lf\n",lfThrust[i]);
printf("\t  Weight(lbs): %lf\n",lfWeight[i]);
printf("\t  Flow(lbs/sec): %lf\n",lfFlow[i]);
printf("\t  Cross-sectional area(sqft): %lf\n",lfCrossSection[i]);
printf("\t  Drag coefficient: %lf\n",lfDrag[i]);
printf("\t  \"Cut-off\" time(secs): %lf\n",lfCutOff[i]);
printf("\t  Step size(secs): %lf\n",lfStepSize[i]);
}
printf("\nPitch program: \n");
printf("\t  Vertical rise time(secs): %lf\n",lfVertRise);
printf("\t  Pitch angle(degrees from vert): %lf\n",lfPitchAngle);
printf("\t  Pitch Program end time(secs): %lf\n",lfPitchEnd);
printf("\t  Launch azimuth(degrees from north): %lf\n",lfLaunchAzimuth);

     
PLAT = lfLatitude;
PLONG = lfLongitude;
Latitude = PLAT;
Longitude = PLONG;
HEIGHT = lfHeight;
AZ = lfLaunchAzimuth;
PANG = lfPitchAngle; 
VEND = lfVertRise;
PEND = lfPitchEnd;
		

for(i=0; i<NumStages; i++) 
{
COT[i] = lfCutOff[i];
THRUST[i] = lfThrust[i] * 0.4535924;  
FLOW[i] = lfFlow[i] * 0.04625455;
WEIGHT[i] = lfWeight[i] * 0.04625455;
XSEC[i] = lfCrossSection[i] * 0.3048 * 0.3048;
CD[i] = lfDrag[i];    
TIN[i] = lfStepSize[i];  
}

Velocity = 1.0;

printf("\n\n\nDATA\n:\n");

OneRun15(X);
return 0;
}