function  [z, out_data, res_vec, varargout] = advanced_Newton_Raphson_solver(z, f_and_jac, xxdotu_z, ...
  model, params, g, v, stap, geop, trap, verbosity)
% -- NEWTON SOLVER --
% starting from initial guess z, iteratively look for solution z
% for which f(z)=0
% the solver is blind to the underlying physics contained in f_and_jac
% each step requires evaluation of f(z) and df_dz(z)
% OUTPUT: the parameter vector z solving f(x(z), xdot(z), u(z)) = 0
% INPUTS: * function handles f_and_jac, x_z, xdot_z, u_z provide the
%         parametrization for the case of interest

% parameters Newton solver
tau_init = 1; red_tau = 0.5; inc_tau = 1.2; tau_min = 1e-5; tau_max = 1e2; nr_iter = 100; res_tol = 1e-10;

sum_sq_res_acc = Inf; f_acc = []; df_dz_acc = []; z_dir = []; tau = [];

inewt = 1;
res_vec = zeros(1, nr_iter);
while inewt <= nr_iter
  % -----------EVALUATION f AND JAC
  [f, df_dz, stap, geop, trap, inner_jacs] = f_and_jac(z, stap, geop, trap, inewt-1);
  % -----------EVALUATION RESIDUE
  sum_sq_res = .5*sum(f.^2);
  f_eval = f;
  df_dz_eval = df_dz;
  res_vec(inewt) = sum_sq_res;
  if sum_sq_res < res_tol
    if verbosity > 0
      fprintf('State equations solved in %u iterations\n', inewt);
    end
    out_data = get_out_data(xxdotu_z, z, g, v, stap, geop, trap, model, params);
    res_vec = res_vec(res_vec ~= 0);
    % varargout containing the jacs of interest for optimization
    % routine
    if nargout == 4
      jacs.df_dz = df_dz;
      % jacs.df_dx = inner_jacs.df_dx;
      % jacs.df_dxdot = inner_jacs.df_dxdot;
      jacs.df_du = inner_jacs.df_du;
      varargout{1} = jacs;
    end
    return
  end
  acceptance_flag = accept_step(sum_sq_res_acc, f_acc, df_dz_acc, sum_sq_res, f_eval, df_dz_eval, z_dir, tau);
  if acceptance_flag == -1
    if verbosity > 2
      plot_iteration(xxdotu_z, z, g, v, model);
    end
    tau = tau_init;
    z_acc = z; sum_sq_res_acc = sum_sq_res;
    f_acc = f_eval; df_dz_acc = df_dz_eval;
    % -----------NEWTON DESCENT DIRECTION
    [L,U,P] = lu(df_dz);
    z_dir = -U\(L\(P*f));
  else
    res_vec(inewt) = sum_sq_res_acc;
    if acceptance_flag == 1
      factor_tau = red_tau;
    else
      factor_tau = inc_tau;
    end
    tau = factor_tau*tau;
    z = z_acc;
    if verbosity > 1
      fprintf('it %u: step rejected by Wolfe conditions; tau set to value %.4f\n', inewt, tau);
    end
    if tau < tau_min || tau > tau_max
      error('RAPTOR:IncreasingRes',...
        'tau (%4.2e) is lower than the minimum value (%4.2e), no solution found (increasing residue)', tau, tau_min);
    end
  end
  z = z + tau*z_dir;
  unphys_flag = z_validity_flag(xxdotu_z, z, g, v, model);
  while unphys_flag ~= -1
    if verbosity > 1
      fprintf('it %u: state not valid; tau reduced to value %.4f\n', inewt, red_tau*tau);
    end
    tau = red_tau*tau;
    z = z_acc + tau*z_dir;
    if tau < tau_min
      error('RAPTOR:UnphysicalState',...
        'tau (%4.2e) is lower than the minimum value (%4.2e), no solution found (unphysical state, flag %.2d)', tau, tau_min, unphys_flag);
    end
    unphys_flag = z_validity_flag(xxdotu_z, z, g, v, model);
  end
  inewt = inewt+1;
  % maximum nr of iterations?
  if inewt == nr_iter + 1
    error('RAPTOR:MaxIter',...
      'maximum number of iterations (%.2d) reached, no solution can be found', nr_iter);
  end
end
end

function flag = accept_step(sum_sq_res_acc, f_acc, df_dz_acc, sum_sq_res, f_eval, df_dz_eval, z_dir, tau) 
if isinf(sum_sq_res_acc)
  flag = -1;
else
  % Wolfe conditions parameters
  c1 = 1e-4; c2 = .9;

  % sufficient decrease 
  wolfe1 = sum_sq_res <= sum_sq_res_acc + c1 * tau * f_acc' * df_dz_acc * z_dir;
  % curvature 
  wolfe2 = f_eval' * df_dz_eval * z_dir >= c2 * f_acc' * df_dz_acc * z_dir;

  if ~wolfe1
    flag = 1;
  elseif ~wolfe2
    flag = 2;
  else
    flag = -1;
  end
end
end

function flag = z_validity_flag(xxdotu_z, z, g, v, model)
[x, ~, ~, ~, ~, ~] = xxdotu_z(z);
flag = check_state_validity(x, g, v, model);
end

function [] = plot_iteration(xxdotu_z, z, g, v, model)
[x, ~, xdot, ~, ~, ~] = xxdotu_z(z);
RAPTOR_plot_iteration(x, xdot, g, v, model)
pause(1)
end

function out_data = get_out_data(xxdotu_z, z, g, v, stap, geop, trap, model, params)
[x, ~, xdot, ~, u, ~] = xxdotu_z(z);
out_data = RAPTOR_out(x, g, v, xdot, u, 1, stap, geop, trap, model, params);
end