function HW3_2015_solutions
%
    close all   
 %
 %%  Problem 1
 %
    L = 1; % Length of lens
    c = 5; % Constant determining ratio of electrostatic force to mass
    figure1 = figure;
    axes1 = axes('Parent',figure1,'YGrid','on','XGrid','on',...
    'FontSize',14);
    axis equal;
    hold on;
%    
    for i=1:3   % Loop over the three possible cases
%    
    initial_w = [i*0.5,0,0,1];   % Set the initial conditions
    tstart = 0; tstop = 1.2;
%    
    options = odeset('RelTol',0.0000001);
    [times,sols] = ode45(@charged_particle,[tstart,tstop],initial_w,options);   
    plot(sols(:,2),sols(:,1),'LineWidth',2)
    end
    xlabel('z','FontSize',16,'FontName','Times New Roman','FontAngle','italic');
    ylabel('r','FontSize',14,'FontName','Times','FontAngle','italic');
    title('Trajectories through an electrostatic lens','FontSize',20);
    
    
%
%% ODE function for problem 1
%
    function dwdt = charged_particle(t,w)
       r = w(1); z = w(2); vr = w(3); vz = w(4);
       Fz = c*z;
       Fr = -c*r/2;
       if (z>L) Fr=0; Fz=0; end
       drdt = vr; dzdt = vz; dvrdt = Fr; dvzdt = Fz;
       dwdt = [drdt;dzdt;dvrdt;dvzdt];       
    end
%
%% Problem 2
%
   m = 8600;  % Capsule mass, kg
   g = 9.81;  % Gravitational acceleration, m/s^2
   A = pi*5^2/4; % Capsule area, m^2
   Cd = 1;  % Drag coefft
   Re = 6371*10^3;  % Earth radius (m)
   d = 5000;   % Density decay length, (m)
   rho0 = 1.2;   % Mass density, kg/m^3
   R0 = Re+122000;  % re-entry altitude, m
   V0 = 32000/3.6;  % Re-entry velocity, m/s
   
%  This is the test with zero drag

   phi = 0; % Re-entry angle
   Cd = 0; % Drag set to zero
   
   initial_w = [R0;0;-V0*sin(phi);V0*cos(phi)/R0];
   tstart = 0;
   tstop = 240*60;
   options = odeset('Event',@space_event,'RelTol',0.000001);
   [times,sols] = ode45(@space_vehicle,[tstart,tstop],initial_w,options);
 
   for i=1:length(times)
       xx(i) = sols(i,1)*cos(sols(i,2))/1000;
       yy(i) = sols(i,1)*sin(sols(i,2))/1000;
   end
   
    figure2 = figure;
    axes2 = axes('Parent',figure2,'YGrid','on','XGrid','on',...
    'FontSize',14);
    axis square;
    ylim([-15000 15000]);
    xlim([-15000 15000]);
    hold on
    plot(xx,yy,'LineWidth',2)
    xlabel('x (km)','FontSize',16,'FontName','Times New Roman','FontAngle','italic');
    ylabel('y (km)','FontSize',14,'FontName','Times','FontAngle','italic');
    title('Predicted orbit with no drag','FontSize',18);
   
   
    phivals = [3,4.15,5,9];   % All the required phi values
    Cd = 1;
   
   for k = 1:4 
       phi = phivals(k)*pi/180;
       initial_w = [R0;0;-V0*sin(phi);V0*cos(phi)/R0];
       [times,sols] = ode45(@space_vehicle,[tstart,tstop],initial_w,options);
       AA = [];
       for i = 1:length(times)
           R = sols(i,1);  theta = sols(i,2);  vr = sols(i,3); omega = sols(i,4);
           dwdt = space_vehicle(times(i),sols(i,:)); % use the EOM function to find the accels
           dvrdt = dwdt(3); domegadt = dwdt(4);      % Extract the required derivatives
           ar = dvrdt - R*omega^2;  % r component of a
           atheta = R*domegadt + 2*vr*omega;  % theta component of a
           AA(i) = sqrt(ar^2 + atheta^2); % Acceleration magnitude
       end
       figure3 = figure;
       axes3 = axes('Parent',figure3,'YGrid','on','XGrid','on',...
        'FontSize',14);   
       plot(times,sols(:,1)-Re,'LineWidth',2)
       box(axes3,'on');
       hold(axes3,'all');
       anglestr = num2str(phivals(k));
       ylabel('Altitude (km)','FontSize',16,'FontName','Times New Roman','FontAngle','italic');
       xlabel('Time (s)','FontSize',14,'FontName','Times','FontAngle','italic');
       alttit = ['Altitude for  ' anglestr ' degree entry'];
       title(alttit,'FontSize',18);
       figure4 = figure;
       axes4 = axes('Parent',figure4,'YGrid','on','XGrid','on',...
        'FontSize',14);
       box(axes4,'on');
       hold(axes4,'all');
       plot(times,AA/g,'LineWidth',2)
       ylabel('Acceleration (g)','FontSize',16,'FontName','Times New Roman','FontAngle','italic');
       xlabel('Time (s)','FontSize',14,'FontName','Times','FontAngle','italic');
       acctit = ['Acceleration for  ' anglestr ' degree entry'];
       title(acctit,'FontSize',18);
   end
%
%% ODE function for problem 2 
%
    function dwdt = space_vehicle(t,w)
        R = w(1); theta = w(2); vr = w(3); omega = w(4);
        V = sqrt(vr^2+(R*omega)^2);
        B = g*Re^2/R^2;
        C = (rho0*Cd*A*V/(2*m))*exp(-(R-Re)/d);
        drdt = vr; dthetadt = omega;
        dvrdt = -B+R*omega^2-C*vr; domegadt = -2*vr*omega/R - C*omega;
        dwdt = [drdt;dthetadt;dvrdt;domegadt];
    end
    function [ev,stop,dir] = space_event(t,w)
        R = w(1); 
        ev = R-Re;
        stop = 1;
        dir = -1;
    end
%
%% Problem 3
%
    h = 1;   %Desired altitude
    m = 10/1000; %    Mass, in kg
    rho = 1.2; % Mass density of air, kg
    L = 0.02; %L in m
    g = 9.81; % Gravitational accel, m/s^2
    CC = 2*m*sqrt(m*g/(pi*rho*L^2)); % The constant C
    c = 0.0012; % Air resistance coefficient
    KP = 1;
    KDvals = [0,0.2,0.2];
    KIvals = [0,0,0.2];

    tstart = 0;
    tstop = 20;
    initial_w = [0,0,0];
    figure5 = figure;
    axes5 = axes('Parent',figure5,'YGrid','on','XGrid','on',...
         'FontSize',14);
    box(axes5,'on');
    hold(axes5,'all');
    for k = 1:3
      KI = KIvals(k);
      KD = KDvals(k);
      [time,sols] = ode45(@quadcopter_control,[tstart,tstop],initial_w);
      Displaystring = ['KP = ',num2str(KP),' KD = ',num2str(KD),' KI = ',num2str(KI)];
      plot(time,sols(:,1),'LineWidth',2,'DisplayName',Displaystring)
    end
    legend1 = legend(axes5,'show');
    set(legend1,...
    'Position',[0.648299859055674 0.793781435756744 0.217758985200846 0.107910379515318]);
   ylabel('Altitude (m)','FontSize',16,'FontName','Times New Roman','FontAngle','italic');
   xlabel('Time (s)','FontSize',14,'FontName','Times','FontAngle','italic');
   acctit = 'Altitude for controlled quadcopter';
   title(acctit,'FontSize',18);



%% ODE function 
    function dwdt = quadcopter_control(t,w)
        y = w(1); vy = w(2); Q = w(3);
        e = h-y;
        dedt = -vy;
        P = KP*e + KD*dedt + KI*Q;
        dydt = vy;
        dvydt = 8*P/(vy*m+CC) - c*vy^2/m -g;
        dQdt = e;
        dwdt = [dydt;dvydt;dQdt];
    end
    
end