clear all;

% Define all needed functions here !

% helper function - G(x)
G    = @(x)( [ 10*(x(2)-x(1)^2);
              1-x(1);
	      3*sqrt(10)*(x(4)-x(3)^2);
	      1-x(3);
	      sqrt(10)*(x(2)+x(4)-2)
	      (x(2)-x(4))/sqrt(10)]);

% Jacobian of G
DXG  = @(x)( [ -20*x(1) 10 0 0 ;
	      -1       0  0 0;
	      0        0 3*sqrt(10)*[-2*x(3), 1];
	      0        0 -1 0;
	      0        sqrt(10)*[1 0 1];
	      0       [1 0 -1]/sqrt(10)]);

% Derivatives of the DG
DXX1G = [ -20 0 0 0; zeros(5,4)]; 
DXX2G = zeros(6,4);
DXX3G = [ zeros(2,4); 0 0 -6*sqrt(10) 0; zeros(3,4)]; 
DXX4G = zeros(6,4);

% Function g(x) and its Jacobian
g = @(x)(DXG(x)'*G(x));
Dg =@(x)([DXX1G'*G(x) ,DXX2G'*G(x), DXX3G'*G(x),  DXX4G'*G(x)]+ DXG(x)'*DXG(x) );

% Initial Guess
x0 = [-3 -1 -3 -1]';

% function h(x,a) goes into the predcorr
h   = @(x,a)( g(x) +(a-1)*(g(x0)) );
DXh = @(x,a)( Dg(x) );
DAh = @(x,a)( g(x0) );

% u - version
Uf  = @(u)( h( u(1:(length(u)-1)) , u( length(u)) ) );
DUf = @(u)( [DXh(u(1:(length(u)-1)) ,u(length(u))) g(x0)]) ; 

% initial point
x = x0
a = 0;
u = [x0;a];

% Max iteration and initial Delta
maxIter = 1000;
Delta = 0.1;

% Save iterates into these variables
U = [u];

% for the first step select v0 from the N(fx(x)).  
% use the following :  Q*R*x = 0 => R x = 0 , if x != 0
% note, that eig - normalizes all eigenvetors => |v| = 1

[Q,R] = qr( DUf(u));
[X,D] = eig( [R ; zeros(1,length(u))]);

I = find(abs(diag(D))<1e-10);
v = X(:,I(1));

while u(5) < 1;

  % take a Delta step  
  uold = u;
  u= u + Delta*v;
  
  % iterate with Newton and QR-dec.( use non-stationary Newton )
  u = gnewton(u, Uf, DUf);
 
  % construct new direction 
  v = (u-uold)/norm(u-uold);

  if( u(5) > 1+1e-8)
    u = uold;
    Delta = Delta/2;
  end
  
  U = [U u];
  
end



% Plot error etc.. figures

figure;
for i=1:1:size(U,2)
  error(i) = norm(g(U(1:4,i)));
end

semilogy(1:size(U,2),error,'r*');
