#include "Satellite.h"

int Satellite::calculate_r() {

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

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

int Satellite::calculate_V() {

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

Matrix& Satellite::calculate_F(){
	calculate_V();
	double Sb = m_As / m_ms;
	m_F = -Sb * m_density * m_v * m_DVS;
	return m_F;
}

Matrix& Satellite::recalculate_F(const Matrix& i_DVS) {
	double v = sqrt(pow(i_DVS(0, 0), 2) + pow(i_DVS(1, 0), 2) + pow(i_DVS(2, 0), 2));
	double Sb = m_As / m_ms;
	m_F = -Sb * m_density * v * i_DVS;
	return m_F;
}

Matrix Satellite::step_calculate_F(const Matrix& i_DCS, const Matrix& i_DVS, const double step_time)
{
		return (m_moon.calculateFm(i_DCS, step_time) + m_sun.calculateFs(i_DCS, step_time) + m_sun.calculatePs(i_DCS, m_ks, m_As, m_ms, step_time)
		+ m_earth.recalculateAccelerationCC(step_time, i_DCS)
		+ recalculate_F(i_DVS));
}

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

int Satellite::integration_step(double i_time) {
	std::vector<double> step_time{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
	std::vector<Matrix> F{}; //массив хранящий ускорения по подшагам
	std::vector<Matrix> A{}; //массив хранящий коэффициенты полинома
	std::vector<Matrix> α{}; //массив хранящий параметры коэффициентов полинома
	Matrix buff_DCS{ m_DCS };
	Matrix buff_DVS { m_DVS };
	Matrix coef_c{8,8};
	
	for (short i = 0; i < 8; i++)
		step_time.at(i) = (i_time - m_start_time) * coef_time_step.at(i);
	
	coef_c(0, 0) = 1;
	for (short i = 0; i < 8; i++) {
		for (short j = 1; j < 8; 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, 0);
		}
	}
	for (int i = 0; i < 8; i++) {
		F.push_back(Matrix{ 3, 1 });
		A.push_back(Matrix{ 3, 1 });
		α.push_back(Matrix{ 3, 1 });
	}
	
	for (short i = 0; i < 8; i++) {
		buff_DCS = (m_DCS + step_time.at(i) * m_DVS + (1 / 2) * F.at(0) * pow(step_time.at(i), 2)
			+ (1 / 6) * A.at(0) * pow(step_time.at(i), 3) + (1 / 12) * A.at(1) * pow(step_time.at(i), 4)
			+ (1 / 20) * A.at(2) * pow(step_time.at(i), 5) + (1 / 30) * A.at(3) * pow(step_time.at(i), 6)
			+ (1 / 42) * A.at(4) * pow(step_time.at(i), 7));
		buff_DVS = (m_DVS + F.at(0) * step_time.at(i) + (1 / 2) * A.at(0) * pow(step_time.at(i), 2)
			+ (1 / 3) * A.at(1) * pow(step_time.at(i), 3) + (1 / 4) * A.at(2) * pow(step_time.at(i), 4)
			+ (1 / 5) * A.at(3) * pow(step_time.at(i), 5) + (1 / 6) * A.at(4) * pow(step_time.at(i), 5)
			+ (1 / 7) * A.at(5) + pow(step_time.at(i), 7));
		F.push_back(step_calculate_F(buff_DCS, buff_DVS, step_time.at(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(4) + coef_c(6, 4) * α.at(6);
		A.at(5) = α.at(5) + coef_c(6, 5) * α.at(6);
	}
	buff_DCS = (m_DCS + i_time * m_DVS + (1 / 2) * F.at(0) * pow(i_time, 2)
		+ (1 / 6) * A.at(0) * pow(i_time, 3) + (1 / 12) * A.at(1) * pow(i_time, 4)
		+ (1 / 20) * A.at(2) * pow(i_time, 5) + (1 / 30) * A.at(3) * pow(i_time, 6)
		+ (1 / 42) * A.at(4) * pow(i_time, 7));
	buff_DVS = (m_DVS + F.at(0) * i_time + (1 / 2) * A.at(0) * pow(i_time, 2)
		+ (1 / 3) * A.at(1) * pow(i_time, 3) + (1 / 4) * A.at(2) * pow(i_time, 4)
		+ (1 / 5) * A.at(3) * pow(i_time, 5) + (1 / 6) * A.at(4) * pow(i_time, 5)
		+ (1 / 7) * A.at(5) + pow(i_time, 7));
	m_DCS = buff_DCS;
	m_DVS = buff_DVS;

	std::cout << "buff_DCS = " << buff_DCS << " buff_DVS = " << buff_DVS;
	return 0;
}