%% ---- Commande du controleur sur maquette (plaque alu) ----

% Recuperation des donnees
clc; close all; 
load('ABCtild.mat'); % contient 'Atild', 'Btild', 'Ctild','deltaT','n','nu'
R = eye(nu,nu);
Q = eye(n,n)*10;
Ae = [Atild,zeros(n,nu);
    eye(nu,nu)*deltaT,zeros(n-nu,n-nu),eye(nu,nu)];
Be = [Btild;
    zeros(nu,nu)];
Qi = eye(nu,nu)*1e-3;
Qe = [Q,zeros(n,nu);
    zeros(nu,n),Qi];
Re = R;
Ke = dlqr(Ae,Be,Qe,Re);
Ky = Ke(:,1:nu);
K = Ke(:,1:n);
Ki = Ke(:,n+(1:nu));


% --------- Envoi du controle a la maquette --------------

% Initialisation
try
    fclose(s);
catch
    warning('s ne peut pas être fermé');
end

s = serial('COM11');
set(s,'BaudRate',9600,'Parity','none')
set(s,'DataBits',8)
set(s,'StopBits',1)

fopen(s);
pause(2);

%periode d'echantillonnage
deltaT = 42;

%duree de la manip
TotalTime_min = 60;
TotalTime_sec = TotalTime_min*60;
coups = round(TotalTime_sec/deltaT);
T1 = zeros(1,coups);
T2 = zeros(1,coups);
u1 = zeros(1,coups);
u2 = zeros(1,coups);

figure(1);
subplot(2,1,1);
axis([0 TotalTime_min 0 150]); 
ani1=animatedline('Color','r');
xlabel('temps (min)')
ylabel('température (°C)')
title('capteur 1') 
subplot(2,1,2);
axis([0 TotalTime_min 0 150]);
ani2=animatedline('Color','b');
xlabel('temps (min)')
ylabel('température (°C)')
title('capteur 2') 

figure(2);
subplot(2,1,1);
axis([0 TotalTime_min 0 255]); 
ani3=animatedline('Color','r');
xlabel('temps (min)')
ylabel('valeur de la commande')
title('U1') 
subplot(2,1,2);
axis([0 TotalTime_min 0 255]);
ani4=animatedline('Color','b');
xlabel('temps (min)')
ylabel('valeur de la commande')
title('U2')

x = 0:deltaT/60:TotalTime_min;
U1 = check_u(100);
U2 = check_u(100);
w = strcat('0',num2str(U1,'%03d'),num2str(U2,'%03d'));

% Experience 
TSTART = tic;
eTime = 0;
delta_u = 60;


%% Controle avec l'observateur (Simuler Xk+1chapeau avec Uk = -Ky y* + KI yI - K Xkchapeau)
R0 = eye(size(R));
Q0 = eye(size(Q))*1000;
Ko = (dlqr(Atild',Ctild',Q0,R0))';

X_est  = randn(n,1);
y =  [0;0]; %temp de la plaque - temp du point de fonctionnemeent
Tf = [127;127];
Ttarget = [80;80];
y_star = Ttarget - Tf;    % temperature ciblees - temp du point de fonctionnemeent
yi = [0;0];

u = Tf;

State_est = [X_est];
sortieY = [y];


for k = 1:coups
    while(eTime<k*deltaT)
        eTime = toc(TSTART);
    end
    
    if eTime>20*60 
            Ttarget = [55;60];
            y_star = Ttarget - Tf; 
    end
%     if eTime>40*60
%             Ttarget = [70;50];
%             y_star = Ttarget - Tf; 
%    end
    TempTarget(:,k)=Ttarget;
    % Determination du controleur U
    yi = yi + deltaT*(y-y_star);  
    X_est  = Atild*X_est + Btild*u + Ko*(y - Ctild * X_est);
    u = -K*X_est + Ky*y_star - Ki*yi;
    u(u>255)=255;
    
    % stockage:
    State_est = [State_est ,X_est]; 
    U(:,k)=u;

    % Ecriture de w sur aduino 
    U1 = check_u(u(1));
    U2 = check_u(u(2));
    w = strcat('0',num2str(U1,'%03d'),num2str(U2,'%03d')); 
    fwrite(s,w,'uint8');
    %Lecture de T1/T2 de l'arduino
    temp = fscanf(s,'%s', 9);
    T1(k) = str2num(temp(:,1:4))/10;
    T2(k) = str2num(temp(:,6:9))/10;
    y = [T1(k);T2(k)]-Tf;
    sortieY = [sortieY, y];
    
    % affichage dynamique (tt les deltaT)
    addpoints(ani1,x(k),T1(k));    
    addpoints(ani2,x(k),T2(k));
    drawnow
    addpoints(ani3,x(k),U1);
    addpoints(ani4,x(k),U2);
    drawnow
    
    disp(['U = ',num2str(U1), ' ; ', num2str(U2)]);
    disp(['T-Ttarget = ',num2str(T1(k)-Ttarget(1)), ' ; ', num2str(T2(k)-Ttarget(2))]);
end

fclose(s);
save('test3.mat','T1', 'T2', 'u1', 'u2','x');
% test1 :
% TempsTarget:  20 min à 100,100
%               20 min à 60,60
%               20 min à 70,50
