%% ENGN0040 2021 Homework #2
%
%% Problem 3
%

syms t
close all
w = 4*pi/15; R = 1.5;
y1 = R*(cos(w*(t-5))-1);
x = R*sin(w*(t-5));
y2 = R*(-cos(w*(t-5))+1);
vy1 = diff(y1,t); vy2 = diff(y2,t); vx = diff(x,t)
ay1 = diff(vy1,t); ay2 = diff(vy2,t); ax = diff(vx,t)
figure
fplot(y1,x,[5,12.5],'Color',[1,0,0])
hold on
fplot(y2,x,[12.5,20],'Color',[1,0,0])
xlabel('y (m)')
ylabel('x (m)')
title('Quadcopter trajectory')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',20,'xdir','reverse','XGrid','on','YGrid',...
    'on');
figure
p1 =  fplot(vy1,[5,12.5],'Color',[1,0,0]);
hold on
p2 = fplot(vy2,[12.5,20],'Color',[1,0,0]);
p3 = fplot(vx,[5,20],'Color',[0,0,1]);
xlabel('time (s)')
ylabel('Velocity (m/s)')
title('Quadcopter velocity')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',20,'XGrid','on','YGrid',...
    'on');
legend1 = legend([p1,p3],'v_y','v_x');
set(legend1,...
    'Position',[0.41665983606557377 0.208333329529014 0.229166666666668 0.187022895474015],...
    'FontAngle','italic',...
    'FontSize',14);
figure
p1 =  fplot(ay1,[5,12.5],'Color',[1,0,0]);
hold on
p2 = fplot(ay2,[12.5,20],'Color',[1,0,0]);
p3 = fplot(ax,[5,20],'Color',[0,0,1]);
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
title('Quadcopter acceleration')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',20,'XGrid','on','YGrid',...
    'on');
legend1 = legend([p1,p3],'a_y','a_x');
set(legend1,...
    'Position',[0.71665983606557377 0.208333329529014 0.229166666666668 0.187022895474015],...
    'FontAngle','italic',...
    'FontSize',14);

figure
p1 =  fplot(6*pi/15,[5,20],'Color',[1,0,0]);
hold on
p3 = fplot(0,[5,20],'Color',[0,0,1]);
ylim([-0.1,1.5])
xlabel('time (s)')
ylabel('Velocity (m/s)')
title('Quadcopter velocity')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',20,'XGrid','on','YGrid',...
    'on');
legend1 = legend([p1,p3],'v_L','v_T');
set(legend1,...
    'Position',[0.61665983606557377 0.308333329529014 0.229166666666668 0.187022895474015],...
    'FontAngle','italic',...
    'FontSize',14);

figure
p1 =  plot([5,12.5,12.5,20],[-(6*pi/15)^2/1.5,-(6*pi/15)^2/1.5,(6*pi/15)^2/1.5,(6*pi/15)^2/1.5],'Color',[1,0,0]);
hold on
p3 = fplot(0,[5,20],'Color',[0,0,1]);
ylim([-1.5,1.5])
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
title('Quadcopter acceleration')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',20,'XGrid','on','YGrid',...
    'on');
legend1 = legend([p1,p3],'a_T','a_L');
set(legend1,...
    'Position',[0.61665983606557377 0.308333329529014 0.229166666666668 0.187022895474015],...
    'FontAngle','italic',...
    'FontSize',14);

%
%% Problem 5
%

syms t
syms x(t) z(t)
x(t) = -0.6471*t^3-1.0593*t^2 + 6.7421*t - 0.0394;
z(t) = -0.8033*t^3+2.0538*t^2-1.5713*t+0.0039;
% Plot the trajectory
figure
fplot(x(t),z(t),[0,1.5],'LineWidth',2)

axis equal
ylim([-1,0])
xlabel('x (m)')
ylabel('z (m)')
title('Trajectory of a seagull')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',20,'XGrid','on','YGrid',...
    'on');
% Find the speed (magnitude of velocity)
V = sqrt(diff(x(t),t)^2 + diff(z(t),t)^2);
figure
fplot(V,[0,1.5],'LineWidth',2)
xlabel('Time (s)')
ylabel('Speed (m/s)')
title('Speed of a seagull')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',20,'XGrid','on','YGrid',...
    'on');
%Velocity and acceleration vectors
v = [diff(x(t),t),diff(z(t),t)];
a = [diff(x(t),t,2),diff(z(t),t,2)];
% Tangent and normal
tangent = v/V;
normal = [-tangent(2),tangent(1)];
at = dot(a,tangent);
an = dot(a,normal);
% Plot an and at
figure
fplot(at,[0,1.5],'LineWidth',2,'DisplayName','a_t')
hold on
fplot(an,[0,1.5],'LineWidth',2,'DisplayName','a_n')
xlabel('Time (s)')
ylabel('Acceleration (m/s^2)')
title('Acceleration of a seagull')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',20,'XGrid','on','YGrid',...
    'on');
legend1 = legend(axes1,'show');
set(legend1,...
    'Position',[0.2665983606557377 0.708333329529014 0.229166666666668 0.187022895474015],...
    'FontAngle','italic',...
    'FontSize',14);
% Lift and drag forces
m = 0.37; % Seagull mass
g = 9.81;
FD = -m*g*tangent(2) - m*at;
FL = m*g*normal(2) + m*an;
figure
fplot(FD,[0,1.5],'LineWidth',2,'DisplayName','F_D')
hold on
fplot(FL,[0,1.5],'LineWidth',2,'DisplayName','F_L')
xlabel('Time (s)')
ylabel('Force (N)')
title('Forces acting on a seagull')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',20,'XGrid','on','YGrid',...
    'on');
legend1 = legend(axes1,'show');
set(legend1,...
    'Position',[0.2665983606557377 0.308333329529014 0.229166666666668 0.187022895474015],...
    'FontAngle','italic',...
    'FontSize',14);

%

syms c x xx v t real
x = (1-exp(-t))^2
v = diff(x,t)
a = diff(v,t)
axx = simplify(subs(a,t,-log(1-sqrt(xx))))
vxx = simplify(subs(v,t,-log(1-sqrt(xx))))

%
%% Problem 6
%

clear all
syms s S x y real
V0 = 0.44704*25;
V1 = 0.44704*55;
amax = 0.2*9.81;
at = amax/sqrt(1 + pi^2/log(V1^2/V0^2)^2)
theta = (pi/(2*log(V1^2/V0^2))*log(1 + 2*at*s/V0^2))
L = (V1^2-V0^2)/(2*amax) * sqrt(1 + pi^2/log(V1^2/V0^2)^2)
x = simplify(int(cos(theta),s,[0,S]));
y = simplify(int(sin(theta),s,[0,S]));
figure
fplot(real(x),real(y),[0,L],'LineWidth',2,'Color',[1,0,0])
axis equal
axis square
xlabel('x')
ylabel('y')
title('Shape of highway ramp')
grid on