#include "Satellite.h"

int Satellite::satellite_number = 0;
const std::vector<double> Satellite::coef_time_step { 0.000000000000000000, 0.056262560526922147, //коэффиценты для разбития 
							     					  0.180240691736892365, 0.352624717113169637,  // шага на подинтервалы
                                                      0.547153626330555383, 0.734210177215410532,
										              0.885320946839095768, 0.977520613561287501, 1 };

double calculate_E(const double M, const double e) {
	double buffer_Ek{ M };
	double buffer_Ek1{ 0 };
	for (short i = 0; i < 4; i++) {
		//buffer_Ek1 = M + e * sin(buffer_Ek);
		buffer_Ek1 = buffer_Ek - ((buffer_Ek - e*sin(buffer_Ek) - M) / (1 - e * cos(buffer_Ek)));
		buffer_Ek = buffer_Ek1;
	}
	return buffer_Ek1;
}

int Satellite::calculate_r(const Matrix& DCS) {

	m_r = sqrt(pow(DCS(0, 0), 2) + pow(DCS(1, 0), 2) + pow(DCS(2, 0), 2));
	return 0;
}

int Satellite::calculate_density(const Matrix& DCS)
{
	calculate_r(DCS);
	m_h = m_r - Radius_Earth * (1 - compression_earth*pow((DCS(2,0)/m_r), 2));
	m_density = 2E-13 * exp((-m_h + 200) / 60);
	return 0;
}

int Satellite::calculate_V(const Matrix& DVS) {

	m_v = sqrt(pow(DVS(0, 0), 2) + pow(DVS(1, 0), 2) + pow(DVS(2, 0), 2));
	return 0;
}

Matrix& Satellite::calculate_F(const Matrix& i_DCS, const Matrix& i_DVS) {
	calculate_V(i_DVS);
	calculate_density(i_DCS);
	double Sb = m_param.cross_sectional_radius / m_param.mass;
	m_F = -Sb * m_density * m_v * i_DVS;
	return m_F;
}

void Satellite::step_calculate_F(const Matrix& i_DCS, const Matrix& i_DVS, std::vector<Matrix>& F, const double step_time, const short step_integration)
{
	F.at(step_integration) = m_moon->calculateFm(i_DCS, step_time) + m_sun->calculateFs(i_DCS, step_time) +
		m_sun->calculatePs(i_DCS, m_param, step_time) + m_earth->recalculateAccelerationCC(step_time, i_DCS) +
		calculate_F(i_DCS, i_DVS);
	//F.at(step_integration) = i_DCS + step_time ;
}



void calc_α(std::vector<Matrix>& α,const std::vector<Matrix>& F,  const std::vector<double>& step_time, int current_moment) {
	switch (current_moment)
	{
	case 1:
		α[0] = (F[1] - F[0]) / step_time[1];
		break;
	case 2:
		α[1] = (((F[2] - F[0]) / step_time[2]) - α[0]) / (step_time[2] - step_time[1]);
		break;
	case 3:
		α[2] = ((((F[3] - F[0]) / step_time[3]) - α[0]) / 
			(step_time[3] - step_time[1]) - α[1]) /
			(step_time[3] - step_time[2]);
		break;
	case 4:
		α[3] = ((((F[4] - F[0]) / step_time[4] - α[0]) / 
			(step_time[4] - step_time[1]) - α[1]) / 
			(step_time[4] - step_time[2]) - α[2]) / 
			(step_time[4] - step_time[3]);
		break;
	case 5:
		α[4] = (((((F[5] - F[0]) / (step_time[5]) - α[0]) / 
			(step_time[5] - step_time[1]) - α[1]) / 
			(step_time[5] - step_time[2]) - α[2]) / 
			(step_time[5] - step_time[3]) - α[3]) / 
			(step_time[5] - step_time[4]);
		break;
	case 6:
		α[5] = ((((((F[6] - F[0]) / step_time[6] - α[0]) / 
			(step_time[6] - step_time[1]) - α[1]) / 
			(step_time[6] - step_time[2]) - α[2]) / 
			(step_time[6] - step_time[3]) - α[3]) / 
			(step_time[6] - step_time[4]) - α[4]) / 
			(step_time[6] - step_time[5]);
		break;
	case 7:
		α[6] = (((((((F[7] - F[0]) / step_time[7] - α[0]) / 
			(step_time[7] - step_time[1]) - α[1]) / 
			(step_time[7] - step_time[2]) - α[2]) / 
			(step_time[7] - step_time[3]) - α[3]) / 
			(step_time[7] - step_time[4]) - α[4]) / 
			(step_time[7] - step_time[5]) - α[5]) / 
			(step_time[7] - step_time[6]);
		break;
	}
}

int Satellite::initialization_A()
{
	for (short i = 0; i < 9; i++)
		m_A.push_back(Matrix{ 3, 1 });
	return 0;
}

Satellite::Satellite(double time_TDB, satellite_parameters& i_param, Matrix& i_DCS, Matrix& i_DVS, 
	Moon* ptr_moon, Sun* ptr_sun, Earth* ptr_earth): 
	m_start_time{ time_TDB }, 
	last_moment_integration{ time_TDB }, 
	m_param{ i_param }, 
	m_DCS{ i_DCS }, 
	m_DVS{ i_DVS }, 
	m_moon{ ptr_moon }, 
	m_sun{ ptr_sun }, 
	m_earth{ ptr_earth }, 
	m_Id{ satellite_number++ }, 
	m_Keo{ 0.0,0.0,0.0,0.0,0.0,0.0 }
{
	satellite_number++;
	initialization_A();
	calculate_r(m_DCS);
	calculate_density(m_DCS);
	
	calculate_V(m_DVS);
}
Satellite::Satellite(double time_MJD, satellite_parameters& i_param, Kepler_elements_orbit& i_Keo, 
	Moon* ptr_moon, Sun* ptr_sun, Earth* ptr_earth): 
	m_param{ i_param }, 
	m_start_time{ time_MJD }, 
	last_moment_integration{ time_MJD }, 
	m_Keo{i_Keo}, 
	m_moon{ ptr_moon },
	m_sun{ ptr_sun }, 
	m_earth{ ptr_earth },
	m_Id{ satellite_number++ }
{
	m_Keo.convertKeplerToDC(m_DCS, m_DVS, m_earth->gravitational_parameter_Earth);
	//std::cout << std::setprecision(20)<< m_Keo << "\nStart DCS: " << m_DCS << "Start DVS: " << m_DVS;
	m_Keo.convertDCtoKepler(m_DCS, m_DVS, m_earth->gravitational_parameter_Earth);
	//std::cout << m_Keo;
	m_r = m_Keo.get_r();
	m_v = m_Keo.get_V();
	initialization_A();
}

int Satellite::integration_step(double end_time_integration, double step){
	std::vector<double> step_time{};
	std::vector<double> global_step_time{};
	std::vector<Matrix> F{}; //массив хранящий ускорения по подшагам
	std::vector<Matrix> A{}; //массив хранящий коэффициенты полинома
	std::vector<Matrix> α{m_A}; //массив хранящий параметры коэффициентов полинома
	Matrix buff_DCS{};
	Matrix buff_DVS{};
	Matrix coef_c{ coef_time_step.size(),coef_time_step.size() };
	for (short i = 0; i < coef_time_step.size(); i++) {
		step_time.push_back(step * coef_time_step.at(i));
		global_step_time.push_back((end_time_integration - last_moment_integration) * coef_time_step.at(i) + last_moment_integration);
	}
	coef_c(0, 0) = 1;
	for (short i = 0; i < coef_time_step.size(); i++) {
		for (short j = 1; j < coef_time_step.size(); j++) {
			if (i == j)
				coef_c(i, j) = 1;
			else if (i == 0)
				coef_c(j, i) = -step_time.at(j) * coef_c(j - 1, 0);
			else
				coef_c(i, j) = coef_c(i-1,j-1) - step_time.at(i) * coef_c(i - 1, j);
		}
	}
	
	for (int i = 0; i < coef_time_step.size(); i++) {
		F.push_back(Matrix{ 3, 1 });
		A.push_back(Matrix{ 3, 1 });
	}
	for (short i = 0; i < 9; i++) {
		buff_DCS = m_DCS + step_time.at(i) * m_DVS +
			(1.0 / 2.0) * F.at(0) * pow(step_time.at(i), 2) +
			(1.0 / 6.0) * A.at(0) * pow(step_time.at(i), 3) +
			(1.0 / 12.0) * A.at(1) * pow(step_time.at(i), 4) +
			(1.0 / 20.0) * A.at(2) * pow(step_time.at(i), 5) +
			(1.0 / 30.0) * A.at(3) * pow(step_time.at(i), 6) +
			(1.0 / 42.0) * A.at(4) * pow(step_time.at(i), 7) +
			(1.0 / 56.0) * A.at(5) * pow(step_time.at(i), 8) +
			(1.0 / 72.0) * A.at(6) * pow(step_time.at(i), 9);
		buff_DVS = m_DVS + F.at(0) * step_time.at(i) +
			(1.0 / 2.0) * A.at(0) * pow(step_time.at(i), 2) +
			(1.0 / 3.0) * A.at(1) * pow(step_time.at(i), 3) +
			(1.0 / 4.0) * A.at(2) * pow(step_time.at(i), 4) +
			(1.0 / 5.0) * A.at(3) * pow(step_time.at(i), 5) +
			(1.0 / 6.0) * A.at(4) * pow(step_time.at(i), 5) +
			(1.0 / 7.0) * A.at(5) * pow(step_time.at(i), 7) +
			(1.0 / 8.0) * A.at(6) * pow(step_time.at(i), 8);
		step_calculate_F(buff_DCS, buff_DVS, F, global_step_time.at(i), i);
		calc_α(α, F, step_time, i);
		
		A.at(0) = α.at(0) + 
			coef_c(1, 0) * α.at(1) + 
			coef_c(2, 0) * α.at(2) + 
			coef_c(3, 0) * α.at(3) + 
			coef_c(4, 0) * α.at(4) + 
			coef_c(5, 0) * α.at(5) + 
			coef_c(6, 0) * α.at(6);
		A.at(1) = α.at(1) + 
			coef_c(2, 1) * α.at(2) + 
			coef_c(3, 1) * α.at(3) + 
			coef_c(4, 1) * α.at(4) + 
			coef_c(5, 1) * α.at(5) + 
			coef_c(6, 1) * α.at(6);
		A.at(2) = α.at(2) + 
			coef_c(3, 2) * α.at(3) + 
			coef_c(4, 2) * α.at(4) + 
			coef_c(5, 2) * α.at(5) + 
			coef_c(6, 2) * α.at(6);
		A.at(3) = α.at(3) + 
			coef_c(4, 3) * α.at(4) + 
			coef_c(5, 3) * α.at(5) + 
			coef_c(6, 3) * α.at(6);
		A.at(4) = α.at(4) + 
			coef_c(5, 4) * α.at(5) + 
			coef_c(6, 4) * α.at(6);
		A.at(5) = α.at(5) + coef_c(6, 5) * α.at(6);
		A.at(6) = α.at(6);
	}

	m_DCS = buff_DCS;
	m_DVS = buff_DVS;
	m_A = α;
	last_moment_integration = end_time_integration;

	m_Keo.convertDCtoKepler(m_DCS, m_DVS, m_earth->gravitational_parameter_Earth);
	return 0;
}

double Kepler_elements_orbit::convertGradToRad(double grad)
{
	return grad * (M_PI / 180);
}

double Kepler_elements_orbit::convertRadToGrad(double rad)
{
	return rad * (180 / M_PI);
}

int Kepler_elements_orbit::calculateConstantVectors(const Matrix& DCS, const Matrix& DVS, const double gp)
{
	constantVectorAreas.at(1) = DCS(1, 0) * DVS(2, 0) - DCS(2, 0) * DVS(1, 0);
	constantVectorAreas.at(2) = DCS(2, 0) * DVS(0, 0) - DCS(0, 0) * DVS(2, 0);
	constantVectorAreas.at(3) = DCS(0, 0) * DVS(1, 0) - DCS(1, 0) * DVS(0, 0);
	constantVectorAreas.at(0) = sqrt(constantVectorAreas.at(1) * constantVectorAreas.at(1) +
		constantVectorAreas.at(2) * constantVectorAreas.at(2) +
		constantVectorAreas.at(3) * constantVectorAreas.at(3));
	constantVectorLaplace.at(1) = -gp * (DCS(0, 0) / r) + constantVectorAreas.at(3) * DVS(1, 0) - constantVectorAreas.at(2) * DVS(2, 0);
	constantVectorLaplace.at(2) = -gp * (DCS(1, 0) / r) + constantVectorAreas.at(1) * DVS(2, 0) - constantVectorAreas.at(3) * DVS(0, 0);
	constantVectorLaplace.at(3) = -gp * (DCS(2, 0) / r) + constantVectorAreas.at(2) * DVS(0, 0) - constantVectorAreas.at(1) * DVS(1, 0);
	constantVectorLaplace.at(0) = sqrt(constantVectorLaplace.at(1) * constantVectorLaplace.at(1) +
		constantVectorLaplace.at(2) * constantVectorLaplace.at(2) +
		constantVectorLaplace.at(3) * constantVectorLaplace.at(3));
	return 0;
}

int Kepler_elements_orbit::calculateAccessoryParameter(const double gp)
{
	p = a * (1 - e*e); //вычисление фокального парметра
	E = calculate_E(convertGradToRad(M), e); // вычисление эксцентрической аномалии
	r = a * (1 - e * cos(E)); // вычисление радиус вектора до спутника

	double sin_N = (sqrt(1-e*e)*sin(E)) / (1-e*cos(E));
	double cos_N = (cos(E) - e) / (1 - e * cos(E));
	N = atan2(sin_N, cos_N);
	dN = sqrt(p * gp)/(r*r); // Вычисление первой производной от истинной аномалии
	T = 2 * M_PI * sqrt(pow(a, 3) / gp);
	double sin_u = sin_N * cos(convertGradToRad(ω)) + cos_N * sin(convertGradToRad(ω));
	double cos_u = cos_N * cos(convertGradToRad(ω)) - sin_N * sin(convertGradToRad(ω));
	u = atan2(sin_u, cos_u);
	

	velocity.at(0) = sqrt((gp / p) * (1 + e * e + 2 * e * cos_N)); //вычисление модуля векотра скорости
	velocity.at(1) = sqrt(gp / p) * e * sin_N;
	velocity.at(2) = sqrt(gp / p) * (1 + e * cos_N);

	return 0;
}

int Kepler_elements_orbit::calculate_i()
{
	if (constantVectorAreas.at(3) >= 0)
		i = acos(abs(constantVectorAreas.at(3) / constantVectorAreas.at(0)));
	else
		i = M_PI - acos(abs(constantVectorAreas.at(3) / constantVectorAreas.at(0)));
	return 0;
}

int Kepler_elements_orbit::calculate_Ω()
{
	if ((constantVectorAreas.at(1) >= 0) && (constantVectorAreas.at(2) <= 0))
		Ω = atan(abs(constantVectorAreas.at(1) / constantVectorAreas.at(2)));
	else if ((constantVectorAreas.at(1) >= 0) && (constantVectorAreas.at(2) > 0))
		Ω = M_PI - atan(abs(constantVectorAreas.at(1) / constantVectorAreas.at(2)));
	else if ((constantVectorAreas.at(1) < 0) && (constantVectorAreas.at(2) >= 0))
		Ω = M_PI + atan(abs(constantVectorAreas.at(1) / constantVectorAreas.at(2)));
	else 
		Ω = 2*M_PI + atan(abs(constantVectorAreas.at(1) / constantVectorAreas.at(2)));
	return 0;
}

int Kepler_elements_orbit::calculate_ω()
{
	double buff_sin, buff_cos;
	if (cos(i) != 0) {
		buff_sin = (-constantVectorLaplace.at(1) * sin(Ω) + constantVectorLaplace.at(2) * cos(Ω)) /
			(constantVectorLaplace.at(0) * cos(i));
		buff_cos = (constantVectorLaplace.at(1) * cos(Ω) + constantVectorLaplace.at(2) * sin(Ω)) /
			constantVectorLaplace.at(0);
	}
	else {
		buff_cos = constantVectorLaplace.at(2) / (constantVectorLaplace.at(0) * sin(Ω));
		buff_sin = constantVectorLaplace.at(3) / constantVectorLaplace.at(0);
	}
	ω = atan2(buff_sin, buff_cos);
	return 0;
}

int Kepler_elements_orbit::calculate_u(const Matrix& DCS)
{
	double buff_sin, buff_cos;
	buff_sin = DCS(2,0) / (r * sin(i));
	buff_cos = DCS(0, 0) / r * cos(Ω) + DCS(1, 0) / r * sin(Ω);
	u = atan2(buff_sin, buff_cos);
	return 0;
}

int Kepler_elements_orbit::calculate_N()
{
	double buff_sin, buff_cos;
	buff_sin = sin(u) * cos(ω) - cos(u) * sin(ω);
	buff_cos = cos(u) * cos(ω) + sin(u) * sin(ω);
	N = atan2(buff_sin, buff_cos);
	return 0;
}

int Kepler_elements_orbit::RevereseCalculateE()
{
	double buff_sin, buff_cos;
	buff_sin = (sqrt(1-e*e)*sin(N)) / (1+e*cos(N));
	buff_cos = (cos(N) + e) / (1 + e*cos(N));
	E = N + atan2((buff_sin*cos(N) - buff_cos*sin(N)),buff_cos*cos(N) + buff_sin*sin(N));
	return 0;
}

int Kepler_elements_orbit::ReverseCalculateM()
{
	M = E - e * sin(E);
	return 0;
}

int Kepler_elements_orbit::convertKeplerToDC(Matrix& DCS, Matrix& DVS, const double gp)
{
	calculateAccessoryParameter(gp);
	DCS(0, 0) = r * (cos(u) * cos(convertGradToRad(Ω)) - sin(u) * sin(convertGradToRad(Ω)) * cos(convertGradToRad(i)));
	DCS(1, 0) = r * (cos(u) * sin(convertGradToRad(Ω)) + sin(u) * cos(convertGradToRad(Ω)) * cos(convertGradToRad(i)));
	DCS(2, 0) = r * sin(u) * sin(convertGradToRad(i));

	DVS(0, 0) = velocity.at(1) * (DCS(0, 0) / r) - (sin(u) * cos(convertGradToRad(Ω)) 
		+ cos(u) * sin(convertGradToRad(Ω)) * cos(convertGradToRad(i))) * velocity.at(2);
	DVS(1, 0) = velocity.at(1) * (DCS(1, 0) / r) - (sin(u) * sin(convertGradToRad(Ω))
		- cos(convertGradToRad(Ω)) * cos(u) * cos(convertGradToRad(i))) * velocity.at(2);
	DVS(2, 0) = velocity.at(1) * (DCS(2, 0) / r) + cos(u) * sin(convertGradToRad(i)) * velocity.at(2);
	return 0;
}

int Kepler_elements_orbit::convertDCtoKepler(const Matrix& DCS, const Matrix& DVS, const double gp)
{
	r = sqrt(DCS(0,0) * DCS(0, 0) + DCS(1, 0) * DCS(1, 0) + DCS(2, 0) * DCS(2, 0));
	calculateConstantVectors(DCS, DVS, gp);
	e = (constantVectorLaplace.at(0) / gp);
	p = (constantVectorAreas.at(0) * constantVectorAreas.at(0)) / gp;
	a = p / (1 - e * e);

	calculate_i();
	calculate_Ω();
	calculate_ω();
	calculate_u(DCS);
	calculate_N();
	RevereseCalculateE();
	ReverseCalculateM();
	return 0;
}

int Kepler_elements_orbit::setKeo(double i_a, double i_e, double i_i, double i_Ω, double i_ω, double i_M)
{
	a = i_a;
	e = i_e;
	i = i_i;
	Ω = i_Ω;
	ω = i_ω;
	M = i_M;
	return 0;
}

std::ostream& operator<<(std::ostream& out, const Kepler_elements_orbit& keo)
{
	out << "Semi-major axis = " << keo.a << "\neccentricity = " << keo.e 
		<< "\nthe angle of inclination of the orbit in degrees = " << keo.i 
		<< "\nlongitude of the ascending node in degrees = " << keo.Ω
		<< "\nthe perigee argument in degrees = " << keo.ω 
		<< "\nthe magnitude of the average anomaly in degrees = " << keo.M << "\n";
	return out;
}