#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include "PitchBlkOut.h"
#define RK_order 4
#define N_Max 100
static const int Neq = 2;


void XP_System(int Neq, double T, double Y[], double DY[]);
void RK4_System(int n, double h, double T, double x[], int nsteps);

double interpolate(int numberofpairs, double x[], double y[], double bigx, int k);
double product(double x[], double bigx, int i, int upperbound,int lowerbound) ;
double summation(double x[], double y[], int numberofpairs, double bigx, int upperbound, int lowerbound);

double pressure23(double altitude);
double temperature23(double altitude);
double gravity23(double alt);
double celerity23(double temperature);

void onerun();

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

 int anykey;

int numstages;
int control;
int stage=1;

//A  compute in atmospheric routine
double pressure;
double temperature;

//F compute in the function evaluation part of the integration
double velocity;
double c[3]; //unit vector for velocity

//
double celerity;
double mach;
double q;
double af;

//G compute in gravity routine
double gravity;
double g[3];

//O compute in output routine
double lattitude;
double longitude;
double altitude;
double acceleration;
double range;

//P  compute in pitch program routine
double az;
double pang;
double vend;  //vertical rise time
double pend;
//
double f[3];


//s  adjust values at staging
double COT[2];
double thrust[2];
double weight[2];
double flow[2];
double crosssection[2];
double CD[2];
double TI[2];

//I input quantities
double plat;
double plong;
double height;

//three coefficients for unit vectors
double ft;
double fg;
double fa;


void onerun()
{
	int nsteps;
	int i=0;
	double y[7];
	
	g[0] = 0.0;
	g[1] = 0.0;
	g[2] = -1.0;
	
	y[0] =0.0;
	y[1] =0.0;
	y[2] = height;
	y[3] =0.0;
	y[4] =0.0;
	y[5] =0.0;				
	y[6] = weight[0];				
	velocity =1.0;
	acceleration =2.0;
	temperature = temperature23(height);
	celerity = celerity23(temperature);
	gravity = gravity23(height);
	mach =  velocity/celerity;
	pressure = pressure23(height);
	q = 0.7 * pressure * (mach * mach);
	f[3] =1.0;

	for (i = 0; i < numstages; i++)		
	{
		stage =i;
		nsteps = (int) COT[i] / TI[i];
		y[6] = weight[i];
		RK4_System(7, TI[i], 0, y, nsteps);
	}	
}

void XP_System(int Neq, double T, double Y[], double DY[])
{

	int i;
	double mass;
	temperature = temperature23(Y[2]);	
	celerity = celerity23(temperature);					
	velocity = sqrt( (Y[3]*Y[3]) + (Y[4]*Y[4]) + (Y[5]*Y[5]) );		
	gravity = gravity23(Y[2]);	
	mach =  velocity / celerity;
	printf("\n\n pressure is currently %lf sending y2 to pressure. y2 is %lf \n\n", pressure, Y[2]);
	pressure = pressure23(Y[2]);
	printf("\n\n pressure is currently %lf \n\n", pressure);
	q = 0.7 * pressure * (mach * mach);
	
	mass = weight[stage] - flow[stage] * T;

	ft = thrust[stage];
	fg = mass*gravity;
	fa = CD[stage] * q * crosssection[stage];	
	
	acceleration = (ft - fa) / (9.80655 * mass);
	af = fa;

	if (T!=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;
	}
	

		
	
	
	printf("Pressure = %lf, Temperature = %lf, Celerity = %lf, Gravity = %lf\n", pressure, temperature, celerity, g[2]);
	
	
	PITCH(T, az, pang, vend, pend, f, c);

	DY[0] = Y[3];								
	DY[1] = Y[4];								
	DY[2] = Y[5];
/*	
	DY[3] = ((thrust[stage] * f[0]) - (fa * c[0])) / Y[6];
	DY[4] = ((thrust[stage] * f[1]) - (fa * c[1])) / Y[6];	
	DY[5] = ((thrust[stage] * f[2]) - g[2] - (fa * c[2])) / Y[6];
*/

	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]);
	
	//acceleration = sqrt(pow(DY[0], 2.0) + pow(DY[0], 2.0) + pow(DY[0], 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++)
	{
	scanf("%d",&anykey);	
		
		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 product(double x[], double bigx, int i, int upperbound,int lowerbound) 
{ 
     
	double result = 1;  
	int j = 0;
     
	for(j=lowerbound; j<=upperbound; j++) 
     	{ 
		if (j!=i)
		{ 
			result = (result) * ((bigx - x[j]) / (x[i]- x[j]));  
		} 
	} 
	return result; 
}




double summation(double x[], double y[], int numberofpairs, double bigx, int upperbound, int lowerbound)
{

	double sum = 0;
	int i=lowerbound;
	numberofpairs=upperbound;
	
	/*
	printf("inside summation\n");
	printf("bigx is %lf\n", bigx);
	printf("upperbound is is %d lowerbound is %d\n", upperbound, lowerbound);
	printf("npair is %d\n", numberofpairs);	*/
	

	for(i=lowerbound; i<=upperbound; i++) 
	{ 
		sum+=(y[i] * product(x,bigx,i,upperbound,lowerbound)); 
	} 
	return sum; 
}





double interpolate(int numberofpairs, double x[], double y[], double bigx, int k)
{
	int i=0,j=0,width=0;
	int upperbound=0,lowerbound = 0;
	double fx=0;
	
/*
	printf("bigx is %lf\n", bigx);
	printf("k is %d\n", k);
	printf("npair is %d\n", numberofpairs);
*/	



	for (i=0;i<numberofpairs;i++) 
	{
		if (x[i]>bigx)
		{
			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 > numberofpairs)  
		{
			lowerbound=lowerbound-(abs(upperbound-numberofpairs));
			upperbound=numberofpairs;
		}
	}

	fx = summation(x,y,numberofpairs,bigx,upperbound,lowerbound);
	//printf("fx is %lf\n", fx);
	return fx;
}




double temperature23(double altitude)
{
	double tempalt;
	double X[20];
	double Y[20];
	double temp = 0;


	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;

	
	tempalt = altitude;
	temp= interpolate(15,X,Y,tempalt,4);

	return temp;


}



double pressure23(double alt)
{
	double press =0.0;
	
	double X[6];
	//X = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
	double results[6];	
	
	int i =0;
	double tempsum=0;

	alt = alt/1000;

	if (alt > 65)
	{
		press =0;
	}
	else	
	{
		if (alt > 25)
		{  //we got these values by running gauss....

		/*	X[0] = -0.000002;
			X[1] = 0.000521;
			X[2] = -0.051042;
			X[3] = 2.472917;
			X[4] = -61.125000;
			X[5] = 665.000000;
*/
		X[0] = -2.0833e-06;
		X[1] = 5.2083e-04;
		X[2] = -5.1042e-02;
		X[3] = 2.4729;
		X[4] = -6.1125e+01;
		X[5] = 6.6500e+02;
			
			
		}
		else
		{
	/*
			X[0] = -0.000967;
			X[1] = 0.078333;
			X[2] = -2.379167;
			X[3] = 34.291670;
			X[4] = -251.166700;
			X[5] = 1000.000000;
*/
		X[0] = -9.6667e-04;
		X[1] = 7.8333e-02;
		X[2] = -2.3792;
		X[3] = 3.4292e+01;
		X[4] = -2.5117e+02;
		X[5] = 1000;

		}
		
		
		results[0] = (X[0] * (pow ( alt, 5)) );
		results[1] = (X[1] * (pow ( alt, 4)) );
		results[2] = (X[2] * (pow ( alt, 3)) );
		results[3] = (X[3] * (pow ( alt, 2)) );
		results[4] = (X[4] * (pow ( alt, 1)) );
		results[5] = (X[5]);
		
		
		printf("%lf * %lf ^ 5  = %lf\n", X[0], alt, results[0]);
		printf("%lf * %lf ^ 4  = %lf\n", X[1], alt, results[1]);
		printf("%lf * %lf ^ 3  = %lf\n", X[2], alt, results[2]);
		printf("%lf * %lf ^ 2  = %lf\n", X[3], alt, results[3]);
		printf("%lf * %lf ^ 1  = %lf\n", X[4], alt, results[4]);
		printf("%lf * %lf ^ 0  = %lf\n", X[5], alt, results[5]);
		

		press = results[0] + results[1] + results[2] + results[3] + results[4] + results[5];   



	}
	
	return press * 1000 * .010197162;
}




void output(int j, double T, double Y[])
{
	
	double chglongt, chglatt;
/*
	chglatt = (lattitude + Y[0]) / (1852.0 * 60.0);		
	chglongt = (longitude + Y[1]) / (1852.0 * 60.0);

	lattitude = lattitude + chglatt;
	longitude = longitude + chglongt;

*/

	lattitude = -(Y[0] /(1852 * 60)) + lattitude;
	longitude = +(Y[1] /(1852 * 60)) + longitude;

	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], fa, lattitude, longitude,
		sqrt(pow(Y[0], 2.0) + pow(Y[1], 2.0))/1852.0, velocity, acceleration, COT[stage], TI[stage]);

}


double gravity23(double alt)
{
	return (3.98622e+14) / (pow(((6.378178e+06) + (alt)),2));
}

double celerity23(double temperature)
{
	return sqrt(((1.4)*((8.31432e+03) / (28.9644)) * temperature));
}


int main()
{
int i;
double testalt=0.0;
double testpress=0.0;
	//gather input section==================================================================

printf("gotta test the pressure enter an altitude in KM\n");
scanf("%lf",&testalt);
testpress = pressure23(testalt);
printf("the pressure is %lf \n", testpress);


	printf("enter the number of stages\n");
	scanf("%d",&numstages);
	printf("enter control variable\n");
	scanf("%d",&control);
	printf("enter lattitude\n");
	scanf("%lf",&lattitude);
	printf("enter longitude\n");
	scanf("%lf",&longitude);
	printf("enter height\n");
	scanf("%lf",&height);
	
	//loop twice	
	for(i=0;i<2;i++)
	{
		printf("enter thrust\n");
		scanf("%lf",&thrust[i]);
		thrust[i] = thrust[i] * 0.4535924;
		
		printf("enter weight\n");
		scanf("%lf",&weight[i]);
		weight[i] = weight[i] * .04625455;

		printf("enter flow rate\n");
		scanf("%lf",&flow[i]);
		flow[i] = flow[i] * .04625455;
		//flow[i] = (flow[i] * -1);

		printf("enter cross sectional area\n");
		scanf("%lf",&crosssection[i]);
		crosssection[i] = crosssection[i] * (0.3048 * 0.3048);

		printf("enter drag coefficient\n");
		scanf("%lf",&CD[i]);
		printf("enter cut off time\n");
		scanf("%lf",&COT[i]);
		printf("enter integration step size\n");
		scanf("%lf",&TI[i]);
	
	}
	
	printf("enter vertical rise time\n");
	scanf("%lf",&vend);
	
	printf("enter pitch angle\n");
	scanf("%lf",&pang);
	
	
	printf("enter time of end of pitch program\n");
	scanf("%lf",&pend);
	
	printf("enter launch azimuth (in degrees from north)\n");
	scanf("%lf",&az);
	



//ECHO input section==================================================================
	printf("\n echo\n");
	printf("\nenter the number of stages\t");
	printf("%d",numstages);
	printf("\nenter control variable\t");
	printf("%d",control);
	printf("\nenter lattitude\t");
	printf("%lf",lattitude);
	printf("\nenter longitude\t");
	printf("%lf",longitude);
	printf("\nenter height\t");
	printf("%lf",height);

	//loop twice	
	for(i=0;i<2;i++)
	{
	
		printf("\nenter thrust\t");
		printf("%lf",thrust[i]);
		printf("\nenter weight\t");
		printf("%lf",weight[i]);
		printf("\nenter flow rate\t");
		printf("%lf",flow[i]);
		printf("\nenter cross sectional area\t");
		printf("%lf",crosssection[i]);
		printf("\nenter drag coefficient\t");
		printf("%lf",CD[i]);
		printf("\nenter cut off time\t");
		printf("%lf",COT[i]);
		printf("\nenter integration step size \t");
		printf("%lf",TI[i]);
	}

	printf("\nenter vertical rise time \t");
	printf("%lf",vend);
	printf("\nenter pitch angle \t");
	printf("%lf",pang);
	printf("\nenter time of end of pitch program \t");
	printf("%lf",pend);
	printf("\nenter launch azimuth (in degrees from north) \t");
	printf("%lf",az);
//=========================================================================




onerun();

return 0;
}