clear all
%==========
% Ce code permet le calcul des valeurs propres du système dynamique pour
% étudier la stabilité mais aussi le contrôle de la stabilité par deux
% méthodes: le contrôle exacte et celui par l'opposé du signe de la vitesse
% de tangage
%==========
% Données
roe=1000;% masse volumique de l'eau
df=2;%longueur du foil
ds=2; %longueur du safran
Sf=.2;% surace du foil opposé à l'écoulement
Ss=.1;% surface du safran 
%
L=1;%Longueur servant a l'adimensionnement des coefficients aero
h=5;% distance entre le foil et le safran
raddeg=pi/180;% coefficient pour transformer des degrés en radians
m=1000;% poids du bateau
J0=10000;% inertie du bateau en tangage autour de O
a=.2;% distance du centre de gravité au point O (a>=> G en arriere de O
%============
npv=300;% nombre de points de calcul pour la vitesse
umin=0;% vitesse minimale
umax=10;% vitesse maximale
icomp=complex(0,1);
du=(umax-umin)/npv;% pas en vitesse
mur1=linspace(umin,umax,npv);% stockage des parties réelles des valeurs propres
mur2=linspace(umin,umax,npv);% stockage des parties réelles des valeurs propres
mur3=linspace(umin,umax,npv);% stockage des parties réelles des valeurs propres
mur4=linspace(umin,umax,npv);% stockage des parties réelles des valeurs propres
mui1=linspace(umin,umax,npv);% stockage des parties imaginaires des valeurs propres
mui2=linspace(umin,umax,npv);% stockage des parties imaginaires des valeurs propres
mui3=linspace(umin,umax,npv);% stockage des parties imaginaires des valeurs propres
mui4=linspace(umin,umax,npv);% stockage des parties imaginaires des valeurs propres
vit=linspace(umin,umax,npv);
%
M=[m,-a*m;-a*m,J0];%Matrice des inerties
M1=M^(-1);% Inverse de la matrice des inerties
%========
% anngles de braquage safran beta et foil alpha
%========
beta=raddeg*3; %braquage du safran
alpha=raddeg*4;% braquage du foil (initial)
%=======
 %Données contrôle
%=======
a0=1;b0=1;% coefficient dans le critere de contrôle
%=====
for i=1:npv% boucle en vitesse pour construire les matrice C et K et calculer leur valeurs propres
    u=umin+(i-1)*du; 
    c11=-(roe*u/2)*(Ss*dczsnaca(beta)+Sf*dczfnaca(alpha));
    c12=(roe*u/2)*(Ss*(2*ds*czsnaca(beta)+h*dczsnaca(beta))+Sf*(2*df*cos(alpha)*czfnaca(alpha)-df*sin(alpha)*dczsnaca(alpha)));
    c21=-(roe*L*u/2)*(Ss*dcmsnaca(beta)+Sf*dcmfnaca(alpha))+(roe*u/2)*(Ss*h*dczsnaca(beta)-Sf*df*sin(alpha)*dczfnaca(alpha));
    c22=(roe*L*u*Ss/2)*(2*ds*cmsnaca(beta)+h*dcmsnaca(beta))+(roe*Sf*df*u*L/2)*(-sin(alpha)*dcmfnaca(alpha)+2*cos(alpha)*cmfnaca(alpha));
    c22=c22-(roe*Ss*h*u/2)*(2*ds*czsnaca(beta)+h*dczsnaca(beta))+(u*Sf*df^2/2)*(sin(2*alpha)*czfnaca(alpha)-sin(alpha)^2*dczfnaca(alpha));
    k11=100000;% raideur artificielle correspondant au fait que la voilure est flexible et que les foils ne peuvent (doivent) pas sortir de l'eau
    k12=-(roe*u^2/2)*(Ss*dczsnaca(beta)+Sf*dczfnaca(alpha));
    k21=0;
    k22=-(roe*L*u^2/2)*(Ss*dcmsnaca(beta)+Sf*dcmfnaca(alpha))-(roe*u^2/2)*(Sf*df*sin(alpha)*dczfnaca(alpha)-Ss*h*dczsnaca(beta));
    k22=k22-((roe*u^2)/2)*(Sf*df*cos(alpha)*czfnaca(alpha)+Ss*ds*czsnaca(beta));
    C=[c11,c12;c21,c22];%C=-[1,.2;.2,1]*0;
    %
    K=[k11,k12;k21,k22];%K=[1,0;.0,2];
    %
    M2=[eye(2),zeros(2);zeros(2),M];
    K2=[zeros(2),eye(2);K,-icomp*C];
    %===========
    %Matrice du système dynamique
    %===========
    A2=M2^(-1)*K2;
    %===========
    %calcul des valeurs propres de A2
    %===========
    %vp=linspace(1,4,4)*0;
    vp=eig(A2);
    murv=real(vp)/(2*pi);
    muiv=imag(vp);
    mur1(i)=max(max(abs(murv(1)),abs(murv(2))),max(abs(murv(3)),abs(murv(4))));
    mur2(i)=min(min(abs(murv(1)),abs(murv(1))),min(abs(murv(3)),abs(murv(4))));
    mui1(i)=muiv(3);mui2(i)=muiv(2);
    mur3(i)=-mur1(i);mur4(i)=-mur2(i);
    mui3(i)=muiv(3);mui4(i)=muiv(4);
end

%=============

figure
 subplot(2,1,1)
plot(vit,mur1,'k','LineWidth',2)
hold on
plot(vit,mur2,'k','LineWidth',2)
hold on
plot(vit,mur3,'k','LineWidth',2)
hold on
plot(vit,mur4,'k','LineWidth',2)
xlabel('Velocity of the boat m/s')
ylabel('Frequencies')
title('Frequencies')
subplot(2,1,2)
plot(vit,mui1,'k','LineWidth',2)
hold on
plot(vit,mui2,'k','LineWidth',2)
hold on
plot(vit,mui3,'k','LineWidth',2)
hold on
plot(vit,mui4,'k','LineWidth',2)
xlabel('Velocity of the boat m/s')
ylabel(' Damping')
title('Damping')
%======

% Resolution dynamique
%===========
T=12.;%temps sur lequel on calcule le controle
npt=4000;% nombre de pas de temps
dt=T/npt;%pas de temps
temps=(linspace(1,npt,npt)'-1)*dt;
z=linspace(1,npt,npt)'*0;
dz=linspace(1,npt,npt)'*0;
gamma=linspace(1,npt,npt)'*0;
dgamma=linspace(1,npt,npt)'*0;
%=======
X=[z,gamma];
Xdot=linspace(1,npt,npt)'*0;
delta=linspace(1,npt,npt)*0;
deltadot=linspace(1,npt,npt)*0;
%==============
% conditions initiales
%==============
z0=0.;dz0=.2;gamma0=.0*raddeg;dgamma0=10*raddeg;
X(1,1)=z0;
X(1,2)=gamma0;
X(2,1)=z0+dt*dz0;
X(2,2)=gamma0+dt*dgamma0;
%============
%============
CM=M1*C;
KM=M1*K;
%vppk=eig(KM);
E=[(roe*u/2)*(Sf)*(2*df*cos(alpha)*czfnaca(alpha)-df*sin(alpha)*dczsnaca(alpha))+.01,(roe*Sf*df*u*L/2)*(-sin(alpha)*dcmfnaca(alpha)+2*cos(alpha)*cmfnaca(alpha))+.01];
Ec=M1*E';
xx=(roe*L*u^2/2)*(Sf*dcmfnaca(alpha))-(roe*u^2/2)*(Sf*df*sin(alpha)*dczfnaca(alpha));
    xx=xx+((roe*u^2)/2)*(Sf*df*cos(alpha)*czfnaca(alpha));
B=[(roe*u^2/2)*(Sf*dczfnaca(alpha))+.1,xx+.1];
Bc=M1*B';
%==========
%=======
% Calcul du contrôle exact
%=======


lsec=linspace(1,4,1)*0;
Sol=linspace(1,4,1)*0;
Gramm=zeros(4);
%P1=zeros(2,npt);
DP1=zeros(2,npt);
%Q1=zeros(2,npt);
DQ1=zeros(2,npt);
aux=linspace(1,npt,npt)*0;
CMt=CM';
KMt=KM';
%===========
coef=sqrt(a0*b0);
r=sqrt(a0/b0);
%============
%=======
for i1=1:4
   Phi=linspace(1,4,4)*0;
    Phi(i1)=1;
    
    P1=resoladj(npt,dt,CMt,KMt,Phi);
    for ikl=1:2
    DP1(ikl,1)=0;
    for ider=2:npt
        DP1(ikl,ider)=(P1(ikl,ider)-P1(ikl,ider-1))/dt;
    end
    end
    for ip0=1:npt
    sum1=0;
        for ip=2:ip0 
            f=Ec(1)*DP1(1,ip)-Bc(1)*P1(1,ip)+Ec(2)*DP1(2,ip)-Bc(2)*P1(2,ip);
        sum1=sum1+dt*f*sinh(r*(ip0-ip)*dt);
        end
        aux(ip0)=sum1;
    end
    for it=1:npt
    delta(it)=((sinh(r*dt*it)/sinh(r*dt*npt))*aux(npt)-aux(it))/coef;
    end
    
    %========
        for i2=1:4
        deltaPhi=linspace(1,4,4)*0;
        
        deltaPhi(i2)=1;
                    
        Q1=resoladj(npt,dt,CMt,KMt,deltaPhi);
        for ikl=1:2
        DQ1(ikl,1)=0;
        for ider=2:npt
            DQ1(ikl,ider)=(Q1(ikl,ider)-Q1(ikl,ider-1))/dt;
        end
        end
        
        sum2=0;
        for kt=1:npt
           g=Ec(1)*DQ1(1,kt)-Bc(1)*Q1(1,kt)+Ec(2)*DQ1(2,kt)-Bc(2)*Q1(2,kt);
           sum2=sum2+dt*delta(kt)*g;
        end
        Gramm(i1,i2)=sum2;
        end
end
  vp=eig(Gramm) ;%valeurs propres de Gramm qui doivent etre postives
    lsec(1)=dz0-CM(1,1)*z0-CM(1,2)*gamma0;
    lsec(2)=dgamma0-CM(2,1)*z0-CM(2,2)*gamma0;
    lsec(3)=-z0;
    lsec(4)=-gamma0;
lasolution=Gramm\lsec';
%==============
P1=resoladj(npt,dt,CMt,KMt,lsec);
for i1=1:2
DP1(i1,1)=(P1(i1,2)-P1(i1,1))/dt;
    for it=2:npt

DP1(i1,it)=(P1(i1,it)-P1(i1,it-1))/dt;
    end
end


%=================
%=================
for ip0=1:npt
    sum1=0;
        for ip=1:ip0 
            f=Ec(1)*DP1(1,ip)-Bc(1)*P1(1,ip)+Ec(2)*DP1(2,ip)-Bc(2)*P1(2,ip);
        sum1=sum1+dt*f*sinh(r*(ip0-ip)*dt);
        end
        aux(ip0)=sum1;
end
deltamax=100*raddeg;
    for it=1:npt
    delta(it)=min(deltamax,max(-deltamax,((sinh(r*dt*it)/sinh(r*dt*npt))*aux(npt)-aux(it))/coef))/r;
    end
 %========
 %========

for it=2:npt-1   
deltadot(it)=(delta(it+1)-delta(it-1))/(2*dt);
end
deltadot(1)=deltadot(2);
deltadot(npt)=deltadot(npt-1);
    %=========
    %==========

for ii=3:npt
    for jj=1:2
        sum=0;
        for k=1:2
            sum=sum-dt*CM(jj,k)*(X(ii-1,k)-X(ii-2,k))+dt^2*KM(jj,k)*X(ii-1,k);
        end  
        
    X(ii,jj)=2*X(ii-1,jj)-X(ii-2,jj)+dt^2*(Bc(jj)*delta(ii-1)+Ec(jj)*deltadot(ii-1))-sum;
    z(ii)=X(ii,1);gamma(ii)=X(ii,2);
     end
   

end
%==========
        figure
    subplot(3,1,1)
plot(temps,1*z,'k','LineWidth',2)
xlabel('Time')
ylabel('Heaving')
title(['Heaving with exact control V(m/s)=',num2str(umax)])
subplot(3,1,2)
plot(temps,1*gamma,'k','LineWidth',2)
title(['Pitching with exact control V(m/s)=',num2str(umax)])
xlabel('Time')
ylabel('Pitching')
subplot(3,1,3)
plot(temps,1*delta,'k','LineWidth',2)
xlabel('Time')
ylabel('Control')
title(['Exact Control V(m/s)=',num2str(umax)])
%=====
%=====
%   methode otusa
%======
% Resolution dynamique
%===========
temps=(linspace(1,npt,npt)'-1)*dt;
z=linspace(1,npt,npt)'*0;
dz=linspace(1,npt,npt)'*0;
gamma=linspace(1,npt,npt)'*0;
dgamma=linspace(1,npt,npt)'*0;
%=======
X=[z,gamma];
Xdot=linspace(1,npt,npt)'*0;
delta=linspace(1,npt,npt)*0;
deltadot=linspace(1,npt,npt)*0;
%==============
% conditions initiales
%==============
z0=0.;dz0=.2;gamma0=.0*raddeg;dgamma0=10*raddeg;
deltamax=2*raddeg;
X(1,1)=z0;
X(1,2)=gamma0;
X(2,1)=z0+dt*dz0;
X(2,2)=gamma0+dt*dgamma0;
%============
%============
CM=M1*C;
KM=M1*K;
vppk=eig(KM);
E=[C(1,2),C(2,2)];
Ec=M1*E';
B=[-K(1,2),-K(2,2)];
Bc=M1*B';
%==========
for ii=3:npt
    if (ii>= 500)
        deltamax=1.3*raddeg;
    end
    if (ii>= 1000)
        deltamax=.65*raddeg;
    end
    if (ii>= 2000)
        deltamax=.4*raddeg;
    end
    if(ii>=3000)
        deltamax=.1*raddeg;
    end
    for jj=1:2
        sum=0;
        for k=1:2
            sum=sum-dt*CM(jj,k)*(X(ii-1,k)-X(ii-2,k))+dt^2*KM(jj,k)*X(ii-1,k);
        end  
        
    X(ii,jj)=2*X(ii-1,jj)-X(ii-2,jj)+dt^2*Bc(jj)*deltadot(ii-1)-sum;
    if(jj==2)
        if(abs(X(ii,2)-X(ii-1,2))<= .000001)
            deltadot(ii)=0;
        else
        Xdot(ii)=sign(X(ii,2)-X(ii-1,2));
        deltadot(ii)=-deltamax*Xdot(ii);
        end
    end
    end
   
end
%===========
figure
for k=1:npt
    z(k)=X(k,1);
    gamma(k)=X(k,2);
end
subplot(3,1,1)
plot(temps,z,'k','LineWidth',2)
xlabel ('Time')
ylabel('Heaving')
title(['Heaving V(m/s)=',num2str(umax)])
subplot(3,1,2)
plot(temps,gamma,'k','LineWidth',2)
title (['Pitching V(m/s)=',num2str(umax)])
xlabel('Time')
ylabel('Pitching')
subplot(3,1,3)
plot(temps,deltadot,'k','LineWidth',2)
xlabel('Time')
ylabel('Control')
title(['Control OTUSA type using the steering box V(m/s)=',num2str(umax)])