function Control1_2_JMS(model)
% Solution to exercise 2 of control 1.
% JMS, Oct.2021

% Set parameters
if ~exist('model')
  model = 'A';
end
if model=='A'
  dr = 1.5e-4;
  p = 0.5;
  dphi = 4*pi*1e-3;
  q = 0.5;
  dt = 1e-2;
  l = [1,0,0,0,0,0,0,0,0,1];
elseif model=='B'
  dr = 2e-4;
  p = 0.3;
  dphi = 2*pi*1e-3;
  q = 0.4;
  dt = 1e-2;
  l = [0,1,0,0,0,0,0,0,1,0];
elseif model=='C'
  dr = 2.5e-4;
  p = -0.5;
  dphi = 3*pi*1e-3;
  q = 0.5;
  dt = 1e-2;
  l = [0,0,1,0,0,0,0,1,0,1];
elseif model=='D'
  dr = 1e-4;
  p = 0.8;
  dphi = 4*pi*1e-3;
  q = -0.6;
  dt = 1e-2;
  l = [0,0,0,1,0,0,1,0,0,0];
else
  error('Control1_1 ERROR: unexpected model')
end
ns = 2^10-1;

% Find random sequence and trajectory's polar coordinates
r(1) = 0;
phi(1) = 0;
for n = 1:ns
  s(n) = l(10);
  r(n+1) = r(n) + dr*(1+p*s(n));
  phi(n+1) = phi(n) + dphi*(1+q*(1-s(n)));
  l = [ xor(l(3),l(10)), l(1:9) ];      % update l vector with xor function
end

% Count zeros and ones in sequence s
nz = sum(s==0);
no = sum(s==1);
is = 2:ns-1;
nsz = (s(1)==0 & s(2)~=0) + ...            % number of single zeros
      sum(s(is-1)~=0 & s(is)==0 & s(is+1)~=0) + (s(ns-1)~=0 & s(ns)==0);
is = 2:ns-2;
ndz = (s(1)==0 & s(2)==0 & s(3)~=0) + ...  % number of double zeros
      sum(s(is-1)~=0 & s(is)==0 & s(is+1)==0 & s(is+2)~=0) + ...
      (s(ns-2)~=0 & s(ns-1)==0 & s(ns)==0);
fprintf('Total number of zeros in s  = %d\n',nz)
fprintf('Total number of ones in s   = %d\n',no)
fprintf('Number of single zeros in s = %d\n',nsz)
fprintf('Number of double zeros in s = %d\n',ndz)
fprintf('Ratio of single to double zeros = %f\n',nsz/ndz)

% Find trajectory's cartesian coordinates and velocity
nt = numel(r);
t = dt*[0:nt-1];
x = r.*cos(phi);
y = r.*sin(phi);
vx = derivative_JMS(t,x);
vy = derivative_JMS(t,y);
vt = sqrt(vx.^2+vy.^2);
L = integral_JMS(t,vt);

% Plot trajectory (y vs x)
figure(11)
plot(x,y,'LineWidth',1.5)
axis equal
grid on
xlabel('x (m)')
ylabel('y (m)')
set(gca,'FontSize',15)

% Plot trajectory's coordinates vs time
figure(12)
plot( t,x, t,y, t,r, t,L, 'LineWidth',1.5)
legend('x','y','r','L')
grid on
xlabel('time (s)')
ylabel('distance (m)')
set(gca,'FontSize',15)

% Plot velocity vs time
figure(13)
plot( t,vx, t,vy, t,vt )
legend('v_x','v_y','v_t')
grid on
xlabel('time (s)')
ylabel('velocity (m/s)')
set(gca,'FontSize',15)

end

