%% 2 / Construction du modèle
% pour un controle optimal sur plaque chaude
% On en ressort les caractéristique du modèle : A, B C

%% i) Récupération des données Expérience 
clc;close all ;clear all ;
% ---- Données Sonia/Sophie -----
load('Data_sonia.mat'); % T1,T2,u1,u2
deltaT = 42;
Tmax = 5940;
t = 0:deltaT:Tmax-deltaT;
% ------------------------------

% ---- Données du prof --------
% data = load('Data_prof.mat');
% t = data.ToSave(:,1)';
% u1 = data.ToSave(:,2)';
% u2 = data.ToSave(:,3)';
% T1 = data.ToSave(:,4)';
% T2 = data.ToSave(:,5)';
% -------------------------------

% ---- Données Guillaume ------
% load('Data_guillaume.mat');
% u2 = THETA2(1:length(THETA3));
% u1 = THETA3 ;
% T1 = valeurs_temperature_droite;
% T2 = valeurs_temperature_gauche;
% t= temps;
% ------------------------------


%% ii) Conditionnement des données expérimentales
idx = find(u1 > u1(1));
U1 = u1(idx(1):end) - u1(1);
U2 = u2(idx(1):end) - u2(1);
Y1 = T1(idx(1):end) - T1(idx(1));
Y2 = T2(idx(1):end) - T2(idx(1));
t2 = t(idx(1):end)-t(idx(1));

figure, 
subplot(211),hold on;
plot(t2,Y1,'r'), plot(t2,U1,'--k'), 
title('Données expérimentales centrées'), 
xlabel('temps (min)'), ylabel('T1(°c)  /  u1'), legend('Température T1','commande u1','Location','southeast');
subplot(212),hold on;
plot(t2,Y2,'b'), plot(t2,U2,'--k'), 
xlabel('temps (min)'), ylabel('T2(°c)  /  u2'), legend('Température T2','commande u2','Location','southeast');


%% iii) Algo du modèle (ici option 2)

% 1 ----- Construire Matrice Yh1,Yh2 et Uh1,Uh2
i = 5;
j = length(U1) -2*i;

Uh1 = zeros(2*i , j);
Uh2 = zeros(2*i , j);
Yh1 = zeros(2*i , j);
Yh2 = zeros(2*i , j);
for k = 1 : i   
    Uh1(2*k-1,:)= U1(k:j-1+k);
    Uh2(2*k-1,:)= U1(k+i:j-1+k+i);
    Yh1(2*k-1,:)= Y1(k:j-1+k);
    Yh2(2*k-1,:)= Y1(k+i:j-1+k+i);

    Uh1(2*k,:)=U2(k:j-1+k);
    Uh2(2*k,:)= U2(k+i:j-1+k+i);
    Yh1(2*k,:)= Y2(k:j-1+k);
    Yh2(2*k,:)= Y2(k+i:j-1+k+i);
end


% 2 ----- Construire Matrice M1 - M2
M1 = zeros(2*i * 2  , j);
M2 = zeros(2*i * 2  , j);

M1(1:2*i,:)=Uh1;
M1(2*i+1:end, : )=Yh1;

M2(1:2*i,:)=Uh2;
M2(2*i+1:end, : )=Yh2;

% 3 ----- Décomposition en svd (single value decomposition) associé à M1 et M2
% Recherche des bases orthonormé de M1 et M2:
Q1 = orth(M1');
Q2 = orth(M2');
a=Q1'*Q1;
[Y,S,Z] = svd(Q1'*Q2); % Q1'*Q2 - Y*S*Z'

u = Q1*Y;
v = Q2*Z; % vecteur principaux 
v=v';  % v est transpose pour qu'il se presente comme dans le theoreme 2 (j colonnes)


%% ------- Détermination de A,B,C pour différent n (dimension de la variable d'état X)----------
% On choisira le plus petit donnant n donnant une erreur acceptable (entre Yest et Y obtenu)

% 4 ----- Détermination de n (dimension de la variable d'état X)
nn = 1:10;
for n = nn
    % 5 ---- Construire X = [Xi, ..., Xi+j-2]
        Xtild = v(1:n,:);
        Xk = Xtild(:,1:end-1);
        Xkp1 = Xtild(:,2:end);
        Uk = [ U1(i+(1:j-1));
            U2(i+(1:j-1)) ];
        R= [Xk;
            Uk ] ;
        Mg =Xkp1';
        Md=R';

        p = Md\Mg;
        MatAB = p' ; %Xkp1 * pinv(R);%Pv_R;  ;
        
    % 6 ---- Calcul de A,B,C par methode du moindre carré
        A = MatAB(:,1:n);
        B = MatAB(:,n+1:end);

        Y = zeros(2,j);
        Y(1,:) = Y1(i+(1:j));
        Y(2,:) = Y2(i+(1:j));
        
        Mg=Y'; % Transpose Y
        Md = Xtild';
        C = Md\Mg;
        C= C';
    
    %% iiii) Vérification du résultat obtenu
    
        % simulation
        X_est  = zeros(n,1);
        State = [X_est];
        for k = 1:length(Y1)-1
            X_est  = A*X_est + B*[U1(k);U2(k)];
            State = [State,X_est];
        end
        Y_est = C*State;
        % erreur sur chaque sorties Y1 et Y2
        err(n)= sum((Y1-Y_est(1,:)).^2) + sum((Y2-Y_est(2,:)).^2);
        
%         figure,
%         subplot(2,1,1); stairs(Y1,'r'); hold on;  stairs(Y_est(1,:),'k');
%         title(['Comparaison Y reel et estimé avec n= ',num2str(n)]);ylabel('Y1');legend('Y1','Y1 estime');
%         subplot(2,1,2); stairs(Y2,'r'); hold on, stairs(Y_est(2,:),'k');ylabel('Y2');legend('Y2','Y2 estime');
    
end

% Evolution de l'erreur en fonction de la valeur de n
figure, plot(nn,err,'x-'); title('erreur sur le modèle en fonction de n');
xlabel('n'); ylabel('erreur');


%% ------- Détermination des paramètre A,B,C du modèle (avec le n choisi) ----------
n=4;
Xtild = v(1:n,:);
Xk = Xtild(:,1:end-1);
Xkp1 = Xtild(:,2:end);
Uk = [ U1(i+(1:j-1));
    U2(i+(1:j-1)) ];
R= [Xk;
    Uk ] ;
Mg =Xkp1';
Md=R';
p = Md\Mg;
MatAB = p' ; %Xkp1 * pinv(R);%Pv_R;  ;

% paramètre A,B,C du modèle
A = MatAB(:,1:n);
B = MatAB(:,n+1:end);
Y = zeros(2,j);
Y(1,:) = Y1(i+(1:j));
Y(2,:) = Y2(i+(1:j));
Mg=Y'; % Transpose Y
Md = Xtild';
C = Md\Mg;
C= C';

% simulation (pour visualisation)
X_est  = zeros(n,1);
State = [X_est];
for k = 1:length(Y1)-1
    X_est  = A*X_est + B*[U1(k);U2(k)];
    State = [State,X_est];
end
Y_est = C*State;

figure,
subplot(2,1,1); stairs(Y1,'r'); hold on;  stairs(Y_est(1,:),'k');
title(['Comparaison Y reel et estimé avec n= ',num2str(n)]);ylabel('Y1');legend('Y1','Y1 estime');
subplot(2,1,2); stairs(Y2,'r'); hold on, stairs(Y_est(2,:),'k');ylabel('Y2');legend('Y2','Y2 estime');
      
% Sauvegarde des caractéristique du modèle
save('parametres_du_modele.mat', 'A','B','C','deltaT');


%% -------------- Transformation de A,B,C sous la forme canonique souhaité -------------
% s'effectue dans le script "FORME_CANONIQUE.m"

%% Simulation du modèle avec Atild, Btild, Ctild (forme canonique de A,B,C):
% simulation (pour visualisation)
load('ABCtild.mat');
X_est2  = zeros(n,1);
State = [X_est2];
for k = 1:length(Y1)-1
    X_est2  = Atild*X_est2 + Btild*[U1(k);U2(k)];
    State = [State,X_est2];
end
Y_est2 = Ctild*State;

figure,
subplot(2,1,1); stairs(Y1,'r'); hold on;  stairs(Y_est(1,:),'k'), hold on;  stairs(Y_est2(1,:),'c');
title(['Comparaison Y reel et estimé avec n= ',num2str(n)]);ylabel('Y1');legend('Y1','Y1 estime','Y1 estime tild');

subplot(2,1,2); stairs(Y2,'r'); hold on, stairs(Y_est(2,:),'k'); hold on, stairs(Y_est(2,:),'c');
ylabel('Y2');legend('Y2','Y2 estime','Y2 estime tild');



