#include "Satellite.h"

double calc_i(const double c, const double c1, const double c2, const double c3) {
	if (c3 >= 0) return acos(c3 / c);
	else if (c3 == 0 && (pow(c1, 2) + pow(c2, 2) != 0)) return (M_PI_2);
	else if (c3 < 0) return (M_PI - acos(c3 / c));
	else return -2;
}

double calc_Ω(const double c1, const double c2) {
	if (c1 >= 0 && c2 <= 0) return atan(c1 / c2);
	else if ((c1 >= 0) && (c2 > 0)) return (M_PI - atan(c1 / c2));
	else if ((c1 < 0) && (c2 >= 0)) return (M_PI + atan(c1 / c2));
	else if ((c1 < 0) && (c2 < 0)) return (2 * M_PI - atan(c1 / c2));
	return -2;
}

double calc_ω(const double i, const double Ω, const double l, const double l1, const double l2, const double l3) {
	if (cos(i) != 0)
		return (atan2(sin((-l1 * sin(Ω) + l2 * cos(Ω)) / (l * cos(i))), cos((l1 * cos(Ω) + l2 * sin(Ω)) / (l))));
	else
		return atan2(sin(l3 / l), cos(l2 / (l * sin(Ω))));
}

double calc_u(const double x, const double y, const double z, const double i, const double Ω, const double r) {
	return (atan2((z) / (r * sin(i)), ((x / r) * cos(Ω) + y / r * sin(Ω))));
}

double calc_N(const double u, const double ω) {
	return (atan2((sin(u) * cos(ω) - cos(u) * sin(ω)), (cos(u) * cos(ω) + sin(u) * sin(ω))));
}

int calc_component_V(double p, double e, double N, double& Vr, double& Vτ, const double& gpE) {
	Vr = sqrt(gpE / p) * e * sin(N);
	Vτ = sqrt(gpE / p) * (1 + e * cos(N));
	return 0;
}

double calc_θ(const double Vr, const double Vτ, const double V) {
	return (atan2(Vr / V, Vτ / V));
}

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_Ek = buffer_Ek1;
	}
	return buffer_Ek1;
}

int calculeteKeplerToCoord(Matrix& DCS, const double r, const double u, const double Ω, const double i) {
	DCS(0, 0) = r * (cos(u) * cos(Ω) - sin(u) * sin(Ω) * cos(i));
	DCS(1, 0) = r * (cos(u) * sin(Ω) + sin(u) * cos(Ω) * cos(i));
	DCS(2, 0) = r * sin(u) * sin(i);
	return 1;
}

int calculateKeplerToSpeed(Matrix& DVS, const Matrix& DCS, const double r, const double u, const double Ω, const double i, const double V_r, const double V_τ) {

	DVS(0, 0) = DCS(0, 0) / r * V_r + (-sin(u) * cos(Ω) - cos(u) * sin(Ω) * cos(i)) * V_τ;
	DVS(1, 0) = DCS(1, 0) / r * V_r + (-sin(u) * sin(Ω) - cos(u) * cos(Ω) * cos(i)) * V_τ;
	DVS(2, 0) = DCS(2, 0) / r * V_r + cos(u) * sin(i) * V_τ;
	return 0;
}

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)
{
	Matrix o = m_moon->calculateFm(i_DCS, step_time) + m_sun->calculateFs(i_DCS, step_time);
	Matrix t = m_sun->calculatePs(i_DCS, m_ks, m_As, m_ms, step_time) + m_earth->recalculateAccelerationCC(step_time, i_DCS);
	Matrix th = t + recalculate_F(i_DVS);
	Matrix f = o + th;
	return f;
}

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::convertStateVectorToDCe()
{
	m_DCS(0, 0) = m_eeo.r * (cos(m_eeo.u) * cos(m_eeo.Ω) - sin(m_eeo.u) * sin(m_eeo.Ω)*cos(m_eeo.i));
	m_DCS(1, 0) = m_eeo.r * (cos(m_eeo.u) * sin(m_eeo.Ω) + sin(m_eeo.u) * cos(m_eeo.Ω) * cos(m_eeo.i));
	m_DCS(2, 0) = m_eeo.r * sin(m_eeo.u) * sin(m_eeo.i);

	m_DVS(0, 0) = (m_DCS(0,0) / m_eeo.r) * m_eeo.v * sin(m_eeo.θ) +
		(-sin(m_eeo.u) * cos(m_eeo.Ω) - cos(m_eeo.u) * sin(m_eeo.Ω) * cos(m_eeo.i)) * m_eeo.v * cos(m_eeo.θ);
	m_DVS(1, 0) = (m_DCS(1, 0) / m_eeo.r) * m_eeo.v * sin(m_eeo.θ) +
		(-sin(m_eeo.u) * sin(m_eeo.Ω) - cos(m_eeo.u) * cos(m_eeo.Ω) * cos(m_eeo.i)) * m_eeo.v * cos(m_eeo.θ);
	m_DVS(2, 0) = (m_DCS(2, 0) / m_eeo.r) * m_eeo.v * sin(m_eeo.θ) + cos(m_eeo.u) * sin(m_eeo.i) * m_eeo.v * cos(m_eeo.θ);
	return 0;
}

int Satellite::convertStateVectorToDCm() {
	m_DCS(0, 0) = m_meo.r * (cos(m_meo.u) * sin(m_meo.Ω) + sin(m_meo.u) * cos(m_meo.Ω) * cos(m_meo.i));
	m_DCS(1, 0) = m_meo.r * sin(m_meo.u) * sin(m_meo.i);
	m_DCS(2, 0) = m_meo.r * (cos(m_meo.u) * cos(m_meo.Ω) - sin(m_meo.u) * sin(m_meo.Ω) * cos(m_meo.i));

	m_DVS(0, 0) = (m_DCS(0, 0) / m_meo.r) * m_meo.v * sin(m_meo.θ) +
		(-sin(m_meo.u) * sin(m_meo.Ω) + cos(m_meo.u) * cos(m_meo.Ω) * cos(m_meo.i)) * m_meo.v * cos(m_meo.θ);
	m_DVS(1, 0) = (m_DCS(1, 0) / m_meo.r) * m_meo.v * sin(m_meo.θ) + cos(m_meo.u) * sin(m_meo.i) * m_meo.v * cos(m_meo.θ);
				
	m_DVS(2, 0) = (m_DCS(2, 0) / m_meo.r) * m_meo.v * sin(m_meo.θ) +
		(-sin(m_meo.u) * cos(m_meo.Ω) - cos(m_meo.u) * sin(m_meo.Ω) * cos(m_meo.i)) * m_meo.v * cos(m_meo.θ);
	return 0;
}

int Satellite::convertDCeTOStateVecror()
{
	double Vr; //радиальная скорость
	double Vτ; //трансверсальная скорость
	double c_1 = m_DCS(1, 0) * m_DVS(2, 0) - m_DCS(2, 0) * m_DVS(1, 0);
	double c_2 = m_DCS(2, 0) * m_DVS(0, 0) - m_DCS(0, 0) * m_DVS(2, 0);
	double c_3 = m_DCS(0, 0) * m_DVS(1, 0) - m_DCS(1, 0) * m_DVS(0, 0);
	double c = sqrt(pow(c_1, 2) + pow(c_2, 2) + pow(c_3, 2));

	calculate_r();
	calculate_V();
	m_eeo.r = m_r;
	m_eeo.v = m_v;
	
	double l_1 = -m_earth->gravitational_parameter_Earth * (m_DCS(0, 0) / m_eeo.r) + m_DVS(1, 0) * c_3 - m_DVS(2, 0) * c_2;
	double l_2 = -m_earth->gravitational_parameter_Earth * (m_DCS(1, 0) / m_eeo.r) + m_DVS(2, 0) * c_1 - m_DVS(0, 0) * c_3;
	double l_3 = -m_earth->gravitational_parameter_Earth * (m_DCS(2, 0) / m_eeo.r) + m_DVS(0, 0) * c_2 - m_DVS(1, 0) * c_1;
	double l = sqrt(pow(l_1, 2) + pow(l_2, 2) + pow(l_3, 2));

	double e = l / m_earth->gravitational_parameter_Earth;
	double p = pow(c, 2) / m_earth->gravitational_parameter_Earth;

	m_eeo.i = calc_i(c, c_1, c_2, c_3);
	m_eeo.Ω = calc_Ω(c_1, c_2);
	double ω = calc_ω(m_eeo.i, m_eeo.Ω, l, l_1, l_2, l_3);
	m_eeo.u = calc_u(m_DCS(0, 0), m_DCS(1, 0), m_DCS(2, 0), m_eeo.i, m_eeo.Ω, m_eeo.r);
	double N = calc_N(m_eeo.u, ω);

	calc_component_V(p, e, N, Vr, Vτ, m_earth->gravitational_parameter_Earth);
	m_eeo.θ = calc_θ(Vr, Vτ, m_eeo.v);
	return 0;
}

int Satellite::convertDCmTOSteteVector()
{
	Matrix buff_DCS{ m_DCS };
	Matrix buff_DVS{ m_DVS };

	m_DCS(0, 0) = buff_DCS(2, 0);
	m_DCS(1, 0) = buff_DCS(0, 0);
	m_DCS(2, 0) = buff_DCS(1, 0);

	m_DVS(0, 0) = buff_DVS(2, 0);
	m_DVS(1, 0) = buff_DVS(0, 0);
	m_DVS(2, 0) = buff_DVS(1, 0);

	convertDCeTOStateVecror();

	m_DCS = buff_DCS;
	m_DVS = buff_DVS;

	return 0;
}

int Satellite::convertKeplertoDC()
{
	double p{m_Keo.a*(1 - pow(m_Keo.e, 2))};
	double Vr; //радиальная скорость (км/с)
	double Vτ; //тангенциальная скорость (км/с)
	m_Keo.E = calculate_E(m_Keo.convertGradToRad(m_Keo.M), m_Keo.e);
	m_Keo.N = 2 * atan(sqrt((1 + m_Keo.e) / (m_Keo.e)) * tan(m_Keo.E / 2));
	m_Keo.u = m_Keo.N + m_Keo.ω;
	
	m_eeo.r = m_Keo.a*(1-m_Keo.e*cos(m_Keo.E));
	m_eeo.u = m_Keo.u;
	m_eeo.i = m_Keo.i;
	m_eeo.Ω = m_Keo.Ω;

	Vr = sqrt(m_earth->gravitational_parameter_Earth / p) * m_Keo.e * sin(m_Keo.N);
	Vτ = sqrt(m_earth->gravitational_parameter_Earth / p) * (1 + m_Keo.e * cos(m_Keo.N));
	calculeteKeplerToCoord(m_DCS, m_eeo.r, m_Keo.convertGradToRad(m_Keo.u), m_Keo.convertGradToRad(m_Keo.Ω), m_Keo.convertGradToRad(m_Keo.i));
	calculateKeplerToSpeed(m_DVS, m_DCS, m_eeo.r, m_Keo.convertGradToRad(m_Keo.u), m_Keo.convertGradToRad(m_Keo.Ω),
		m_Keo.convertGradToRad(m_Keo.i), Vr, Vτ);
	return 0;
}

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

Satellite::Satellite(double time_TDB, double i_As, double i_ms, double i_ks,
	Matrix& i_DCS, Matrix& i_DVS, Moon* ptr_moon, Sun* ptr_sun, Earth* ptr_earth) : m_As{ i_As }, m_ms{ i_ms }, m_ks{ i_ks },
	m_start_time{ time_TDB }, last_moment_integration{ time_TDB }, m_DCS{ i_DCS }, m_DVS{ i_DVS },
	m_moon{ ptr_moon }, m_sun{ ptr_sun }, m_earth{ ptr_earth }
{
	initialization_A();
};

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{m_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 - last_moment_integration) * 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 });
		α.push_back(Matrix{ 3, 1 });
	}
	std::cout << "20 = " << buff_DVS(2, 0) << std::endl;
	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.at(i) = (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;
	m_A = A;

	last_moment_integration = i_time;
	std::cout << "buff_DCS = " << buff_DCS << " buff_DVS = " << buff_DVS << std::endl;
	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);
}