#include "SateOrbit.h"
//#define EIGEN_USE_MKL_ALL
//#define EIGEN_VECTORIZE_SSE4_2
//#include <mkl.h>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <time.h>
//#include <mkl.h>

using namespace std;
using namespace Eigen;



OrbitPoly::OrbitPoly()
{
}

OrbitPoly::OrbitPoly(int polynum, Eigen::MatrixX<double> polySatellitePara, double SatelliteModelStartTime)
{
	if (polySatellitePara.rows() != polynum||polySatellitePara.cols()!=6) {
		throw exception("?????????????????");
	}
	this->polySatellitePara = polySatellitePara;
	this->SatelliteModelStartTime = SatelliteModelStartTime;
	this->polynum = polynum;
}

OrbitPoly::~OrbitPoly()
{
}

Eigen::MatrixX<double> OrbitPoly::SatelliteSpacePoint(Eigen::MatrixX<double> satellitetime)
{
	Eigen::MatrixX<double> result= SatelliteSpacePoints(satellitetime, this->SatelliteModelStartTime, this->polySatellitePara, this->polynum);
	return result;
}

Eigen::MatrixX<double> OrbitPoly::SatelliteSpacePoint(long double satellitetime) {

	if (this->polySatellitePara.rows() != polynum || this->polySatellitePara.cols() != 6) {
		throw exception("the size of satellitetime has error!! row: p1,p2,p3,p4 col: x,y,z,vx,vy,vz ");
	}
	// ?????????
	double satellitetime2 =double( satellitetime - this->SatelliteModelStartTime);
	Eigen::MatrixX<double> satetime(1, polynum);
	for (int i = 0; i < polynum; i++) {
		satetime(0, i) = pow(satellitetime2, i);
	}

	// ????
	Eigen::MatrixX<double> satellitePoints(1, 6);
	satellitePoints = satetime * polySatellitePara;
	return satellitePoints;
}

/// <summary>
/// ????????????????????
/// </summary>
/// <param name="satellitetime">???????</param>
/// <param name="SatelliteModelStartTime">??????????</param>
/// <param name="polySatellitePara">??????[x1,y1,z1,vx1,vy1,vz1;  x2,y2,z2,vx2,vy2,vz2; .....  ]</param>
/// <param name="polynum">??????????</param>
/// <returns>????????</returns>
Eigen::MatrixX<double> SatelliteSpacePoints(Eigen::MatrixX<double>& satellitetime, double SatelliteModelStartTime, Eigen::MatrixX<double>& polySatellitePara, int polynum)
{
	if (satellitetime.cols() != 1) {
		throw exception("the size of satellitetime has error!!");
	}
	if (polySatellitePara.rows() != polynum || polySatellitePara.cols() != 6) {
		throw exception("the size of satellitetime has error!! row: p1,p2,p3,p4 col: x,y,z,vx,vy,vz ");
	}
	// ?????????
	int satellitetime_num = satellitetime.rows();
	satellitetime = satellitetime.array() - SatelliteModelStartTime;
	Eigen::MatrixX<double> satelliteTime(satellitetime_num, polynum);
	for (int i = 0; i < polynum; i++) {
		satelliteTime.col(i) = satellitetime.array().pow(i);
	}

	// ????
	Eigen::MatrixX<double> satellitePoints(satellitetime_num, 6);
	satellitePoints = satelliteTime * polySatellitePara;
	return satellitePoints;
}