#include "dynInteg.h"

void dynInteg1(double* dynRhs, double* path, double* x, double* u, double* p, cmscp_auxdata* auxdata) {
	// 有动力下惯性姿态角控制
	// x：状态变量：地心距、经度、纬度、速度、当地速度倾角、负的当地航迹方位角、质量、时间
	// u：相对发惯系的俯仰和偏航角

	double A0, lon0, B0, alpha, beta, nu, u1[4] = { 0 }, dynRhs0[8] = { 0 }, path0[5] = { 0 }, state[8] = { 0 };
	cmscp_scales* scales;

	scales = auxdata->scales;
	A0 = auxdata->launchA;
	lon0 = auxdata->launchLon;
	B0 = auxdata->launchB;

	// 有量纲化
	state[0] = x[0] * scales->length;
	state[1] = x[1];
	state[2] = x[2];
	state[3] = x[3] * scales->speed;
	state[4] = x[4];
	state[5] = x[5];
	state[6] = x[6] * scales->mass;
	state[7] = x[7] * scales->time;

	// 控制变量
	A_H2B1(&alpha, &beta, &nu,
		state[5], state[4], state[2], state[1], u[0], u[1], auxdata->gammaT0,
		A0, lon0, B0, state[7]);
	u1[0] = auxdata->thrust;
	u1[1] = alpha;
	u1[2] = beta;
	u1[3] = nu;

	// 动力学方程
	dynamics0(dynRhs0, path0, state, u1, auxdata);

	// 输出无量纲化参数
	// 动力学右手边项(有可能dynRhs和dynRhs0个数不一样，所以设置两个变量)
	if (dynRhs != NULL)
	{
		dynRhs[0] = dynRhs0[0] / scales->speed;
		dynRhs[1] = dynRhs0[1] * scales->time;
		dynRhs[2] = dynRhs0[2] * scales->time;
		dynRhs[3] = dynRhs0[3] / scales->acceleration;
		dynRhs[4] = dynRhs0[4] * scales->time;
		dynRhs[5] = dynRhs0[5] * scales->time;
		dynRhs[6] = dynRhs0[6] / scales->mass * scales->time;
		dynRhs[7] = dynRhs0[7];
	}

	// 路径约束函数值
	if (path != NULL) {
		path[0] = path0[0] / scales->length;//高度
		path[1] = path0[1] / (scales->density * scales->speed * scales->speed);//动压
		path[2] = path0[2];//过载
		path[3] = path0[3];//纵程角
		path[4] = path0[4];//横程角
	}
}

void dynInteg2(double* dynRhs, double* path, double* x, double* u, double* p, cmscp_auxdata* auxdata) {
	// 无动力下惯性姿态角控制
	// x：状态变量：地心距、经度、纬度、速度、当地速度倾角、负的当地航迹方位角、质量、时间
	// u：相对发惯系的俯仰和偏航角

	double A0, lon0, B0, u1[4] = { 0 }, dynRhs0[8] = { 0 }, path0[5] = { 0 }, state[8] = { 0 };
	cmscp_scales* scales;

	scales = auxdata->scales;
	A0 = auxdata->launchA;
	lon0 = auxdata->launchLon;
	B0 = auxdata->launchB;

	// 有量纲化
	state[0] = x[0] * scales->length;
	state[1] = x[1];
	state[2] = x[2];
	state[3] = x[3] * scales->speed;
	state[4] = x[4];
	state[5] = x[5];
	state[6] = x[6] * scales->mass;
	state[7] = x[7] * scales->time;

	// 动力学方程
	dynamics0(dynRhs0, path0, state, u1, auxdata);

	// 输出无量纲化参数
	// 动力学右手边项
	if (dynRhs!=NULL)
	{
		dynRhs[0] = dynRhs0[0] / scales->speed;
		dynRhs[1] = dynRhs0[1] * scales->time;
		dynRhs[2] = dynRhs0[2] * scales->time;
		dynRhs[3] = dynRhs0[3] / scales->acceleration;
		dynRhs[4] = dynRhs0[4] * scales->time;
		dynRhs[5] = dynRhs0[5] * scales->time;
		dynRhs[6] = dynRhs0[6] / scales->mass * scales->time;
		dynRhs[7] = dynRhs0[7];
	}

	// 路径约束函数值
	if (path != NULL) {
		path[0] = path0[0] / scales->length;//高度
		path[1] = path0[1] / (scales->density * scales->speed * scales->speed);//动压
		path[2] = path0[2];//过载
		path[3] = path0[3];//纵程角
		path[4] = path0[4];//横程角
	}
}


void dynInteg3(double* dynRhs, double* path, double* x, double* u, double* p, cmscp_auxdata* auxdata) {
	// 正常再入动力学模型，倾侧角为控制变量
	// x：状态变量：地心距、经度、纬度、速度、当地速度倾角、负的当地航迹方位角、质量、时间
	// u：倾侧角

	double A0, lon0, B0, u1[4] = { 0 }, dynRhs0[8] = { 0 }, path0[5] = { 0 }, state[8] = { 0 };
	cmscp_scales* scales;

	scales = auxdata->scales;
	A0 = auxdata->launchA;
	lon0 = auxdata->launchLon;
	B0 = auxdata->launchB;

	// 有量纲化
	state[0] = x[0] * scales->length;
	state[1] = x[1];
	state[2] = x[2];
	state[3] = x[3] * scales->speed;
	state[4] = x[4];
	state[5] = x[5];
	state[6] = x[6] * scales->mass;
	state[7] = x[7] * scales->time;

	// 控制变量
	u1[3] = u[0];

	// 动力学方程
	dynamics0(dynRhs0, path0, state, u1, auxdata);

	// 输出无量纲化参数
	// 动力学右手边项
	if (dynRhs != NULL)
	{
		dynRhs[0] = dynRhs0[0] / scales->speed;
		dynRhs[1] = dynRhs0[1] * scales->time;
		dynRhs[2] = dynRhs0[2] * scales->time;
		dynRhs[3] = dynRhs0[3] / scales->acceleration;
		dynRhs[4] = dynRhs0[4] * scales->time;
		dynRhs[5] = dynRhs0[5] * scales->time;
		dynRhs[6] = dynRhs0[6] / scales->mass * scales->time;
		dynRhs[7] = dynRhs0[7];
	}

	// 路径约束函数值
	if (path != NULL) {
		path[0] = path0[0] / scales->length;//高度
		path[1] = path0[1] / (scales->density * scales->speed * scales->speed);//动压
		path[2] = path0[2];//过载
		path[3] = path0[3];//纵程角
		path[4] = path0[4];//横程角
	}
}