function ground_track
%% GROUND TRACK DE UN SATELITE
% UTN - FRH
% MECANICA ORBITAL
% -------------------------------------------------------------------------
% Este programa calcula y grafica la trayectoria en tierra (ground track)
% de un satelite a partir de sus elementos orbitales.
%
% -------------------------------------------------------------------------
%% NOTAS ACLARATORIAS
%
% El programa calcula la posicion del satelite en distintos instantes de
% tiempo, considerando la variacion de la ascension recta del nodo
% ascendente y del argumento del perigeo debido al efecto del segundo
% armonico zonal J2.
%
% La posicion calculada en el sistema ecuatorial geocentrico se transforma
% al sistema fijo a la Tierra para obtener la longitud y latitud
% correspondientes a la posicion del satelite.
%
% Finalmente, se representan las coordenadas obtenidas sobre un mapa
% terrestre para obtener el ground track.
%
% -------------------------------------------------------------------------
%% VARIABLES
%
% mu          - Parametro gravitacional de la Tierra [km^3/s^2]
% deg         - Factor de conversion de grados a radianes
% J2          - Segundo armonico zonal de la Tierra
% Re          - Radio ecuatorial de la Tierra [km]
% we          - Velocidad angular de rotacion de la Tierra [rad/s]
%
% n_periods   - Numero de periodos orbitales a representar
% T           - Periodo orbital [s]
% e           - Excentricidad de la orbita
% incl        - Inclinacion orbital [rad]
% wpo         - Argumento del perigeo inicial [rad]
% Wo          - Ascension recta del nodo ascendente inicial [rad]
% Mo          - Anomalia media inicial [rad]
%
% a           - Semieje mayor de la orbita [km]
% h           - Momento angular especifico [km^2/s]
% Eo          - Anomalia excentrica inicial [rad]
% TAo         - Anomalia verdadera inicial [rad]
% to          - Tiempo inicial medido desde el paso por el perigeo [s]
% tf          - Tiempo final de calculo [s]
%
% fac         - Factor comun de las ecuaciones de variacion
% Wdot        - Tasa de regresion del nodo ascendente [rad/s]
% wpdot       - Tasa de avance del argumento del perigeo [rad/s]
%
% ra          - Longitud del satelite [deg]
% dec         - Latitud del satelite [deg]
% n_curves    - Numero de curvas que componen el ground track
% RA          - Longitudes separadas por curva [deg]
% Dec         - Latitudes separadas por curva [deg]
%
% -------------------------------------------------------------------------
%% FUNCIONES UTILIZADAS
%
% ra_and_dec_from_r - Calcula longitud y latitud a partir del vector de
%                     posicion en el sistema fijo a la Tierra.
%
% kepler_E          - Resuelve la ecuacion de Kepler mediante el metodo
%                     de Newton-Raphson.
%
% sv_from_coe       - Calcula el vector de estado a partir de los elementos
%                     orbitales clasicos.
%
% find_ra_and_dec   - Calcula la posicion del satelite durante la orbita.
%
% form_separate_curves - Separa las curvas del ground track.
%
% plot_ground_track - Grafica el ground track.
%
% print_orbital_data - Muestra los resultados orbitales.
%
% -------------------------------------------------------------------------
%% LIMPIEZA
close all       % Cierra todas las ventanas abiertas
clear           % Borra las variables del Workspace
clc             % Limpia el Command Window

%% CONSTANTES
deg = pi/180;                          % Conversion de grados a radianes
mu  = 398600;                          % Parametro gravitacional [km^3/s^2]
J2  = 0.00108263;                      % Segundo armonico zonal
Re  = 6378;                            % Radio de la Tierra [km]
we  = (2*pi + 2*pi/365.26)/(24*3600); % Velocidad angular terrestre [rad/s]

%% DATOS DE ENTRADA
n_periods = 3;                  % Numero de periodos a representar

T = (24*60*60)/15.49724271;     % Periodo orbital [s]
e = 0.0010236;                  % Excentricidad

incl = 51.6386*deg;             % Inclinacion [rad]
wpo  = 50.4886*deg;             % Argumento del perigeo inicial [rad]
Wo   = 200.1168*deg;            % Ascension recta inicial [rad]
Mo   = 112.8263*deg;            % Anomalia media inicial [rad]

%% CALCULOS ORBITALES
% Semieje mayor
a = (mu*(T/(2*pi))^2)^(1/3);

% Momento angular especifico
h = sqrt(mu*a*(1 - e^2));

% Anomalia excentrica inicial
Eo = kepler_E(e, Mo);

% Anomalia verdadera inicial
TAo = 2*atan(tan(Eo/2)*sqrt((1 + e)/(1 - e)));

% Tiempo inicial medido desde el paso por el perigeo
to = Mo*(T/(2*pi));

% Tiempo final de calculo
tf = to + n_periods*T;

%% TASAS DE VARIACION ORBITAL
% Factor comun de las ecuaciones de variacion
fac = -3/2*sqrt(mu)*J2*Re^2/(1 - e^2)^2/a^(7/2);

% Tasa de regresion del nodo ascendente
Wdot = fac*cos(incl);

% Tasa de avance del argumento del perigeo
wpdot = fac*(5/2*sin(incl)^2 - 2);

%% CALCULO DE LA TRAYECTORIA
[ra, dec] = find_ra_and_dec(to, tf, T, e, h, mu, ...
                            Wo, Wdot, wpo, wpdot, ...
                            incl, we);

%% SEPARACION DE LAS CURVAS
[RA, Dec, n_curves] = form_separate_curves(ra, dec);

%% GRAFICA DEL GROUND TRACK
plot_ground_track(ra, dec, RA, Dec, n_curves);

%% RESULTADOS
print_orbital_data(h, e, a, T, incl, TAo, to, ...
                   Wo, Wdot, wpo, wpdot, mu);

end

%% =========================================================================
%% FUNCIONES AUXILIARES
%% =========================================================================

function [ra, dec] = ra_and_dec_from_r(r)
%% ASCENSION RECTA Y DECLINACION
% Esta funcion calcula la ascencion recta y la declinacion a partir del
% vector posicion en sistema ecuatorial geocentrico
% Entradas:
% r   - Vector de posicion [km]
%
% Salidas:
% ra  - Longitud [deg]
% dec - Latitud [deg]

%% Calculo de los cosenos directores
l = r(1)/norm(r);
m = r(2)/norm(r);
n = r(3)/norm(r);

%% Calculo de la declinacion
dec = asind(n);

%% Calculo de la ascension recta
if m > 0
    ra = acosd(l/cosd(dec));
else
    ra = 360 - acosd(l/cosd(dec));
end

end %ra_and_dec_from_r
%% =========================================================================
function E = kepler_E(e, M)
%% RESOLUCION DE LA ECUACION DE KEPLER
% Esta funcion utiliza el metodo de Newton para resolver la
% ecuacion de Kepler:
%       E - e*sin(E) = M
% Entradas:
% e - Excentricidad
% M - Anomalia media [rad]
%
% Salida:
% E - Anomalia excentrica [rad]

%% DATOS DE CALCULO
tolerance = 1e-8;

%% ESTIMACION INICIAL
if M < pi
    E = M + e/2;
else
    E = M - e/2;
end 

%% ITERACION DE NEWTON
ratio = 1;
while abs(ratio) > tolerance
    ratio = (E - e*sin(E) - M)/(1 - e*cos(E));
    E = E - ratio;
end

end %kepler_E
%% =========================================================================
function [r, v] = sv_from_coe(coe, mu)
%% ELEMENTOS ORBITALES Y VECTOR DE ESTADO
% Esta función calcula el vector de estado (r,v) a partir de los
% elementos orbitales clásicos (coe).
%
% Entradas:
% mu  - Parametro gravitacional [km^3/s^2]
% coe - Elementos orbitales [h e RA incl w TA]
% donde
%       h    - momento angular (km^2/s)
%       e    - excentricidad
%       RA   - ascensión recta del nodo ascendente (rad)
%       incl - inclinación de la órbita (rad)
%       w    - argumento del perigeo (rad)
%       TA   - anomalía verdadera (rad)
% Salidas:
% r - Vector de posicion [km]
% v - Vector de velocidad [km/s]

%% DATOS DE ENTRADA
h    = coe(1);
e    = coe(2);
RA   = coe(3);
incl = coe(4);
w    = coe(5);
TA   = coe(6);

%% VECTOR DE POSICION EN EL SISTEMA PERIFOCAL
rp = (h^2/mu)*(1/(1 + e*cos(TA))) * ...
    (cos(TA)*[1; 0; 0] + sin(TA)*[0; 1; 0]);

%% VECTOR DE VELOCIDAD EN EL SISTEMA PERIFOCAL
vp = (mu/h) * ...
    (-sin(TA)*[1; 0; 0] + (e + cos(TA))*[0; 1; 0]);

%% MATRIZ DE ROTACION R3(RA)
R3_W = [ cos(RA)  sin(RA)  0
        -sin(RA)  cos(RA)  0
         0        0        1];

%% MATRIZ DE ROTACION R1(incl)
R1_i = [ 1  0           0
         0  cos(incl)   sin(incl)
         0 -sin(incl)   cos(incl)];

%% MATRIZ DE ROTACION R3(w)
R3_w = [ cos(w)  sin(w)  0
        -sin(w)  cos(w)  0
         0       0        1];

%% MATRIZ DE TRANSFORMACION
Q_pX = (R3_w*R1_i*R3_W)';

%% VECTOR DE ESTADO
r = Q_pX*rp;
v = Q_pX*vp;

%% CONVERSION A VECTORES FILA
r = r';
v = v';

end %sv_from_coe
%% =========================================================================

%% =========================================================================
%% SUBFUNCIONES
%% =========================================================================
function [ra, dec] = find_ra_and_dec(to, tf, T, e, h, mu, ...
                                     Wo, Wdot, wpo, wpdot, ...
                                     incl, we)
% CALCULO DE LA TRAYECTORIA
% Esta funcion calcula la longitud y la latitud del satelite durante el
% intervalo de tiempo seleccionado.

% VECTOR DE TIEMPOS
times = linspace(to, tf, 1000);

% INICIALIZACION DE VECTORES
ra  = zeros(length(times), 1);
dec = zeros(length(times), 1);

% CALCULO DE LA POSICION
for i = 1:length(times)
    % Tiempo actual
    t = times(i);
    % Anomalia media
    M = 2*pi/T*t;
    % Anomalia excentrica
    E = kepler_E(e, M);
    % Anomalia verdadera
    TA = 2*atan(tan(E/2)*sqrt((1 + e)/(1 - e)));
    % Vector de posicion en el sistema perifocal
    r = h^2/mu/(1 + e*cos(TA)) * ...
        [cos(TA); sin(TA); 0];
    % Ascension recta del nodo en el instante t
    W = Wo + Wdot*t;
    % Argumento del perigeo en el instante t
    wp = wpo + wpdot*t;
    
    % Matriz de rotacion R3(W)
    R1 = [ cos(W)  sin(W)  0
          -sin(W)  cos(W)  0
           0       0       1];
    % Matriz de rotacion R1(incl)
    R2 = [ 1  0           0
           0  cos(incl)   sin(incl)
           0 -sin(incl)   cos(incl)];

    % Matriz de rotacion R3(wp)
    R3 = [ cos(wp)  sin(wp)  0
          -sin(wp)  cos(wp)  0
           0        0        1];
    % Transformacion de perifocal a ecuatorial geocentrico
    QxX = (R3*R2*R1)';

    % Posicion en el sistema ecuatorial geocentrico
    R = QxX*r;

    % Rotacion de la Tierra
    theta = we*(t - to);
    Q = [ cos(theta)  sin(theta)  0
         -sin(theta)  cos(theta)  0
          0           0           1];

    % Posicion en el sistema fijo a la Tierra
    r_rel = Q*R;

    % Ascension recta y declinacion
    [alpha, delta] = ra_and_dec_from_r(r_rel);

    % Almacenamiento
    ra(i)  = alpha;
    dec(i) = delta;
end
end %find_ra_and_dec
%% =========================================================================
function [RA, Dec, n_curves] = form_separate_curves(ra, dec)
% SEPARACION DE LAS CURVAS DEL GROUND TRACK
% Esta funcion separa las curvas del ground track cuando se produce un
% salto significativo entre dos valores consecutivos de longitud.

% PARAMETROS
tol = 100;

% INICIALIZACION
curve_no = 1;
n_curves = 1;
k = 0;

RA  = cell(1, 1);
Dec = cell(1, 1);

ra_prev = ra(1);

% IDENTIFICACION DE CURVAS
for i = 1:length(ra)

    if abs(ra(i) - ra_prev) > tol

        curve_no = curve_no + 1;
        n_curves = n_curves + 1;
        k = 0;

    end

    k = k + 1;

    RA{curve_no}(k)  = ra(i);
    Dec{curve_no}(k) = dec(i);

    ra_prev = ra(i);
end
end %form_separate_curves
%% =========================================================================
function plot_ground_track(ra, dec, RA, Dec, n_curves)
% GRAFICA DEL GROUND TRACK
% CARGA DE DATOS TOPOGRAFICOS 
load topo topo topomap1

% COORDENADAS DEL MAPA
longitude = 0:359;
latitude  = -89:90;

% GRAFICA DE LOS CONTINENTES
contour(longitude, latitude, topo, [0 0], 'black');
hold on

% GROUND TRACK
for i = 1:n_curves
    plot(RA{i}, Dec{i}, 'LineWidth', 1.2);
end

% PUNTOS INICIAL Y FINAL
plot(ra(1), dec(1), 'o');
plot(ra(end), dec(end), 'o');
text(ra(1), dec(1), ' Inicio');
text(ra(end), dec(end), ' Fin');

% ECUADOR
line([0 360], [0 0], 'Color', 'k');

% CONFIGURACION DE LA GRAFICA
xlabel('Longitud este [deg]');
ylabel('Latitud [deg]');
title('Ground Track del satelite');
axis equal
axis([0 360 -90 90]);

% CUADRICULA
grid on
xticks(0:30:360);
yticks(-90:15:90);

% LEYENDA
legend('Continentes', 'Ground Track', ...
       'Inicio', 'Fin', 'Ecuador', ...
       'Location', 'best');
hold off
end %plot_ground_track
%% =========================================================================
function print_orbital_data(h, e, a, T, incl, TAo, to, ...
                            Wo, Wdot, wpo, wpdot, mu)
% PRESENTACION DE RESULTADOS
% CALCULO DEL VECTOR DE ESTADO INICIAL
coe = [h e Wo incl wpo TAo];
[ro, vo] = sv_from_coe(coe, mu);

% ENCABEZADO
fprintf('\n');
fprintf('============================================================\n');
fprintf('                    DATOS ORBITALES\n');
fprintf('============================================================\n');

% ELEMENTOS ORBITALES
fprintf('\nMomento angular           = %g km^2/s', h);
fprintf('\nExcentricidad             = %g', e);
fprintf('\nSemieje mayor             = %g km', a);
fprintf('\nPeriodo orbital           = %g horas', T/3600);
fprintf('\nInclinacion               = %g deg', incl*180/pi);
fprintf('\nAnomalia verdadera inicial = %g deg', TAo*180/pi);
fprintf('\nTiempo desde el perigeo   = %g horas', to/3600);

% ASCENSION RECTA DEL NODO
fprintf('\n\nAscension recta inicial   = %g deg', Wo*180/pi);
fprintf('\nTasa de regresion del nodo = %g deg/periodo', Wdot*180/pi*T);

% ARGUMENTO DEL PERIGEO
fprintf('\n\nArgumento del perigeo inicial = %g deg', wpo*180/pi);
fprintf('\nTasa de avance del perigeo    = %g deg/periodo', ...
    wpdot*180/pi*T);

% VECTOR DE POSICION
fprintf('\n\nVector de posicion inicial:\n');
fprintf('r0 = [%12g, %12g, %12g] km', ...
    ro(1), ro(2), ro(3));
fprintf('\nMagnitud de r0 = %g km\n', norm(ro));

% VECTOR DE VELOCIDAD
fprintf('\nVector de velocidad inicial:\n');
fprintf('v0 = [%12g, %12g, %12g] km/s', ...
    vo(1), vo(2), vo(3));
fprintf('\nMagnitud de v0 = %g km/s\n', norm(vo));

% FINAL
fprintf('\n============================================================\n');
fprintf('\n');
end %print_orbital_data