% Set Newton-Raphson convergence parameters
Mmax = 10;
restol = 1e-8;

% Initialize vector for ODE integration
f = zeros(6,1);
w = zeros(6,1);
res = zeros(6,1);
v(:,1) = [u0; v0; w0; x0; y0; z0];

% Start iterative loop
for n = 2:N+1,

% Begin sub-iteration loop
  m = 0; 
  curres = restol + 1; % Doing this forces at least one sub-iteration to occur
  w = v(:,n-1);
  while ((m < Mmax) & (curres > restol)),
    
% Calculate rhs
  [f, pf_pw] = part_pf(w, p);

% Calculate residual
  res = w - v(:,n-1) - dt*f;
  curres = norm(res);

% Calculate linearization of residual with respect to w
    pres_pw = eye(size(pf_pw)) - dt*pf_pw;

% Calculate dw from pres_dw*dw = -res
    dw = -pres_pw\res;
    w  = w + dw;

% Increment sub-iteration counter
    m = m + 1;

  end

  % Check on convergence and update states
  if (curres > restol),
    fprintf('Maximum number of Newton sub-iterations occurred\n');
  end

  v(:,n) = w;
  
end