% Solution to problem 2 of control 1A
% JMS, Oct.2020

% Set parameters
clear
version = 'A'
if version=='A'
  b = 0.5;
  w = pi;
elseif version=='B'
  b = 0.8;
  w = 2*pi;
elseif version=='C'
  b = 0.4;
  w = pi/2;
elseif version=='D'
  b = 0.2;
  w = pi/4;
end
kA = 3;
kB = 6;
N = 1e3;
tmax = 2*pi/w;

% Find trajectories
[RA,npA,lA] = rhodonea(kA,b,N);
[RB,npB,lB] = rhodonea(kB,b,N);
lA, lB

% Find velocity and acceleration
dt = tmax/(N-1);                 % time interval
t = 0:dt:tmax;
r = RB;
v(1,:) = derivative(t,r(1,:));   % velocity vector
v(2,:) = derivative(t,r(2,:));
a(1,:) = derivative(t,v(1,:));   % acceleration vector
a(2,:) = derivative(t,v(2,:));
vmod = sqrt(sum(v.^2));          % velocity modulus
amod = sqrt(sum(a.^2));          % acceleration modulus
an = a-v.*sum(a.*v)./vmod.^2;    % normal acceleration vector
anmod = sqrt(sum(an.^2));        % normal acceleration modulus

% Plot trajectories
figure(11)
plot( RA(1,:),RA(2,:), RB(1,:),RB(2,:), 'LineWidth',2 )
legend( ['np=',num2str(npA)], ['np=',num2str(npB)] )
xlabel('x (m)')
ylabel('y (m)')
axis equal
set(gca,'FontSize',20)

% Plot velocity components
figure(12)
plot(t,v(1,:),t,v(2,:),t,vmod,'LineWidth',2)
legend('v_x','v_y','v_{tot}','Location','SouthEast')
xlabel('t (s)')
ylabel('v (m/s)')
set(gca,'FontSize',20)
grid on

% Plot normal acceleration
figure(13)
plot(t,amod,'k', t,anmod,'r--','LineWidth',2)
legend('a_{total}','a_{normal}')
xlabel('t (s)')
ylabel('a_n (m/s^2)')
set(gca,'FontSize',20)
grid on
