#include <TString.h>
#include <iostream>
#include <string>
#include <TVector3.h>
#include <TMath.h>
#include <math.h>
#include <TRotation.h>

using namespace std;

class Orbit
{
	private:
	double EARTH_RADIUS; //[Km]
	double MU; //Mass parameter [km^3 s^-2]
	double OMEGA_EARTH; //[rad/s]
	TVector3 OMEGA_EARTH_VECT;
	double ALPHA_G0; //Right ascension of Greenwich meridian[deg]
	double LAT_GS; //Ground station latitude [deg];
	double LONG_GS; //Ground station longitude [deg];
	double H_GS; //Ground station altitude [m];
	TVector3 R_GS_TOPO; //[Km]position of the GS in the topocentric frame
	//Space relative data (Finding position of sc in geo)
	TVector3 SC_RHO; //[Km]position of the SC in the topocentric frame (rho)
	TVector3 SC_TOPO;//[Km]position of the SC in the topocentric frame (r)

	//Date
	TVector3 t0; //Starting time (Day, hours, min)
	TVector3 t; //Time (Day, hours, min)

	public:
	//Functions
	Orbit();
	double timeConversion(TVector3 vector);
	double GetDeltaT();
	void SetTime(int gg, int hr, int min);
	double GetAlphaGreen();
	double GetAlphaGsGeo();
	double GetDeltaGsGeo();
	TRotation TopoGeoMat();
	TVector3 GetScPositionGeo();
	TVector3 GetScVelocityGeo();
	TVector3 GetMomentum();
	double GetEccentricity();
	TVector3 GetNodeVector();
	double GetSemilatumRectum();
	double GetSemiaxis();
	double GetEnergy();
	double GetInclination();
	double GetCapOmega();
	double GetSmallOmega();
	TVector3 GetP1();
	TVector3 GetP2();
	TVector3 GetK();
	TRotation GeoPerMat();
	double GetMeanMotion();
	double GetOrbitalPeriod();
	double GetNOrbits();
	TVector3 GetI();
	TVector3 GetJ();
	double GetTrueAnomaly();
	

};


//*************************************************************

Orbit::Orbit()
{	
	EARTH_RADIUS = 6731; //[Km]
	MU = 3.99e5; //Mass parameter [km^3 s^-2]
	OMEGA_EARTH=729.2e-7; //[rad/s]
	OMEGA_EARTH_VECT.SetXYZ(0.0,0.0,OMEGA_EARTH);
	ALPHA_G0 =100.0; //Right ascension of Greenwich meridian[deg]
	LAT_GS=41.0; //Ground station latitude [deg];
	LONG_GS=17.0; //Ground station longitude [deg];
	H_GS=0.0; //Ground station altitude [m];
	R_GS_TOPO.SetXYZ(0.0,0.0,(EARTH_RADIUS+H_GS)); //[Km]position of the GS in the topocentric frame
	//Space relative data (Finding position of sc in geo)
	SC_RHO.SetXYZ(0.0,0.0,229.0); //[Km]position of the SC in the topocentric frame (rho)
	SC_TOPO =SC_RHO + R_GS_TOPO;//[Km]position of the SC in the topocentric frame (r)

	//Date
	t0.SetXYZ(1.0,0.0,0.0); //Starting time (Day, hours, min)
	t.SetXYZ(12.0,12.0,0.0); //Time (Day, hours, min)
}
double Orbit::timeConversion(TVector3 vector)
{
	double converted =vector.X()*86400.0 +vector.Y()*3600.0 +vector.Z()*60.0;
	return converted;
}



double Orbit::GetDeltaT()
{
	double deltat =timeConversion(t-t0); //delta t in sec
	return deltat;
}
void Orbit::SetTime(int gg, int hr, int min)
{
	t.SetX(gg);
	t.SetY(hr);
	t.SetZ(min);
}
double  Orbit::GetAlphaGreen()
{
	double alpha_g0 =ALPHA_G0*TMath::DegToRad(); //alpha of greenwich in [rad]
	double alpha_green =fmod((alpha_g0+OMEGA_EARTH*GetDeltaT()) , (double)TMath::TwoPi());	
	return alpha_green;
}
double Orbit::GetAlphaGsGeo()
{
	double alpha_gs_geo =GetAlphaGreen()+LONG_GS*TMath::DegToRad();
	return alpha_gs_geo;
}
double Orbit::GetDeltaGsGeo()
{
	double delta_gs_geo =(LAT_GS)*TMath::DegToRad();
	return delta_gs_geo;
}
TRotation Orbit::TopoGeoMat()
{
	//Coordination Transformation Matrix from Topographic to Geocentric eq.
	TRotation r1;
	TRotation r2;
	TRotation r3;
	r1.RotateZ(GetAlphaGsGeo());
	r2.RotateY(TMath::PiOver2()-GetDeltaGsGeo());
	r3 =r1*r2;
	return r3;
}
TVector3 Orbit::GetScPositionGeo()
{
	TVector3 r_sc_geo = TopoGeoMat() * SC_TOPO;
	return r_sc_geo;
}	
TVector3 Orbit::GetScVelocityGeo()
{
	TVector3 w(0.0, -8.36323, 0.0);//Observed Velocity of satellite in topocentric frame
	TVector3 wE_T(TopoGeoMat().ZX()*OMEGA_EARTH,TopoGeoMat().ZY()*OMEGA_EARTH,TopoGeoMat().ZZ()*OMEGA_EARTH); //Velocity of Earth in Topocentric frame
	TVector3 u_trasc=wE_T.Cross(SC_TOPO);
	TVector3 v_sc_topo=w+u_trasc; //Velocity of sc in topocentric frame
	TVector3 v_sc_geo=TopoGeoMat() *v_sc_topo;
	return v_sc_geo;
}

TVector3 Orbit::GetMomentum()
{
	TVector3 h=GetScPositionGeo().Cross(GetScVelocityGeo());
	return h;
}
double Orbit::GetEccentricity()
{
	TVector3 e_vett=GetScVelocityGeo().Cross(GetMomentum())*(1.0/MU)-GetScPositionGeo()*(1.0/GetScPositionGeo().Mag());
	double eccentricity=e_vett.Mag();
	return eccentricity;
}
TVector3 Orbit::GetNodeVector()
{
	TVector3 g3(0.0, 0.0, 1.0); //g3 is z axes in geo
	TVector3 n_vers_geo=g3.Cross(GetMomentum());
	return n_vers_geo;
}
double Orbit::GetSemilatumRectum()
{
	double p= GetMomentum().Mag2()/MU;
	return p;
}
double Orbit::GetSemiaxis()
{
	TVector3 e_vett=GetScVelocityGeo().Cross(GetMomentum())*(1.0/MU)-GetScPositionGeo()*(1.0/GetScPositionGeo().Mag());
	double a= GetSemilatumRectum()/(1.0-e_vett.Mag2());
	return a;
}
double Orbit::GetEnergy()
{
	double E=-MU/(2*GetSemiaxis());
	return E;
}
double Orbit::GetInclination()
{
	TVector3 g3(0.0, 0.0, 1.0); //g3 is z axis in geo
	double inclination=g3.Angle(GetMomentum());
	return inclination;
}
double Orbit::GetCapOmega()
{
	TVector3 g1( 1.0, 0.0, 0.0); //g1 is x axis in geo
	double OMEGA=g1.Angle(GetNodeVector());
	return OMEGA;
}
double Orbit::GetSmallOmega()
{
	TVector3 e_vett=GetScVelocityGeo().Cross(GetMomentum())*(1.0/MU)-GetScPositionGeo()*(1.0/GetScPositionGeo().Mag());
	double omega=e_vett.Angle(GetNodeVector());
	return omega;
}


TVector3 Orbit::GetP1()
{
	TVector3 e_vett=GetScVelocityGeo().Cross(GetMomentum())*(1.0/MU)-GetScPositionGeo()*(1.0/GetScPositionGeo().Mag());
	TVector3 p1 =e_vett*(1.0/e_vett.Mag());
	return p1;
}
TVector3 Orbit::GetK()
{
	TVector3 k=GetMomentum()*(1.0/GetMomentum().Mag());
	return k;
}
TVector3 Orbit::GetP2()
{
	TVector3 p2=GetK().Cross(GetP1());
	return p2;
}
TRotation Orbit::GeoPerMat()
{
	TRotation r1;
	TRotation r2;
	TRotation r3;
	TRotation Tgp;
	r1.RotateZ(GetCapOmega());
	r2.RotateY(GetInclination());
	r3.RotateZ(GetSmallOmega());
	Tgp =r1*r2*r3;//Transformation matrix from geo to perifocal reference frame
	return Tgp;
}
double Orbit::GetMeanMotion()
{
	double mm =pow(MU/pow(GetSemiaxis(),3.0),0.5);
	return mm;
}
double Orbit::GetOrbitalPeriod()
{
	double T_orbit=TMath::TwoPi()*pow(GetSemiaxis(),1.5)/pow(MU,0.5);
	return T_orbit;
}
double Orbit::GetNOrbits()
{
	double N_orbit=TMath::TwoPi()/OMEGA_EARTH/GetOrbitalPeriod();
	return N_orbit;
}
TVector3 Orbit::GetI()
{
	TVector3 i =GetScPositionGeo().Unit();
	return i;
}
TVector3 Orbit::GetJ()
{
	TVector3 j =GetK().Cross(GetI()).Unit();
	return j;
}
double Orbit::GetTrueAnomaly()
{
	double nu=GetI().Angle(GetP1());
	return nu;
}