function [root,B]=broyden1(func,x0,B,varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Usage: [root,B]=broyden(func,x0,B,varargin)
% Solve f(x)=0 with Broydens method
%
% Inputs:  func     - name of function which return the residuals
%          x0       - initial estimated solution 
%          B        - estimate Jacobi-matrix
%          varargin - parameters used by the function 'func'
%
% Outputs: root     - solution vector (x)
%          B        - updated estimated Jacobi-matrix
%
% Written by   : John C. Morud               ??
% Last modified: Marius S. Govatsmark        10.jun.2000
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Connect matrixes and variables
delta=1e-4;        %Pertubation step - numerical differentials
nx=max(size(x0));  %Number of algebraic variables
x=x0;              %Initial solution vector - broyden
iter=0;            %Counter - number of iterations

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Calculate norm to residual for the initial solution vector
f=feval(func,x,varargin{:});
error=norm(f);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Calculate Jacobi-matrixes 
if nargin==2 | isempty(B)
   for i=1:nx
      p=0*x; p(i)=delta;
 ...
 J(:,i)=(feval(func,x+p,varargin{:})-feval(func,x-p,varargin{:}))/(2*delta);
      %disp(sprintf('Broyden.m - Jacobi iter=%d',i))    
   end
%Jacobi matrix structure
   %J 
   %global J, disp('Start'), J, size(J), spy(J), rank(J); x0
   %disp('Start'), J, size(J), spy(J), rank(J); x0
   B=inv(J);
end %if

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%MAIN LOOP
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while error>1e-7       

   iter=iter+1;
   dx=-B*f;
    xnew=x+dx;
   fnew=feval(func,xnew,varargin{:});
 
   count=0;
   while norm(fnew)>error |~isreal(fnew) %Backtrack if norm(f) not
                                        %decreasing or x isn't real.
      if ~isreal(fnew)
         disp('Backtrack because of not real fnew');
      end  
      count=count+1;
      if count>100; break;end;
      dx=dx/2;
      if count==4    %If not success on 4th backtrack, reevaluate Jacobian
         for i=1:nx
            p=0*x; p(i)=delta;
            J(:,i)=(feval(func,x+p,varargin{:})-feval(func,x-p,varargin{:}))/(2*delta);
	 end 
         if rank(J)<max(size(x))
	    for i=1:length(x)
	      x(i)
	    end  
	    J
            rank(J), spy(J); pause  %Check if have lost rank
	  end 
         B=inv(J);
         dx=-B*f;
      end %if
      xnew=x+dx;
      fnew=feval(func,xnew,varargin{:});
   end %while

   df=fnew-f;
   temp=B*df;
   %norm(df), norm(dx)
   B=B+(dx-temp)*((dx'*B)/(dx'*temp));
   x=xnew;
   f=fnew;
   error=norm(f);
  
   if iter>100
     'IKKE KONVERGENS I BROYDEN'
      break
   end
 
end %while

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
root=x;





