function Control3_2_JMS(model)
% Solution to problem 2 of control 3
% Jose Soler, May.2022

% Set system parameters
if model=='A'
  l = 1.2;           % internal force parameter (m) 
  C = 10;            % internal force parameter (N/m^3)
  D = 3;             % external force parameter (N/m^2)
  m = [0.8, 1.2];    % particle masses (kg)
elseif model=='B'
  l = 1.2;
  C = 4;
  D = 3;
  m = [1.2, 1.8];
elseif model=='C'
  l = 1.2;
  C = 10;
  D = 3;
  m = [1.7, 1.3];
elseif model=='D'
  l = 1.2;
  C = 4;
  D = 3;
  m = [1.7, 1.3];
else
  error('Wrong model')
end
r0 = [ [ 0.0;  0.0; 0.8], [0.0; 0.7; 0.0] ]; % initial positions (m)
v0 = [ [-0.1; -0.5; 0.0], [1.0; 0.0; 0.0] ]; % initial velocities (m/s)
dt = 1e-3;                                   % integration time interval (s)
tmax = 30;                                   % max integration time (s)

% Find trajectories and angular momenta
[r,v] = verlet_JMS( @(r)myforce_JMS(r,l,C,D), m, r0, v0, dt, tmax );
t = 0:dt:tmax;
for it = 1:numel(t)
  ra(:,it) = r(:,1,it);
  rb(:,it) = r(:,2,it);
  la(:,it) = m(1)*cross(r(:,1,it),v(:,1,it));
  lb(:,it) = m(2)*cross(r(:,2,it),v(:,2,it));
end
rcm = (m(1)*ra + m(2)*rb) / (m(1)+m(2));     % trajectory of center of mass (CM)
l = la+lb;                                   % total angular momentum

% Plot center of mass trajectory
figure(11)
plot3(rcm(1,:),rcm(2,:),rcm(3,:),'LineWidth',1.5)
axis equal
xlabel('x_{cm} (m)')
ylabel('y_{cm} (m)')
zlabel('z_{cm} (m)')
grid on
set(gca,'FontSize',15)

% Plot particle distances to CM
figure(12)
plot(t,vecnorm(ra-rcm),t,vecnorm(rb-rcm),'LineWidth',1.5)
legend('a','b')
xlabel('time (s)')
ylabel('distance to CM (m)')
grid on
set(gca,'FontSize',15)

% Plot angular momentum
figure(13)
plot( t,la(1,:), t,lb(1,:), t,l(1,:), t,l(2,:), t,l(3,:), 'LineWidth',1.5)
legend('L_x^a','L_x^b','L_x','L_y','L_z','Location','SouthEast')
xlabel('time (s)')
ylabel('angular momentum (kg*m^2/s)')
grid on
set(gca,'FontSize',15)

end % function
