/*
 *   Analysis C
 */

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

#define MAXN 10   // used as the max index in alot of the array declarations
#define MAX_ITERATIONS 10
double Pressure26(double alt);
double Temperature26(double alt);
double Celerity26(double temp, double alt);
double Gravity26(double alt);
double convertMilliBarsToKSM(double millibars);
double func(double x[MAXN], int degree, double xVal);

void Search26(int MaxIter, double endBoundConditions[], double Pert[], double Tol[]);
void OneRun(double X[]);

// Runge-Kutta functions
void RK4_System(int n, double h, double *tref, double X[], int nsteps);
void XP_System(int Neq, double T, double Y[], double DY[]);
void output(int k, double T, double Y[]);

// Gauss functions
void gaussSolve(double a[MAXN][MAXN], double b[MAXN], double x[MAXN], int n);
void gauss(double a[MAXN][MAXN], int l[MAXN], int n);
void solve(double a[MAXN][MAXN], double b[MAXN], double x[MAXN], int l[MAXN], int n);
double max(double a, double b);

void header();

struct pair {
	double x;
	double y;
};

#define MAXSTAGES 10
int numStages;
int PCONTROL=0, TEMP_PCONTROL=0;
int currentStage;
double COT[MAXSTAGES], Thrust[MAXSTAGES], Weight[MAXSTAGES];
double Flow[MAXSTAGES], CrossSection[MAXSTAGES], CD[MAXSTAGES], TI[MAXSTAGES], AZ, PANG, VEND, PEND;
double PLAT,PLONG,HEIGHT;
double Velocity, Gravity, Pressure, Temperature, Celerity, Mach, Q, AF, Acceleration, Range;
double C[3], F[3], G[3]={0.0, 0.0, -1.0};
// double Latitude, Longitude, Altitude, Acceleration, Range;



int main()
{
	double iLat, iLong, iHeight;
	double iThrust[MAXSTAGES], iWeight[MAXSTAGES], iFlow[MAXSTAGES], iCrossSection[MAXSTAGES], iDrag[MAXSTAGES], iCutOff[MAXSTAGES], iStepSize[MAXSTAGES];
	double iVertRise, iPitchAngle, iPitchEnd, iLaunchAzimuth;
	double iDesiredValue[3], iPerturbation[3], iTolerance[3];
	int i;

	header();


	// get all the input from stdin
	printf("PPCONTROL values:\n");
	printf("\tNumber of stages: ");
	scanf("%d", &numStages);
	printf("\tPrint PPCONTROL: ");
	scanf("%d", &TEMP_PCONTROL);
	
	printf("Launch Point\n");
	printf("\tLatitude (degrees): ");
	scanf("%lf", &iLat);
	printf("\tLongitude (degrees): ");
	scanf("%lf", &iLong);
	printf("\tHeight (meters): ");
	scanf("%lf", &iHeight);

	for(i=0; i<numStages; i++)
	{
		printf("The vehicle, Stage %d\n", i+1);
		printf("\tThrust (pounds): ");
		scanf("%lf", &iThrust[i]);
		printf("\tWeight (pounds): ");
		scanf("%lf", &iWeight[i]);
		printf("\tFlow (pounds/sec): ");
		scanf("%lf", &iFlow[i]);
		printf("\tCross-sectional area (square feet): ");
		scanf("%lf", &iCrossSection[i]);
		printf("\tDrag coefficient: ");
		scanf("%lf", &iDrag[i]);
		printf("\tCut-off time (secs): ");
		scanf("%lf", &iCutOff[i]);
		printf("\tStep size (secs): ");
		scanf("%lf", &iStepSize[i]);
	}
	
	printf("Pitch program:\n");
	printf("\tVertical rise time (secs): ");
	scanf("%lf", &iVertRise);
	printf("\tPitch angle (degrees from verticle): ");
	scanf("%lf", &iPitchAngle);
	printf("\tTime of end of pitch program (secs): ");
	scanf("%lf", &iPitchEnd);
	printf("\tLaunch azimuth (degrees from north): ");
	scanf("%lf", &iLaunchAzimuth);
	
	for (i=0; i<3; i++) {
		printf("Search Parameters (%d): Desired Value, Perturbation, Tolerance\n", i);
		scanf("%lf %lf %lf", &iDesiredValue[i], &iPerturbation[i], &iTolerance[i]);
	}



	// print out input values
	printf("PPCONTROL values:\n");
	printf("\tNumber of stages: %d\n", numStages);
	printf("\tPrint PPCONTROL: %d\n", TEMP_PCONTROL);
	
	printf("Launch Point\n");
	printf("\tLatitude: %lf\n", iLat);
	printf("\tLongitude: %lf\n", iLong);
	printf("\tHeight: %lf\n", iHeight);
        
	for(i=0; i<numStages; i++)
	{
		printf("The vehicle, Stage %d\n", i+1);
		printf("\tThrust (pounds): %lf\n", iThrust[i]);
		printf("\tWeight (pounds): %lf\n", iWeight[i]);
		printf("\tFlow (pounds/sec): %lf\n", iFlow[i]);
		printf("\tCross-sectional area (square feet): %lf\n", iCrossSection[i]);
		printf("\tDrag coefficient: %lf\n", iDrag[i]);
		printf("\tCut-off time (secs): %lf\n", iCutOff[i]);
		printf("\tStep size (secs): %lf\n", iStepSize[i]);
	}

	printf("Pitch program:\n");
	printf("\tVertical rise time (secs): %lf\n", iVertRise);
	printf("\tPitch angle (degrees from verticle): %lf\n", iPitchAngle);
	printf("\tTime of end of pitch program (secs): %lf\n", iPitchEnd);
	printf("\tLaunch azimuth (degrees from north): %lf\n", iLaunchAzimuth);

	for (i=0; i<3; i++) {
		printf("Search Parameters (%d): Desired Value, Perturbation, Tolerance\n", i);
		printf("\t%lf %lf %lf\n", iDesiredValue[i], iPerturbation[i], iTolerance[i]);
	}



	// fix units
	PLAT = iLat;
	PLONG = iLong;
	HEIGHT = iHeight;
	for(i=0; i<numStages; i++)
	{
		Thrust[i] = iThrust[i] * 0.4535924;
		Weight[i] = iWeight[i] * 0.04625455;
		Flow[i] = iFlow[i] * 0.04625455;
		CrossSection[i] = iCrossSection[i] * pow(0.3048,2.0);
		CD[i] = iDrag[i];
		COT[i] = iCutOff[i];
		TI[i] = iStepSize[i];
	}
	VEND = iVertRise;
	PANG = iPitchAngle;  // don't convert to radians afterall
	PEND = iPitchEnd;    // don't convert to radians afterall
	AZ = iLaunchAzimuth;


	Search26( MAX_ITERATIONS, iDesiredValue, iPerturbation, iTolerance);

	printf("Azimuth=%lf, Pitch angle=%lf, Thrust=%lf\n", AZ, PANG, Thrust[0]);

	return 0;
}



void Search26(int MaxIter, double endBoundConditions[], double Pert[], double Tol[])
{
	double X2[MAXN];
	double u[MAXN][MAXN]; // for storing the final values for each pertubation,  [pertubation][value]
	double standard[MAXN];
	double A[MAXN][MAXN];
	double B[MAXN];
	double indepVars[MAXN];  // for storing original independant vars, Azimuth, PitchAngle, Thrust
	double AnsX[MAXN];  // for storing results from gauss
	double h=0.0;
	int n=1, i=0, j=0;
	
	// initialize everything
	for (i=0; i<3; i++)
	{
		standard[i] = 0.0;
		B[i] = 0.0;
		//AnsX[i] = 0.0;
		for (j = 0; j < MAXN; j++)
		{
			A[i][j] = 0.0;
		}
	}
	
	indepVars[0] = AZ;
	indepVars[1] = PANG;
	indepVars[2] = Thrust[0];
	
	for (i=0; i<MaxIter; i++)
	{
		// standard run
		AZ = indepVars[0];
		PANG = indepVars[1];
		Thrust[0] = indepVars[2];

		OneRun(X2);
		standard[0] = X2[0];
		standard[1] = X2[1];
		standard[2] = X2[2];
		//printf("x[0]: %lf\n", standard[0]);
		//printf("x[1]: %lf\n", standard[1]);


		B[0] = endBoundConditions[0] - standard[0];
		B[1] = endBoundConditions[1] - standard[1];
		B[2] = endBoundConditions[2] - standard[2];
		
		// determine if were close enough already
		if ( fabs(B[0]/endBoundConditions[0])<Tol[0] && 
			fabs(B[1]/endBoundConditions[1])<Tol[1] &&
			fabs(B[2]/endBoundConditions[2])<Tol[2] )
		{
			printf("Within tolerance. Breaking. Completed in %d iterations.\n", i+1);
			break;
		}


		
		// Pertubation 1
		AZ = indepVars[0] * (1.0+Pert[0]);
		PANG = indepVars[1];
		Thrust[0] = indepVars[2];

		OneRun(X2);
		u[0][0] = X2[0];
		u[0][1] = X2[1];
		u[0][2] = X2[2];


		// Pertubation 2
		AZ = indepVars[0];
		PANG = indepVars[1] * (1.0+Pert[1]);
		Thrust[0] = indepVars[2];

		OneRun(X2);
		u[1][0] = X2[0];
		u[1][1] = X2[1];
		u[1][2] = X2[2];


		// Pertubation 3
		AZ = indepVars[0];
		PANG = indepVars[1];
		Thrust[0] =  indepVars[2] * (1.0+Pert[2]);

		OneRun(X2);
		u[2][0] = X2[0];
		u[2][1] = X2[1];
		u[2][2] = X2[2];


		A[0][0] = (u[0][0] - standard[0]) / (Pert[0] * indepVars[0]);
		A[0][1] = (u[1][0] - standard[0]) / (Pert[1] * indepVars[1]);
		A[0][2] = (u[2][0] - standard[0]) / (Pert[2] * indepVars[2]);

		A[1][0] = (u[0][1] - standard[1]) / (Pert[0] * indepVars[0]);
		A[1][1] = (u[1][1] - standard[1]) / (Pert[1] * indepVars[1]);
		A[1][2] = (u[2][1] - standard[1]) / (Pert[2] * indepVars[2]);

		A[2][0] = (u[0][2] - standard[2]) / (Pert[0] * indepVars[0]);
		A[2][1] = (u[1][2] - standard[2]) / (Pert[1] * indepVars[1]);
		A[2][2] = (u[2][2] - standard[2]) / (Pert[2] * indepVars[2]);
		
		gaussSolve(A, B, AnsX, 3);
		
		indepVars[0] += AnsX[0];
		indepVars[1] += AnsX[1];
		indepVars[2] += AnsX[2];
		
		//printf("G1: %lf     G2: %lf\n", X[2], X[3]);
	}

	// run one more time for user output
	PCONTROL = TEMP_PCONTROL;
	OneRun(X2);
	
	printf("X=%lf, Y=%lf, Z=%lf\n", X2[0], X2[1], X2[2]);
}



void OneRun(double X[])
{
	int i, nsteps;
	double time = 0.0;
	//double X[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
	double Fg,Ft,Fa;

	for (i=0; i<7; i++) {
		X[i] = 0.0;
	}
	X[2] = HEIGHT;

	/*
	F[0]=0; F[1]=0; F[2]=1;
	C[0]=0; C[1]=0; C[2]=0;

	X[2] = HEIGHT;

	Pressure = Pressure26(HEIGHT);
	Temperature = Temperature26(HEIGHT);
	Celerity = Celerity26(Temperature, HEIGHT);
	Gravity = Gravity26(HEIGHT);
	Velocity = 1;
	Mach =  Velocity / Celerity;
	Q = 0.7 * Pressure * Mach * Mach;
	Ft = Thrust[0];											// thrust
	Fg = Weight[0] * Gravity;								// gravity
	Fa = CD[0] * Q * CrossSection[0];						// drag
	Acceleration = (Ft - Fa) / (Gravity * Weight[0]);
	*/
	
	for (i=0; i<numStages; i++)
	{
		currentStage = i;
		nsteps = COT[i]/TI[i];
		X[6] = Weight[i];
		RK4_System(7, TI[i], &time, X, nsteps);
	}
	
}




// Runge-Kutta functions
void RK4_System(int n, double h, double *tref, double X[], int nsteps)
{
	int i=0, j=0;
	double t;
	double Y[n];
	double K[4][n];

	t=*tref;
	
	if (PCONTROL)
	{
		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+h/2.0, 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.0, 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 = t + h;
		//Time = Time + h;
		
		if (PCONTROL)
		{
			output(j, t, X);
		}
	}

	*tref = t;
}

void XP_System(int Neq, double T, double Y[], double DY[])
{
		double Fg,Ft,Fa;
		
		Pressure = Pressure26(Y[2]);
		Temperature = Temperature26(Y[2]);
		Celerity = Celerity26(Temperature, Y[2]);
		Gravity = Gravity26(Y[2]);
		
		if ( T==0.0 ) {
			Velocity = 1;
		} else {
			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[currentStage];									// thrust
		Fg = Gravity * Y[6];										// gravity
		Fa = CD[currentStage] * Q * CrossSection[currentStage];		// drag
		Acceleration = (Ft - Fa) / (Gravity * (Y[6]));
		//Range = sqrt( Y[0]*Y[0] + Y[1]*Y[1] + Y[2]*Y[2] );
		Range = sqrt( Y[0]*Y[0] + Y[1]*Y[1] )/1852.0;
		
		
		if( T==0.0 )
		{
			C[0] = 0.0; C[1] = 0.0; C[2] = 0.0;
		}
		else
		{
			C[0] = Y[3] / Velocity;
			C[1] = Y[4] / Velocity;
			C[2] = Y[5] / Velocity;
		}
		
		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]) / (Y[6]);
        DY[4] = (Ft * F[1] + Fg * G[1] - Fa * C[1]) / (Y[6]);
        DY[5] = (Ft * F[2] + Fg * G[2] - Fa * C[2]) / (Y[6]);
        DY[6] = -Flow[currentStage];
}

void output(int j, double T, double Y[])
{
	double Latitude, Longitude;
	int k=0, l=0;
	
	Latitude = ((PLAT*1852.0*60.0) + Y[0])/(1852.0*60.0);
	Longitude = ((PLAT*1852.0*60.0) + Y[1])/(1852.0*60.0);

	/*
	if( T==0.0 )
	{
		C[0] = 0.0; C[1] = 0.0; C[2] = 0.0;
	}
	else
	{
		C[0] = Y[3] / Velocity;
		C[1] = Y[4] / Velocity;
		C[2] = Y[5] / Velocity;
	}*/
	
	BLKOUT(1, PCONTROL, T, Y, C, F, G, PANG, VEND, PEND, AZ,
		Flow[currentStage], CrossSection[currentStage], Thrust[currentStage],
		Gravity, Pressure, Temperature, Mach, Celerity,
		Q, CD[currentStage], AF, Latitude, Longitude, Range, Velocity,
		Acceleration, COT[currentStage], TI[currentStage]);
}
// End Runge-Kutta functions






// returns pressure in Kilograms/square meter. requires altitude in meters.
double Pressure26(double alt)
{
	double PressureCurveFit1[] = { -9.6666667e-04, 7.8333333e-02, -2.3791667e+00, 3.4291667e+01, -2.5116667e+02, 1.0000000e+03};
	double PressureCurveFit2[] = { -2.0833333e-06, 5.2083333e-04, -5.1041667e-02, 2.4729167e+00, -6.1125000e+01, 6.6500000e+02};
	
	alt = alt/1000;
	
	if (alt < 25) {
		return convertMilliBarsToKSM( func(PressureCurveFit1, 6, alt) );
		//return func(PressureCurveFit1, 5, alt);
	} else if (alt < 65) {
		return convertMilliBarsToKSM( func(PressureCurveFit2, 6, alt) );
		//return func(PressureCurveFit2, 5, alt);
	} else if (alt >= 65) {
		return 0;
	} else {
		printf("Error: Pressure(%lf): Can't process invalid altitude.\n", alt);
	}
}

// returns temperature in Kalvens. requires altitude in meters.
double Temperature26(double alt)
{
	int n=15, i=0;
	double y=0;
	struct pair table[15];
	double TemperatureAry[2][15] = { {0.0, 5000.0, 10000.0, 15000.0, 20000.0, 25000.0, 30000.0, 35000.0, 40000.0, 45000.0, 50000.0, 55000.0, 60000.0, 65000.0, 70000.0},
						{290.0, 240.0, 220.0, 225.0, 230.0, 245.0, 255.0, 265.0, 270.0, 275.0, 270.0, 255.0, 235.0, 215.0, 200.0} };

	if ( alt>65000.0 ) {
		return 180.0;
	}
	
	// since my lagrange function used a different format for data, I have to convert
	for (i=0; i<n; i++)
	{
		table[i].x = TemperatureAry[0][i];
		table[i].y = TemperatureAry[1][i];
	}
	
	lagrange_interpolate(n, table, 4, alt, &y, 0);
	
	return y;
}


// returns speed of sound in meters/second.  accepts temperature in Kelvans, altitude in meters.
double Celerity26(double temp, double alt)
{
	if (alt >= 65000.0)
	{
		return 1000.0;
	}
	else
	{
		return ( pow(1.4 * 8314.32 / 28.9644 * temp, 0.5) );
	}
}

// returns acceleration of gravity, not force.  requires altitude in meters.
double Gravity26(double alt)
{
	return (398622000000000.0/pow(6378178.0+alt,2.0));
}

// converts pressure in milibars to Kilograms/square meter.
double convertMilliBarsToKSM(double millibars)
{
	return (millibars * 1000.0 * 0.010197162);
}

// helper function for curve fit, point evaluation.  x is array of polynomial coefficients, xVal is X value to be interprated.
double func(double x[MAXN], int degree, double xVal)
{
	int i, power;
	double sum = 0.0;
	power = degree-1;
	for (i=0;i<degree;i++)
	{
		sum = sum + pow(xVal, power) * x[i];
		power--;
	}
	return sum;
}


int lagrange_interpolate( int n, struct pair table[], int k, double x, double *y, int debug )
{
	int i=0, j=0;
	int halfk=0, halfk_top=0;
	int start_point=0, points_less_x=0, points_greater_x=0;
	double l=0, ans=0, denom=0;
	
	
	// if k > n,  then fail.
	if (k>n) {
		return 0;
	}
	
	// if less than floor(k/2) data points to left or right
	halfk = k/2;  // using an int here is on purpose
	halfk_top = k - halfk;
	
	
	// count the number of data points above and below x.
	if (debug) { printf("lagrange_interpolate(): calculating location of x in table.\n"); }
	for (i=0; i<n; i++) {
		if (table[i].x < x) {
			points_less_x++;
		} else {
			points_greater_x++;
		}
	}
	if (debug>2) { printf("lagrange_interpolate(): points_less_x=%i\tpoints_greater_x=%i.\n", points_less_x, points_greater_x); }
	
	
	// figure out the starting data point for K points
	//  start_point is 0 indexed
	if (debug) { printf("lagrange_interpolate(): calculating starting data point.\n"); }
	if ( halfk > points_less_x ) {
		// then use all of them
		start_point = 0;
	} else if ( halfk_top > points_greater_x ) {
		// then use all of those
		start_point = n - k;
	} else {
		start_point = points_less_x - halfk;
	}
	
	
	if (debug) { printf("lagrange_interpolate(): starting interpolation.\n"); }
	for (i=0; i<k; i++) {
		
		// calc l
		l = 1;
		for (j=0; j<k; j++) {
			if ( i!=j ) {
				
				if (debug>2) { printf("lagrange_interpolate(): i=%i\tj=%i.\n", (start_point+i), (start_point+j)); }
				
				// double check demoinator is not zero
				//   even though this should never happen
				denom = table[start_point+i].x - table[start_point+j].x;
				if ( denom ) {
					l = l * (  (x - table[start_point+j].x)/denom );
					
					if (debug>3) { printf("lagrange_interpolate(): calculating l%i\tl=%lf.\n", (i+1), l); }
				} else {
					if (debug>2) { printf("lagrange_interpolate(): strange, demoninator in l%i is zero.\n", (i+1)); }
				}
			}
		}
		
		ans += table[start_point+i].y * l;
		
		if (debug>2) { printf("lagrange_interpolate(): using data point: y%i=%lf.\n", (start_point+i), table[start_point+i].y ); }
		if (debug>2) { printf("lagrange_interpolate(): calculating y\ty=%lf.\n", ans); }
	}
	
	*y = ans;	
	
	return 1;	
}



// Gauss functions
void gaussSolve(double a[MAXN][MAXN], double b[MAXN], double x[MAXN], int n)
{
	int l[MAXN];
	gauss(a, l, n);
	solve(a, b, x, l, n);
}

void gauss(double a[MAXN][MAXN], int l[MAXN], int n)
{
	double s[MAXN];
	int i,j,k,p;
	double r,rmax,smax,xmult;
	
	//printf("n: %i\n", n);
	
	for (i=0; i<n; i++)
	{
		l[i] = i;
		smax = 0;
		
		for (j=0; j<n; j++)
		{
			//printf("p: %lf\n", fabs(a[i][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[ l[i] ][k]/s[ l[i] ] );
			if (r>rmax)
			{
				rmax = r;
				j = i;
			}
		}
		p = l[k];
		l[k] = l[j];
		l[j] = p;
		
		
		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];
			}
		}
	
	}
	
}

void solve(double a[MAXN][MAXN], double b[MAXN], double x[MAXN], int l[MAXN], int n)
{
	double sum;
	int i,j,k;
	
	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];
	}
}

double max(double a, double b)
{
	return ( (a>b) ? (a):(b));
}
// End Gauss functions


void header()
{
		printf("\nJuan Velasquez NN:26 Numerical Analysis\n\n");
}