% C3_1.m
% Solve problem 1 of control 3. 2015/04/17
% J.M.Soler

% Set simulation parameters
clear all
q = 1.e-6;   % particle charge in C
m = 1.e-2;   % particle mass in kg
D1 = -2.e6;  % first potential constant in N/(C*m)
D2 =  3.e7;  % second potential constant in N/(C*m^2)
D3 = -1.e8;  % third potential constant in N/(C*m^3)
x0 = 2.e-1;  % initial position in m
v0 = 0.71;   % initial velocity in m/s
tf = 3.0;    % final time in s
dt = 0.002;  % time step in s

% Set force and energy functions
myForce = @(x)(q*(D1*x + D2*x.^2 + D3*x.^3));
myEpot = @(x)(-q*(D1*x.^2/2 + D2*x.^3/3 + D3*x.^4/4));

% Integrate trajectory
[x,v,a] = verlet( myForce, m, x0, v0, dt, tf );
x=squeeze(x); v=squeeze(v); a=squeeze(a); 
t = (0:dt:tf);

% Plot trajectory
figure(1)
subplot(3,1,1)
    plot(t,x)
    xlabel('t (s)')
    ylabel('x (m)')
subplot(3,1,2)
    plot(t,v)
    xlabel('t (s)')
    ylabel('v (m/s)')
subplot(3,1,3)
    plot(t,a)
    xlabel('t (s)')
    ylabel('a (m/s^2)')

% Plot energies
Ek = 0.5*m*(v.^2);
Ep = myEpot(x);
figure(2)
plot( t,Ek, t,Ep, t,Ek+Ep )
legend('E_{kin}','E_{pot}','E_{tot}')
xlabel('t (s)')
ylabel('E (J)')

% Find turning points
xmin = min(x)
xmax = max(x)

% Find oscillation period
nt = numel(t);
it = find( x(2:nt).*x(1:nt-1)<0 & ...
           x(2:nt) -x(1:nt-1)>0);      % steps crossing x=0 with v>0
T = (it(2)-it(1))*dt                   % period between two such steps
