function HW3_2014

   close all
%% Problem 1
   Vx = 1;
   Vy = 1;
   m = 0.5;
   g = 9.81;
   c = 0.001;
   t = linspace(0,2.5,100);
   figure1 = figure;
   axes1 = axes('Parent',figure1,'FontSize',16);
   box(axes1,'on');
   hold(axes1,'all');   
   [xx,yy] = trajectory(t);
   plot(xx,yy,'Displayname','c=0.001','LineWidth',2,'Color',[1,0,0]); 
   hold on
   c = 1;
   [xx,yy] = trajectory(t);
   plot(xx,yy,'Displayname','c=1','LineWidth',2,'Color',[0,1,0]);    
   c = 10;
   [xx,yy] = trajectory(t);
   plot(xx,yy,'Displayname','c=10','LineWidth',2,'Color',[0,0,1]);
   legend1 = legend(axes1,'show');
   set(legend1,...
    'Position',[0.692669137210187 0.687705456936231 0.155074116305587 0.172912557527942],...
    'FontSize',14);
    xlabel('x','FontSize',16);
    ylabel('y','FontSize',16); 
    
   function [X,Y] = trajectory(t)
       for i = 1:length(t)
           X(i) = (m*Vx/c)*(1-exp(-c*t(i)/m));
           Y(i) = (Vy + m*g/c)*(m/c)*(1-exp(-c*t(i)/m))-m*g*t(i)/c;
       end
    end
   
   
%% Problem 2

   m = 6000;      % Helicopter mass, kg
   c = 0.8;       % Drag coeft, Ns^2/m^2;
   g = 9.81;      % Gravitational const, m/s^2;
   T = 150;       % Power cycle period, sec
   L = 5.2;       % Rotor length, L
   CC = sqrt(m*g/(pi*1.2*L^2));
   thrust = @thrust1;
   initial_w = [0;0;0;0];    % Initial position and velocity
   [time,sol] = ode45(@helicopter,[0:0.5:150],initial_w)
 
%   animate_helicopter(sol);
   xlabel('x (m)','FontSize',16);
   ylabel('y (m)','FontSize',16);

   
   figure2 = figure
   axes1 = axes('Parent',figure2,'FontSize',16);
   box(axes1,'on');
   hold(axes1,'all');
   plot(time,sol(:,3),'Displayname','vx','LineWidth',2,'Color',[1,0,0]);
   plot(time,sol(:,4),'Displayname','vy','LineWidth',2,'Color',[0,0,1]);
   legend1 = legend(axes1,'show');
   set(legend1,...
    'Position',[0.182635059701732 0.238493438994125 0.139794494811663 0.128535804457516],...
    'FontSize',14);
    xlabel('time (s)','FontSize',16);
    ylabel('Velocity (m/s)','FontSize',16); 
 
    function dwdt = helicopter(t,w)
       x = w(1); y = w(2); vx = w(3); vy = w(4);
       dxdt = vx;
       dydt = vy;
       V = sqrt(vx^2+vy^2);
       [Fl,Ft] = thrust(t,vx,vy);
       dvxdt = (Ft-c*V*vx)/m;
       dvydt = (Fl-c*V*vy)/m -g;
       dwdt = [dxdt;dydt;dvxdt;dvydt];      
    end
    function [Lift,Thrust] = thrust1(t,vx,vy)
       Power = m*g*CC*(1+0.05*sin(2*pi*t/T));
       Lift = Power/(vy/2 + CC);
       Thrust = 200*sin(1.8*pi*t/T);
    end

%% Problem 2

    lambda = -1.0;
    mu = 0;
    Omega = 2;
    initial_w = [1.;0.;0.;1];
    options = odeset('RelTol',0.00001)
    [time,sols] = ode45(@ion,[0,400],initial_w,options);
    figure2 = figure;
    axes1 = axes('Parent',figure2,'FontSize',16);
    box(axes1,'on');
    hold(axes1,'all');
    plot(sols(:,1),sols(:,2),'Displayname','(a)','Color',[1,0,0])
    hold on
    initial_w = [0.8;0.;0.;0.25];
    [time,sols] = ode45(@ion,[0,400],initial_w,options);
    plot(sols(:,1),sols(:,2),'Displayname','(b)','Color',[0,1,0])
    
    mu = 8;
    initial_w = [2.5;0.;0.;0.25];
    [time,sols] = ode45(@ion,[0,400],initial_w,options);
    plot(sols(:,1),sols(:,2),'Displayname','(c)','Color',[0,0,1]) 
    legend1 = legend(axes1,'show');
    set(legend1,...
    'Position',[0.182635059701732 0.238493438994125 0.139794494811663 0.128535804457516],...
    'FontSize',14);
     xlabel('x','FontSize',16);
     ylabel('y','FontSize',16); 
     figure3 = figure
     axes1 = axes('Parent',figure3,'FontSize',16);
     box(axes1,'on');
     hold(axes1,'all');
     initial_w = [1.5;0.;0.;0.25];
     [time,sols] = ode45(@ion,[0,400],initial_w,options);
     plot(sols(:,1),sols(:,2),'Color',[1,0,0],'LineWidth',2) 
     xlabel('x','FontSize',16);
     ylabel('y','FontSize',16);    
 
    function dwdt = ion(t,w)
       x = w(1); y = w(2); vx=w(3); vy = w(4);
       dxdt = vx;
       dydt = vy;
       r = sqrt(x^2+y^2);
       dvxdt = (lambda+mu*cos(Omega*t))*x/r^2;
       dvydt = (lambda+mu*cos(Omega*t))*y/r^2;
       dwdt = [dxdt;dydt;dvxdt;dvydt]; 
    end

    L0 = 1.2;
    dL = 0.25;
    Omega = 1.9*sqrt(g/L0);
    g = 9.81;
    initial_w = [0.1;0];
    [time,sol] = ode45(@pendulum,[0,50],initial_w);
    figure4 = figure;
     axes1 = axes('Parent',figure4,'FontSize',16);
     box(axes1,'on');
     hold(axes1,'all');
     plot(time,sol(:,1),'Color',[1,0,0],'LineWidth',2);
     xlabel('time (s)','FontSize',16);
     ylabel('\theta','FontSize',16);        
%    animate_pendulum(time,sol,L0,dL,Omega)
%% Problem 2

    function dwdt = pendulum(t,w)
        theta = w(1); omega = w(2);
        dthetadt = omega;
        domegadt = (-2*dL*Omega*cos(Omega*t)*omega-g*sin(theta))/(L0+dL*sin(Omega*t));
        dwdt = [dthetadt;domegadt];
    end


end
% function animate_helicopter(sol)
% 
% [m,n] = size(sol);
% figure99 = figure;
% wmin = min(min(sol(:,1)),min(sol(:,2)));
% wmax = max(max(sol(:,1)),max(sol(:,2)));
% wmin = wmin - (wmax-wmin)*0.05;
% wmax = wmax + (wmax-wmin)*0.05;
% for i = 1:m
%     clf
%     axes1 = axes('Parent',figure99,'FontSize',16);
%     plot(sol(1:i,1),sol(1:i,2))
%     xlim([wmin wmax]);
%     ylim([wmin wmax]);
%     axis square
%     hold on
%     plothelicopter(sol(i,1),sol(i,2))
%     pause(0.05);
% end
% 
% 
% 
% end
% function plothelicopter(x0,y0)
% x = [-1,-1];
% y = [0,3];
% x = x + x0;
% y = y + y0;
% plot(x,y,'LineWidth',2,'Color',[0,0,0])
% hold on
% x = [-6,4];
% y = [3,3];
% x = x + x0;
% y = y + y0;
% plot(x,y,'LineWidth',2,'Color',[0,0,0])
% 
% t = (1/16:1/8:1)'*2*pi;
% x = x0+2*sin(t);
% y = y0+2*cos(t);
% x(6) = x(6)-5;
% x(7) = x(7)-5;
% y(6) = y(6)+1.5;
% y(7) = y(7) + 1.0;
% fill(x,y,'r')
% 
% 
% end
% function animate_pendulum(time,sols,L,dL,Omega)
% 
% for i=1:length(time)
% xp(i) =    (L+dL*sin(Omega*time(i)))*sin(sols(i,1));
% yp(i) =  - (L+dL*sin(Omega*time(i)))*cos(sols(i,1));
% end
% maxv = max(max(xp),max(yp));
% minv = min(min(xp),min(yp));
% %if (minv>0) minv = -0.1; end
% %if (maxv<0) maxv = 0.1; end
% dx = 0.505*(maxv-minv);
% xcen = 0.5*(maxv+minv);
% ycen = 0.5*(maxv+minv);
% figure;
% [m,n] = size(sols);
% for i = 1:m
%     clf
%     ylim([ycen-dx ycen+dx]);
%     xlim([xcen-dx xcen+dx]);
%     axis square;
%     hold on;
%     plot(xp(i),yp(i),'ro','MarkerSize',11,'MarkerFaceColor',[1,0,0]);
%     plot([0,xp(i)],[0,yp(i)],'LineWidth',2)
%     pause(0.05)
% end
% 
% end
