clear all;
close all;
clc;
%% 1
load('SysIdenData.mat')
t = LogData.time;
y_act = LogData.signals(1).values(:,2);
y_actm = LogData.signals(1).values(:,1);
u_act = LogData.signals(2).values;
Ts = 0.75;

figure
subplot(2,1,1);
plot(t,y_act,'b');
hold on
plot(t,y_actm,'r');
hold off
grid on;
xlim([0 2000]);
ylim([0 4]);
title('Actual Output Signal');
xlabel('Time (sec)');
ylabel('Water Level (V)');
legend('Noise-Reduced Output', 'Measured Output');

subplot(2,1,2);
plot(t,u_act);
grid on;
xlim([0 2000]);
ylim([0 4]);
title('Actual Input Signal');
xlabel('Time (sec)');
ylabel('Pump Voltage (V)');
legend('Actual Input');
%%
u_offset = 2.05;
u = u_act - u_offset;

y_offset = mean(y_act((735/0.75):(750/0.75)));
y = y_act - y_offset;

y = y(800:1600);
u = u(800:1600);
t = t(800:1600) - t(800);

figure
subplot(2,1,1);
plot(t, y, 'r');
grid on;
xlim([0 1000]);
ylim([-1.5 1])
title('Actual Offset-Free, Truncated Output Signal');
xlabel('Time(sec)');
ylabel('Water Level (V)');
legend('Actual Output');

subplot(2,1,2);
plot(t, u);
grid on;
xlim([0 1000]);
ylim([-1.5 1])
title('Actual Offset-Free, Truncated Input Signal');
xlabel('Time(sec)');
ylabel('Pump Voltage (V)');
legend('Actual Input');

%%
phi = [];
Y = [];
for k=10 : round(length(y)/2)
    temp = [y(k-1) y(k-2) u(k-1) u(k-2)];
    phi = [phi;temp];
    Y = [Y;y(k)];
end

theta_h = inv(phi' * phi) * phi' * Y;
a1 = -theta_h(1);
a2 = -theta_h(2);
b1 = theta_h(3);
b2 = theta_h(4);

z = tf('z',Ts);
T = (b1*z + b2)/(z^2 + a1*z + a2);

G = [0 1; -a2 -a1];
H = [0; 1];
C = [b2 b1];
D = 0;
SS = ss(G,H,C,D,Ts);
%%
figure
subplot(2,1,1);
Y_sim1 = lsim(T,u(k+1:end));
plot(t(k+1:end),Y_sim1);
hold on
plot(t(k+1:end),y(k+1:end));
legend('Simulated Output','Actual Output');
title('Offset-Free Model Verification (2nd Half)');
xlabel('Time (sec)');
ylabel('Water Level (V)');
ylim([-2 2]);

subplot(2,1,2);
Y_sim2 = lsim(T,u);
plot(t,Y_sim2);
hold on;
plot(t,y);
legend('Simulated Output','Actual Output');
title('Offset-Free Model Verification (2nd Half)');
xlabel('Time (sec)');
ylabel('Water Level (V)');
ylim([-2 2]);

%% 2
eigenvals = eig(G);
G_obs = G';
H_obs = C';
C_obs = H';
D_obs = 0;

L_ndb_sp = acker(G_obs,H_obs,[0.92 0.92]);
sys_cl_ndb_sp = ss((G_obs - H_obs*L_ndb_sp),H_obs,C_obs,D_obs,Ts);
dcgain_ndb_sp = dcgain(sys_cl_ndb_sp);

K_db = (G_obs^2)*inv(obsv(G_obs,C_obs))*[0 1]';

%% 3
load('SFLogData');
treal = SFLogData.time;
yref = SFLogData.signals(1).values(:,1);
yreal = SFLogData.signals(1).values(:,2);
ureal = SFLogData.signals(2).values;


[Y_sim, t_sim, x_sim] = lsim(sys_cl_ndb_sp,(1/dcgain_ndb_sp) * yref);

figure;
subplot(2,1,1);
plot(treal,yreal,'b');
hold on
plot(treal,yref,'r');
hold on
plot(treal,Y_sim,'g');
grid on;
xlim([0 600]);
ylim([0 4]);
title('Output Feedback Control Results');
xlabel('Time (sec)');
ylabel('Water Level (V)');
legend('Actual', 'Reference','Simulated');

[Y_sim, t_sim, x_sim] = lsim(sys_cl_ndb_sp,(1/dcgain_ndb_sp) * (yref-y_offset));

u_sim = [];
for count = 1:length(treal)
    u_sim(count) = (1/dcgain_ndb_sp * (yref(count)-y_offset)) - (L_ndb_sp * x_sim(count,:)') + u_offset;
end

subplot(2,1,2);
plot(treal,ureal,'b');
hold on;
plot(treal,u_sim,'r');
grid on;
xlim([0 600]);
ylim([0 3]);
title('Control Input Signal');
xlabel('Time (sec)');
ylabel('Pump Voltage (V)');
legend('Actual Control Input', 'Simuated Control Input');
%%
Gc = Kp + (Ki*h)/(z-1);
Gcl = (Gc*T)/(1+Gc*T);

Gu = Gc/(1+Gc*T);
lsim(Gcl,yref);