% clear variables
% clc
 set(groot,'DefaultTextInterpreter','latex','DefaultLegendInterpreter','latex'),
 set(groot,'DefaultAxesFontName','Times','DefaultAxesFontSize',12),
%
% Parameters
%
 theta0 = deg2rad(12);
 theta  = theta0+deg2rad(1);
 m0 = 1000;
 m  = m0+70;
 g = 9.81;
 Cr = 0.01;
 rho = 1.3;
 Cd = 0.32;
 alpha = 2.4;
%
 veq = 200/9;
 Feq = 0.5*rho*Cd*alpha*veq^2 + m0*g*(sin(theta0)+Cr*cos(theta0));
%
 P0 = tf(1,[m0 rho*Cd*alpha*veq]);
 P  = tf(1,[m rho*Cd*alpha*veq]);
%
 t = linspace(0,61,2001);
 amax = .5;
 ynew = 25/9;
 rv = amax*t.*(t<ynew/amax) + ynew*(t>=ynew/amax);
 u = m0*amax*(t<ynew/amax) + rho*Cd*alpha*veq*rv;
 dm = (m0-m)*g*(sin(theta0)+Cr*cos(theta0));
 ds = 2*m0*g*(cos((theta0+theta)/2)-Cr*sin((theta0+theta)/2))*sin((theta0-theta)/2);
 y0 = lsim(P0,u,t);
 ym = lsim(P,u+dm,t);
 ys = lsim(P0,u+ds,t);
%
% Linear simulation
%
 figure(1),
 plot(t,3.6*(y0+veq),'LineWidth',3,'Color',[.2 .2 .7]),
 hold on,
 plot(t,3.6*(ys+veq),'LineWidth',3,'Color',[.7 .2 .2]),
 plot(t,3.6*(ym+veq),'LineWidth',3,'Color',[.2 .7 .2]),
 hold off,
 axis([t(1) t(end) 3.6*(veq-4.7)-1 3.6*(veq+ynew)+1]),
 grid on,
 set(gca,'FontSize',14,'TickLabelInterpreter','latex'),
 set(gca,'XTick',sort(unique(round(100*[0 30 60 ynew/amax])/100))),
 set(gca,'YTick',sort(unique(round(36*[veq,veq+ynew,veq+ynew-2*m0*g*(cos((theta0+theta)/2)-Cr*sin((theta0+theta)/2))*sin((theta-theta0)/2)/(rho*Cd*alpha*veq),veq+ynew-(m-m0)*g*(sin(theta0)+Cr*cos(theta0))/(rho*Cd*alpha*veq)])/10))),
 xlabel('Time (sec)'),
 ylabel('Velocity (km/h)'),
 lgd = legend({'\ nominal response','\ slope change $12^\circ\to13^\circ$','\ mass change $1000\to1070$'},'Location','SouthEast','FontSize',16);
 % print('Q2v','-depsc')
%
% Nonlinear simulation
%
 m = m0; theta = theta0;
 opts = odeset('RelTol',1e-8,'AbsTol',1e-8);
 [tv,v] = ode45(@(t,v) f(t,v,m,m0,0.5*rho*Cd*alpha,m*g*(sin(theta)+Cr*cos(theta)),veq,Feq,amax,ynew),t,veq,opts);
 figure(2),
 plot(t,3.6*(y0+veq),'LineWidth',3,'Color',[.2 .2 .7]),
 hold on,
 plot(tv,3.6*v,'LineWidth',3,'Color',[.2 .7 .7]),
 hold off,
 axis([t(1) t(end) 3.6*veq-.4 3.6*(veq+ynew)+.4]),
 grid on,
 set(gca,'FontSize',14,'TickLabelInterpreter','latex'),
 set(gca,'XTick',sort(unique(round(100*[0 30 60 ynew/amax])/100))),
 set(gca,'YTick',sort(unique(round(36*[veq,veq+ynew,sqrt(veq^2+2*veq*ynew)])/10))),
 xlabel('Time (sec)'),
 ylabel('Velocity (km/h)'),
 lgd = legend({'\ nominal linear response','\ nominal nonlinear response'},'Location','SouthEast','FontSize',16);
 % print('Q2vnl','-depsc')
%
%
% derivative for nonlinear simulations
%
function dvdt = f(t,v,m,m0,a,Frg,veq,Feq,amax,ynew),
    rv = amax*t.*(t<ynew/amax) + ynew*(t>=ynew/amax);
    u = m0*amax*(t<ynew/amax) + 2*a*veq*rv + Feq;
    dvdt = (u - a*v^2 - Frg) / m;
end
