#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);
double max(double a, double b);

void OneRun23();

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

void Gauss(int n, double a[10][10], double b[10], double x[10]);

void Search23();

double X[100];

 int anykey;



int MaxIter=10;

double G1,G2,G3,Pert1,Pert2,Pert3;
double B1,B2,B3,Tol1,Tol2,Tol3;
double Xmin,Xmax;
int NSUB=20;
double TIME[2000],Ys[2000];
 
 
 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;

//saved c matrix


/*
double X1final;
double X2final;
double X3final;
*/

void Search23()
{
	double sta, stb, stc;

	double pert1a, pert1b, pert1c;
	double pert2a, pert2b, pert2c;
	double pert3a, pert3b, pert3c;
	
	double error1, error2, error3;

	int i = 0, j = 0;
	double A[10][10], B[10]; 
		//X[10];
	double temp, temp2, temp3;
	double resultX[10];

	double saveAZ, savePA, saveTH;
	saveAZ = az;
	savePA = pang;
	saveTH = thrust[0];
	

	for (i = 0; i < 10; i++)
	{
		B[i] = 0.0;
		X[i] = 0.0;
		for (j = 0; j < 10; j++)
			A[i][j] = 0.0;
	}
	//printf("you are here2  MaxIter is %d \n", MaxIter);

	for(i = 0; i < MaxIter; i++)
	{	/*	
	G1 =az;
	G2 =pang;
	G3 =thrust[0];
	*/

		int p=0;
		int q=0;



		az = saveAZ;
		pang = savePA;
		thrust[0] = saveTH;

		OneRun23();  //do a regular run
		sta = X[0];  //save the regular results
		stb = X[1];
		stc = X[2];

		//printf("sta is %lf stb is %lf stc is %lf",sta,stb,stc);
		//printf("B1 is %lf B2 is %lf B3 is %lf",B1,B2,B3);
		
		B[0] = (B1 - sta);
		B[1] = (B2 - stb);
		B[2] = (B3 - stc);


		/*
		if (fabs(X[0] - B1) < Tol1 && fabs(X[1] - B2) < Tol2 && fabs(X[2] - B3) < Tol3)
		{
			printf("Tolerence met. Completed in %d iterations.\n", i+1);
	
			break; 
		}
		*/

	//	printf ("stc - B3 = %lf\n", fabs(stc-B3));
	//	printf ("stc - B3 / B3  \n ", fabs(fabs(stc-B3)/fabs(B3));

		error1 = (fabs(B1 - sta) / fabs(B1));
		error2 = (fabs(B2 - stb) / fabs(B2));
		error3 = (fabs(B2 - stc) / fabs(B3));
		
		printf ("erro1 is %lf error2 is %lf error3 is %lf\n tol1 is %lf tol2 is %lf tol3 is %lf" ,error1, error2, error3, Tol1, Tol2, Tol3);
		
		//if (fabs(fabs(B[0])/fabs(B1)) < Tol1 && fabs(fabs(B[1])/fabs(B2)) < Tol2 && fabs(fabs(B[2])/fabs(B3)) < Tol3)   // check for tolerance
		
		//if ((error1 < Tol1) && (error2 < Tol2) && (error3 < Tol3))
		if (fabs(fabs(B[0])/fabs(B1)) < Tol1 && fabs(fabs(B[1])/fabs(B2)) < Tol2 && fabs(fabs(B[2])/fabs(B3)) < Tol3)   // check for tolerance
		{
			printf("Tolerence met. Completed in %d iterations.\n", i+1);
            break;            		
		}
		
		
		//change AZ
		az = ( saveAZ * (Pert1 +1));
		G1 = az;
		//restore other 2
		pang=savePA;
		thrust[0]=saveTH;
		OneRun23();  //perturbation run 1
		pert1a = X[0]; 
		pert1b = X[1];
		pert1c = X[2];
		
		
		//change PANG
		pang = ( savePA * (Pert2 +1));
		G2 = pang;
		//restore other 2
		az=saveAZ;
		thrust[0]=saveTH;
		OneRun23();  //perturbation run 2
		   pert2a = X[0]; 
		pert2b = X[1];
		pert2c = X[2];


		//change thrust
		thrust[0] = ( saveTH * (Pert3 +1));
		G3 = thrust[0];	
		//restore other 2
		az = saveAZ;
		pang=savePA;
		OneRun23();  //perturbation run 3
	    pert3a = X[0]; 
		pert3b = X[1];
		pert3c = X[2];

		/*
		G1 = saveAZ;
		G2 = savePA;
		G3 = saveTH;
		*/
	
		
		
		
			

		A[0][0] = (pert1a - sta) / (Pert1 * G1); 
		A[0][1] = (pert2a - sta) / (Pert1 * G2);
		A[0][2] = (pert3a - sta) / (Pert1 * G3);
		A[1][0] = (pert1b - stb) / (Pert2 * G1);
		A[1][1] = (pert2b - stb) / (Pert2 * G2);
		A[1][2] = (pert3b - stb) / (Pert2 * G3);
		A[2][0]=  (pert1c - stc) / (Pert3 * G1);
		A[2][1]=  (pert2c - stc) / (Pert3 * G2);
		A[2][2]=  (pert3c - stc) / (Pert3 * G3);
/*
		for (p =0; p <3; p++)
		{	
			for (q=0; q<3; q++)
			{
				//printf(" %lf ", A[p][q]);
			}
			//printf("\n");
		}
*/
	
	printf("Azimuth=%lf, Pitch angle=%lf, Thrust=%lf\n", az, pang, thrust[0]);
	printf("X[0] = %lf, X[1] = %lf, X[2] = %lf\n", X[0], X[1], X[2]);   // feed back

		X[0] = 0;
		X[1] = 0;
		X[2] = 0;

		Gauss(3, A, B, resultX);
		
		saveAZ += resultX[0];
		savePA += resultX[1];		
		saveTH += resultX[2];	
		
		/*
		az += resultX[0];
		pang += resultX[1];		
		thrust[0] += resultX[2];	
		*/				


	}
}


void OneRun23()
{
	int nsteps;
	int tempsteps;
	int i=0;
	double y[7];
	
	g[0] = 0.0;
	g[1] = 0.0;
	g[2] = -1.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[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[2] =1.0;

	for (i = 0; i < numstages; i++)		
	{
		stage =i;
		nsteps = (int) COT[i] / TI[i];
		y[6] = weight[i];
		//printf("you are here\n");
			
		
		if (i == 0)
		{
			
			//printf("runing if i =0\n");
			RK4_System(7, TI[i], 0, X, nsteps);
		}
		else
		{	
			//printf("runing else\n");
			RK4_System(7, TI[i], 100, X, ( (int) ((COT[i]-COT[i-1]) / TI[i]) )  );
		}
	}	
}

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, gravity);
	
	
	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]);
	
	
	
}

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);
	
	//scanf("%d",&anykey);	

	for(j = 1; j <= nsteps; j++)
	{
		//printf("step %d \n", 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 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);

	if (altitude >= 65000)
	{
		temp = 180;
	
	
	}

	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;
	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;
	pressure = pressure23(Y[2]);
	
	
	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);


	 
	chglatt = (fabs(Y[0]) /(1852.0 * 60.0)) + lattitude;  
	chglongt = (fabs(Y[1]) /(1852.0 * 60.0)) + 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, chglatt, chglongt,
		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)
{
	
	if (temperature == 180)
	{
		return 1000;
	}

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

}




double max(double a, double b)
{
	if(a > b)
	{
		return a;
	}
	else
	{
		return b;
	}
}




void Gauss(int n, double a[10][10], double b[10], double x[10])
{
	int I[10];
	double s[10];
	int i = 0;
	int j = 0;
	int k = 0;
	double r; 
	double rmax = 0;
	double smax = 0; 
	double xmult = 0; 
	double temp = 0; 
	double sum = 0;

	for(i=0; i < n; i++)
	{
		I[i] = i;
		smax = 0;
		for(j=0; j<n; j++)
		{ 
			smax = max(smax, fabs(a[i][j]));
		}
		s[i] = smax;
	} 
	for(k = 0; k < n-1; k++)
	{
		rmax = 0;
		for(i = k; i < n; i++)
		{
		r = fabs(a[I[i]][k] / s[I[i]]);
	    if(r > rmax)
	    {
			rmax = r;
			j = i;
	    }
		}
		temp = I[j];
		I[j] = I[k];
		I[k] = temp;

		for(i = k + 1; i<n; i++)
		{
			xmult = a[I[i]][k] / a[I[k]][k];
			a[I[i]][k] = xmult;
			for(j = k + 1; j < n; j++)
			{
				a[I[i]][j] = a[I[i]][j] - (xmult)*(a[I[k]][j]);
			}
		}
	}
 
	for(i = 0;  i < 4 ; i++)
	x[i] = 0;
	i = k = sum = 0;
	for(k=0; k<n-1; k++)
	{
		for(i=k+1; i<n; i++)
		{
			b[I[i]] = b[I[i]] - a[I[i]][k] * b[I[k]];
		}
	} 
	x[n-1] = b[I[n-1]] / a[I[n-1]][n-1];
	for(i = n-2; i >=0; i--)
	{  
		sum = b[I[i]];
		for(j=i+1; j<n; j++)
		{
			sum = sum -  a[I[i]][j] *x[j];
		}
		x[i] = sum / a[I[i]][i];
	}

}


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

	c[0]=0.0; 
	c[1]=0.0; 
	c[2]=0.0;

	G1 =az;
	G2 =pang;
	G3 =thrust[0];

scanf("\n enter B1 \t");
	scanf("%lf",&B1);
scanf("\n enter pert1 \t");
	scanf("%lf",&Pert1);
scanf("\n enter Tol1 \t");
	scanf("%lf",&Tol1);

scanf("\n enter B2 \t");
	scanf("%lf",&B2);
scanf("\n enter pert2 \t");
	scanf("%lf",&Pert2);
scanf("\n enter Tol2 \t");
	scanf("%lf",&Tol2);


scanf("\n enter B3 \t");
	scanf("%lf",&B3);
scanf("\n enter pert3 \t");
	scanf("%lf",&Pert3);
scanf("\n enter Tol3 \t");
	scanf("%lf",&Tol3);


//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);

	
printf("\n enter b1 \t");
	printf("%lf",B1);
printf("\n enter pert1 \t");
	printf("%lf",Pert1);
printf("\n enter Tol1 \t");
	printf("%lf",Tol1);

printf("\n enter B2 \t");
	printf("%lf",B2);
printf("\n enter pert2 \t");
	printf("%lf",Pert2);
printf("\n enter Tol2 \t");
	printf("%lf",Tol2);


printf("\n enter B3 \t");
	printf("%lf",B3);
printf("\n enter pert3 \t");
	printf("%lf",Pert3);
printf("\n enter Tol3 \t");
	printf("%lf",Tol3);


//=========================================================================



Search23();
printf("search completed\n final answer for Azimuth, Pich angle and Thrust is\n");
printf("Azimuth=%lf, Pitch angle=%lf, Thrust=%lf\n", az, pang, thrust[0]);
	/*
	printf("Azimuth=%lf, Pitch angle=%lf, Thrust=%lf\n", az, pang, thrust[0]);
	printf("X[0] = %lf, X[1] = %lf, X[2] = %lf\n", X[0], X[1], X[2]);   // feed back
	*/	

	
return 0;
}