function [out,B,step1,tt] = secant_cont(func,x0,y,yold,B,da,mc,mi,varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Usage: [out,B,step1,tt] = secant_cont1(func,x0,y,yold,B,da,mc,mi,varargin)
% Secant continuation methods
% This is a "quick and dirty version, in which no updates are done, even
% when no convergence is obtained in the Broyden step
%
% Last constraint in 'func' is removed. 
% Remember: Update parameter/setpoint to residual function:
%           function DF=func(ux,var,ysp) 
%           p=var{1};
%           DF=[....;y-ysp]; 
%
% Inputs:  func     - name of function which return the residuals
%          x0       - initial solution vector
%          y        - 'free variable value' in last solution 
%          yold     - 'free variable value' in solution before last solution 
%          B        - estimate Jacobi-matrix
%          da       - (wanted) steplength
%          mc       - number of step 
%          mi       - maximum number of iterations (broyden) - default 100
%          varargin - parameters used by the function 'func'
%
% Outputs: out      - solution vectors [x1 x2 ...]
%          B        - updated estimate of Jacobi-matrix
%          step1    - step lengths (norm(x-x_old))
%          tt       - calculation times 
%
% Written by   : J.C.Morud(?), A.C.Christiansen, I.J.Halvorsen.
% Last modified: M.S.Govatsmark        12.jun.2000
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Connect constants 
delta   =da;       %Wanted step length
max_cont=mc;       %Number of step
max_iter=mi;       %Maximum iterations
nx=length(x0);     %Number of algebraic variables

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Initial matrixes
out=zeros(nx,max_cont);    %Solution vectors [x1 x2 ...]
tt=zeros(1,max_cont);      %Calculation time - each step
step1=zeros(1,max_cont);   %Actual step length - each step

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Solution 1 (x)
[x,B]=broyden(func,x0,B,varargin,y);
x0=x;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Extra solution - old
[x,B]=broyden(func,x0,B,varargin,yold);
x_old=x;
x=x0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for teller=1:max_cont, %Counter - stepnumber 

%Secant direction and (wanted) length   
   n = (x-x_old)/norm((x-x_old),2)*delta;  

%Predicted new solution   
   x_pred = x+n;
   x_old=x;       

%Calculate new solution - broyden method (n and x-x_old normal)   
   t=cputime;
   [x,B]=broyden_secant('broyden_func',x_pred,B,n,max_iter,varargin,func);
   tt(teller)=cputime-t;
   out(:,teller) = x;  
   step1(teller)=norm((x-x_old),2);
   disp(sprintf('Step number: %d Calculation time: %0.4f Step length: %0.4f',teller,tt(teller),step1(teller)));
   
end %for

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%% BROYDEN %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [root,B]=broyden(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   : ........
% Last modified: Marius S. Govatsmark        12.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
%   global J, size(J), spy(J), rank(J);
   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))
            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;
   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;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%% BROYDEN_SECANT %%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [root,B]=broyden_secant(func,x0,B,n,max_iter,varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Usage: function [root,B]=broyden_secant(func,x0,B,n,max_iter,varargin)
% Solve f(x)=0 with Broydens method
%
% Inputs:  func     - name of function which return the residuals
%          x0       - initial estimated solution / predicted solution
%          B        - estimate Jacobi-matrix
%          n        - estimate Jacobi-matrix
%          max_iter - maximum number of iterations (broyden)
%          varargin - parameters used by the function 'func'
%
% Outputs: root     - solution vector (x)
%          B        - updated estimated Jacobi-matrix
%
% Written by   : ........
% Last modified: Marius S. Govatsmark        12.jun.2000
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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


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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Calculate Jacobi-matrixes 
%if teller==1 
if isempty(B)
   for i=1:nx
      p=0*x; p(i)=delta;
%      disp(sprintf('Broyden_secant.m - Jacobi iter=%d',i));
      J(:,i)=(feval(func,x+p,x_pred,n,varargin)-feval(func,x-p,x_pred,n,varargin))/(2*delta); %dydx=(y(x+h)-y(x-h))/(2*h)
   end  
   B=inv(J);
end %if


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

   iter=iter+1;
   dx=-B*f;
   xnew=x+dx;
   fnew=feval(func,xnew,x_pred,n,varargin);

   count=0;
   while norm(fnew)>error  %Backtrack if norm(f) not decreasing
      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,x_pred,n,varargin)-feval(func,x-p,x_pred,n,varargin))/(2*delta)';
         end
         B=inv(J);
         dx=-B*f;
      end %if
      xnew=x+dx;
      fnew=feval(func,xnew,x_pred,n,varargin);
      disp(sprintf('Backtrack: max(abs(dx))=%0.4e norm(fnew)=%0.4e count=%d', max(abs(dx)),norm(fnew),count));       
   end %while

   df=fnew-f;
   temp=B*df;
   B=B+(dx-temp)*((dx'*B)/(dx'*temp)); %Update estimate of Jacobian matrix
   x=xnew;
   f=fnew;
   error=norm(f);

   if iter>max_iter
     'IKKE KONVERGENS I BROYDEN'
      break
   end

end %while


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



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%% BROYDEN_FUNC %%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function res=broyden_func(x,x_pred,n,varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Usage: function res=broyden_func(x,x_pred,n,varargin)
% Calculate residuals to broyden_secant
%
% Inputs:  func     - name of function which return the residuals
%          x        - estimated solution from broyden_secant
%          x_pred   - predicted solution - secant direction and steplength
%          n        - estimate Jacobi-matrix
%          varargin - parameters used by the function 'func'
%
% Outputs: res      - residual vector connected to solution vector (x)
%
% Written by   : ........
% Last modified: Marius S. Govatsmark        12.jun.2000
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Residual function used od secant- continuation methods
%msg20000612

func=varargin{1}{2};
varargin=varargin{1}{1};

%Residual from function evaluation
res=feval(func,x,varargin,0);

%Replace last constraint from function evaluation with orthogonal constraint
res(size(res,1))=n'*(x_pred-x); 











