#include <stdio.h>
#include <math.h>
#include "PitchBlkOut.h"
#define N_Max 100

double Celerity14(double temp);
double Gravity14(double alt);
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 OneRun14();

double Pressure14(double altitude);
double Temprature14(double alt);
double interpolate(double x[], double y[], int k, double Desired, int N_pair);
double Sum_Calculate(double x[], double Desired, int i, int upperBound,int lowerBound);
double Summation(double x[], double y[], int N_pair, double Desired, int upperBound, int lowerBound);

int NumStages;
int CONTROL;
int stage;
	double m;
	double Ft,Fa,Fg;

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;


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 main()
{


	int i;
	
	printf("Kwang-kook, Kim(Shine) NN:14 Numerical Analysis CS407\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:\n",i);
		printf("  Thrust:\n");
		scanf("%lf",&iThrust[i]);
		printf("  Weight:\n");
		scanf("%lf",&iWeight[i]);
		printf("  Flow:\n");
		scanf("%lf",&iFlow[i]);
		printf("  Cross-sectional area:\n");
		scanf("%lf",&iCrossSection[i]);
		printf("  Drag coefficient:\n");
		scanf("%lf",&iDrag[i]);
		printf("  Cut-off time:\n");
		scanf("%lf",&iCutOff[i]);
		printf("  Step size:\n");
		scanf("%lf",&iStepSize[i]);

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

	// Echo Input of Data
	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:\n",i);
		printf("  Thrust:%lf\n",iThrust[i]);
		printf("  Weight:%lf\n",iWeight[i]);
		printf("  Flow:%lf\n",iFlow[i]);
		printf("  Cross-sectional area:%lf\n",iCrossSection[i]);
		printf("  Drag coefficient:%lf\n",iDrag[i]);
		printf("  Cut-off time:%lf\n",iCutOff[i]);
		printf("  Step size:%lf\n",iStepSize[i]);
	}

	printf("Pitch program:\n");
	printf("  Vertical rise time:%lf\n",iVertRise);
	printf("  Pitch angle:%lf\n",iPitchAngle);
	printf("  Pitch Program end time:%lf\n",iPitchEnd);
	printf("  Launch azimuth:%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;

	OneRun14();
	
}

void OneRun14()
{
	int i;
	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];


	Pressure = Pressure14(alt);
	Temperature = Temprature14(alt);
	Celerity = Celerity14(Temperature);
	Gravity = Gravity14(alt);

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

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

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

	for(i=0;i<NumStages;i++)
	{
		stage=i;
		X[6] = Weight[i];
		RK4_System(7,TI[i],0.0,X,(int)(iCutOff[i] / TI[i]));
	}

	//RK4_System(7,TI[stage],0.0,X,(int)(iCutOff[stage] / TI[stage]));

}

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

	alt = Y[2];
	m = Weight[stage] - Flow[stage] * T;

	Pressure = Pressure14(alt);
	Temperature = Temprature14(alt);
	Celerity = Celerity14(Temperature);
	Gravity = Gravity14(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);

	Ft = Thrust[stage];
	Fg = (m) * Gravity;
	Fa = CD[stage] * Q * CrossSection[stage];

	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 * (m));
	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]) / (m);
	DY[4] = (Ft * F[1] + Fg * G[1] - Fa * C[1]) / (m);
	DY[5] = (Ft * F[2] + Fg * G[2] - Fa * C[2]) / (m);
	DY[6] = -(m / Y[6]) * Flow[stage];
}

void output(int j, double T, double Y[])
{
	int i;

	double temp_Latitude=0.0,temp_Longitude=0.0, temp_T;
	for( i = 0; i < 7; i++)
		printf("Y[%d] = %lf\n",i,Y[i]);

	
	temp_Latitude = -(Y[0] / (1852.0 * 60.0)) + Latitude; //x
	temp_Longitude = (Y[1] / (1852.0 * 60.0)) + Longitude; //y
	Range = sqrt(pow(Y[0],2.0) + pow(Y[1],2.0)) / 1852.0;
	temp_T+=T;

	BLKOUT(1, CONTROL,temp_T, Y, C,F, G,PANG, VEND, PEND, AZ,
			-Flow[stage],CrossSection[stage],Thrust[stage],
			Gravity,Pressure,Temperature,Mach,Celerity,Q,CD[stage],AF,
			temp_Latitude,temp_Longitude,Range, Velocity,Acceleration,COT[stage],TI[stage]);
}

double Pressure14(double altitude)
{
	double x, evaluate;
	double A[N_Max];
	int i,j;

	x = altitude / 1000.0;
	if(x > 65.0)
	{
		return 0.0;
	}
	
	else if(x < 25.0)
	{
		A [ 0 ] = -9.666667e-04;
		A [ 1 ] = 7.833333e-02;
		A [ 2 ] = -2.379167e+00;
		A [ 3 ] = 3.429167e+01;
		A [ 4 ] = -2.511667e+02;
		A [ 5 ] = 1.000000e+03;
		evaluate=0.0;
		
		for(i=5; i>=0; i--)
		{
			evaluate+=A[i] * pow(x, (double)(5-i));
		}
		return evaluate * 1000.0 * 0.010197162;
	}
	else if(x > 0.0)
	{
		A [ 0 ] = -2.083333e-06;
		A [ 1 ] = 5.208333e-04;
		A [ 2 ] = -5.104167e-02;
		A [ 3 ] = 2.472917e+00;
		A [ 4 ] = -6.112500e+01;
		A [ 5 ] = 6.650000e+02;
		
		evaluate = 0.0;
		for(j=5; j>=0; j--)
		{
			evaluate+=A[j] * pow(x, (double)(5-j));
		}
		return evaluate * 1000.0 * 0.010197162;
	}
}

double Temprature14(double alt)
{
	double X[100], Y[100];
	double temp;
	
	X[0]=0.0, Y[0]=290.0;
	X[1]=5000.0, Y[1]=240.0;
	X[2]=10000.0, Y[2]=220.0;
	X[3]=15000.0, Y[3]=225.0;
	X[4]=20000.0, Y[4]=230.0;
	X[5]=25000.0, Y[5]=245.0;
	X[6]=30000.0, Y[6]=255.0;
	X[7]=35000.0, Y[7]=265.0;
	X[8]=40000.0, Y[8]=270.0;
	X[9]=45000.0, Y[9]=275.0;
	X[10]=50000.0, Y[10]=270.0;
	X[11]=55000.0, Y[11]=255.0;
	X[12]=60000.0, Y[12]=235.0;
	X[13]=65000.0, Y[13]=215.0;
	X[14]=70000.0, Y[14]=200.0;

	temp = interpolate(X,Y,4,alt / 1000.0,15);
	return (alt > 65000.0 ? 180.0 : temp);
}


double Celerity14(double temp)
{
	double result;

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

double Gravity14(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);
}

void RK4_System(int n, double h, double T, double x[], int nsteps)
{
	double Y[n];							
	double K[4][n];
	int i = 0; 
	int j = 0;

	output(0, T, x);
	
	for(j = 1; 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 + 0.5 * h, Y, K[1]);
		for(i = 0; i < n; i++)
		{
			Y[i] = x[i] + 0.5 * h * K[1][i];
		}


		XP_System(n, T + 0.5 * h, 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+=h;
		output(j, T, x);
	}
}

double interpolate(double x[], double y[], int k, double Desired, int N_pair)
{
	int i=0,j=0,width=0;
	int upperBound=0,lowerBound = 0;
	double fX=0;
	
	for (i=0;i<N_pair;i++) 
	{
		if (x[i]>Desired)
		{
			lowerBound=i-1;
			upperBound=i;
			break;
		}
	}
	width = (k/2) - 1;
	upperBound+=width;
	lowerBound-=width;

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

	fX = Summation(x,y,N_pair,Desired,upperBound,lowerBound);
	return fX;
}

double Summation(double x[], double y[], int N_pair, double Desired, int upperBound, int lowerBound)
{
	double sum = 0;
	int i=lowerBound;
	N_pair=upperBound;

	for(i=lowerBound;i<=upperBound;i++) 
	{ 
          sum+=(y[i] * Sum_Calculate(x,Desired,i,upperBound,lowerBound)); 
	} 
	return sum; 
}

double Sum_Calculate(double x[], double Desired, int i, int upperBound,int lowerBound) 
{ 
     
     double result = 1;  
	 int j = 0;
     for(j=lowerBound; j<=upperBound; j++) 
     { 
          if (j!=i)  
          { 
			   result*=((Desired - x[j]) / (x[i]- x[j]));  
          } 
     } 
     return result; 
}