#include "sensor_thrust.h"

#include "liblog.h"

#include <math.h>
#include <time.h>
#include <stdlib.h>
#include "GNCFunction.h"

//static double ImpCost = 0.0; /* ImpCost	从仿真开始起，推力器累计冲量消耗 */

void GetThrustMeas(double ThrustForce[3], double ThrustTorque[3], double* SatMass, double SatMtxI[3][3], double JetPWMPlan[12], double JetPWMPeriod, double JetPos[3][12], double JetDir[3][12], 
				   double ThrustIsp, double InitGasMass, double DryMassData[5], double GasMassPos[3], double DryMassPosData[3][5], int Cmd, double InitGasMtxI[3][3], double DryMtxIData[3][15]);
void GetJetForce(double JetForce[12], double JetPWMPlan[12], double JetPWMPeriod);
void Jet2Thrust(double ThrustForce[3], double ThrustTorque[3], double JetForce[12], double JetPos[3][12], double JetDir[3][12], double SatMassPos[3]);
void SatMassProperty(double* SatMass, double SatMassPos[3], double SatMtxI[3][3], double InitGasMass, double DryMassData[5], double GasMassPos[3], double DryMassPosData[3][5], double GasCost, int Cmd, double InitGasMtxI[3][3], double DryMtxIData[3][15]);



//输出:ThrustForce[3], ThrustTorque[3], 
//	   SatMass, SatMtxI[3][3],
//输入: JetPWMPlan[12]
void sensor_ThrustMeas(double ThrustForce[3], double ThrustTorque[3], double* SatMass, double SatMtxI[3][3], double JetPWMPlan[12])
{
	double JetPWMPeriod, JetPos[3][12], JetDir[3][12], ThrustIsp, SatMassTemp1;
	
	/*喷口PWM调制周期，即GNC控制周期，与控制程序周期相同*/
    JetPWMPeriod = 1; 
	/*比例系数*/
	ThrustIsp = 343.0;
	/*本体系下的喷口安装位置，列向量 * 12，m*/
    JetPos[0][0] = -0.233; JetPos[0][1] = -0.233; JetPos[0][2] = 0.383;  JetPos[0][3] = 0.383;  JetPos[0][4] = 0.18;   JetPos[0][5] = -0.18;  JetPos[0][6] = 0.18;  JetPos[0][7] = -0.18; JetPos[0][8] = 0;      JetPos[0][9] = 0;      JetPos[0][10] = 0;     JetPos[0][11] = 0;
    JetPos[1][0] = 0;      JetPos[1][1] = 0;      JetPos[1][2] = 0;      JetPos[1][3] = 0;      JetPos[1][4] = -0.183; JetPos[1][5] = -0.183; JetPos[1][6] = 0.183; JetPos[1][7] = 0.183; JetPos[1][8] = 0.156;  JetPos[1][9] = -0.156; JetPos[1][10] = 0.156; JetPos[1][11] = -0.156;
    JetPos[2][0] = 0.147;  JetPos[2][1] = -0.147; JetPos[2][2] = 0.147; JetPos[2][3] = -0.147; JetPos[2][4] = 0;      JetPos[2][5] = 0;      JetPos[2][6] = 0;     JetPos[2][7] = 0;     JetPos[2][8] = -0.323; JetPos[2][9] = -0.323; JetPos[2][10] = 0.323; JetPos[2][11] = 0.323;
	/*本体系下的推力方向，列向量 * 12，单位方向矢量*/
	JetDir[0][0] = 1; JetDir[0][1] = 1; JetDir[0][2] = -1; JetDir[0][3] = -1; JetDir[0][4] = 0; JetDir[0][5] = 0; JetDir[0][6] = 0;  JetDir[0][7] = 0;  JetDir[0][8] = 0; JetDir[0][9] = 0; JetDir[0][10] = 0;  JetDir[0][11] = 0;
    JetDir[1][0] = 0; JetDir[1][1] = 0; JetDir[1][2] = 0;  JetDir[1][3] = 0;  JetDir[1][4] = 1; JetDir[1][5] = 1; JetDir[1][6] = -1; JetDir[1][7] = -1; JetDir[1][8] = 0; JetDir[1][9] = 0; JetDir[1][10] = 0;  JetDir[1][11] = 0;
    JetDir[2][0] = 0; JetDir[2][1] = 0; JetDir[2][2] = 0;  JetDir[2][3] = 0;  JetDir[2][4] = 0; JetDir[2][5] = 0; JetDir[2][6] = 0;  JetDir[2][7] = 0;  JetDir[2][8] = 1; JetDir[2][9] = 1; JetDir[2][10] = -1; JetDir[2][11] = -1;
	/*平台结构变化指令，1舱门关闭（默认），2舱门开启，3左载荷先发射，4右载荷先发射，5载荷全发射*/
	int Cmd = 1;
	/*初始推进剂质量，kg*/
	double InitGasMass = 3.55;//6.5 * 0.7692;
	/*平台质量，kg，5种工况，1 * 5*/
	double DryMassData[5] = { 48.381, 48.381, 46.013, 45.898, 43.53 };//{ 34.99881, 34.99881, 32.312115, 32.22625, 29.535285 };
	/*推进剂质心卫星位置，m，推力器布局坐标系*/
	double GasMassPos[3] = { -25 * 1e-3, 0.0, -201.44 * 1e-3 };
	/*平台质心位置，m，推力器布局坐标系，5种工况，3 * 5*/
	double DryMassPosData[3][5] = { {8.39 * 0.001, 8.03 * 0.001, 10.29 * 0.001, 10.19 * 0.001, 12.96 * 0.001},
									{ -8.92 * 0.001, -7.94 * 0.001, -15.98 * 0.001, 0.19 * 0.001, -8.00 * 0.001},
									{ -4.72 * 0.001, -4.72 * 0.001, 0.25 * 0.001, 1.30 * 0.001, 7.50 * 0.001} };
	/*初始推进剂转动惯量，kg·m3，取决于推进剂质心且对齐到推力器布局坐标系*/
	double InitGasMtxI[3][3] = { {0.0571059330000000, 0, -0.00222918000000000},
								 {0, 0.0319816850000000, 0},
								 {-0.00222918000000000, 0, 0.0678860230000000} };
	/*平台转动惯量，kg·m3，取决于平台质心且对齐到推力器布局坐标系，5种工况，3* (3 * 5)*/
	double DryMtxIData[3][15] = { {1.86687640893600, 0.0286157968140000, -0.439427130912000, 1.89572816175600, 0.0409105154700000, -0.437885017878000, 1.87455028889500, 0.0482311009420000, -0.445634256610000, 1.90556951219600, 0.0362650204340000, -0.452313634868000, 1.89121192802100, 0.0438251188440000, -0.462662149911000},
								  {0.0286157968140000, 2.3271403853280, -0.0114310573740000, 0.0409105154700000, 2.31908819868000, -0.0163531346220000, 0.0482311009420000, 2.29461077938400, -0.00115876103900000, 0.0362650204340000, 2.32061908341600, -0.0323862321540000, 0.0438251188440000, 2.29928834970900, -0.0167186100900000},
								  {-0.439427130912000, -0.0114310573740000, 1.20621470673000, -0.437885017878000, -0.0163531346220000, 1.22701427290200, -0.445634256610000, -0.00115876103900000, 1.17827923915600, -0.452313634868000, -0.0323862321540000, 1.19281841776200, -0.462662149911000, -0.0167186100900000, 1.14977779874100} };

	GetThrustMeas(ThrustForce, ThrustTorque, &SatMassTemp1, SatMtxI, JetPWMPlan, JetPWMPeriod, JetPos, JetDir,
				  ThrustIsp, InitGasMass, DryMassData, GasMassPos, DryMassPosData, Cmd, InitGasMtxI, DryMtxIData);
	*SatMass = SatMassTemp1;

    //printf("ThrustForce is %10.5f \t %10.5f \t %10.5f\r\n", ThrustForce[0], ThrustForce[1], ThrustForce[2]);
	//printf("ThrustTorque is %10.5f \t %10.5f \t %10.5f\r\n", ThrustTorque[0], ThrustTorque[1], ThrustTorque[2]);
	//spdlog::info("函数{0} ThrustForce为:{1:.6f}, {2:.6f}, {3:.6f},ThrustTorque为:{4:.6f}, {5:.6f}, {6:.6f}",__FUNCTION__,
	//				ThrustForce[0], ThrustForce[1], ThrustForce[2],
	//				ThrustTorque[0], ThrustTorque[1], ThrustTorque[2]);
	spdlog::info("函数{0} ThrustForce为:{1:.6f}, {2:.6f}, {3:.6f},{4:.6f}, {5:.6f}, {6:.6f}, {7:.6f}", __FUNCTION__,
		ThrustForce[0], ThrustForce[1], ThrustForce[2],
		ThrustTorque[0], ThrustTorque[1], ThrustTorque[2],
		*SatMass);
}


/* 获得微推力器三轴推力、力矩以及气体消耗 */
void GetThrustMeas(double ThrustForce[3], double ThrustTorque[3], double* SatMass, double SatMtxI[3][3], double JetPWMPlan[12], double JetPWMPeriod, double JetPos[3][12], double JetDir[3][12], 
				   double ThrustIsp, double InitGasMass, double DryMassData[5], double GasMassPos[3], double DryMassPosData[3][5], int Cmd, double InitGasMtxI[3][3], double DryMtxIData[3][15]) {

	/* 微推力器模拟模块 */
	/* 输入量 */
	/* FullTankForceStd		充气后推力器推力标称值 */
	/* NullTankForceStd		触发充气推力器推力标称值 */
	/* TankForceDscPara		推力下降系数，N/(N*s) */
	/* TankGasTime			二级储箱充气时长 */
	/* JetPWMPlan			喷口PWM调制指令值，即GNC输出的推力器指令 */
	/* JetPWMPeriod			喷口PWM调制周期，即GNC控制周期 */
	/* tNowSec				仿真时刻，s */
	/* JetPos				本体系下的喷口安装位置，列向量*12，m */
	/* JetDir				本体系下的推力方向，列向量*12，单位方向矢量 */
	/* SatMassPos			二级储箱充气时长 */
	/* ThrustIsp			比例系数 */
	/* 输出量 */
	/* ThrustForce			推力器合力 */
	/* ThrustTorque			合力矩*/
	/* GasCost				储气消耗*/

	static double ImpCost = 0.0; /* ImpCost	从仿真开始起，推力器累计冲量消耗 */
	double JetForce[12], SatMassPos[3], GasCost, SatMassTemp;

	GetJetForce(JetForce, JetPWMPlan, JetPWMPeriod);
	for (int i = 0; i < 12; i++)
	{
		ImpCost += JetForce[i];
	}
	GasCost = ImpCost / ThrustIsp;
	
	SatMassProperty(&SatMassTemp, SatMassPos, SatMtxI, InitGasMass, DryMassData, GasMassPos, DryMassPosData, GasCost, Cmd, InitGasMtxI, DryMtxIData);
	*SatMass = SatMassTemp;
	Jet2Thrust(ThrustForce, ThrustTorque, JetForce, JetPos, JetDir, SatMassPos);
}


/* 推力器12喷口推力输出模拟 */
void GetJetForce(double JetForce[12], double JetPWMPlan[12], double JetPWMPeriod) {

	/* 推力器12喷口推力输出模拟模块 */
	/* 输入量 */
	/* FullTankForceStd		充气后推力器推力标称值 */
	/* NullTankForceStd		触发充气推力器推力标称值 */
	/* TankForceDscPara		推力下降系数，N/(N*s) */
	/* TankGasTime			二级储箱充气时长 */
	/* JetPWMPlan			喷口PWM调制指令值，即GNC输出的推力器指令 */
	/* JetPWMPeriod			喷口PWM调制周期，即GNC控制周期 */
	/* tNowSec				仿真时刻，s */
	/* ImpCost				从仿真开始起，推力器累计冲量消耗 */
	/* 输出量 */
	/* JetForce				推力器12喷口推力输出值（1s内平均推力） */

	/*充气后推力器推力标称值*/
	double FullTankForceStd = 40 * 1e-3; //45 * 1e-3;
	// 当前二级储箱压力（推力）
	double TankForce = FullTankForceStd;

	for (int ii = 0; ii < 12; ii++) {
			JetForce[ii] = TankForce * (JetPWMPlan[ii] / JetPWMPeriod);
	}
}


/* 获得本体系三轴推力和力矩 */
void Jet2Thrust(double ThrustForce[3], double ThrustTorque[3], double JetForce[12], double JetPos[3][12], double JetDir[3][12], double SatMassPos[3]) {

	/* 推力器合力和合力矩模拟模块 */
	/* 输入量 */
	/* JetForce		推力器12喷口推力输出值（1s内平均推力） */
	/* JetPos		本体系下的喷口安装位置，列向量*12，m */
	/* JetDir		本体系下的推力方向，列向量*12，单位方向矢量 */
	/* SatMassPos	二级储箱充气时长 */
	/* 输出量 */
	/* ThrustForce	推力器合力 */
	/* ThrustTorque 合力矩*/

	double iiJetForceVec[3], iiJetPosVec[3], iiJetTorque[3], ThrustForceTemp[3], ThrustTorqueTemp[3];

	ThrustForceTemp[0] = 0.0; ThrustForceTemp[1] = 0.0; ThrustForceTemp[2] = 0.0;
	ThrustTorqueTemp[0] = 0.0; ThrustTorqueTemp[1] = 0.0; ThrustTorqueTemp[2] = 0.0;
	for (int ii = 0; ii < 12; ii++) {
		for (int j = 0; j < 3; j++) {
			iiJetForceVec[j] = JetForce[ii] * JetDir[j][ii];
		}
		for (int j = 0; j < 3; j++) {
			iiJetPosVec[j] = JetPos[j][ii] - SatMassPos[j];
		}
		//iiJetTorque = cross( iiJetPosVec,iiJetForceVec );
		iiJetTorque[0] = iiJetPosVec[1] * iiJetForceVec[2] - iiJetPosVec[2] * iiJetForceVec[1];
		iiJetTorque[1] = iiJetPosVec[2] * iiJetForceVec[0] - iiJetPosVec[0] * iiJetForceVec[2];
		iiJetTorque[2] = iiJetPosVec[0] * iiJetForceVec[1] - iiJetPosVec[1] * iiJetForceVec[0];

		/*mtxAdd((double*)ThrustForceTemp, (double*)ThrustForceTemp, (double*)iiJetForceVec, 3, 1);
		mtxAdd((double*)ThrustTorqueTemp, (double*)ThrustTorqueTemp, (double*)iiJetTorque, 3, 1);*/
		for (int j = 0; j < 3; j++) {
			ThrustForceTemp[j] += iiJetForceVec[j];
			ThrustTorqueTemp[j] += iiJetTorque[j];
		}
	}
	mtxCpy((double*)ThrustForce, (double*)ThrustForceTemp, 3, 1);
	mtxCpy((double*)ThrustTorque, (double*)ThrustTorqueTemp, 3, 1);
}


/* 追踪星质量特性 */
void SatMassProperty(double* SatMass, double SatMassPos[3], double SatMtxI[3][3], double InitGasMass, double DryMassData[5], double GasMassPos[3], double DryMassPosData[3][5], double GasCost, int Cmd, double InitGasMtxI[3][3], double DryMtxIData[3][15]) {

	/* 推力器合力和合力矩模拟模块 */
	/* 输入量 */
	/* InitGasMass		初始推进剂质量，kg */
	/* DryMassData		平台质量，kg，5种工况，1*5； */
	/* GasMassPos		推进剂质心卫星位置，m，推力器布局坐标系 */
	/* DryMassPosData	平台质心位置，m，推力器布局坐标系，5种工况，3*5 */
	/* GasCost			累积冲量消耗质量，kg*/
	/* Cmd				平台结构变化指令，1舱门关闭（默认），2舱门开启，3左载荷先发射，4右载荷先发射，5载荷全发射*/
	/* 输出量 */
	/* SatMassPos		整星质心位置，m,推力器布局坐标系 */

	//推进剂剩余质量
	double GasMass = InitGasMass - GasCost;
	//卫星平台质量，根据指令选择
	int ii = Cmd - 1;
	double DryMass = DryMassData[ii];
	//整星质量
	*SatMass = DryMass + GasMass;
	//卫星平台质心位置，根据指令选择
	double DryMassPos[3] = { DryMassPosData[0][ii], DryMassPosData[1][ii], DryMassPosData[2][ii] };
	for (int i = 0; i < 3; i++)
	{
		SatMassPos[i] = (GasMass * GasMassPos[i] + DryMass * DryMassPos[i]) / *SatMass;
	}
	//推进剂转动惯量，与剩余质量成正比
	double GasMtxI[3][3], DryMtxI[3][3], dGasPos[3], dDryPos[3], dGasPosMtx[3][3], dDryPosMtx[3][3], temp1[3][3], temp2[3][3];

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			GasMtxI[i][j] = (GasMass / InitGasMass) * InitGasMtxI[i][j];
		}
	}
	//卫星平台转动惯量，根据指令选择
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			DryMtxI[i][j] = DryMtxIData[i][3 * Cmd - 3 + j];
		}
	}
	//整星转动惯量计算
	mtxSub((double*)dGasPos, GasMassPos, (double*)SatMassPos, 3, 1);
	mtxSub((double*)dDryPos, DryMassPos, (double*)SatMassPos, 3, 1);
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			dGasPosMtx[i][j] = dGasPos[i] * dGasPos[j] * GasMass;
			dDryPosMtx[i][j] = dDryPos[i] * dDryPos[j] * GasMass;
		}
	}
	mtxAdd((double*)temp1, (double*)GasMtxI, (double*)dGasPosMtx, 3, 3);
	mtxAdd((double*)temp2, (double*)DryMtxI, (double*)dDryPosMtx, 3, 3);
	mtxAdd((double*)SatMtxI, (double*)temp1, (double*)temp2, 3, 3);
}