% Control3_3.m
% Problem 3, Comuptación I, Control 3. 2017/04/21
% J.M.Soler

% Set parameters
clear all
R=0.85;     % trajectory radius of mass A (m)
mB=0.9;     % mass B (kg)
k=8.8;      % elastic constant between A and B (N/m)
g=9.8;      % gravitational acceleration (m/s^2)
dd=5e-2;    % bin size for histogram of A-B distances (m)

% Load data
load('Control3_datos.mat');

% Find energies
Ec=0.5*mB*sum(vB.^2);        % kinetic energy
Ep=mB*g*rB(2,:);             % gravitational potential energy

% Find external work of elastic force
nt=numel(tt);                % number of time steps
it=2:nt;                     % index for time intervals
fB=-k*(rB-rA);               % elastic force on B
fB=(fB(:,it-1)+fB(:,it))/2;  % mean force on each time interval
drB=rB(:,it)-rB(:,it-1);     % position increments
dW=sum(fB.*drB);             % work on B in each time interval
Wext=[0,cumsum(dW)];         % external work on B, as a function of time
DE=Ec+Ep-Wext;               % nnergy difference

% Plot energies
figure(1)
plot(tt,Ec,'-r',tt,Ep,'-k',tt,Wext,'-b',tt,DE,'-g');
axis tight;
xlabel('time(s)');
ylabel('Energy (J)');
legend('Kinetic','Potential','Spring work','\DeltaE');

% Find probability density of A-B distances
dAB=sqrt(sum((rA-rB).^2));   % A-B distances
db=[0:dd:1.8];               % bin edges
pd=histc(dAB,db)/nt/dd;      % prob. density

% Plot probability density of dAB
figure(2)
bar(db,pd,'histc');
xlabel('d_{AB} (m)');
ylabel('Prob(d_{AB}) (m^{-1})');

% Show probability that dAB<R
fprintf('Prob(dAB<R) = %f\n',sum(dAB<R)/nt);
