%function problem_1_hw2
%
% Script to do simulation for the first problem of HW 2 for 
% ME 511, CSUF, Spring 2009
%
% By Nasser Abbasi
%

close all

%
% DATA INPUT. Modify as needed.
%
xo = 1;           %initial position (to the right of equilibrium)
g  = 9.8;
m  = 10;           %mass of object
k  = 4;            %spring stiffness
w  = sqrt(k/m);    %natural frequency
theta = 0;         %angle of inclination (actually not needed at all!)
t  = 0:0.5:10*pi/w; %time scale to run the simulation

x  = zeros(length(t),1);              %x(t)
current_speed = zeros(length(t),1);   %v(t)

normal_force = m*g*cos(theta);

%pick a static friction coeff smaller than k*x0/N to allow mass to move
mu_static    = 0.15*(k*xo/normal_force);

%pick a dynamic friction coeff smaller than the static (this is normal)
mu_dynamic   = 0.5*mu_static;

static_force = normal_force*mu_static;

% The following needed to find where the mass will stop
number_of_legs = floor(t(end)/(pi/w));
peaks = zeros(number_of_legs,1);
p     = mu_dynamic*g*cos(theta)/w^2;
oldx  = xo;
found = false;

%
% LOOP to first find where the mass will stop
%
stop_time = 0;
for n=1:number_of_legs
    time_of_peak = n*(pi/w);
    peaks(n)     = (xo-(2*n-1)*p)*cos(w*time_of_peak)+(-1)^(n+1)*p;    
        
    line([(n-1)*(pi/w) n*(pi/w)],...
        [(-1)^(n+1)*static_force/k (-1)^(n+1)*static_force/k],...
        'LineStyle','-','Color','k');
    line([n*(pi/w) n*(pi/w)],...
        [static_force/k -static_force/k],...
        'LineStyle','-','Color','k');   
    hold on;
    
    if abs(peaks(n))<static_force/k && ~found
       stop_time = time_of_peak;
       found     = true;       
    end
end
%line([0,t(end)],[0 0],'LineStyle','-','Color','r');
ymax=3*max(peaks);

%
% Now start the simulation
%
for i=1:length(t)    
    n = ceil(t(i)/(pi/w));    
    
    x(i) = (xo-(2*n-1)*p)*cos(w*t(i))+(-1)^(n+1)*p;    
    
    if t(i)>stop_time
        x(i) = oldx;
        current_speed(i) = 0;        
    else
       oldx = x(i);
       current_speed(i) = -w*(xo-(2*n-1))*p*sin(w*t(i));       
    end
    h = plot(t(1:i),x(1:i),'-b',t(1:i),current_speed(1:i),'--r');         
    ylim([-ymax ymax]);
    xlim([0 t(end)]);
    title(sprintf('t=%4.1f   x(t)=%4.1f   v(t)=%4.1f',t(i),x(i),...
        current_speed(i)));   
    xlabel('time (sec)'); ylabel('x(t) & v(t)');
    if i > 1
        delete(hp);
    end
    hp=plot(t(i),x(i),'o',...
                'MarkerEdgeColor','k',...
                'MarkerFaceColor','g',...
                'MarkerSize',8);         
    drawnow
    legend(h,'position','speed');    
            
end
