#include "SystemCoords.h"
int ConvertSC::calculatePrecessionParam() {
	m_pp.m_ζα = RS * (2306.2181 + (0.30188 + 0.017998 * m_ts) * m_ts) * m_ts;
	m_pp.m_θα = RS * (2004.3109 - (0.42665 + 0.041833 * m_ts) * m_ts) * m_ts;
	m_pp.m_zα = RS * (2306.2181 + (1.09468 + 0.018203 * m_ts) * m_ts) * m_ts;
	return 0;
}

int ConvertSC::calculateMatrixPrecession() {
	Matrix a = Matrix::RotationOY(m_pp.m_θα);
	Matrix b = Matrix::RotationOZ(-m_pp.m_ζα);
	Matrix c = Matrix::RotationOZ(-m_pp.m_zα);
	Matrix R = a * b;
	m_precession = c * R;
	return 0;
}

int ConvertSC::calculateFundamentalArguments() {
	m_fa.λ = m_rg * (218.31643250 + (481267.8812772222 - (0.00161167 - 0.00000528 * m_ts) * m_ts) * m_ts);
	m_fa.l = m_rg * (134.96298139 + (477198.8673980556 + (0.00869722 + 0.00001778 * m_ts) * m_ts) * m_ts);
	m_fa.L = m_rg * (357.52772333 + (35999.05034 - (0.00016028 + 0.00000333 * m_ts) * m_ts) * m_ts);
	m_fa.F = m_rg * (93.27191028 + (483202.0175380555 - (0.00368250 - 0.00000306 * m_ts) * m_ts) * m_ts);
	m_fa.D = m_rg * (297.85036306 + (445267.11148 - (0.00191417 - 0.00000528 * m_ts) * m_ts) * m_ts);
	return 0;
}

int ConvertSC::calculateNutationParameters() {
	Δψ = RS * ((-17.1996 - 0.01742 * m_ts) * sin(m_fa.λ - m_fa.F) + (0.2062 + 0.00002 * m_ts) * sin(2 * m_fa.λ - 2 * m_fa.F) +
		0.0046 * sin(m_fa.λ - 2 * m_fa.l + m_fa.F) + 0.0011 * sin(2 * m_fa.l - 2 * m_fa.F) -
		(1.3187 + 0.00016 * m_ts) * sin(2 * m_fa.λ - 2 * m_fa.D) + (0.1426 - 0.00034 * m_ts) * sin(m_fa.L) -
		(0.0517 - 0.00012 * m_ts) * sin(2 * m_fa.λ + m_fa.L - 2 * m_fa.D) + (0.0217 - 0.00005 * m_ts) *
		sin(2 * m_fa.λ - m_fa.L - 2 * m_fa.D) + (0.0129 + 0.00001 * m_ts) * sin(m_fa.λ + m_fa.F - 2 * m_fa.D) +
		0.0048 * sin(2 * m_fa.l - 2 * m_fa.D) - 0.0022 * sin(2 * m_fa.F - 2 * m_fa.D) + (0.0017 - 0.00001 * m_ts) * sin(2 * m_fa.L) -
		(0.0016 - 0.00001 * m_ts) * sin(2 * m_fa.λ + 2 * m_fa.L - 2 * m_fa.D) - 0.0015 * sin(m_fa.λ + m_fa.L - m_fa.F) -
		0.0012 * sin(m_fa.λ - m_fa.L - m_fa.F));

	Δψ += RS * ((-0.2274 - 0.00002 * m_ts) * sin(2 * m_fa.λ) + (0.0712 + 0.00001 * m_ts) * sin(m_fa.l) -
		(0.0386 + 0.00004 * m_ts) * sin(m_fa.λ + m_fa.F) - 0.0301 * sin(2 * m_fa.λ + m_fa.l) - 0.0158 * sin(m_fa.l - 2 * m_fa.D) +
		0.0123 * sin(2 * m_fa.λ - m_fa.l) + 0.0063 * sin(2 * m_fa.D) + (0.0063 + 0.00001 * m_ts) * sin(m_fa.λ + m_fa.l - m_fa.F) -
		(0.0058 + 0.00001 * m_ts) * sin(m_fa.λ - m_fa.l - m_fa.F) - 0.0059 * sin(2 * m_fa.λ - m_fa.l + 2 * m_fa.D) -
		0.0051 * sin(m_fa.λ + m_fa.l + m_fa.F) - 0.0038 * sin(2 * m_fa.λ + 2 * m_fa.D) + 0.0029 * sin(2 * m_fa.l) +
		0.0029 * sin(2 * m_fa.λ + m_fa.l - 2 * m_fa.D) - 0.0031 * sin(2 * m_fa.λ + 2 * m_fa.l) + 0.0026 * sin(2 * m_fa.F) +
		0.0021 * sin(m_fa.λ - m_fa.l + m_fa.F) + 0.0016 * sin(m_fa.λ - m_fa.l - m_fa.F + 2 * m_fa.D) -
		0.0013 * sin(m_fa.λ + m_fa.l - m_fa.F - 2 * m_fa.D) - 0.0010 * sin(m_fa.λ - m_fa.l + m_fa.F + 2 * m_fa.D));
	Δε = RS * ((9.2025 + 0.00089 * m_ts) * cos(m_fa.λ - m_fa.F) - (0.0895 - 0.00005 * m_ts) * cos(2 * m_fa.λ - 2 * m_fa.F) -
		0.0024 * cos(m_fa.λ - 2 * m_fa.l + m_fa.F) + (0.5736 - 0.00031 * m_ts) * cos(2 * m_fa.λ - 2 * m_fa.D) +
		(0.0054 - 0.00001 * m_ts) * cos(m_fa.L) + (0.0224 - 0.00006 * m_ts) * cos(2 * m_fa.λ + m_fa.L - 2 * m_fa.D) -
		(0.0095 - 0.00003 * m_ts) * cos(2 * m_fa.λ - m_fa.L - 2 * m_fa.D) - 0.0070 * cos(m_fa.λ + m_fa.F - 2 * m_fa.D));
	Δε += RS * ((0.0977 - 0.00005 * m_ts) * cos(2 * m_fa.λ) + 0.0200 * cos(m_fa.λ + m_fa.F) + (0.0129 - 0.00001 * m_ts) *
		cos(2 * m_fa.λ + 2 * m_fa.l) - 0.0053 * cos(2 * m_fa.λ - m_fa.l) - 0.0033 * cos(m_fa.λ + m_fa.l - m_fa.F) +
		0.0032 * cos(m_fa.λ - m_fa.l - m_fa.F) + 0.0026 * cos(2 * m_fa.λ - m_fa.l + 2 * m_fa.D) + 0.0027 * cos(m_fa.λ + m_fa.l + m_fa.F) +
		0.0016 * cos(2 * m_fa.λ + 2 * m_fa.D) - 0.0012 * cos(2 * m_fa.λ + m_fa.l - 2 * m_fa.D) + 0.0013 * cos(2 * m_fa.λ + 2 * m_fa.l) -
		0.0010 * cos(m_fa.λ - m_fa.l + m_fa.F));
	return 0;
};

int ConvertSC::calculateMatrixNutation() {
	Matrix a = Matrix::RotationOZ(-Δψ);
	Matrix b = Matrix::RotationOX(m_ε);
	Matrix c = Matrix::RotationOX(-m_ε - Δε);
	Matrix buff = a * b;
	m_nutation = c * buff;
	return 0;
}

int ConvertSC::rewrite() {
	calculateTime();
	calculatePrecessionParam();
	calculateMatrixPrecession();
	calculateAngleInclination();
	calculateFundamentalArguments();
	calculateNutationParameters();
	calculateMatrixNutation();
	return 0;
}

Matrix Earth_rot_matrix(double GTST) {
	return Matrix::RotationOZ(GTST);
}

std::ostream& operator<<(std::ostream& out, precession_parameters& input_pp)
{
	out << "-------------------------------------------------------------"
		<< "\nPrecession parameters\n" << "ζα = " << input_pp.m_ζα
		<< "\nθα = " << input_pp.m_θα << "\nm_zα = " << input_pp.m_zα
		<< "\n-------------------------------------------------------------";
	return out;
}

std::ofstream& operator<<(std::ofstream& out, precession_parameters& input_pp)
{
	out << input_pp;
	return out;
}

std::ostream& operator<<(std::ostream& out, fundamental_arguments& input_fa)
{
	out << "\n-------------------------------------------------------------"
		<< "\nfundamental arguments"
		<< "\nλ = " << input_fa.λ << "\nl = " << input_fa.l
		<< "\nL = " << input_fa.L << "\nF = " << input_fa.F
		<< "\nD = " << input_fa.D;
	return out;
}

std::ofstream& operator<<(std::ofstream& out, fundamental_arguments& input_fa)
{
	out << input_fa;
	return out;
}

std::ostream& operator<<(std::ostream& out, ConvertSC& input_CSC)
{
	out << "\n*************************************************************"
		<< "\nFields of the ConvertCS class"
		<< "\nm_ε = " << input_CSC.m_ε << "\nΔψ = " << input_CSC.Δψ
		<< "\nm_Δε = " << input_CSC.Δε << "\nm_t = " << input_CSC.m_t
		<< "\nm_ts = " << input_CSC.m_ts << "\nprecession parameters = " << input_CSC.m_pp
		<< "\precession matrix = " << input_CSC.m_precession << "\nutation matrix = " << input_CSC.m_nutation
		<< "\fundamental arguments = " << input_CSC.m_fa;
	return out;
}

std::ofstream& operator<<(std::ofstream& out, ConvertSC& input_CSC)
{
	out << input_CSC;
	return out;
}

Matrix AEC_to_CC(Matrix& Precession) {
	return (Precession.Transpose());
}

Matrix CC_to_TEC(Matrix& Precession, Matrix& Nutation)
{
	return (Nutation * Precession);
}

Matrix TEC_to_CC(Matrix& TEC)
{
	return TEC.Transpose();
}

Matrix CC_to_TC(Matrix& Precession, Matrix& Nutation, Matrix& ERM)
{
	Matrix buf = CC_to_TEC(Precession, Nutation);
	return (ERM * buf);
}

Matrix TC_to_CC(Matrix& CC_to_TC)
{
	return CC_to_TC.Transpose();
}

Matrix SC_to_DC(Matrix& GEC) {
	return(Matrix{ 3, 1, {GEC(2,0) * cos(GEC(1, 0)) * cos(GEC(0,0)),
						  GEC(2,0) * cos(GEC(1, 0)) * sin(GEC(0,0)),
						  GEC(2,0) * sin(GEC(1, 0)) } });
};