% calcul du systeme controle
%=========
clear all
m=1.;
J0=1;
k=10;
c=2;
a=-.02;
V=3.2;
z0=.0;
dz0=.0;
alpha0=.1;
dalpha0=1;
beta0=.05;
ro=1.2;
S=1.;
L=1.;
a0=1;
b0=10;
r=sqrt(a0/b0);
NPT=6000;
T=3;
dt=T/NPT;
bL=1;
temp=linspace(1,T+1,NPT)-1;
temp2=linspace(1,2*T+1,2*NPT)-1;
z=linspace(1,2,NPT)*0.;
alpha=linspace(1,2,NPT)*0.;
z2=linspace(1,2,2*NPT)*0.;
alpha2=linspace(1,2,2*NPT)*0.;
%=========
M=[m, a*cos(alpha0);a*cos(alpha0),J0];
K=[k ,-ro*S*V^2*dcz0(alpha0)/2;0., c-ro*S*V^2*L*dcm0(alpha0)/2];
R=linsolve(M,K);
Rad=linsolve(M,K');
%

F0=[ro*S*V^2*cz0(alpha0)/2,ro*S*L*V^2*cm0(alpha0)/2];
B0=[ro*S*V^2*dczg(beta0)/2,ro*S*L*V^2*dcmg(beta0)/2];
%B0=[0,1];
C0=-[bL*ro*S*V*(dczg(beta0)+cxg(beta0))/2,bL*ro*S*L*V*(2*cmg(beta0)+dcmg(beta0))/2];
%C0=[1,1];
xeq=linsolve(K,F0');
C=linsolve(M,C0');
F=linsolve(M,F0');
B=linsolve(M,B0');
beta=linspace(1,2,NPT)*0;
betadot=linspace(1,2,NPT)*0;
P1dot=linspace(1,2,NPT)*0.;
P2dot=linspace(1,2,NPT)*0.;
%==============
% critere de controlabilite
%==============
%base duale
%=====
bipB=norm(B0)^2;
bipC=norm(C0)^2;
bipBC=B0*C0';
delta=bipB*bipC-bipBC;
matin=[bipC,-bipBC;-bipBC,bipC]/delta;
semB=[3 , 1];
semC=[1  ,1];
Bstar=matin*semB';
Cstar=matin*semC';
f1=B0*(Rad*Bstar);
f2=B0*(Rad*Cstar);
f3=C0*(Rad*Bstar);
f4=C0*(Rad*Cstar);
root1=(-f3+sqrt(f3^2-4*f4))/2;
root2=(-f3-sqrt(f3^2-4*f4))/2;
%
rav1=root1^3+f1*root1+f2;
rav2=root2^3+f1*root2+f2;
%==========
Avis='si le nombre ci-dessous est nul il n y a pas controlabilite'
test=abs(rav1*rav2/f2)
%==========
condinit= M*[z0;alpha0];
xeqM=M*xeq;
vitesseinit=M*[dz0;dalpha0]; 
z0M=condinit(1);
alpha0M=condinit(2);
dz0M=vitesseinit(1);
dalpha0M=vitesseinit(2);
%===============
PHIHUM=calculgramp(NPT,T,dt,z0M,dz0M,alpha0M,dalpha0M,xeqM,Rad,B0,C0,F0,a0,b0);
[P1,P2]=calculetatadj(NPT,dt,PHIHUM,Rad);
%===============
P1dot(1)=PHIHUM(2);
P2dot(1)=PHIHUM(4);
%===============
for ijk=2:NPT
    P1dot(ijk)=(P1(ijk)-P1(ijk-1))/dt;
    P2dot(ijk)=(P2(ijk)-P2(ijk-1))/dt;
end 
%==============
cc=sinh(r*T);
intj=0;
for jj=1:NPT
    intj=intj+(C0(1)*P1dot(jj)+C0(2)*P2dot(jj)-B0(1)*P1(jj)-B0(2)*P2(jj))*sinh(r*(NPT-jj+1)*dt);
end
intj=intj*dt;
for kl=1:NPT
rich=0.;
for jj=1:kl
   rich=rich+(C0(1)*P1dot(jj)+C0(2)*P2dot(jj)-B0(1)*P1(jj)-B0(2)*P2(jj))*sinh(r*(kl-jj+1)*dt)*dt;
end
   beta(kl)=(1/(b0*r))*((sinh(r*dt*(kl-1))/cc)*intj-rich);
    
end
beta(1)=0;
%beta(NPT)=0;
for jj=2:NPT-1
     betadot(jj)=(beta(jj)-beta(jj-1))/dt;
end
betadot(1)=betadot(2);
betadot(NPT)=betadot(NPT-1);
%=======
figure
subplot(1,3,1)
plot(temp,beta,'LineWidth',2)
xlabel('Temps')
ylabel('Loi de controle exacte');
title('controle exact')
%pause
z(1)=z0;
z(2)=z0(1)+dt*dz0(1);
alpha(1)=alpha0;
alpha(2)=alpha(1)+dt*dalpha0;
X1=[z(1),alpha(1)]';
X2=[z(2),alpha(2)]';
%=============
% resolution du systeme edo
%===========

for i=3:NPT
X3=2*X2-X1+dt^2*(F+B*beta(i-1)+C*betadot(i-1)-R*X2);
z(i)=X3(1);
alpha(i)=X3(2);
X1=X2;
X2=X3;
end

%=======
subplot(1,3,2)
plot(temp,z,temp,alpha,'LineWidth',2)
xlabel('Temps');
ylabel('Solution en z (bleue) et alpha (rouge)')
title('Solution contrôlée ')

for i=1:NPT
    z2(i)=z(i);
    alpha2(i)=alpha(i);
X3=2*X2-X1+dt^2*(F-R*X2);
z2(i+NPT)=X3(1);
alpha2(i+NPT)=X3(2);
X1=X2;
X2=X3;
end
subplot(1,3,3)
plot(temp2,z2,temp2,alpha2,'LineWidth',2)
xlabel('Temps')
ylabel('Solution avec coupure du contrôle a T')
title('Solution prolongée après que le contrôle soit coupé')