% FIRST PART: integrates equation of relative motion between two masses m1 and m2. m1
% is the Earth, m2 a spacecraft
% SECOND PART: plots the trajectory using the Keplerian orbit solution in terms of true anomaly 

% overall: it solves uncollected hw 1

%NOTE: it does not work past 1 orbital period -- you find out why!

clear all
clc
close all


%constants of the problem START%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
G = 6.6742*10^(-20); %km^3/(kg*s^2) gravitational constant
m1 = 5.972*10^24; %kg mass of Earth
m2 = 1000; %kg mass of S/C
mu = G*(m1+m2); %km^3/s^2 gravitational parameter
%constants of the problem END%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


r0 = [8000; 0; 6000]; %km 
v0 = [0; 7; 0]; %km/s 
Y0 = [r0; v0]; %initial state vector (km & km/s)

tf = 1*4*3600; %s days (4 hrs)
TSPAN = [0 tf]; %duration of integration in seconds
OPTIONS = odeset('Maxstep', 60);
[TOUT,YOUT] = ode45(@relacc,TSPAN,Y0,OPTIONS);

%%%%%%%%%%%%%%%%%%
%%%QUESTION 1%%%%%
%%%%%%%%%%%%%%%%%%
%computing constant vectors
h = cross(r0, v0); % angular momentum vector
e = cross(v0, h)/mu - r0/norm(r0); %eccentricity vector

%defining the coordinate system fixed with orbital plane (perifocal
%coordinate system)
z_hat = h/norm(h); %unit vector normal to orbital plane
x_hat = e/norm(e); %unit vector along e
y_hat = cross(z_hat, x_hat); %completing the right-handed coordinate system

%defining theta values
n = 360; %number of points per orbit (fineness of plot)
theta = linspace(0, 2*pi, n);
r_norm = (norm(h)^2/mu)*(1./(1+norm(e)*cos(theta)));
for i=1:n
    r(:,i) = r_norm(i)*cos(theta(i))*x_hat + r_norm(i)*sin(theta(i))*y_hat;
end

%plotting orbit from ODE45
figure(1)
plot3(YOUT(:,1),YOUT(:,2),YOUT(:,3),'r','linewidth',2)
grid on
hold on
axis equal
plot3(YOUT(1,1),YOUT(1,2),YOUT(1,3),'r*')
xlabel('x (km)'); ylabel('y (km)'); zlabel('z (km)');
[X,Y,Z] = sphere;
surf(6378*X, 6378*Y, 6378*Z)

%%%%%%%%%%%%%%%%%%
%%%QUESTION 2%%%%%
%%%%%%%%%%%%%%%%%%
%plotting from Keplerain orbit equation
plot3(r(1,:),r(2,:),r(3,:),'k')

%%%%%%%%%%%%%%%%%%
%%%QUESTION 3%%%%%
%%%%%%%%%%%%%%%%%%
[size1 size2] = size(TOUT); %defining number of time steps created by ode45
[maxr k] = max(sqrt(YOUT(:,1).^2+YOUT(:,2).^2+YOUT(:,3).^2)); %locating apogee and the index in the vector
%creating true anomaly vector
for i=1:size1
    %using dot product between r and e vectors to compute true anomaly
    %before you call acos you need to make sure it does not give you
    %complex outputs, when the argument is outside [-1 1]
    argument = (YOUT(i,1)*e(1) + YOUT(i,2)*e(2) + YOUT(i,3)*e(3)) ...
        /(norm([YOUT(i,1); YOUT(i,2); YOUT(i,3)])*norm(e));
    if norm(argument) >=1
        argument = sign(argument);
    else
        theta_ode45(i) = acos(argument);
    end
    % after apogee the inverse cosine will not give angles greater than pi,
    % so, we fix that!
    if i>=k
        theta_ode45(i) =  2*pi - theta_ode45(i);
    end
end

%creating velocity vector from true anomaly parameterization
for i=1:size1
    vr = (mu/norm(h))*norm(e)*sin(theta_ode45(i));
    vp = (mu/norm(h))*(1+norm(e)*cos(theta_ode45(i)));
    v(1:3, i) = vr*[YOUT(i,1); YOUT(i,2); YOUT(i,3)]/norm([YOUT(i,1); YOUT(i,2); YOUT(i,3)]) + ...
                vp*cross(h, [YOUT(i,1); YOUT(i,2); YOUT(i,3)])/norm(cross(h, [YOUT(i,1); YOUT(i,2); YOUT(i,3)]));
end

%%%%%%%%%%%%%%%%%%
%%%QUESTION 4%%%%%
%%%%%%%%%%%%%%%%%%
compare_vel = v(1:3,:)-YOUT(:,4:6)';
max(sqrt(compare_vel(1,:).^2+compare_vel(2,:).^2+compare_vel(3,:).^2))



