#include "stdafx.h"
#include "SatelliteOribtModel.h"
#include "GeoOperator.h"
#include <boost/math/tools/polynomial.hpp> // 多项式
#include "BaseTool.h"
#include <QDebug>
#include <QFile>
#include <QXmlStreamWriter>



PolyfitSatelliteOribtModel::PolyfitSatelliteOribtModel()
{
	this->oribtStartTime = 0;
	this->beamAngle = 0;
	this->RightLook = true;
	this->cycletime = 0;
	this->minAzAngle = 0;
	this->maxAzAngle = 0;
	this->referenceAzAngle = 0;
	this->referenceTimeFromStartTime = 0;
	this->AntnnaAxisX = Point3{ 1,0,0 };
	this->AntnnaAxisY = Point3{ 0,1,0 };
	this->AntnnaAxisZ = Point3{ 0,0,1 };
	this->Pxchisq = 0;
	this->Pychisq = 0;
	this->Pzchisq = 0;
	this->Vxchisq = 0;
	this->Vychisq = 0;
	this->Vzchisq = 0;
	this->Pt = 1;
	this->Gri = 1;
}

PolyfitSatelliteOribtModel::~PolyfitSatelliteOribtModel()
{
	 //TODO: 析构函数


}

QString PolyfitSatelliteOribtModel::getSatelliteOribtModelParamsString()
{
	QString result = "";
	result += this->polyfitPx.size()-1 + "\n";
	result += "----------- poly Position X  -------------------------\n";
	for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
		result += QString::number(this->polyfitPx[i], 'e', 6) + "\n";
	}
	result += "----------- poly Position Y  -------------------------\n";
	for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
		result += QString::number(this->polyfitPy[i], 'e', 6) + "\n";
	}
	result += "----------- poly Position Z  -------------------------\n";
	for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
		result += QString::number(this->polyfitPz[i], 'e', 6) + "\n";
	}
	result += "----------- poly Position Vector X  ------------------\n";
	for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
		result += QString::number(this->polyfitVx[i], 'e', 6) + "\n";
	}
	result += "----------- poly Position Vector Y  ------------------\n";
	for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
		result += QString::number(this->polyfitVy[i], 'e', 6) + "\n";
	}
	result += "----------- poly Position Vector Z  ------------------\n";
	for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
		result += QString::number(this->polyfitVz[i], 'e', 6) + "\n";
	}
	result+= "------------------------------------------------------\n";

	return result;
}

SatelliteOribtNode PolyfitSatelliteOribtModel::getSatelliteOribtNode(double& timeFromStartTime, bool& antAzAngleFlag)
{
	// 位置、速度
	SatelliteOribtNode node;
	ErrorCode state = getSatelliteOribtNode(timeFromStartTime, node, antAzAngleFlag);
    return node;
}

ErrorCode PolyfitSatelliteOribtModel::getSatelliteOribtNode(double& timeFromStartTime, SatelliteOribtNode& node, bool& antAzAngleFlag)
{
	// 卫星坐标，速度方向
	node.time = timeFromStartTime;
	node.Px=0;
	node.Py=0;
	node.Pz=0;
	node.Vx=0;
	node.Vy=0;
	node.Vz=0;

	for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
		node.Px += this->polyfitPx[i] * pow(timeFromStartTime, i);
		node.Py += this->polyfitPy[i] * pow(timeFromStartTime, i);
		node.Pz += this->polyfitPz[i] * pow(timeFromStartTime, i);
		node.Vx += this->polyfitVx[i] * pow(timeFromStartTime, i);
		node.Vy += this->polyfitVy[i] * pow(timeFromStartTime, i);
		node.Vz += this->polyfitVz[i] * pow(timeFromStartTime, i);
	}

	
	node.beamAngle = this->beamAngle; // 波位角
	ErrorCode Azstatecode=this->getAzAngleInCurrentTimeFromStartTime(timeFromStartTime, node.AzAngle); // 摆动角
	if (Azstatecode != ErrorCode::SUCCESS) {
		return Azstatecode;
	}	else {}

	
	if (!antAzAngleFlag) {
		return ErrorCode::SUCCESS;
	}	else {
	}
	// 计算卫星天线指向
	ErrorCode state = getAntnnaDirection(node);

	if (state != ErrorCode::SUCCESS) {
		return state;
	}	else {}

	state = getZeroDopplerAntDirect(node);
	if (state != ErrorCode::SUCCESS) {
		return state;
	}
	else {}
	return ErrorCode::SUCCESS;
}


ErrorCode PolyfitSatelliteOribtModel::getSatelliteAntDirectNormal(SatelliteOribtNode& Rs, Vector3D& Rt, SatelliteAntDirect& antNode)
{
	//Vector3D Rts = Vector3D{Rt.x-Rs.Px,Rt.y- Rs.Py,Rt.z-Rs.Pz}; // Rts t-->s
	antNode.Xst = (Rt.x - Rs.Px); // 卫星 -->  地面
	antNode.Yst = (Rt.y - Rs.Py);
	antNode.Zst = (Rt.z - Rs.Pz);
	antNode.Vxs = Rs.Vx;		  // 卫星速度
	antNode.Vys = Rs.Vy;
	antNode.Vzs = Rs.Vz;
	// 天线指向在天线坐标系下的值
	antNode.Xant = (antNode.Xst * (Rs.AntYaxisY * Rs.AntZaxisZ - Rs.AntYaxisZ * Rs.AntZaxisY) + antNode.Xst * (Rs.AntXaxisZ * Rs.AntZaxisY - Rs.AntXaxisY * Rs.AntZaxisZ) + antNode.Xst * (Rs.AntXaxisY * Rs.AntYaxisZ - Rs.AntXaxisZ * Rs.AntYaxisY)) / (Rs.AntXaxisX * (Rs.AntYaxisY * Rs.AntZaxisZ - Rs.AntZaxisY * Rs.AntYaxisZ) - Rs.AntYaxisX * (Rs.AntXaxisY * Rs.AntZaxisZ - Rs.AntXaxisZ * Rs.AntZaxisY) + Rs.AntZaxisX * (Rs.AntXaxisY * Rs.AntYaxisZ - Rs.AntXaxisZ * Rs.AntYaxisY));
	antNode.Yant = (antNode.Yst * (Rs.AntYaxisZ * Rs.AntZaxisX - Rs.AntYaxisX * Rs.AntZaxisZ) + antNode.Yst * (Rs.AntXaxisX * Rs.AntZaxisZ - Rs.AntXaxisZ * Rs.AntZaxisX) + antNode.Yst * (Rs.AntYaxisX * Rs.AntXaxisZ - Rs.AntXaxisX * Rs.AntYaxisZ)) / (Rs.AntXaxisX * (Rs.AntYaxisY * Rs.AntZaxisZ - Rs.AntZaxisY * Rs.AntYaxisZ) - Rs.AntYaxisX * (Rs.AntXaxisY * Rs.AntZaxisZ - Rs.AntXaxisZ * Rs.AntZaxisY) + Rs.AntZaxisX * (Rs.AntXaxisY * Rs.AntYaxisZ - Rs.AntXaxisZ * Rs.AntYaxisY));
	antNode.Zant = (antNode.Zst * (Rs.AntYaxisX * Rs.AntZaxisY - Rs.AntYaxisY * Rs.AntZaxisX) + antNode.Zst * (Rs.AntXaxisY * Rs.AntZaxisX - Rs.AntXaxisX * Rs.AntZaxisY) + antNode.Zst * (Rs.AntXaxisX * Rs.AntYaxisY - Rs.AntYaxisX * Rs.AntXaxisY)) / (Rs.AntXaxisX * (Rs.AntYaxisY * Rs.AntZaxisZ - Rs.AntZaxisY * Rs.AntYaxisZ) - Rs.AntYaxisX * (Rs.AntXaxisY * Rs.AntZaxisZ - Rs.AntXaxisZ * Rs.AntZaxisY) + Rs.AntZaxisX * (Rs.AntXaxisY * Rs.AntYaxisZ - Rs.AntXaxisZ * Rs.AntYaxisY));
	// 计算theta 与 phi
	antNode.Norm = std::sqrt(antNode.Xant*antNode.Xant+antNode.Yant*antNode.Yant+antNode.Zant*antNode.Zant); // 计算 pho
	antNode.ThetaAnt = std::acos(antNode.Zant / antNode.Norm); // theta 与 Z轴的夹角
	antNode.PhiAnt = (antNode.Yant*std::sin(antNode.ThetaAnt)/std::abs(antNode.Yant*std::sin(antNode.ThetaAnt)))  * std::acos(antNode.Xant/(antNode.Norm*std::sin(antNode.ThetaAnt)));
	return ErrorCode::SUCCESS;
}

void PolyfitSatelliteOribtModel::setSatelliteOribtStartTime(long double oribtStartTime)
{
	this->oribtStartTime = oribtStartTime;
}

long double PolyfitSatelliteOribtModel::getSatelliteOribtStartTime()
{
	return this->oribtStartTime;
}

void PolyfitSatelliteOribtModel::setbeamAngle(double beamAngle, bool RightLook)
{
	this->beamAngle = beamAngle;
	this->RightLook = RightLook;
}

void PolyfitSatelliteOribtModel::setAzAngleRange(double cycletime, double minAzAngle, double maxAzAngle, double referenceAzAngle, double referenceTimeFromStartTime)
{
	this->cycletime = cycletime;
	this->minAzAngle = minAzAngle;
	this->maxAzAngle = maxAzAngle;
	this->referenceAzAngle = referenceAzAngle;
	this->referenceTimeFromStartTime = referenceTimeFromStartTime;
}

double PolyfitSatelliteOribtModel::getAzAngleInCurrentTimeFromStartTime(double& currentTime)
{
	// 计算角度
	double Azangle = 0;
	ErrorCode state = getAzAngleInCurrentTimeFromStartTime(currentTime, Azangle);
	return Azangle;
}

/// 
///   X=Y x Z          摆动角(-）				    摆动角（+）
///      Z				  Z					            Z		
/// 	 ^				   \						   /
/// 	 |					\						  /
///  y<--X---			 y<--X---				 y<--X---
///      |				     |\    				    /|    
/// 	 |					   \				   /  	
/// 

ErrorCode PolyfitSatelliteOribtModel::getAzAngleInCurrentTimeFromStartTime(double& currentTime, double& AzAngle)
{
	if (std::abs(this->maxAzAngle - this->minAzAngle) < 1e-7||std::abs(this->cycletime)<1e-7) {
		AzAngle=this->referenceAzAngle;
		return ErrorCode::SUCCESS;
	}
	else {
		AzAngle = this->referenceAzAngle + (currentTime - this->referenceTimeFromStartTime) / this->cycletime * (this->maxAzAngle - this->minAzAngle);
		return ErrorCode::SUCCESS;
	}
}

void PolyfitSatelliteOribtModel::setAntnnaAxisX(double X, double Y, double Z)
{
	this->AntnnaAxisX = Point3{ X, Y, Z };
}

void PolyfitSatelliteOribtModel::setAntnnaAxisY(double X, double Y, double Z)
{
	this->AntnnaAxisY = Point3{ X, Y, Z };
}

void PolyfitSatelliteOribtModel::setAntnnaAxisZ(double X, double Y, double Z)
{
	this->AntnnaAxisZ = Point3{ X, Y, Z };
}

///           左视(+）				        右视（-）
///      Z				  Z					            Z		
/// 	 ^				   \						   /
/// 	 |					\						  /
///  x<--O---			 x<--O---				 x<--O---
///      |				     |\    				    /|    
/// 						   \				   /  	
/// 
ErrorCode PolyfitSatelliteOribtModel::getAntnnaDirection(SatelliteOribtNode& node)
{
	bool flag = false;
	double nexttime = node.time + 1e-6;
	SatelliteOribtNode node1 = this->getSatelliteOribtNode(nexttime, flag);

	//qDebug() << "getAntnnaDirection  corrdination " << std::endl;

	double Vx = (node1.Px - node.Px);
	double Vy = (node1.Py - node.Py);
	double Vz = (node1.Pz - node.Pz);

	// 代码测试部分
	//node.Px = 0;
	//node.Py = 0;
	//node.Pz = 1;

	//Vx = 1, Vy = 0, Vz = 0;


	// 1. 计算天线指向
	Eigen::Vector3d axisZ0 = { -1 * node.Px ,-1 * node.Py,-1 * node.Pz };	// z 轴 --波位角为0时，天线指向的反方向
	Eigen::Vector3d axisX0 = { Vx,Vy,Vz  };	// x 轴 --飞行方向
	Eigen::Vector3d axisY0 = axisZ0.cross(axisX0);			// y 轴 --右手定则  -- 初始坐标系

	//qDebug() << "axis_X0=[ " << axisX0.x() << "," << axisX0.y() << "," << axisX0.z() << "]" << std::endl;
	//qDebug() << "axis_Y0=[ " << axisY0.x() << "," << axisY0.y() << "," << axisY0.z() << "]" << std::endl;
	//qDebug() << "axis_Z0=[ " << axisZ0.x() << "," << axisZ0.y() << "," << axisZ0.z() << "]" << std::endl;

	double rotateAngle = this->RightLook ? -this->beamAngle : this->beamAngle; // 旋转角度 左（逆时针）：theta , 右（顺时针）： -theta 
	//qDebug() << "rotateAngle=" << rotateAngle << std::endl;
	//qDebug() << "Look side:\t" << (this->RightLook ? "right" : "left") << std::endl;
	// 1.2. 根据波位角，确定卫星绕X轴-飞行轴
	Eigen::Matrix3d rotateMatrixBeam = rotationMatrix(axisX0, rotateAngle*d2r); // 旋转矩阵
	axisZ0=rotateMatrixBeam*axisZ0; // 旋转矩阵
	axisY0=rotateMatrixBeam*axisY0;
	axisX0=rotateMatrixBeam*axisX0;
	// 1.3. 根据方位角，确定卫星绕Y轴旋转
	double azangle = node.AzAngle;
	Eigen::Matrix3d rotateMatrixAzAngle = rotationMatrix(axisY0, azangle * d2r); // 旋转矩阵
	axisZ0 = rotateMatrixAzAngle * axisZ0; // 旋转矩阵
	axisY0 = rotateMatrixAzAngle * axisY0;
	axisX0 = rotateMatrixAzAngle * axisX0;

	// 1.4. 计算实际指向
	node.AntDirecX = axisZ0[0];
	node.AntDirecY = axisZ0[1];
	node.AntDirecZ = axisZ0[2];

	// 2. 计算天线坐标系,方便计算天线方向图

	node.AntXaxisX =axisX0[0]; // 实际天线坐标系 在 WGS84 坐标系
	node.AntXaxisY =axisX0[1];
	node.AntXaxisZ =axisX0[2];

	node.AntYaxisX =axisY0[0];
	node.AntYaxisY =axisY0[1];
	node.AntYaxisZ =axisY0[2];
					 
	node.AntZaxisX =axisZ0[0];
	node.AntZaxisY =axisZ0[1];
	node.AntZaxisZ =axisZ0[2]; 
	 

	//qDebug() << "axis_X=[" << axisX0.x() << "," << axisX0.y() << "," << axisX0.z() << "]" << std::endl;
	//qDebug() << "axis_Y=[" << axisY0.x() << "," << axisY0.y() << "," << axisY0.z() << "]" << std::endl;
	//qDebug() << "axis_Z=[" << axisZ0.x() << "," << axisZ0.y() << "," << axisZ0.z() << "]" << std::endl;
	//qDebug() << "------------------------------------" << std::endl;
	return ErrorCode::SUCCESS;
}

ErrorCode PolyfitSatelliteOribtModel::getZeroDopplerAntDirect(SatelliteOribtNode& node)
{
	bool flag = false;
	double nexttime = node.time + 1e-6;
	SatelliteOribtNode node1 = this->getSatelliteOribtNode(nexttime, flag);

	// 1. 计算天线指向
	Eigen::Vector3d axisZ0 = { -1 * node.Px ,-1 * node.Py,-1 * node.Pz };	// z 轴 --波位角为0时，天线指向的反方向
	Eigen::Vector3d axisX0 = { (node1.Px-node.Px)  , (node1.Py - node.Py)  , (node1.Pz - node.Pz)   };	// x 轴 --飞行方向
	Eigen::Vector3d axisY0 = axisZ0.cross(axisX0);			// y 轴 --右手定则  -- 初始坐标系

	double rotateAngle = this->RightLook ? -this->beamAngle : this->beamAngle; // 旋转角度 左（逆时针）：theta , 右（顺时针）： -theta 

	// 1.2. 根据波位角，确定卫星绕X轴-飞行轴
	Eigen::Matrix3d rotateMatrixBeam = rotationMatrix(axisX0, rotateAngle * d2r); // 旋转矩阵
	axisZ0 = rotateMatrixBeam * axisZ0; // 旋转矩阵
	axisY0 = rotateMatrixBeam * axisY0;
	axisX0 = rotateMatrixBeam * axisX0;
	// 1.3. 根据方位角，确定卫星绕Y轴旋转
	double azangle = 0;
	Eigen::Matrix3d rotateMatrixAzAngle = rotationMatrix(axisY0, azangle * d2r); // 旋转矩阵
	axisZ0 = rotateMatrixAzAngle * axisZ0; // 旋转矩阵
	axisY0 = rotateMatrixAzAngle * axisY0;
	axisX0 = rotateMatrixAzAngle * axisX0;

	// 1.4. 计算实际指向
	node.zeroDopplerDirectX = axisZ0[0];
	node.zeroDopplerDirectY = axisZ0[1];
	node.zeroDopplerDirectZ = axisZ0[2];

	return ErrorCode::SUCCESS;
}

void PolyfitSatelliteOribtModel::addOribtNode(SatelliteOribtNode node)
{
	this->oribtNodes.push_back(node);
}



ErrorCode PolyfitSatelliteOribtModel::polyFit(int polynum/*=3*/, bool input_timeFromReferenceTime)
{
	if (polynum > this->oribtNodes.size() - 1) {
		return ErrorCode::OrbitNodeNotEnough;
	}
	else {}

	this->polyfitPx=std::vector<double>(polynum+1,0);
	this->polyfitPy=std::vector<double>(polynum+1,0);
	this->polyfitPz=std::vector<double>(polynum+1,0);	 
	this->polyfitVx=std::vector<double>(polynum+1,0);
	this->polyfitVy=std::vector<double>(polynum+1,0);
	this->polyfitVz=std::vector<double>(polynum+1,0);	 

	std::vector<double> timeArr = std::vector<double>(this->oribtNodes.size(), 0);
	std::vector<double> PxArr = std::vector<double>(this->oribtNodes.size(), 0);
	std::vector<double> PyArr = std::vector<double>(this->oribtNodes.size(), 0);
	std::vector<double> PzArr = std::vector<double>(this->oribtNodes.size(), 0);
	std::vector<double> VxArr = std::vector<double>(this->oribtNodes.size(), 0);
	std::vector<double> VyArr = std::vector<double>(this->oribtNodes.size(), 0);
	std::vector<double> VzArr = std::vector<double>(this->oribtNodes.size(), 0);

	// 数据准备
	for (long i = 0; i < this->oribtNodes.size(); i++) {
		if (input_timeFromReferenceTime) {
			timeArr[i] = this->oribtNodes[i].time;
		}
		else {
			timeArr[i] = this->oribtNodes[i].time - this->oribtStartTime; // 以参考时间为基准
			qDebug() << this->oribtNodes[i].time << "\t--->\t"<< timeArr[i];
		}
		PxArr[i] = this->oribtNodes[i].Px;
		PyArr[i] = this->oribtNodes[i].Py;
		PzArr[i] = this->oribtNodes[i].Pz;
		VxArr[i] = this->oribtNodes[i].Vx;
		VyArr[i] = this->oribtNodes[i].Vy;
		VzArr[i] = this->oribtNodes[i].Vz;
	}

	// 拟合Px Py Pz Vx Vy Vz
	ErrorCode codePx = polynomial_fit(timeArr, PxArr, polynum, this->polyfitPx, this->Pxchisq);
	ErrorCode codePy = polynomial_fit(timeArr, PyArr, polynum, this->polyfitPy, this->Pychisq);
	ErrorCode codePz = polynomial_fit(timeArr, PzArr, polynum, this->polyfitPz, this->Pzchisq);
	ErrorCode codeVx = polynomial_fit(timeArr, VxArr, polynum, this->polyfitVx, this->Vxchisq);
	ErrorCode codeVy = polynomial_fit(timeArr, VyArr, polynum, this->polyfitVy, this->Vychisq);
	ErrorCode codeVz = polynomial_fit(timeArr, VzArr, polynum, this->polyfitVz, this->Vzchisq);
	

	// 打印 this->polyfitPx
	qDebug() << "polyfit value:";
	qDebug() << "Px" << "\t" << "Py" << "\t" << "Pz" << "\t" << "Vx" << "\t" << "Vy" << "\t" << "Vz";
	for (long i = 0; i < this->polyfitPx.size(); i++) { // 求解平方
		qDebug() << this->polyfitPx[i] << "\t" << this->polyfitPy[i] << "\t" << this->polyfitPz[i] << "\t" << this->polyfitVx[i] << "\t" << this->polyfitVy[i] << "\t" << this->polyfitVz[i];
	}
 
 
	// 评价拟合情况
	double starttime = this->oribtStartTime;
	qDebug() << "oribt refrence time: " << starttime;
	qDebug() << "time" << "\t" << "dPx" << "\t" << "dPy" << "\t" << "dPz" << "\t" << "dVx" << "\t" << "dVy" << "\t" << "dVz";
	for (long i = 0; i < timeArr.size(); i++) {
		double time_temp = timeArr[i];
		bool flag = false;
		SatelliteOribtNode tempnode;
		this->getSatelliteOribtNode(time_temp, tempnode, flag);
		qDebug() << timeArr[i] << "\t" << PxArr[i] - tempnode.Px << "\t" << PyArr[i] - tempnode.Py << "\t" << PzArr[i] - tempnode.Pz << "\t" << VxArr[i] - tempnode.Vx << "\t" << VyArr[i] - tempnode.Vy << "\t" << VzArr[i] - tempnode.Vz;
	}
	return ErrorCode::SUCCESS;
}

double PolyfitSatelliteOribtModel::getPt()
{
	return this->Pt;
}

double PolyfitSatelliteOribtModel::getGri()
{
	return this->Gri;
}

void PolyfitSatelliteOribtModel::setPt(double pt)
{
	this->Pt = pt;
}

void PolyfitSatelliteOribtModel::setGri(double gri)
{
	this->Gri = gri;
}

std::shared_ptr<AbstractSatelliteOribtModel>    CreataPolyfitSatelliteOribtModel(std::vector<SatelliteOribtNode>& nodes,  long double startTime, int polynum)
{
	qDebug() << "CreataPolyfitSatelliteOribtModel \t" << (double)startTime<<"\t" << polynum;


	std::shared_ptr<PolyfitSatelliteOribtModel> ployfitOribtModel= std::make_shared< PolyfitSatelliteOribtModel>();
	for (long i = 0; i < nodes.size(); i++) {
		ployfitOribtModel->addOribtNode(nodes[i]);
	}
	ployfitOribtModel->setSatelliteOribtStartTime(startTime);
	ErrorCode stateCode = ployfitOribtModel->polyFit(polynum,false);
	if (stateCode != ErrorCode::SUCCESS) {
		qDebug() <<QString::fromStdString( errorCode2errInfo(stateCode));
		return nullptr;
	}
	else {
		return ployfitOribtModel;
	}
	return ployfitOribtModel;
}

 

void PolyfitSatelliteOribtModel::saveToXml(const QString& filePath)   {
	QFile file(filePath);
	if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
		// Handle error
		return;
	}

	QXmlStreamWriter xmlWriter(&file);
	xmlWriter.setAutoFormatting(true);
	xmlWriter.writeStartDocument();
	xmlWriter.writeStartElement("PolyfitSatelliteOribtModel");

	// Write basic properties
	xmlWriter.writeTextElement("oribtStartTime", QString::number(oribtStartTime, 'e', 35)); // 科学计数法，保留35位有效数字

	// Write polyfit parameters
	xmlWriter.writeStartElement("PolyfitParameters");
	xmlWriter.writeAttribute("Pxchisq", QString::number(Pxchisq,'e',35));
	xmlWriter.writeAttribute("Pychisq", QString::number(Pychisq,'e',35));
	xmlWriter.writeAttribute("Pzchisq", QString::number(Pzchisq,'e',35));
	xmlWriter.writeAttribute("Vxchisq", QString::number(Vxchisq,'e',35));
	xmlWriter.writeAttribute("Vychisq", QString::number(Vychisq,'e',35));
	xmlWriter.writeAttribute("Vzchisq", QString::number(Vzchisq,'e',35));

	writeVector(xmlWriter, "polyfitPx", polyfitPx);
	writeVector(xmlWriter, "polyfitPy", polyfitPy);
	writeVector(xmlWriter, "polyfitPz", polyfitPz);
	writeVector(xmlWriter, "polyfitVx", polyfitVx);
	writeVector(xmlWriter, "polyfitVy", polyfitVy);
	writeVector(xmlWriter, "polyfitVz", polyfitVz);
	xmlWriter.writeEndElement();

	xmlWriter.writeEndElement(); // End of PolyfitSatelliteOribtModel
	xmlWriter.writeEndDocument();

	file.close();
}

void PolyfitSatelliteOribtModel::writeVector(QXmlStreamWriter& xmlWriter, const QString& name, const std::vector<double>& vec) {
	xmlWriter.writeStartElement(name);
	for (double val : vec) {
		xmlWriter.writeTextElement("Value", QString::number(val,'e',35));
	}
	xmlWriter.writeEndElement();
}



bool PolyfitSatelliteOribtModel::loadFromXml(const QString& filePath) {
	QFile file(filePath);
	if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
		// Handle error
		return false;
	}

	QXmlStreamReader xmlReader(&file);
	while (!xmlReader.atEnd() && !xmlReader.hasError()) {
		QXmlStreamReader::TokenType token = xmlReader.readNext();
		if (token == QXmlStreamReader::StartElement) {
			if (xmlReader.name() == "oribtStartTime") {
				oribtStartTime = xmlReader.readElementText().toDouble();
			}
			else if (xmlReader.name() == "PolyfitParameters") {
				Pxchisq = xmlReader.attributes().value("Pxchisq").toDouble();
				Pychisq = xmlReader.attributes().value("Pychisq").toDouble();
				Pzchisq = xmlReader.attributes().value("Pzchisq").toDouble();
				Vxchisq = xmlReader.attributes().value("Vxchisq").toDouble();
				Vychisq = xmlReader.attributes().value("Vychisq").toDouble();
				Vzchisq = xmlReader.attributes().value("Vzchisq").toDouble();
			}
			else if (xmlReader.name() == "polyfitPx") {
				this->readVector(xmlReader, QString("polyfitPx"), polyfitPx);
			}
			else if (xmlReader.name() == "polyfitPy") {
				this->readVector(xmlReader, QString("polyfitPy"), polyfitPy);
			}
			else if (xmlReader.name() == "polyfitPz") {
				this->readVector(xmlReader, QString("polyfitPz"), polyfitPz);
			}
			else if (xmlReader.name() == "polyfitVx") {
				this->readVector(xmlReader, QString("polyfitVx"), polyfitVx);
			}
			else if (xmlReader.name() == "polyfitVy") {
				this->readVector(xmlReader, QString("polyfitVy"), polyfitVy);
			}
			else if (xmlReader.name() == "polyfitVz") {
				this->readVector(xmlReader, QString("polyfitVz"), polyfitVz);
			}
		}
	}

	if (xmlReader.hasError()) {
		// Handle error
		return false;
	}

	file.close();
	return true;
}


void PolyfitSatelliteOribtModel::readVector(QXmlStreamReader& xmlReader, const QString& name, std::vector<double>& vec) {
	if (xmlReader.name() == name) {
		qDebug() << "read orbit model ,key " << name;
		while (xmlReader.readNextStartElement()) {
			if (xmlReader.name() == "Value") {
				vec.push_back(xmlReader.readElementText().toDouble());
			}
		}
	}
}



long double PolyfitSatelliteOribtModel::getOribtStartTime()
{
	return oribtStartTime;
}

std::vector<double> PolyfitSatelliteOribtModel::getPolyfitPx()
{
	return polyfitPx;
}

std::vector<double> PolyfitSatelliteOribtModel::getPolyfitPy()
{
	return polyfitPy;
}

std::vector<double> PolyfitSatelliteOribtModel::getPolyfitPz()
{
	return polyfitPz;
}

std::vector<double> PolyfitSatelliteOribtModel::getPolyfitVx()
{
	return polyfitVx;
}

std::vector<double> PolyfitSatelliteOribtModel::getPolyfitVy()
{
	return polyfitVy;
}

std::vector<double> PolyfitSatelliteOribtModel::getPolyfitVz()
{
	return polyfitVz;
}