#include "dynamics0.h"

double getAltitude(double lat, double rad) {
	// 迭代求椭球体地球模型的高度
	// lat : 飞行器质心的地心纬度
	// rad : 飞行器质心的地心距离
	// alt : 高度
	int cnt;
	double ae, alphae, e, tol, B, Re, alt, late, error, alt0;

	// 1.地球参数
	ae = 6378140;// 长半轴
	alphae = 1 / 298.257;// 扁率
	e = 1 - alphae;// 偏心率
	tol = 1e-1;// 高度迭代误差容限 单位 m

	// 2.迭代
	// 初值
	B = atan(tan(lat) / (e * e));
	Re = ae * e / sqrt(sin(lat) * sin(lat) + (e * e) * cos(lat) * cos(lat));
	alt = sqrt(rad * rad - Re * Re * sin(B - lat) * sin(B - lat)) - Re * cos(B - lat);
	late = lat - asin(alt * sin(B - lat) / rad);

	// 迭代
	error = tol + 100;
	cnt = 0;
	while (error >= tol) {
		alt0 = alt;
		B = atan(tan(late) / (e * e));
		Re = ae * e / sqrt(sin(late) * sin(late) + (e * e) * cos(late) * cos(late));
		alt = sqrt(rad * rad - Re * Re * sin(B - late) * sin(B - late)) - Re * cos(B - late);
		late = lat - asin(alt * sin(B - late) / rad);
		error = alt - alt0;
		if (error < 0)
		{
			error = -error;
		}
		cnt = cnt + 1;
		if (cnt >= 3) {
			break;
		}
	}
	return alt;
}

// 气动系数
void getCxCyCz(double* Cx, double* Cy, double* Cz, double ma, double alt) {
	double a0, a1, a2, a3, a4, a5, ma2, ma3, ma4;

	Cz[0] = 0;

	if (alt < 95 * 1000)
	{
		if (ma < 0.5) {
			ma = 0.5;
		}
		else if (ma > 32.2) {
			ma = 32.2;
		}
		// 升力拟合公式
		if (ma < 1.1) {
			a0 = 5.036;
			a1 = -12.73;
			a2 = 9.024;
			a3 = -0.902;
			ma2 = ma * ma;
			Cy[0] = a0 + a1 * sqrt(ma) + a2 * ma + a3 * ma2;
		}
		else if ((ma >= 1.1) & (ma < 2.0)) {
			a0 = -3.317;
			a1 = 7.158;
			a2 = -3.583;
			a3 = 0.2245;
			ma2 = ma * ma;
			Cy[0] = a0 + a1 * sqrt(ma) + a2 * ma + a3 * ma2;
		}
		else {
			a0 = 0.7223;
			a1 = -0.1853;
			a2 = 0.03965;
			a3 = -0.0007835;
			a4 = 8.993e-06;
			ma2 = ma * ma;
			ma3 = ma2 * ma;
			Cy[0] = a0 + a1 * sqrt(ma) + a2 * ma + a3 * ma2 + a4 * ma3;
		}

		// 阻力拟合公式
		if (ma < 2.0) {
			a0 = 5.298;
			a1 = -14.51;
			a2 = 13.27;
			a3 = -3.429;
			a4 = 0.4542;
			ma2 = ma * ma;
			ma3 = ma2 * ma;
			Cx[0] = a0 + a1 * sqrt(ma) + a2 * ma + a3 * ma2 + a4 * ma3;
		}
		else if ((ma >= 2.0) & (ma < 4.0)) {
			a0 = 4.631;
			a1 = -4.847;
			a2 = 1.901;
			a3 = -0.08711;
			ma2 = ma * ma;
			Cx[0] = a0 + a1 * sqrt(ma) + a2 * ma + a3 * ma2;
		}
		else {
			a0 = 1.391;
			a1 = -0.2907;
			a2 = 0.09618;
			a3 = -0.003022;
			a4 = 6.594e-05;
			a5 = -6.096e-07;
			ma2 = ma * ma;
			ma3 = ma2 * ma;
			ma4 = ma3 * ma;
			Cx[0] = a0 + a1 * sqrt(ma) + a2 * ma + a3 * ma2 + a4 * ma3 + a5 * ma4;
		}
	}
	else
	{
		Cx[0] = 0;
		Cy[0] = 0;
	}
}

// 大气密度和马赫数
void getDensityMach(double* rho, double* ma, double alt, double speed) {
	double rho0, R0, H, Z, W, T;
	rho0 = 1.225;
	R0 = 6356.766;

	H = alt / 1000;
	Z = R0 * H / (R0 + H);
	// 计算密度,温度
	if (H <= 11.0191) {
		W = (1 - Z / 44.3308);
		T = 288.15 * W;
		rho[0] = rho0 * pow(W, 4.2559);
	}
	else if (H <= 20.0631) {
		W = exp((14.9647 - Z) / 6.3416);
		T = 216.650;
		rho[0] = rho0 * 1.5898 * 0.1 * W;
	}
	else if (H <= 32.1619) {
		W = 1 + (Z - 24.9021) / 221.552;
		T = 221.552 * W;
		rho[0] = rho0 * 3.2722 * 0.01 * pow(W, -35.1629);
	}
	else if (H <= 47.3501) {
		W = 1 + (Z - 39.7499) / 89.4107;
		T = 250.350 * W;
		rho[0] = rho0 * 3.2618 * (1e-3) * pow(W, -13.2011);
	}
	else if (H <= 51.4125) {
		W = exp((48.6252 - Z) / 7.9223);
		T = 270.650;
		rho[0] = rho0 * 9.4920 * (1e-4) * W;
	}
	else if (H <= 71.8020) {
		W = 1 - (Z - 59.4390) / 88.2218;
		T = 247.021 * W;
		rho[0] = rho0 * 2.5280 * (1e-4) * pow(W, 11.2011);
	}
	else if (H < 86) {
		W = 1 - (Z - 78.0303) / 100.2950;
		T = 200.590 * W;
		rho[0] = rho0 * 1.7632 * (1e-5) * pow(W, 16.0816);
	}
	else {
		W = exp((87.2848 - Z) / 5.4700);
		T = 186.870;
		rho[0] = rho0 * 3.6411 * (1e-6) * W;
	}
	// 马赫数计算公式 
	ma[0] = speed / (20.0468 * sqrt(T));
}

// 地球引力加速度
void getGravity(double* gr, double* gw, double rad, double lat, cmscp_auxdata* auxdata) {
	double mu, ae, J2, J;
	mu = auxdata->mu;
	ae = 6378140;
	J2 = 1.08263e-3;
	J = 1.5 * J2;
	gr[0] = -mu / (rad * rad) * (1 + J * (ae * ae / (rad * rad)) * (1 - 5 * sin(lat) * sin(lat)));
	gw[0] = -2 * mu / (rad * rad) * J * ae * ae / (rad * rad) * sin(lat);
}


void dynamics0(double* dynRhs, double* path, double* state, double* control, cmscp_auxdata* auxdata) {
	double we, g0, lon0, lat0, B0, A0, Isp, S, rad, lon, lat, speed, fpa, azi, mass, time, thrust, attAngle, sideAngle, bankAngle, Pxh, Pyh, Pzh, alt, range, crossRange, downRange, A1, rho, ma, q, Cx, Cy, Cz, X, Y, Z, load, gr, gw, slat, clat, tlat, sfpa, cfpa, tfpa, sazi, cazi, cbank, sbank, C_V2, C_f1, C_f2, C_a1, C_a2, raddot, londot, latdot, speeddot, fpadot, azidot, massdot, timedot;
	demat* H2B;

	we = auxdata->omega;// rotational angular velocity of the Earth
	g0 = auxdata->g0;// mean gravitational acceleration at sea level
	lon0 = auxdata->launchLon;// lontitude of the launch site
	lat0 = auxdata->launchLat;// geocentric latitude of the launch site
	B0 = auxdata->launchB;// geodetic latitude of the launch site
	A0 = auxdata->launchA;// azimuth of the launch site
	Isp = auxdata->Isp;// Specific impulse of the rocket engine
	S = auxdata->S;// aerodynamic reference aera

	// state variables
	rad = state[0];// radial distance from Earth's center
	lon = state[1];// lontitude
	lat = state[2];// geocentric latitude
	speed = state[3];// earth - relative velocity
	fpa = state[4];// fight - path angle
	azi = state[5];// negative azimuth angle
	mass = state[6];// mass
	time = state[7];// time

	// control variables
	thrust = control[0];// total thrust of rocket engines
	attAngle = control[1];// angle of attack
	sideAngle = control[2]; // angle of sideship
	bankAngle = control[3]; // angle of bank

	// some auxillary variables
	// computing the thrust vector
	if (fabs(thrust) < 1)
	{
		Pxh = 0.0;
		Pyh = 0.0;
		Pzh = 0.0;
	}
	else
	{
		H2B = createDeMat(3, 3);
		C_H2B(H2B, bankAngle, sideAngle, attAngle); // direction cosine matrix from half-velocity to body frame
		Pxh = H2B->v[0] * thrust;
		Pyh = H2B->v[1] * thrust;
		Pzh = H2B->v[2] * thrust;
		freeDeMat(H2B, 2);// 释放空间
	}

	// computing the altitude
	alt = getAltitude(lat, rad);
	// computing the range, crossrange, and downrange
	range = acos(sin(B0) * sin(lat) + cos(B0) * cos(lat) * cos(lon - lon0));
	A1 = acos((sin(lat) - sin(B0) * cos(range)) / (cos(B0) * sin(range)));
	crossRange = asin(sin(range) * sin(A0 - A1));
	downRange = acos(cos(range) / cos(crossRange));

	// aerodynamic forces in velocity coordinate frame（速度坐标系中的分解）
	getDensityMach(&rho, &ma, alt, speed); // air density
	q = 0.5 * rho * speed * speed;// dynamic press
	getCxCyCz(&Cx, &Cy, &Cz, ma, alt);
	X = Cx * q * S; // aerodynamic drag force
	Y = Cy * q * S; // aerodynamic lift force
	Z = Cz * q * S; // aerodynamic Lateral force

	// normal load
	load = sqrt(Y * Y + X * X + Z * Z) / (mass * g0);

	// The decomposition of gravity in the radial direction of the center of
	// the earthand the angular velocity of the earth's rotation
	getGravity(&gr, &gw, rad, lat, auxdata);

	// abbreviations
	slat = sin(lat);
	clat = cos(lat);
	tlat = tan(lat);
	sfpa = sin(fpa);
	cfpa = cos(fpa);
	tfpa = tan(fpa);
	sazi = sin(azi);
	cazi = cos(azi);
	cbank = cos(bankAngle);
	sbank = sin(bankAngle);

	// terms related to tthe rotation of the earth
	C_V2 = -(we * we) * rad * clat * (slat * cazi * cfpa - clat * sfpa);
	C_f1 = -2 * we * clat * sazi;
	C_f2 = (we * we) * rad / speed * clat * (slat * cazi * sfpa + clat * cfpa);
	C_a1 = 2 * we * (clat * cazi * tfpa - slat);
	C_a2 = (we * we) * rad * clat * slat * sazi / (speed * cfpa);

	// Right-hand side (RHS) of dynamic equations
	raddot = speed * sfpa;
	londot = -speed * cfpa * sazi / (rad * clat);
	latdot = speed * cfpa * cazi / rad;
	speeddot = Pxh / mass - X / mass + gr * sfpa + gw * (cazi * cfpa * clat + sfpa * slat) + C_V2;
	fpadot = Pyh / (mass * speed) + (Y * cbank - Z * sbank) / (mass * speed) + gr * cfpa / speed + gw / speed * (slat * cfpa - clat * cazi * sfpa) + speed * cfpa / rad + C_f1 + C_f2;
	azidot = -Pzh / (mass * speed * cfpa) - (Z * cbank + Y * sbank) / (mass * speed * cfpa)
		- gw * clat * sazi / (speed * cfpa) + speed / rad * tlat * cfpa * sazi + C_a1 + C_a2;
	massdot = -thrust / (Isp * g0);
	timedot = 1.0;

	// output
	dynRhs[0] = raddot;
	dynRhs[1] = londot;
	dynRhs[2] = latdot;
	dynRhs[3] = speeddot;
	dynRhs[4] = fpadot;
	dynRhs[5] = azidot;
	dynRhs[6] = massdot;
	dynRhs[7] = timedot;

	path[0] = alt;
	path[1] = q;
	path[2] = load;
	path[3] = downRange;
	path[4] = crossRange;
}

double denseSum(double* vecIn, int length) {
	int i;
	double vecOut = 0;
	for ( i = 0; i < length; i++)
	{
		vecOut = vecOut + vecIn[i];
	}
	return vecOut;
}