% calcul du systeme controle
%=========
m=1.;
J0=1.;
k=1;
c=2;
a=-.1;
V=2;
z0=0;
dz0=0;
alpha0=1;
dalpha0=0.01;
beta0=.3;
ro=1.2;
S=1.;
L=0.2;
a0=1;
b0=1;
NPT=10;
T=3;
dt=T/NPT;
Nbtermes=3;
temp=linspace(1,T+1,NPT)-1;
betacontrole=linspace(1,2,NPT);
%=========
M=[m, a*cos(alpha0);a*cos(alpha0),J0];
K=[k ,-ro*S*V^2*dcz0(alpha0)/2;0 c-ro*S*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*czg(beta0)/2,ro*S*L*V^2*cmg(beta0)/2];
C0=[ro*S*V^2*dczg(beta0)/2,ro*S*L*V^2*dcmg(beta0)/2];
C=linsolve(M,C0');
F=linsolve(M,F0');
B=linsolve(M,B0');
beta=linspace(1,2,NPT)*0;
P1dot=linspace(1,2,NPT)*0.;
P2dot=linspace(1,2,NPT)*0.;
%===============
PHIHUM=calculgram(Nbtermes,NPT,T,dt,V,z0,dz0,alpha0,dalpha0,beta0);
[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 
step=1;
%===============
for jj=2:NPT-1
    sum=0;
    step=2;
    instant=jj;
    for in=1:Nbtermes
    coef=dt/(a0+b0*(in^2*pi^2/T^2));
     stock=0.;
        for jt=1:NPT
            sinjt=sin((jt-1)*in*pi*dt/T);
            stock=stock+(B(1)*P1(jt)+B(2)*P2(jt)-C(1)*P1dot(jt)-C(2)*P2dot(jt))*sinjt;
        end
        sinjj=sin(in*pi*(jj-1)*dt/T);
sum=sum+stock*coef*sinjj;
    end
betacontrole(jj)=(2/T)*sum;
end
%=======
figure
plot(temp,betacontrole)