% Sedivy Group Project- Satellite in Venus Orbit
% 22 JUL 13
clear all
GMv = 3.249159e5; % Kepler's Constant for Venus (km^3/s^2)
rv = 6052; % Radius of Venus (km)
Max_Iteration = 5e4;
omega_v = -360/20996755.2; % angular velocity of Venus (deg/s)

% Initial Conditions
dt = 5;             % time increment (seconds)
r = 18806+rv;          % initial radius @ starting point(km)
theta = 0;          % inital angle (by definition) (radians)
v_r = 0;            % initial r-component of velocity (zero for circular orbit)(km/s)
v_theta = 3.6153;   % initial theta-component of velocity (km/s)

% Discrvte Parameters and initial values
dr = v_r*dt;            % km
dtheta = v_theta/r*dt;  % radians
r_0 = r;
theta_0 = theta;
long_s_0 = 151.22; % initial longitude of SSP

% Set-up Cartesian coordinate system:
x = r*cos(theta);
y = r*sin(theta);
all_x = x;
all_y = y;

% Initial Satellite Sub-Point (SSP) (@ start point)
lat_s = 0;  % degrees
long_s = 151.22; % degrees
all_lat = lat_s;
all_long = long_s;

% Venus Station
lat_v = 1;  % degrees
long_v = -10; % degrees

% Initial Elevation Calculation
gamma = acosd(cosd(lat_v)*cosd(lat_s)*cosd((long_s - long_v)) + sind(lat_v)*sind(lat_s)); % degrees
if gamma > acosd(rv/r) % Case of negative elevation
    elev = -acosd(sind(gamma)/sqrt(1+(rv/r)^2-2*rv/r*cosd(gamma))); % degrees
else % Positive or zero elevation
    elev = acosd(sind(gamma)/sqrt(1+(rv/r)^2-2*rv/r*cosd(gamma))); % degrees
end;

% Initial Azimuth Calculation   
alpha = abs(asind(sind(abs(long_v - long_s))*cosd(lat_s)/sind(gamma))); % degrees
if lat_s >= atand(tand(lat_v)*cosd(long_s - long_v)) && ((long_s - long_v) >= 0 && (long_s - long_v) <= 180)  % case wherv sat is to the NE of Earth Station
       azim = alpha;        % degrees
elseif lat_s >= atand(tand(lat_v)*cosd(long_s - long_v)) && ((long_s - long_v) < 0 || (long_s - long_v) > 180) % case wherv sat is to NW of Earth Station
       azim = 360 - alpha;  % degrees
elseif lat_s < atand(tand(lat_v)*cosd(long_s - long_v)) && ((long_s - long_v) >= 0 && (long_s - long_v) <= 180) % case wherv sat is to the SE of Earth Station
       azim = 180 - alpha; % degrees
else   % case where sat is to SW of Earth Station
       azim = 180 + alpha; % degrees
end;

% Set up arrays for elev and azim
all_alpha = alpha;
all_elev = elev;
all_azim = azim;
     
while abs(theta - theta_0) <= 2*pi && length(all_x) < Max_Iteration
    % Compute new state information
    r_new = r + dr;
    theta_new = theta + dtheta;
    dr_new = dr + ((r + 0.5*dr)*dtheta^2 - GMv/(r^2)*dt^2);
    dtheta_new = dtheta - (2*dr*dtheta/(r + 0.5*dr));
    
    % Convert to Cartesian and save new state information
    x_new = r_new*cos(theta_new);
    y_new = r_new*sin(theta_new);
    all_x = [all_x; x_new];
    all_y = [all_y; y_new]; 

    % Calcutate and save new Latitude and Longitude
    lat_s_new = 0; % degrees- Satellite is in Equatorial Orbit
    if x_new > 0 && y_new >= 0
        long_s_new = long_s_0 + abs(atand(y_new/x_new)) - omega_v*dt*(length(all_x)-1); % +X,+Y Quadrant, last term accounts for rotation of the earth and # of iterations
    elseif x_new < 0 && y_new >= 0 
        long_s_new = long_s_0 + 180 - abs(atand(y_new/x_new)) - omega_v*dt*(length(all_x)-1); %-X,+Y Quadrant
    elseif x_new < 0 && y_new < 0 
        long_s_new = long_s_0 - 180 + abs(atand(y_new/x_new)) - omega_v*dt*(length(all_x)-1); %-X,+Y Quadrant
    elseif x_new > 0 && y_new < 0 
        long_s_new = long_s_0 - abs(atand(y_new/x_new)) - omega_v*dt*(length(all_x)-1); %-X,+Y Quadrant
    elseif x_new == 0 && y_new >= 0
        long_s_new = long_s_0 + 90 - omega_v*dt*(length(all_x)-1); % +Y Axis (takes care of when atan is undefined)
    elseif x_new == 0 && y_new < 0
        long_s_new = long_s_0 - 90 - omega_v*dt*(length(all_x)-1); % -Y Axis (takes care of when atan is undefined)
    end;
    if long_s_new < -180 % Ensures Longitude is in the Range (-180,180)
       long_s_new = 360 + long_s_new;
    end;
    if long_s_new > 180  % Ensures Longitude is in the Range (-180,180)
       long_s_new = -360 +long_s_new;
    end;
    all_lat = [all_lat, lat_s_new];
    all_long = [all_long, long_s_new];
    
    % New Elevation Caluation
    gamma_new = acosd(cosd(lat_v)*cosd(lat_s_new)*cosd((long_s_new - long_v)) + sind(lat_v)*sind(lat_s_new)); % degrees
    if gamma_new > acosd(rv/r_new) % Case of negative elevation
        elev_new = -acosd(sind(gamma_new)/sqrt(1+(rv/r_new)^2-2*rv/r_new*cosd(gamma_new))); % degrees
    else % Positive or zero elevation
        elev_new = acosd(sind(gamma_new)/sqrt(1+(rv/r_new)^2-2*rv/r_new*cosd(gamma_new))); % degrees
    end;
      
    % New Azimuth Calculation   
    alpha_new = abs(asind(sind(abs(long_v - long_s_new))*cosd(lat_s_new)/sind(gamma_new))); % degrees
    if lat_s_new >= atand(tand(lat_v)*cosd(long_s_new - long_v)) && ((long_s_new - long_v) >= 0 && (long_s_new - long_v) <= 180)  % case wherv sat is to the NE of Earth Station
        azim_new = alpha_new;        % degrees
    elseif lat_s_new >= atand(tand(lat_v)*cosd(long_s_new - long_v)) && ((long_s_new - long_v) < 0 || (long_s_new - long_v) > 180) % case wherv sat is to NW of Earth Station
        azim_new = 360 - alpha_new;  % degrees
    elseif lat_s_new < atand(tand(lat_v)*cosd(long_s_new - long_v)) && ((long_s_new - long_v) >= 0 && (long_s_new - long_v) <= 180)  % case wherv sat is to the SE of Earth Station
        azim_new = 180 - alpha_new; % degrees
    else % case wherv sat is to SW of Earth Station
        azim_new = 180 + alpha_new; % degrees
    end;

    % Storv information for elev and azim
    all_alpha = [all_alpha, alpha_new];
    all_elev = [all_elev, elev_new];
    all_azim = [all_azim, azim_new];

    % Change new state information to currvnt state information for next iteration
    r = r_new;
    dr = dr_new;
    theta = theta_new;
    dtheta = dtheta_new;
end;

% Plot results
figure(1), plot(all_x,all_y)
title('Orbit for Venus Stellite');
xlabel('X (Kilometers)');
ylabel('Y (Kilometers)');

figure(2), plot(all_azim,all_elev);
ylim([0 90]);
title('Look Angles (Sky Track) for Venus Satellite');
xlabel('Azimuth (degrees)');
ylabel('Elevation (degrees)');


figure(3), plot(all_long,all_lat);
xlim([-180 180]);
ylim([-90 90]);
title('Ground Track for Venus Satellite');
xlabel('Longitude (degrees)');
ylabel('Latitude (degrees)');
    
