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

void Search(int stages, double Weight[], double Tol[], double Pert[], double Xfin[]); 
void OneRun(int stages, double Weight[], double X[]); 
void RK4_System(int n, double h, double t, double X[], int nsteps); 
void XP_System(int Neq, double T, double X[], double DY[]); 
void output(int j, double T, double Y[]);


double Celerity14(double temp);
double Gravity14(double alt);
double Pressure14(double altitude);
double Temperature14(double alt);

void Gauss(int n, double a[][3], double x[], double b[]); 
double do_lagrange(double pairs[][2], double x, int k, int num_pairs); 

int print_control, n_stage;		
double Vrt, Pang, Pend, Az, time, Pressure, Temperature, Mach, Celerity, Q, Fa, start_lat, start_long, start_ht, velocity, g;
double Thrust[5], FlowRate[5], XSect[5], CD[5], COT[5], TI[5], F[3], G[3], C[3];
int i, j, k;
double F1S, F1P1, F1P2, F1P3, F2S, F2P1, F2P2, F2P3, F3S, F3P1, F3P2, F3P3;
double I[3], B[3], X[7], Guess[3];
double A[3][3];


void Search(int stages, double Weight[], double Tol[], double Pert[], double Xfin[])
{

	double temp_P_control=print_control;
	print_control=0;
	Guess[0] = Az;
	Guess[1] = Pang;
	Guess[2] = Thrust[0];

	for (i = 0; i < 8; i++)
	{
		printf("\nIteration %d, Run 1\n", i);
		for (j = 0; j < 6; j++)
		{
			X[j] = 0.0;
		}
		X[2] = start_ht;
		Az = Guess[0];
		Pang = Guess[1];
		Thrust[0] = Guess[2];
		printf("\nAZ: %lf PANG: %lf Thrust: %lf\n", Az, Pang, Thrust[0]);	
		
		
		OneRun(stages, Weight, X);
		if (fabs(X[0] - Xfin[0]) < Tol[0] && fabs(X[1] - Xfin[1]) < Tol[1] && fabs(X[2] - Xfin[2]) < Tol[2])
		{
			break; 
		}

		F1S = X[0];
		F2S = X[1];
		F3S = X[2];
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		for (j = 0; j < 6; j++)
		{
			X[j] = 0.0;
		}
		X[2] = start_ht;
		Az += (Pert[0] * Az);
		printf("\nIteration %d, Run 2\n", i);
		OneRun(stages, Weight, X);
		F1P1 = X[0];
		F2P1 = X[1];
		F3P1 = X[2];

//////////////////////////////////////////////////////////////////////////////////////////////////
		for (j = 0; j < 6; j++)
		{
			X[j] = 0.0;
		}
		X[2] = start_ht;
		Az = Guess[0];
		Pang += (Pert[1] * Pang);
		printf("\nIteration %d, Run 3\n", i);
		OneRun(stages, Weight, X);
		F1P2 = X[0];
		F2P2 = X[1];
		F3P2 = X[2];

////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		for (j = 0; j < 6; j++)
		{
			X[j] = 0.0;
		}
		X[2] = start_ht;
		Pang = Guess[1];
		Thrust[0] += (Pert[2] * Thrust[0]);
		printf("\nIteration %d, Run 4\n", i);
		OneRun(stages, Weight, X);
		F1P3 = X[0];
		F2P3 = X[1];
		F3P3 = X[2];
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

		A[0][0] = (F1P1 - F1S) / (Pert[0] * Guess[0]);
		A[0][1] = (F1P2 - F1S) / (Pert[1] * Guess[1]);
		A[0][2] = (F1P3 - F1S) / (Pert[2] * Guess[2]);
		
		A[1][0] = (F2P1 - F2S) / (Pert[0] * Guess[0]);
		A[1][1] = (F2P2 - F2S) / (Pert[1] * Guess[1]);
		A[1][2] = (F2P3 - F2S) / (Pert[2] * Guess[2]);

		A[2][0] = (F3P1 - F3S) / (Pert[0] * Guess[0]);
		A[2][1] = (F3P2 - F3S) / (Pert[1] * Guess[1]);
		A[2][2] = (F3P3 - F3S) / (Pert[2] * Guess[2]);
		B[0] = Xfin[0] - F1S;
		B[1] = Xfin[1] - F2S;
		B[2] = Xfin[2] - F3S;

		Gauss(3, A, I, B);
		for (j = 0; j < 3; j++)
		{
			Guess[j] += I[j];
		}
	}

		print_control=temp_P_control;
		
		for (j = 0; j < 6; j++)
		{
			X[j] = 0.0;
		}
		X[2] = start_ht;
		Az = Guess[0];
		Pang = Guess[1];
		Thrust[0] = Guess[2];
		
		OneRun(stages, Weight, X);

	printf("\nFinal Check: "); 
	printf("\nX = %lf, Y = %lf, Z = %lf\n", X[0], X[1], X[2]);
}

void output(int j, double T, double Y[])
{
	
	int i;
	double met_long, met_lat;
	
	met_lat = start_lat * 1852.0 * 60.0;			
	met_long = start_long * 1852.0 * 60.0;		
	
	if(print_control)
	{
	BLKOUT(1, print_control, time, Y, C, F, G, Pang, Vrt, Pend, Az, -FlowRate[n_stage], XSect[n_stage], Thrust[n_stage], g, 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)) / 1852.0, velocity, (Thrust[n_stage] - Fa) / (9.80665 * Y[6]),
		COT[n_stage], TI[n_stage]);
	}
}

void OneRun(int stages, double Weight[], double X[])
{		
	int nsteps, i;
	
	time = 0.0;
	
	for (i = 0; i < stages; i++)			
	{
		nsteps = (int) COT[i] / TI[i];		
		n_stage = i;				
		X[6] = Weight[i];			
		RK4_System(7, TI[i], time, X, nsteps);		
	}	
}

void RK4_System(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] += (1.0/6.0) * h * (K[0][i] + (2 *  K[1][i]) + (2 * K[2][i]) + K[3][i]);
		}
		
		t += h;
		time += h;			
		output(j, t, X);
	}
}

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

	alt=Y[2];

	velocity = sqrt(pow(Y[3], 2.0) + pow(Y[4], 2.0) + pow(Y[5], 2.0));		
	
	if (velocity != 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, Vrt, Pend, F, C);

	Pressure = Pressure14(X[2]);
	Temperature = Temperature14(X[2]);							
	Celerity = Celerity14(Temperature);					
	Mach =  velocity / Celerity;							
	
	Q = 0.7 * Pressure * pow(Mach, 2.0);						
	Fa = CD[n_stage] * Q * XSect[n_stage];						
	g = Gravity14(X[2]);

	DY[0] = Y[3];									
	DY[1] = Y[4];									
	DY[2] = Y[5];									
	DY[3] = ((Thrust[n_stage] * F[0]) - (Fa * C[0])) / Y[6];			
	DY[4] = ((Thrust[n_stage] * F[1]) - (Fa * C[1])) / Y[6];			
	DY[5] = (((Thrust[n_stage] * F[2]) - (Fa * C[2])) / Y[6]) - g;			
	DY[6] = -FlowRate[n_stage];							
}

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++) 		
		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) {					
		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) {		
		x_end = num_pairs - 1;
		k_count -= (x_end - x_middle);
		k_count -= (x_middle - x_begin);
		x_end+=k_count;
	} else {							
		k_count -= (x_end - x_middle);
		k_count -= (x_middle - x_begin);
		x_end+=k_count;
		if(x_end >= num_pairs) 
		{		
			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;
}



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 Temperature14(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 Celerity14(double temp)
{
	double result;
	result = pow((1.4 * (8314.32 / 28.9644)) * temp,0.5);
	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 Gauss(int n, double a[][3], double x[], double b[])
{	
	
	int L[10];
	double s[10];
	int i, j, k, temp;
	double r, rmax, smax, xmult, sum;
	
	for (i = 0; i < n; i++)
	{
		L[i] = i;
		smax = 0.0;
		for (j = 0; j < n; j++)
		{
			if (fabs(a[i][j]) > smax)
				smax = fabs(a[i][j]);
		}
		s[i] = smax;
	}
	for (k = 0; k < n - 1; k++)
	{
		rmax = 0.0;
		for (i = k; i < n; i++)
		{
			r = fabs(a[L[i]][k] / s[L[i]]);
			if (r > rmax)
			{
				rmax = r;
				j = i;
			}
		}
		temp = L[j];
		L[j] = L[k];
		L[k] = temp;
		for (i = k + 1; i < n; i++)
		{
			xmult = a[L[i]][k] / a[L[k]][k];
			a[L[i]][k] = xmult;
			for (j = k + 1; j < n; j++)
				a[L[i]][j] = a[L[i]][j] - (xmult * a[L[k]][j]);
		}
	}
	for (k = 0; k < n - 1; k++)
	{
		for (i = k + 1; i < n; i++)
			b[L[i]] = b[L[i]] - (a[L[i]][k] * b[L[k]]);
	}
	x[n - 1] = b[L[n - 1]] / a[L[n - 1]][n - 1];
	for(i = n - 2; i >= 0; i--)
	{
		sum = b[L[i]];
		for (j = i + 1; j < n; j++)
			sum = sum - (a[L[i]][j] * x[j]);
		x[i] = sum / a[L[i]][i];
	}
}	

int main()
{
	int closed, i, stages;
	double Weight[5], Pert[3], Tol[3], Xfin[3];

	
	printf("Kwang-kook, Kim(Shine) NN:14 Numerical Analysis for CS407\n");
	scanf("%d %d %lf %lf %lf", &stages, &print_control, &start_lat, &start_long, &start_ht);
	for (i = 0; i < stages; i++)
		scanf("%lf %lf %lf %lf %lf %lf %lf", &Thrust[i], &Weight[i], &FlowRate[i], &XSect[i], &CD[i], &COT[i],
			 &TI[i]);
	scanf("%lf %lf %lf %lf", &Vrt, &Pang, &Pend, &Az);
	for (i = 0; i < 3; i++)
		scanf("%lf %lf %lf", &Xfin[i], &Pert[i], &Tol[i]);	


/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// Echo Input of Data
	printf("\nControl values:\n");
	printf("  Number of stages:%d\n",stages);
	printf("  Print control: %d\n",print_control);
	printf("Launch Point\n");
	printf("  Latitude:%lf\n",start_lat);
	printf("  Longitude:%lf\n",start_long);
	printf("  Height:%lf\n",start_ht);

	for(i = 0; i < stages; i++) 
	{
		printf("\nThe vehicle:\n",i);
		printf("  Thrust:%lf\n",Thrust[i]);
		printf("  Weight:%lf\n",Weight[i]);
		printf("  Flow:%lf\n",FlowRate[i]);
		printf("  Cross-sectional area:%lf\n",XSect[i]);
		printf("  Drag coefficient:%lf\n",CD[i]);
		printf("  Cut-off time:%lf\n",COT[i]);
		printf("  Step size:%lf\n",TI[i]);
	}

	printf("\nPitch program:\n");
	printf("  Vertical rise time:%lf\n",Vrt);
	printf("  Pitch angle:%lf\n",Pang);
	printf("  Pitch Program end time:%lf\n",Pend);
	printf("  Launch azimuth:%lf\n",Az);
	
	for(i = 0; i < stages; i++) 
	{
		printf("\n");
		printf("  Desired value:%lf\n",Xfin[i]);
		printf("  Perturbation:%lf\n",Pert[i]);
		printf("  tolerance:%lf\n",Tol[i]);
	}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	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);
	}

	G[0] = 0.0;										 							
	G[1] = 0.0;
	G[2] = -1.0;

	Search(stages, Weight, Tol, Pert, Xfin);
	printf("\nAzimuth = %lf, Pitch angle = %lf, Thrust = %lf\n", Az, Pang, Thrust[0]);
	return 0;
}