/***************************************************************************
 * Name: gSatTEME.cpp
 *
 * Description: gSatTEME class implementation.
 *              This class abstract all the SGP4 complexity. It uses the
 *              David. A. Vallado code for SGP4 Calculation.
 *
 * Reference:
 *              Revisiting Spacetrack Report #3 AIAA 2006-6753
 *              Vallado, David A., Paul Crawford, Richard Hujsak, and T.S.
 *              Kelso, "Revisiting Spacetrack Report #3,"
 *              presented at the AIAA/AAS Astrodynamics Specialist
 *              Conference, Keystone, CO, 2006 August 21-24.
 *              https://celestrak.org/publications/AIAA/2006-6753/
 ***************************************************************************/

/***************************************************************************
 *   Copyright (C) 2004 by J.L. Canales                                    *
 *   ph03696@homeserver                                                    *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   51 Franklin Street, Suite 500, Boston, MA  02110-1335, USA.             *
 ***************************************************************************/

// GKepFile
#include "gSatTEME.hpp"
//#include <iostream>
//#include <iomanip>
#include <math.h>

#include "mathUtils.hpp"
#include "sgp4io.h"

#define CONSTANTS_SET wgs84
#define TYPERUN_SET   'c'
#define OPSMODE_SET   'i'
#define TYPEINPUT_SET 'm'

#define LATITUDE  0
#define LONGITUDE 1
#define ALTITUDE  2

// Constructors
gSatTEME::gSatTEME(const char *pstrName, char *pstrTleLine1, char *pstrTleLine2)
{
	double startmfe, stopmfe, deltamin; // dummies

	m_SatName = pstrName;

	//set gravitational constants
	getgravconst(CONSTANTS_SET, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2);

	//Parsing TLE_Files and sat variables setting
	twoline2rv(pstrTleLine1, pstrTleLine2, TYPERUN_SET, TYPEINPUT_SET, OPSMODE_SET, CONSTANTS_SET,
	           startmfe, stopmfe, deltamin, satrec);

	// call the propagator to get the initial state vector value
	sgp4(CONSTANTS_SET, satrec,  0.0, m_Position.v,  m_Vel.v);
}

void gSatTEME::setEpoch(gTime ai_time)
{
	gTime     kepEpoch(satrec.jdsatepoch);
	gTimeSpan tSince = ai_time - kepEpoch;

	double dtsince = tSince.getDblSeconds()/KSEC_PER_MIN;
	// call the propagator to get the initial state vector value
	sgp4(CONSTANTS_SET, satrec,  dtsince, m_Position.v,  m_Vel.v);

	m_SubPoint    = computeSubPoint( ai_time);
}

void gSatTEME::setMinSinceKepEpoch(double ai_minSinceKepEpoch)
{
	gTimeSpan tSince( ai_minSinceKepEpoch/KMIN_PER_DAY);
	gTime     Epoch(satrec.jdsatepoch);
	Epoch += tSince;
	// call the propagator to get the initial state vector value
	sgp4(CONSTANTS_SET, satrec,  ai_minSinceKepEpoch, m_Position.v,  m_Vel.v);

	m_SubPoint    = computeSubPoint( Epoch);
}

Vec3d gSatTEME::computeSubPoint(gTime ai_Time)
{
	Vec3d resultVector; // (0) Latitude, (1) Longitude, (2) altitude

	const double theta = atan2(m_Position[1], m_Position[0]); // radians
	resultVector[ LONGITUDE] = fmod((theta - ai_Time.toThetaGMST()), 2.*M_PI);  //radians


	const double r = std::sqrt(Sqr(m_Position[0]) + Sqr(m_Position[1]));
	const double e2 = __f*(2. - __f);
	resultVector[ LATITUDE] = atan2(m_Position[2],r); /*radians*/

	double phi, c;
	do
	{
		phi = resultVector[ LATITUDE];
		c = 1./std::sqrt(1. - e2*Sqr(sin(phi)));
		resultVector[ LATITUDE] = atan2(m_Position[2] + EARTH_RADIUS*c*e2*sin(phi),r);
	}
	while(fabs(resultVector[ LATITUDE] - phi) >= 1E-10);

	resultVector[ ALTITUDE] = r/cos(resultVector[ LATITUDE]) - EARTH_RADIUS*c;/*kilometers*/

	if(resultVector[ LATITUDE] > (M_PI/2.0)) resultVector[ LATITUDE] -= 2.*M_PI;

	resultVector[LATITUDE]  *= M_180_PI;
	resultVector[LONGITUDE] *= M_180_PI;
	if(resultVector[LONGITUDE] < -180.0) resultVector[LONGITUDE] += 360;
	else if(resultVector[LONGITUDE] > 180.0) resultVector[LONGITUDE] -= 360;

	return resultVector;
}
