% Control3_1.m
% Solution to problem 1 of control 3. May.2018
% J.M.Soler, Apr.2018

clear
G = 6.672e-11;     % gravitational constant (N*m^2/kg^2)
M = 5.972e24;      % Earth mass (kg)
R = 6.371e6;       % Earth radius (m)
ha = 6.0e5;        % atmosphere thickness (m)
mur = 0.15;        % drag coeff (kg/s)
ms = 500;           % satellite mass (kg)
r0 = [0;1.2*R;0];  % initial satellite position (m)
v0mod = 9045.5;    % initial satellite velocity (m/s)
theta0 = -pi/10;   % angle of initial velocity with horizon (rad)
v0 = v0mod*[cos(theta0);sin(theta0);0];  % initial velocity vector (m/s)
dt = 1.0;          % integration time step (s)
tmax = 1.e5;       % max. simulation time (s)

% Find trajectory
myforce = @(r,v,t)satelliteForce(ms,r,v);
[r,v] = rungeKutta(myforce,ms,r0,v0,dt,tmax);
r = squeeze(r);
v = squeeze(v);

% Find impact time
t = (0:dt:tmax);         % trajectory times (s)
h = sqrt(sum(r.^2))-R;   % satellite height (m)
itf = find(h<0,1);       % final time step (of impact) (s)
tf = t(itf);             % time of impact, from initial time (s)
lf = pi/2-abs(atan2(r(1,itf),r(2,itf)));  % impact latitude (rad)

% Plot trajectory
figure(1)
phi = (0:pi/30:2*pi);    % mesh of angles to draw circles
plot(R*cos(phi),R*sin(phi),'-k'), hold on    % Erth surface
plot((R+ha)*cos(phi),(R+ha)*sin(phi),'--k')  % atmosphere perimeter
plot(r(1,1),r(2,1),'o', ...                  % initial position
     r(1,itf),r(2,itf),'o')                  % impact position
plot(r(1,1:itf),r(2,1:itf),'-r'), hold off   % satellite trajectory
axis equal, grid on
axis([-15,8,-15,8]*1e6)
xlabel('x (m)')
ylabel('y (m)')

% Find energies
mu = (1-h/ha)*mur;                                  % drag coefficient
mu = max(mu,0);                                     % zero above atmosphere
mu = repmat(mu,3,1);                                % give mu the shape of v
f = -mu.*v;                                         % drag force
W = sum(sum(f(:,1:itf).*v(:,1:itf)))*dt;            % drag work
Ei = -G*M*ms/norm(r0)+0.5*ms*norm(v0)^2;             % initial energy
Ef = -G*M*ms/norm(r(:,itf))+0.5*ms*norm(v(:,itf))^2; % final energy
DE = Ef-Ei;                                         % energy difference

% Write results
fprintf('Time of impact, from initial time = %e s\n',tf)
fprintf('               Latitude of impact = %f deg\n',lf*180/pi)
fprintf('                     Work of drag = %e J\n',W)
fprintf('                    Energy change = %e J\n',DE)
