% Antti Hannukainen 
%
%
% Solve Invariant manifold of the McMillan-map
% 
%

clear all;

a = 1.4;
b = 0.3;

phi_p = @(x)([ 1+x(2)-a*x(1).^2;b*x(1)]);

p(1) = (-(1-b)+sqrt( (1-b)^2+4*a))/(2*a);
p(2) = b*p(1);

% Compute in new coordinates !

phi =  @(x)( phi_p(x+p')-p' );


% Diagonalize

Dphi = Dapprox(phi,[0 ;0]);
[V,D]=eig(Dphi);

% < 1 => Stable manifold
% > 1 => Unstable manifold 
% Both manifolds are one dimensional

as = D(1,1);
au = D(2,2);

% Construct new system in Es,Eu - coordinates !

W = inv(V);

psiS = @(s,u)(W(1,:)*phi( V*[s;u]));
psiU = @(s,u)(W(2,:)*phi( V*[s;u]));

% Solve system

deg = 10;
N = 40;
u = 0.1*rand(1,N);
s = 0.1*rand(1,N);

% construct the B - Matrix ( Only once )
% And S - Matrix

for i=2:deg
  Bu(i-1,:) = as*u.^i - au^i*u.^i;
  Bs(i-1,:) = au*s.^i - as^i*s.^i;
  U(i-1,:) = u.^i;
  S(i-1,:) = s.^i;
end

Bu = Bu';
Bs = Bs';

[Qu,Ru] = qr(Bu);
[Qs,Rs] = qr(Bs);

% Iterate Gamma (G)

Gu = zeros(deg-1,1);
Gs = zeros(deg-1,1);

for i=1:200
  % Construct load !
  
  for k=1:N
    Fu(k,1) = psiS( Gu'*U(:,k),u(k) ) - Gu'*(psiU( Gu'*U(:,k),u(k))).^( (2:deg)');    
    Fs(k,1) = psiU( s(k),Gs'*S(:,k) ) - Gs'*(psiS(s(k),Gs'*S(:,k))).^( (2:deg)');
  end
  
  dGu = Ru\(-Qu'*Fu);
  dGs = Rs\(-Qs'*Fs);
  convu(i) = norm(dGu);
  convs(i) = norm(dGs);
  
  Gu = Gu + dGu;
  Gs = Gs + dGs;
  
end

% Plot the result !

x = -1:0.01:1;
M=8;

XX = repmat(x,M,1);

Y = repmat( (2:(M+1))',1,length(XX));

abu = Gu(1:M);
s = abu'*(XX.^Y);
abu = Gs(1:M);
u = abu'*(XX.^Y);

% To coords.

Xu = V*[x ;u];
Xs = V*[s ;x];

figure;
plot(Xu(1,:),Xu(2,:),'r-')
hold on;
plot(Xs(1,:),Xs(2,:),'g-');

%quiver(0,0,V(1,1),V(2,1));
%quiver(0,0,V(1,2),V(2,2));

set(gca,'Xlim',[-0.2 0.2]);
set(gca,'Ylim',[-0.2 0.2]);

while(1)
  
  [x,y,B] = ginput(1);
  
  if(B == 1)
  
    z(:,1) = [x;y];
  else
    break;
  end
  
  for i=2:N
    
    z(:,i) = phi(z(:,i-1));
    
  end
  for i=1:1:(N-1)
    plot(z(1,[i,i+1]),z(2,[i,i+1]),'k*--' );
    pause(0.1);
  end
  
  
end


