#include "orbit_info.h"
// #include "TyxkDynamics.h"
#include "GNCFunction.h"

#include "liblog.h"
#include "json.hpp"


#include <string>
#include <thread>

#include <fstream>

#include <math.h>
#include <time.h>
//  #include <stdio.h>
#include <stdlib.h>

using namespace nlohmann;
// using namespace Dynamics;


typedef struct __attribute__((packed))
{
    double  Epoch;                      //历元时间
    uint8_t pkg_Cnt;
    BASIC_CMDER CJB_cmder;			//CJB轨道
    BASIC_CMDER Target_cmder;		//目标星轨道
} ORBIT_CMDER;


ORBIT_CMDER orbit_cmder;



STEP_CMDER_RES s_rescmd = {0};

uint8_t set_orbit_flag = 0; //轨道初值赋值标志位
uint8_t orbit_source_flag = 0; //轨道来源标志:0--使用主星传入轨道;1--使用程序默认轨道


double Epoch_J2000 = 233881200; 	//历元信息，累计秒
double posi_J2000[3] = {-41966956.6938834, 4089556.26746777, 92419.0202929388}; 	//J2000位置信息,m
double velo_J2000[3] ={-298.1140306366,-3059.51295367263, 54.487742185087}; 	//J2000速度信息,m/s
double Q_J2000[4] =  {1.0,0.0,0.0,0.0}; 		//惯性系四元数信息

double QuatVVLH[4]= {1.0,0.0,0.0,0.0};		//姿态四元数信息

double Tar_posi_J2000[3] = {-41972144.5417670,4040287.04196300, 92381.2956310000};//目标性初始位置
double Tar_velo_J2000[3] = {-294.436520000000,-3059.96985700000,54.3873050000000};//目标性初始速度

double AngularVelocity[3];	 //惯性系角速度信息

double mjd_2017 = 57754;     //2017.1.1 0:0:0 北京时
//double mjd = 0;

double StepSize = 0.05;		 //动力学递推步长，s


double ThrustForceBody[3], ChaSatMass;

void Dynamic_Model(double* AttiVelDotECI, double* w, double* Tq, double* WheelAM, double* SatMtxI);
void Runge_Kutta(double* AngularVelocity, double h, double* Tq, double* WheelAM, double* SatMtxI);
void KinenaticModel(double AttiVelVVLH[3], double AttiMtxVVLH[3][3], double EulerVVLH[3], double QuatVVLH[4], double AttiVelECI[3], double OrbitVel[3], double tInterval);
void Fun_AccEarth(double Acc_Earth[3], double R_ECEF[3]);
void Fun_orbit(double dots[6], double RV[6]);
void Orbit_Runge_Kutta(double* PosVel, double h);
void ORBIT_J6(double tf, double dt, double RV0[6], double RV[6]);

/// @brief 轨道信息初始化
/// @param 
/// @return 0:设置失败; 1:成功
uint8_t orbit_info_initz(string path)
{
	json sync_j;
    ifstream fin(path);

    if(!fin)
    { 
        cout<<"udp_sever_init配置文件打开失败,采用默认状态!"<<endl;
		spdlog::warn("函数{0}配置文件打开失败,采用默认状态!",__FUNCTION__);

		return 0;
    }
    
    if(fin.good())
    {
        fin>>sync_j;
    }
	else
	{
		return 0;
	}

	orbit_source_flag = sync_j["orbit_source_flag"].get<uint8_t>();

//	printf("orbit_source_flag is:%d",orbit_source_flag);
	StepSize = sync_j["StepSize"].get<double>();
	cout<<"StepSize1 is:"<<StepSize<<endl;

	Epoch_J2000 = sync_j["Epoch_J2000"].get<double>();
	posi_J2000[0] = sync_j["posi_J2000_X"].get<double>();
	posi_J2000[1] = sync_j["posi_J2000_Y"].get<double>();
	posi_J2000[2] = sync_j["posi_J2000_Z"].get<double>();
	velo_J2000[0] = sync_j["velo_J2000_X"].get<double>();
	velo_J2000[1] = sync_j["velo_J2000_Y"].get<double>();
	velo_J2000[2] = sync_j["velo_J2000_Z"].get<double>();

	Tar_posi_J2000[0] = sync_j["Tar_posi_J2000_X"].get<double>();
	Tar_posi_J2000[1] = sync_j["Tar_posi_J2000_Y"].get<double>();
	Tar_posi_J2000[2] = sync_j["Tar_posi_J2000_Z"].get<double>();
	Tar_velo_J2000[0] = sync_j["Tar_velo_J2000_X"].get<double>();
	Tar_velo_J2000[1] = sync_j["Tar_velo_J2000_Y"].get<double>();
	Tar_velo_J2000[2] = sync_j["Tar_velo_J2000_Z"].get<double>();

	fin.close();

	return 1;
}
/// @brief 设置轨道信息;该函数供通信模块调用,将主星传递的信息赋值到本地
///		   仅第一次赋值
/// @param frame:输入量,轨道信息数组 
//  输出:frame
//   AttiVelECI[3]
//   EulerVVLH[3]
//   AttiMtxVVLH[3][3]
//   MtxECI2VVLH[3][3]
/// @return 0:设置失败; 1:成功
uint8_t set_orbit_info(uint8_t *frame,double AttiVelECI[3], double EulerVVLH[3], double AttiMtxVVLH[3][3], double MtxECI2VVLH[3][3])
{

	double orbInfo[6];	
	double QuatECI[4],QuatECI2VVLH[4], QoiX[4][4];

	if(orbit_source_flag==1) //如果使用自定义轨道则给定定值
	{
		if(!set_orbit_flag)
		{
			spdlog::info("轨道初始化成功！");
			//本体轨道信息
			// Epoch_J2000 = 261130015;// 233881200;
			// posi_J2000[0] = 23580586.0537251;//-41966956.6938834;//s_rescmd.resCmder.posi_x_J2000;
			// posi_J2000[1] = -34621080.4670552;//4089556.26746777;//s_rescmd.resCmder.posi_y_J2000;
			// posi_J2000[2] = 3658341.81666531;//92419.0202929388;//s_rescmd.resCmder.posi_z_J2000;
			// velo_J2000[0] = 2550.44489430168;//-298.1140306366;//s_rescmd.resCmder.velo_x_J2000;
			// velo_J2000[1] = 1733.51288095298;//-3059.51295367263;//s_rescmd.resCmder.velo_y_J2000;
			// velo_J2000[2] = -5.46126675290179;//54.487742185087;//s_rescmd.resCmder.velo_z_J2000;
			/* 七公里 */
			/*Epoch_J2000 = 261151615;
			posi_J2000[0] = 34676281.2753897;
			posi_J2000[1] = 24076270.7556943;
			posi_J2000[2] = -109962.795723338;
			velo_J2000[0] = -1737.82925849319;
			velo_J2000[1] = 2518.86637867070;
			velo_J2000[2] = -267.003526384092;*/
			/* 1公里 */
			/*Epoch_J2000 = 261162415;
			posi_J2000[0] = 7615206.74657380;
			posi_J2000[1] = 41520735.9749386;
			posi_J2000[2] = -2677030.64312120;
			velo_J2000[0] = -3011.20121409852;
			velo_J2000[1] = 545.623103173396;
			velo_J2000[2] = -183.096712053141;*/
			/* 150m */
			/*Epoch_J2000 = 261162415;
			posi_J2000[0] = 7615354.12442575;
			posi_J2000[1] = 41520709.5213564;
			posi_J2000[2] = -2677021.70014955;
			velo_J2000[0] = -3011.19925678085;
			velo_J2000[1] = 545.633775145026;
			velo_J2000[2] = -183.097400123677;*/


			//目标星轨道参数
			// Tar_posi_J2000[0] = 23621367.9190000;//-41972144.5417670;
			// Tar_posi_J2000[1] = -34592141.8670000;// 4040287.04196300;
			// Tar_posi_J2000[2] = 3657159.91500000;// 92381.2956310000;
			// Tar_velo_J2000[0] = 2548.34575500000;// -294.436520000000;
			// Tar_velo_J2000[1] = 1736.43994900000;// -3059.96985700000;
			// Tar_velo_J2000[2] = -5.86735000000000;// 54.3873050000000;
			/* 七公里 */
			/*Tar_posi_J2000[0] = 34672232.3889290;
			Tar_posi_J2000[1] = 24081737.4807088;
			Tar_posi_J2000[2] = -112563.071870978;
			Tar_velo_J2000[0] = -1738.23933197031;
			Tar_velo_J2000[1] = 2518.56390864701;
			Tar_velo_J2000[2] = -267.000903391630;*/
			/* 1公里 */
			/*Tar_posi_J2000[0] = 7616189.26558680;
			Tar_posi_J2000[1] = 41520559.6177237;
			Tar_posi_J2000[2] = -2676971.02331015;
			Tar_velo_J2000[0] = -3011.18816531403;
			Tar_velo_J2000[1] = 545.694249650931;
			Tar_velo_J2000[2] = -183.101299190051;*/
			/* 150m */
			/*Tar_posi_J2000[0] = 7615206.74657380;
			Tar_posi_J2000[1] = 41520735.9749386;
			Tar_posi_J2000[2] = -2677030.64312120;
			Tar_velo_J2000[0] = -3011.20121409852;
			Tar_velo_J2000[1] = 545.623103173396;
			Tar_velo_J2000[2] = -183.096712053141;*/


			//姿态角初值
			EulerVVLH[0] = 0.0;
			EulerVVLH[1] = 0.0;
			EulerVVLH[2] = 0.0;

			AttiMtxVVLH[0][0] = 1; AttiMtxVVLH[0][1] = 0; AttiMtxVVLH[0][2] = 0;
			AttiMtxVVLH[1][0] = 0; AttiMtxVVLH[1][1] = 1; AttiMtxVVLH[1][2] = 0;
			AttiMtxVVLH[2][0] = 0; AttiMtxVVLH[2][1] = 0; AttiMtxVVLH[2][2] = 1;
			
			orbInfo[0] = posi_J2000[0]; orbInfo[1] = posi_J2000[1]; orbInfo[2] = posi_J2000[2];
			orbInfo[3] = velo_J2000[0]; orbInfo[4] = velo_J2000[1]; orbInfo[5] = velo_J2000[2];
			
			MtxJtoOGet(MtxECI2VVLH, orbInfo);
			
		
			A2q(QuatECI2VVLH, MtxECI2VVLH);
			qleft(QoiX, QuatECI2VVLH);
			mtxMtp((double*)QuatECI, (double*)QoiX, 4, 4, (double*)QuatVVLH, 4, 1);
			Q_J2000[0] = QuatECI[0];
			Q_J2000[1] = QuatECI[1];
			Q_J2000[2] = QuatECI[2];
			Q_J2000[3] = QuatECI[3];

			//角速度初值
			AngularVelocity[0] = 5.0 * 3.1415926535 / 180.0;//5.0 * 3.1415926535 / 180.0;
			AngularVelocity[1] = 5.0 * 3.1415926535 / 180.0;//5.0 * 3.1415926535 / 180.0;
			AngularVelocity[2] = 5.0 * 3.1415926535 / 180.0;//5.0 * 3.1415926535 / 180.0;
			AttiVelECI[0] = AngularVelocity[0];
			AttiVelECI[1] = AngularVelocity[1];
			AttiVelECI[2] = AngularVelocity[2];

			set_orbit_flag = 1;
		}
		return 0; 
	} 

	//否则使用主星输入轨道 
	else
	{
		if(!set_orbit_flag)
		{
			memcpy((uint8_t *)&orbit_cmder.Epoch,frame,sizeof(ORBIT_CMDER));
			set_orbit_flag = 1;
			
			Epoch_J2000 = orbit_cmder.Epoch;
			posi_J2000[0] = orbit_cmder.CJB_cmder.posi_x_J2000;
			posi_J2000[1] = orbit_cmder.CJB_cmder.posi_y_J2000;
			posi_J2000[2] = orbit_cmder.CJB_cmder.posi_z_J2000;

			velo_J2000[0] = orbit_cmder.CJB_cmder.velo_x_J2000;
			velo_J2000[1] = orbit_cmder.CJB_cmder.velo_y_J2000;
			velo_J2000[2] = orbit_cmder.CJB_cmder.velo_z_J2000;

			Q_J2000[0] = orbit_cmder.CJB_cmder.q0_J2000;
			Q_J2000[1] = orbit_cmder.CJB_cmder.q1_J2000;
			Q_J2000[2] = orbit_cmder.CJB_cmder.q2_J2000;
			Q_J2000[3] = orbit_cmder.CJB_cmder.q3_J2000;

			//目标性轨道参数
			Tar_posi_J2000[0] = orbit_cmder.Target_cmder.posi_x_J2000; //-41972144.5417670;
			Tar_posi_J2000[1] = orbit_cmder.Target_cmder.posi_y_J2000;//4040287.04196300;
			Tar_posi_J2000[2] = orbit_cmder.Target_cmder.posi_z_J2000;//92381.2956310000;
			Tar_velo_J2000[0] = orbit_cmder.Target_cmder.velo_x_J2000;//-294.436520000000;
			Tar_velo_J2000[1] = orbit_cmder.Target_cmder.velo_y_J2000;//-3059.96985700000;
			Tar_velo_J2000[2] = orbit_cmder.Target_cmder.velo_z_J2000;//54.3873050000000;

			orbInfo[0] = posi_J2000[0]; orbInfo[1] = posi_J2000[1]; orbInfo[2] = posi_J2000[2];
			orbInfo[3] = velo_J2000[0]; orbInfo[4] = velo_J2000[1]; orbInfo[5] = velo_J2000[2];
			
			MtxJtoOGet(MtxECI2VVLH, orbInfo);

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

			AttiMtxVVLH[0][0] = 1; AttiMtxVVLH[0][1] = 0; AttiMtxVVLH[0][2] = 0;
			AttiMtxVVLH[1][0] = 0; AttiMtxVVLH[1][1] = 1; AttiMtxVVLH[1][2] = 0;
			AttiMtxVVLH[2][0] = 0; AttiMtxVVLH[2][1] = 0; AttiMtxVVLH[2][2] = 1;
			

			AngularVelocity[0] = 5.0 * 3.1415926535 / 180.0;
			AngularVelocity[1] = 5.0 * 3.1415926535 / 180.0;
			AngularVelocity[2] = 5.0 * 3.1415926535 / 180.0;
			AttiVelECI[0] = AngularVelocity[0];
			AttiVelECI[1] = AngularVelocity[1];
			AttiVelECI[2] = AngularVelocity[2];

			spdlog::info("函数{0}接收到的轨道历元为:{1:.3f},节点号为:{2:2d}, \
							位置为:{3:.3f},{4:.3f},{5:.3f},					\
							速度为:{6:.3f},{7:.3f},{8:.3f},					\
							四元数为:{9:.3f},{10:.3f},{11:.3f},{12:.3f}"
							,__FUNCTION__,Epoch_J2000,
							orbit_cmder.CJB_cmder.pack_id,
							posi_J2000[0],posi_J2000[1],posi_J2000[2],
							velo_J2000[0],velo_J2000[1],velo_J2000[2],
							Q_J2000[0],Q_J2000[1],Q_J2000[2],Q_J2000[3]);

			spdlog::info("函数{0}接收到的目标星轨道历元为:{1:.3f},节点号为:{2:2d}, \
				位置为:{3:.3f},{4:.3f},{5:.3f},					\
				速度为:{6:.3f},{7:.3f},{8:.3f}"
				,__FUNCTION__,Epoch_J2000,
				orbit_cmder.Target_cmder.pack_id,
				Tar_posi_J2000[0],Tar_posi_J2000[1],Tar_posi_J2000[2],
				Tar_velo_J2000[0],Tar_velo_J2000[1],Tar_velo_J2000[2]);

		}
	}	
	return 1;
}


/// @brief 设置轨道信息;该函数供通信模块调用,将主星传递的信息赋值到本地
///		   仅第一次赋值,后续根据初始赋值递推
/// @param frame:输入量,轨道信息数组 
/// @return 0:设置失败; 1:成功
/// /* 输入量 */
	/* SatMtxI			卫星转动惯量矩阵 */
	/* Tq				总力矩 */
	/* WheelAM			动量轮角动量 */
	//SatMass 卫星质量  从微推模型获取
	//SatMtxI 转动惯量  从微推模型获取
	/* 输出量 */
	/* AttiVelECI		角速度W^bi */
	/*orbInfo[6]*/
	/* double Tar_orbInfo[6] */
	// EulerVVLH[3]   欧拉角
	// AttiMtxVVLH[3][3] 姿态转移矩阵
	// MtxECI2VVLH 旋转矩阵

uint8_t generate_orbit_info(double orbInfo[6], double Tar_orbInfo[6], double AttiVelECI[3], double EulerVVLH[3], 
						double AttiMtxVVLH[3][3], double MtxECI2VVLH[3][3], 
						double* mjd, double* Epoch2017,
						double SatMass, double SatMtxI[3][3], 
						double ThrustForce[3], double Tq[3], double WheelAM[3])
{
	double dt_Orb;
	double h, AttiVelVVLH[3], QuatECI[4], OrbitVel[3], tInterval, QuatECI2VVLH[4], QoiX[4][4], PosNorm, VelNorm, W_Orbit;
	double mjd0, pv0[6], Tar_pv0[6];
	double pv[6], Tar_pv[6];
	int NUM = 10;
	
	
	dt_Orb = StepSize;
	h = StepSize / NUM;
	tInterval = StepSize;

	mtxCpy((double*)ThrustForceBody, (double*)ThrustForce, 3, 1);
	ChaSatMass = SatMass;
	
		mjd0 = Epoch_J2000 / 86400.0 - 8.0/24.0 + mjd_2017; //Epoch_J2000更名为Epoch_BJT
		pv0[0] = posi_J2000[0];
		pv0[1] = posi_J2000[1];
		pv0[2] = posi_J2000[2];
		pv0[3] = velo_J2000[0];
		pv0[4] = velo_J2000[1];
		pv0[5] = velo_J2000[2];
		*mjd = mjd0 + dt_Orb / 86400.0;
		//追踪星轨道动力学递推
		ORBIT_J6(dt_Orb, h, pv0, pv);
		MtxJtoOGet(MtxECI2VVLH, pv);
		mtxCpy((double*)orbInfo, (double*)pv, 6, 1);

		//目标星轨道动力学递推
		ThrustForceBody[0] = 0.0; ThrustForceBody[1] = 0.0; ThrustForceBody[2] = 0.0;
		Tar_pv0[0] = Tar_posi_J2000[0];
		Tar_pv0[1] = Tar_posi_J2000[1];
		Tar_pv0[2] = Tar_posi_J2000[2];
		Tar_pv0[3] = Tar_velo_J2000[0];
		Tar_pv0[4] = Tar_velo_J2000[1];
		Tar_pv0[5] = Tar_velo_J2000[2];
		ORBIT_J6(dt_Orb, h, Tar_pv0, Tar_pv);
		mtxCpy((double*)Tar_orbInfo, (double*)Tar_pv, 6, 1);

//相对位置
double DeltaPos[3],DeltaPosVVLH[3];
DeltaPos[0] = pv0[0] - Tar_pv0[0];
DeltaPos[1] = pv0[1] - Tar_pv0[1];
DeltaPos[2] = pv0[2] - Tar_pv0[2];
mtxMtp((double*)DeltaPosVVLH, (double*)MtxECI2VVLH, 3, 3, (double*)DeltaPos, 3, 1);

		// 姿态动力学运动学递推
		PosNorm = norm(pv, 3);
		VelNorm = norm(&pv[3], 3);
		W_Orbit = VelNorm / PosNorm;
		OrbitVel[0] = 0.0;
		OrbitVel[1] = -1 * W_Orbit;
		OrbitVel[2] = 0.0;
		for (int i = 0; i < NUM; ++i) {

			Runge_Kutta((double*)AngularVelocity, (double)h, (double*)Tq, (double*)WheelAM, (double*)SatMtxI);
			mtxCpy((double*)AttiVelECI, (double*)AngularVelocity, 3, 1);
		}
		KinenaticModel(AttiVelVVLH, AttiMtxVVLH, EulerVVLH, QuatVVLH, AttiVelECI, OrbitVel, tInterval);

		Epoch_J2000 = Epoch_J2000 + dt_Orb;
		posi_J2000[0] = pv[0];
		posi_J2000[1] = pv[1];
		posi_J2000[2] = pv[2];
		velo_J2000[0] = pv[3];
		velo_J2000[1] = pv[4];
		velo_J2000[2] = pv[5];

		Tar_posi_J2000[0] = Tar_pv[0];
		Tar_posi_J2000[1] = Tar_pv[1];
		Tar_posi_J2000[2] = Tar_pv[2];
		Tar_velo_J2000[0] = Tar_pv[3];
		Tar_velo_J2000[1] = Tar_pv[4];
		Tar_velo_J2000[2] = Tar_pv[5];

		A2q(QuatECI2VVLH, MtxECI2VVLH);
		qleft(QoiX, QuatECI2VVLH);
		mtxMtp((double*)QuatECI, (double*)QoiX, 4, 4, (double*)QuatVVLH, 4, 1);
		Q_J2000[0] = QuatECI[0];
		Q_J2000[1] = QuatECI[1];
		Q_J2000[2] = QuatECI[2];
		Q_J2000[3] = QuatECI[3];

		*Epoch2017 =Epoch_J2000;

		orbInfo[0] = posi_J2000[0]; orbInfo[1] = posi_J2000[1]; orbInfo[2] = posi_J2000[2];
		orbInfo[3] = velo_J2000[0]; orbInfo[4] = velo_J2000[1]; orbInfo[5] = velo_J2000[2];

		Tar_orbInfo[0] = Tar_posi_J2000[0]; Tar_orbInfo[1] = Tar_posi_J2000[1]; Tar_orbInfo[2] = Tar_posi_J2000[2];
		Tar_orbInfo[3] = Tar_velo_J2000[0]; Tar_orbInfo[4] = Tar_velo_J2000[1]; Tar_orbInfo[5] = Tar_velo_J2000[2];

		spdlog::info("函数{0}的QuatECI为:{1:.1f},{2:.5f},{3:.5f},{4:.5f},AttiVelECI为:{5:.5f},{6:.5f},{7:.5f},{8:.5f},{9:.5f},{10:.5f}",
					__FUNCTION__,
					QuatECI[0], QuatECI[1], QuatECI[2], QuatECI[3],
					AttiVelECI[0], AttiVelECI[1], AttiVelECI[2],
					DeltaPosVVLH[0],DeltaPosVVLH[1],DeltaPosVVLH[2]);

		spdlog::info("函数{0}轨道信息:{1:.1f},{2:.5f},{3:.5f},{4:.5f},{5:.5f},{6:.5f},{7:.5f},\
		{8:.5f},{9:.5f},{10:.5f},{11:.5f},{12:.5f},{13:.5f},{14:.5f},{15:.5f}",
			__FUNCTION__,
			*mjd,
			posi_J2000[0],
			posi_J2000[1],
			posi_J2000[2],
			velo_J2000[0],
			velo_J2000[1],
			velo_J2000[2],
			Q_J2000[0],
			Q_J2000[1],
			Q_J2000[2],
			Q_J2000[3],
			AttiVelECI[0],
			AttiVelECI[1],
			AttiVelECI[2],
			Epoch_J2000);

	return 1;
}

/// @brief 获取轨道信息;该函数供通信模块调用
/// @param frame:输出量,轨道信息数组 
/// @return 1:成功
uint8_t get_orbit_info(uint8_t *frame,uint8_t id)
{

	s_rescmd.Epoch  = Epoch_J2000 ;
	s_rescmd.resCmder.pack_id = id;
	s_rescmd.resCmder.posi_x_J2000 = posi_J2000[0] ;
	s_rescmd.resCmder.posi_y_J2000 = posi_J2000[1] ;
	s_rescmd.resCmder.posi_z_J2000 = posi_J2000[2] ;

	s_rescmd.resCmder.velo_x_J2000 = velo_J2000[0] ;
	s_rescmd.resCmder.velo_y_J2000 = velo_J2000[1] ;
	s_rescmd.resCmder.velo_z_J2000 = velo_J2000[2] ;

	s_rescmd.resCmder.q0_J2000 =  Q_J2000[0] ;
	s_rescmd.resCmder.q1_J2000 =  Q_J2000[1] ;
	s_rescmd.resCmder.q2_J2000 =  Q_J2000[2] ;
	s_rescmd.resCmder.q3_J2000 =  Q_J2000[3] ;

	spdlog::info("函数{0}反馈的轨道历元为:{1:.3f},节点号为:{2:2d},位置为:{3:.3f},速度为:{4:.3f},姿态为:{5:.3f}"
				,__FUNCTION__,Epoch_J2000,
				s_rescmd.resCmder.pack_id,
				posi_J2000[0],
				velo_J2000[0],
				Q_J2000[0]);

	memcpy(frame,(uint8_t *)&s_rescmd.Epoch,sizeof(STEP_CMDER_RES));
	return 1;
}

/// @brief 获取J2000系历元信息
/// @param Epoch:返回的历元数据 
/// @return 0:获取失败; 1:成功
uint8_t get_orbit_epoic(double *Epoch)
{
	*Epoch = Epoch_J2000;

	return 1;
}

/// @brief 获取mjd
/// @param Epoch:返回的位置数据 
/// @return 0:获取失败; 1:成功
//double get_orbit_mjd(void)
//{
//	return mjd;
//}


/// @brief 获取J2000系位置信息
/// @param Epoch:返回的位置数据 
/// @return 0:获取失败; 1:成功
uint8_t get_orbit_posi(double *posi)
{
	posi[0] =  posi_J2000[0];
	posi[1] =  posi_J2000[1];
	posi[2] =  posi_J2000[2];
	return 1;
}

/// @brief 获取J2000系速度信息
/// @param Epoch:返回的速度数据 
/// @return 0:获取失败; 1:成功
uint8_t get_orbit_velo(double *velo)
{
	velo[0] = velo_J2000[0];
	velo[1] = velo_J2000[1];
	velo[2] = velo_J2000[2];
	return 1;
}

/// @brief 获取J2000系姿态四元数信息
/// @param Epoch:返回的姿态四元数数据 
/// @return 0:获取失败; 1:成功
uint8_t get_orbit_Q(double *Q)
{
	Q[0] = Q_J2000[0];
	Q[1] = Q_J2000[1];
	Q[2] = Q_J2000[2];
	Q[3] = Q_J2000[3];

	return 1;
}

/// @brief 获取轨道赋值标志位
/// @param 
/// @return 轨道赋值标志位
uint8_t get_orbit_flag(void)
{
	return set_orbit_flag;
}

/* 动力学模型 */
void Dynamic_Model(double* AttiVelDotECI, double* w, double* Tq, double* WheelAM, double* SatMtxI) {

	/* 动力学模型模块 */
	/* 输入量 */
	/* w				角速度初值 */
	/* Tq				力矩 */
	/* WheelAM			动量轮角动量 */
	/* 输出量 */
	/* AttiVelDotECI				角加速度 */

	double CrossMatrixW[3][3], IW[3], Hs[3];
	double temp1[3], temp2[3], temp3[3][3], temp4[3];

	mtxMtp((double*)IW, (double*)SatMtxI, 3, 3, w, 3, 1);
	mtxAdd((double*)Hs, (double*)IW, (double*)WheelAM, 3, 1);
	CrossMatrixGet(CrossMatrixW, w);
	mtxMtp((double*)temp1, (double*)CrossMatrixW, 3, 3, (double*)Hs, 3, 1);
	mtxSub((double*)temp2, (double*)Tq, (double*)temp1, 3, 1);
	mtxInv((double*)temp3, (double*)SatMtxI, 3);
	mtxMtp((double*)AttiVelDotECI, (double*)temp3, 3, 3, (double*)temp2, 3, 1);
}


/* 四阶龙格库塔算法 */
void Runge_Kutta(double* AngularVelocity, double h, double* Tq, double* WheelAM, double* SatMtxI) {

	/* 四阶龙格库塔算法模块 */
	/* 输入量 */
	/* AngularVelocity	角速度初值 */
	/* h				迭代步长 */
	/* Tq				力矩 */
	/* WheelAM			动量轮角动量 */
	/* 输出量 */
	/* AngularVelocity  角速度终值 */

	double work[3], AttiVelDotECI[3], k0[3], k1[3], k2[3], k3[3];
	int j;

	Dynamic_Model((double*)AttiVelDotECI, AngularVelocity, Tq, WheelAM, SatMtxI);//计算k1
	for (j = 0; j < 3; ++j)
		k0[j] = h * AttiVelDotECI[j];
	for (j = 0; j < 3; ++j)
		work[j] = *(AngularVelocity + j) + 0.5 * k0[j];//用数组work存储w的变化量

	Dynamic_Model((double*)AttiVelDotECI, (double*)work, Tq, WheelAM, SatMtxI);//计算k2
	for (j = 0; j < 3; ++j)
		k1[j] = h * AttiVelDotECI[j];
	for (j = 0; j < 3; ++j)
		work[j] = *(AngularVelocity + j) + 0.5 * k1[j];//用数组work存储w的变化量

	Dynamic_Model((double*)AttiVelDotECI, (double*)work, Tq, WheelAM, SatMtxI);//计算k3
	for (j = 0; j < 3; ++j)
		k2[j] = h * AttiVelDotECI[j];
	for (j = 0; j < 3; ++j)
		work[j] = *(AngularVelocity + j) + k2[j];//用数组work存储w的变化量

	Dynamic_Model((double*)AttiVelDotECI, (double*)work, Tq, WheelAM, SatMtxI);//计算k4
	for (j = 0; j < 3; ++j)
		k3[j] = h * AttiVelDotECI[j];
	for (j = 0; j < 3; ++j)
		work[j] = *(AngularVelocity + j) + (k0[j] + 2 * k1[j] + 2 * k2[j] + k3[j]) / 6;
	mtxCpy((double*)AngularVelocity, (double*)work, 3, 1);

}


/* 运动学更新模拟 */
void KinenaticModel(double AttiVelVVLH[3], double AttiMtxVVLH[3][3], double EulerVVLH[3], double QuatVVLH[4], double AttiVelECI[3], double OrbitVel[3], double tInterval) {

	/* 运动学更新模拟模块 */
	/* 输入量 */
	/* AttiVelECI		姿态角速度Wbi */
	/* OrbitVel			轨道角速度Woi */
	/* 输出量 */
	/* AttiVelVVLH		姿态角速度Wbo */
	/* AttiMtxVVLH		姿态矩阵Abo */
	/* QuatVVLH			姿态四元数qbo */

	double Abo[3][3], Wo[3], woi[3], CrossVelVVLH[4][4], temp[4][4], qbo[4], Rc, ii;
	
	TransQ(Abo, QuatVVLH);
	mtxMtp((double*)woi, (double*)Abo, 3, 3, (double*)OrbitVel, 3, 1);
	AttiVelVVLH[0] = AttiVelECI[0] - woi[0];
	AttiVelVVLH[1] = AttiVelECI[1] - woi[1];
	AttiVelVVLH[2] = AttiVelECI[2] - woi[2];

	CrossVelVVLH[0][0] = 0.0;				   CrossVelVVLH[0][1] = -0.5 * AttiVelVVLH[0]; CrossVelVVLH[0][2] = -0.5 * AttiVelVVLH[1]; CrossVelVVLH[0][3] = -0.5 * AttiVelVVLH[2];
	CrossVelVVLH[1][0] = 0.5 * AttiVelVVLH[0]; CrossVelVVLH[1][1] = 0.0;				   CrossVelVVLH[1][2] = 0.5 * AttiVelVVLH[2];  CrossVelVVLH[1][3] = -0.5 * AttiVelVVLH[1];
	CrossVelVVLH[2][0] = 0.5 * AttiVelVVLH[1]; CrossVelVVLH[2][1] = -0.5 * AttiVelVVLH[2]; CrossVelVVLH[2][2] = 0.0;				   CrossVelVVLH[2][3] = 0.5 * AttiVelVVLH[0];
	CrossVelVVLH[3][0] = 0.5 * AttiVelVVLH[2]; CrossVelVVLH[3][1] = 0.5 * AttiVelVVLH[1];  CrossVelVVLH[3][2] = -0.5 * AttiVelVVLH[0]; CrossVelVVLH[3][3] = 0.0;
	mtxMtp((double*)temp, (double*)CrossVelVVLH, 4, 4, &tInterval, 1, 1);
	for (int i = 0; i < 4; i++) {
		temp[i][i] += 1.0;                                          /* temp = eye(4,4)+tInterval*0.5*(wbo叉乘矩阵） */
	}
	mtxCpy((double*)qbo, (double*)QuatVVLH, 4, 1);
	mtxMtp((double*)QuatVVLH, (double*)temp, 4, 4, (double*)qbo, 4, 1);  /* qbo=qbo(上一时刻)+tInterval*0.5*(wbo叉乘矩阵）*qbo 运动学方程 */
	Rc = 1.0 / norm(QuatVVLH, 4);
	mtxMtp((double*)QuatVVLH, (double*)QuatVVLH, 4, 1, &Rc, 1, 1);
	ii = -1.0;
	if (QuatVVLH[0] < 0) {
		mtxMtp((double*)QuatVVLH, (double*)QuatVVLH, 4, 1, &ii, 1, 1);
	}

	TransQ(AttiMtxVVLH, QuatVVLH);
	qua2euler(EulerVVLH, QuatVVLH, qbo);
}


/* 计算地球引力 */
void Fun_AccEarth(double Acc_Earth[3], double R_ECEF[3]) {
	/*
	 输入:地固系下的卫星位置矢量,m
	 输出:地固系下的卫星加速度矢量，m/s2
	*/
	double GM = 398600.4418e9;         //单位：m3/s3
	double Re = 6378.137e3;            //单位：m
	double J2 = 1.08262617385222e-3;
	double J3 = -2.53241051856772e-6;
	double J4 = -1.61989759991697e-6;
	double J5 = -2.27753590730836e-7;
	double J6 = 5.40666576283813e-7;

	double rx = R_ECEF[0];
	double ry = R_ECEF[1];
	double rz = R_ECEF[2];
	double r = norm(R_ECEF, 3);

	// 中心引力加速度
	double Acc_TB[3];
	double r3 = pow(r, 3);
	for (int i = 0; i < 3; i++) {
		Acc_TB[i] = -1.0 * (GM / r3) * R_ECEF[i];
	}

	// J2摄动加速度
	double r5 = pow(r, 5);
	double Re2 = pow(Re, 2);
	double k0 = -1.0 * (3.0 * GM * J2 * Re2) / (2 * r5);
	double k1 = 5.0 * rz * rz / (r * r);
	double Acc_J2[3];
	Acc_J2[0] = k0 * (1 - k1) * rx;
	Acc_J2[1] = k0 * (1 - k1) * ry;
	Acc_J2[2] = k0 * (3 - k1) * rz;

	// J3摄动加速度
	double r7 = pow(r, 7);
	double Re3 = pow(Re, 3);
	double k01 = -1.0 * (5.0 * J3 * GM * Re3) / (2.0 * r7);
	double k11 = 3.0 * rz - 7.0 * rz * rz * rz / (r * r);
	double k21 = 6.0 * rz * rz - 7.0 * rz * rz * rz * rz / (r * r) - 3.0 / 5.0 * r * r;
	double Acc_J3[3];
	Acc_J3[0] = k01 * rx * k11;
	Acc_J3[1] = k01 * ry * k11;
	Acc_J3[2] = k01 * k21;

	// J4摄动加速度
	//double r7 = pow(r, 7);
	double Re4 = pow(Re, 4);

	double k02 = (15.0 * J4 * GM * Re4) / (8 * r7);
	double k12 = 1.0 - 14.0 * rz * rz / (r * r);
	double k22 = 5.0 - 70.0 * rz * rz / (3 * r * r);
	double k32 = 21.0 * rz * rz * rz * rz / (r * r * r * r);
	double Acc_J4[3];
	Acc_J4[0] = k02 * (k12 + k32) * rx;
	Acc_J4[1] = k02 * (k12 + k32) * ry;
	Acc_J4[2] = k02 * (k22 + k32) * rz;

	// J5摄动加速度
	double r9 = pow(r, 9);
	double Re5 = pow(Re, 5);
	double k03 = (3.0 * J5 * GM * Re5 * rz) / (8 * r9);
	double k001 = rz * rz / (r * r);
	double k002 = pow(k001, 2);
	double k003 = pow(k001, 3);
	double k13 = 35.0 - 210.0 * k001 + 231.0 * k002;
	double k23 = 105.0 - 315.0 * k001 + 231.0 * k002;
	double Acc_J5[3];
	Acc_J5[0] = k03 * k13 * rx;
	Acc_J5[1] = k03 * k13 * ry;
	Acc_J5[2] = k03 * k23 * rz;
	Acc_J5[2] = Acc_J5[2] - 15.0 * J5 * GM * Re5 / (8 * r7);

	// J6摄动加速度
	double Re6 = pow(Re, 6);
	double k04 = -1.0 * (J6 * GM * Re6) / (16.0 * r9);
	double k14 = 35.0 - 945.0 * k001 + 3465.0 * k002 - 3003.0 * k003;
	double k24 = 245.0 - 2205.0 * k001 + 4851.0 * k002 - 3003.0 * k003;
	double Acc_J6[3];
	Acc_J6[0] = k04 * k14 * rx;
	Acc_J6[1] = k04 * k14 * rz;//确定是rz吗?
	Acc_J6[2] = k04 * k24 * rz;

	// 总加速度
	/*for (int i = 0; i < 3; i++) {
		Acc_Earth[i] = Acc_TB[i] + Acc_J2[i] + Acc_J3[i] + Acc_J4[i] + Acc_J5[i] + Acc_J6[i];
	}
	*/
	Acc_Earth[0] = Acc_TB[0] + Acc_J2[0] + Acc_J3[0] + Acc_J4[0] + Acc_J5[0] + Acc_J6[0];
	Acc_Earth[1] = Acc_TB[1] + Acc_J2[1] + Acc_J3[1] + Acc_J4[1] + Acc_J5[1] + Acc_J6[1];
	Acc_Earth[2] = Acc_TB[2] + Acc_J2[2] + Acc_J3[2] + Acc_J4[2] + Acc_J5[2] + Acc_J6[2];

}


/* 轨道动力学 */
void Fun_orbit(double dots[6], double RV[6]) {

	double Acc_Earth[3], SatPosECI[3], MtxECI2Body[3][3], MtxBody2ECI[3][3], ThrustForceECI[3], Acc_Thrust_ECI[3];

	SatPosECI[0] = RV[0];
	SatPosECI[1] = RV[1];
	SatPosECI[2] = RV[2];
	Fun_AccEarth(Acc_Earth, SatPosECI);

	// Acc_Thrust_ECI = MtxBody2ECI* 本体系下三轴推力 / 整星质量
	TransQ(MtxECI2Body, Q_J2000);
	mtxInv((double*)MtxBody2ECI, (double*)MtxECI2Body, 3);
	mtxMtp((double*)ThrustForceECI, (double*)MtxBody2ECI, 3, 3, (double*)ThrustForceBody, 3, 1);

	dots[0] = RV[3];
	dots[1] = RV[4];
	dots[2] = RV[5];
	dots[3] = Acc_Earth[0] + ThrustForceECI[0] / ChaSatMass;
	dots[4] = Acc_Earth[1] + ThrustForceECI[1] / ChaSatMass;
	dots[5] = Acc_Earth[2] + ThrustForceECI[2] / ChaSatMass;

}


/* 轨道的四阶龙格库塔算法 */
void Orbit_Runge_Kutta(double* PosVel, double h) {
	/* 输入量 */
	/* RV			    惯性系下位置速度初始值 */
	/* h				迭代步长 */

	/* 输出量 */
	/* RV		        迭代出的位置速度*/

	double work[6], dots[6], k0[6], k1[6], k2[6], k3[6];
	int j;

	Fun_orbit((double*)dots, PosVel);//计算k1
	for (j = 0; j < 6; ++j)
		k0[j] = h * dots[j];
	for (j = 0; j < 6; ++j)
		work[j] = *(PosVel + j) + 0.5 * k0[j];//用数组work存储w的变化量

	Fun_orbit((double*)dots, (double*)work);//计算k2
	for (j = 0; j < 6; ++j)
		k1[j] = h * dots[j];
	for (j = 0; j < 6; ++j)
		work[j] = *(PosVel + j) + 0.5 * k1[j];//用数组work存储w的变化量

	Fun_orbit((double*)dots, (double*)work);//计算k3
	for (j = 0; j < 6; ++j)
		k2[j] = h * dots[j];
	for (j = 0; j < 6; ++j)
		work[j] = *(PosVel + j) + k2[j];//用数组work存储w的变化量

	Fun_orbit((double*)dots, (double*)work);//计算k4
	for (j = 0; j < 6; ++j)
		k3[j] = h * dots[j];
	for (j = 0; j < 6; ++j)
		work[j] = *(PosVel + j) + (k0[j] + 2 * k1[j] + 2 * k2[j] + k3[j]) / 6.0;
	mtxCpy((double*)PosVel, (double*)work, 6, 1);
}


/* J6摄动轨道递推 */
void ORBIT_J6(double tf, double dt, double RV0[6], double RV[6]) {

	double state[6];
	double t0 = 0;

	for (int i = 0; i < 6; i++) {
		state[i] = RV0[i];
	}

	int steps = (int)((tf - t0) / dt) + 1;

	for (int i = 1; i < steps; i++) {
		Orbit_Runge_Kutta((double*)state, dt);
	}

	for (int i = 0; i < 6; i++) {
		RV[i] = state[i];
	}
}


uint8_t get_orbit_source_flag(void)
{
	return orbit_source_flag;
}