clear all
xi=-.02;
k=2;
y0=.4;
y1=.01;
T=10;
npt=20000;
dt=T/npt;
b=1;
y=linspace(1,npt,npt)*0;
ydot=linspace(1,npt,npt)*0.;
ysanscont=linspace(1,npt,npt)*0.;
yinter=linspace(1,npt,npt)*0.;
temps=linspace(1,T,npt)-1;
p=linspace(1,npt,npt)*0;
beta=linspace(1,npt,npt)*0.;
Phi=[0,0];
q=[0,0];
lambda=zeros(2);
%========
%  on calcule le controle exact lineaire
%=======
for i=1:2
    for k=1:2
    Phi(k)=0;
    end
    Phi(i)=1;
     sol1=resol(npt,Phi,xi,k,dt);
     
    for j=1:2
        for k=1:2
        q(k)=0;
        end
        q(j)=1;
       sol2=resol(npt,q,xi,k,dt);
        s=0;
        for ij=2:npt-1
            s=s+sol1(ij)*sol2(ij);
        end
        s=s+(sol1(1)*sol2(1)+sol1(npt)*sol2(npt))/2;
        s=s*dt*b^2;
        lambda(i,j)=s;
    end 
end
 %mlambda=inv(lambda);
    l(1)=y1+xi*y0;
    l(2)=-y0;
    qq=lambda\l';
   % calcul de p
        betamax=100;
        p(1)=qq(1);
        beta(1)=min(betamax,max(-betamax,-b*p(1)));
        p(2)=qq(1)+dt*qq(2);
        beta(2)=min(betamax,max(-betamax,-b*p(2)));
        %
         for ijk=3:npt
             p(ijk)=2*p(ijk-1)-p(ijk-2)+dt*xi*(p(ijk-1)-p(ijk-2))-k*dt^2*p(ijk-1);
             beta(ijk)=min(betamax,max(-betamax,-b*p(ijk)));
         end
         y(1)=y0; y(2)=y0+dt*y1;
         for ikj=3:npt
         y(ikj)=2*y(ikj-1)-y(ikj-2)-dt*xi*(y(ikj-1)-y(ikj-2))-k*dt^2*y(ikj-1)+dt^2*b*beta(ikj-1);
         end
         
         figure
         subplot(1,2,1)
         plot(temps,y,'k','LineWidth',2)
         xlabel('temps')
         ylabel('Solution de depart')
         title('Initialisation de l algorithme')
          subplot(1,2,2)
         plot(temps,beta,'k','LineWidth',2)
         xlabel('temps')
         ylabel('Controle de depart')
         title('Initialisation de l algorithme')
         %=======
         % application du contrôle linéaire au modèle non linéaire
         %=========
         ysanscont(1)=y(1);
         ysanscont(2)=y(2);
         ydot(1)=y1;
         ydot(2)=y1;
         for ikj=3:npt
         y(ikj)=2*y(ikj-1)-y(ikj-2)-dt*xi*(y(ikj-1)-y(ikj-2))-k*dt^2*y(ikj-1)+dt^2*b*beta(ikj-1)+dt^2*g(ydot(ikj-1));
         ydot(ikj)=(y(ikj)-y(ikj-1))/dt;
         ysanscont(ikj)=2*ysanscont(ikj-1)-ysanscont(ikj-2)-dt*xi*(ysanscont(ikj-1)-ysanscont(ikj-2))-k*dt^2*ysanscont(ikj-1)+dt^2*g((ysanscont(ikj-1)-ysanscont(ikj-2))/dt);
         end
         %=====
         figure
         subplot(1,2,1)
         plot(temps,y,'k','LineWidth',2)
         xlabel('temps')
         ylabel('Solution non lineaire avec controle lineaire')
         title('Solution non lineaire avec controle lineaire')
          subplot(1,2,2)
         plot(temps,ysanscont,'k','LineWidth',2)
         xlabel('temps')
         ylabel('Solution non lineaire sans controle')
         title('Solution non lineaire sans controle')
         %==========
         % Calcul du controle non lineaire
         % boucle d iteration sur n, on part de n=1 avec le controle
         % lineaire
         %========
        
         itermax=15;
         y(1)=y0;y(2)=y0+dt*y1;
         yinter(1)=y0; yinter(2)=y0+dt*y1;
         ydot(1)=y1;ydot(2)=y1;
         ydotinter(1)=y1;ydotinter(2)=y1;
         %====
         for iter=1:itermax
             p(1)=qq(1);
             beta(1)=-b*p(1);
             p(2)=qq(1)+dt*qq(2);
             beta(2)=-b*p(2);
            for ii=3:npt
             yddot=(ydot(ii-1)-ydot(ii-2))/dt;
             p(ii)=2*p(ii-1)-p(ii-2)+dt*(xi-dg(ydot(ii-1)))*(p(ii-1)-p(ii-2))-dt^2*(k+d2g(ydot(ii-1))*yddot)*p(ii-1);
             beta(ii)=-b*p(ii);
             yinter(ii)=2*yinter(ii-1)-yinter(ii-2)-dt*xi*(yinter(ii-1)-yinter(ii-2))-dt^2*(k*yinter(ii-1)-g(ydotinter(ii-1)))+b*beta(ii-1)*dt^2;
             ydotinter(ii)=(yinter(ii)-yinter(ii-1))/dt;
            end
         qqa=[1;0];
         q1=resolnl(npt,qqa,xi,k,ydotinter,dt);
         qqa=[0;1];
         q2=resolnl(npt,qqa,xi,k,ydotinter,dt);
         s1=0;s2=0;
            for ll=2:npt-1
             s1=s1+g(ydotinter(ll))*q1(ll);
             s2=s2+g(ydotinter(ll))*q2(ll);
            end
         %============
         s1=s1+(g(ydotinter(1))*q1(1)+g(ydotinter(npt))*q1(npt))/2;
         s2=s2+(g(ydotinter(1))*q2(1)+g(ydotinter(npt))*q2(npt))/2;
         s1=s1*dt;s2=s2*dt;
         l(1)=y1+xi*y0+s1;
         l(2)=-y0+s2;
         qqp=lambda\l';
         norm(iter)=abs(qqp'*qqp-qq'*qq);
            
         qq=qqp;
            for inew=3:npt
                y(inew)=2*y(inew-1)-y(inew-2)-xi*(y(inew-1)-y(inew-2))*dt+dt^2*(g(ydot(inew-1))+b*beta(inew-1)-k*y(inew-1));
                ydot(inew)=(y(inew)-y(inew-1))/dt;
            end
         end
         %   
         nbiter=linspace(1,itermax,itermax);
         figure
         subplot(2,2,1)
         plot(temps,y,'k','LineWidth',2)
         xlabel('temps')
         ylabel('Solution non lineaire avec controle non lineaire')
         title('Solution non lineaire avec controle non lineaire')
          subplot(2,2,2)
         plot(temps,beta,'k','LineWidth',2)
         xlabel('temps')
         ylabel('Controle non lineaire')
         title('Controle non lineaire')
         subplot(2,2,3)
         plot(nbiter,norm,'k','LineWidth',2)
         xlabel('Iterations')
         ylabel('Evolution de: norme (Pn+1)-norme(Pn)')
         title('Courbe de convergence de l algorithme')
         subplot(2,2,4)
         plot(y,ydot,'k','LineWidth',2)
         xlabel('y')
         ylabel('ydot')
         title('Visualisation dans le plan des phases')