% A flexible transmission
% A not so succesful design
format compact

% Input your RST controller here
r=[1 -1];
s=0.02*[1 0];
t=[1 0 ];
t=t*sum(s)/sum(t);


fs=20;
h=1/fs;

fv = 0.01:0.01:10; 
wv=2*pi*fv;
b0 = [0 0 0 0.28261 0.50666];
a0 = [1 -1.41833 1.58939 -1.31608 0.88642];
b05 = [0 0 0 0.10276 0.18123];
a05 = [1 -1.99185 2.20265 -1.84083 0.89413];
b1 = [0 0 0 0.06408 0.10407];
a1 = [1 -2.09679 2.31962 -1.93353 0.87129];
P0 = tf(b0,a0,h);
P05 = tf(b05,a05,h);
P1 = tf(b1,a1,h);

frba0 = freqz(b0,a0,fv,fs);
frba05 = freqz(b05,a05,fv,fs);
frba1 = freqz(b1,a1,fv,fs);

figure(1)
plot(fv,20*log10(abs(frba0)))
hold on;grid on
plot(fv,20*log10(abs(frba05)),'r')
plot(fv,20*log10(abs(frba1)),'k')
title('The Process')
xlabel('Frequency Hz')
ylabel('dB')


Cfb = tf(s,r,h);
Cff = tf(t,r,h);

% Specification A and B
T=0:h:5;
y0=step(P0*Cff/(1+P0*Cfb),T);
y05=step(P05*Cff/(1+P05*Cfb),T);
y1=step(P1*Cff/(1+P1*Cfb),T);
figure(2)
plot(T,y0)
hold on;grid on
plot(T,y05,'r')
plot(T,y1,'k')
plot([0 5],[1.1 1.1],'k','Linewidth',2)
plot([1.15 5],[0.9 0.9],'k','Linewidth',2)
plot([1.15 5],[1.1 1.1],'k','Linewidth',2)

risetime0 = max(T(find(abs(y0-1)>0.1)))-0.15     % Compensation for 3*h
risetime05 = max(T(find(abs(y05-1)>0.1)))-0.15
risetime1= max(T(find(abs(y1-1)>0.1)))-0.15
risetimemax =  max([risetime0,risetime05,risetime1]);
PASSA =  max([risetime0,risetime05,risetime1])<=1

peak0 = max(y0)
peak05 = max(y05);
peak1 = max(y1)
peakmax = max([peak0 peak05 peak1]);
PASSB = (peakmax <= 1.1)
title('Step Response')

% Specification C
sysd0 = tf(conv(r,[1 0 0 0 0]),conv(a0,r)+conv(b0,s),h);
sysd05 = tf(conv(r,[1 0 0 0 0]),conv(a05,r)+conv(b05,s),h);
sysd1 = tf(conv(r,[1 0 0 0 0]),conv(a1,r)+conv(b1,s),h);
d0 = step(sysd0,T);
d05 = step(sysd05,T);
d1 = step(sysd1,T);
peakd0 = max(abs(d0));
peakd05 = max(abs(d05));
peakd1 = max(abs(d1));
timed0 = max(T(find(abs(d0)>0.1*peakd0)))
timed05 = max(T(find(abs(d05)>0.1*peakd05)))
timed1= max(T(find(abs(d1)>0.1*peakd1)))
timedmax = max([timed0,timed05,timed1]);
PASSC = timedmax <1.2
figure(3)
plot(T,d0,'b',T,d05,'r',T,d1,'k')
hold on;grid on
plot([1.2 max(T)],peakd0*[0.1 0.1],'b','Linewidth',2)
plot([1.2 max(T)],-peakd0*[0.1 0.1],'b','Linewidth',2)
plot([1.2 max(T)],peakd05*[0.1 0.1],'r','Linewidth',2)
plot([1.2 max(T)],-peakd05*[0.1 0.1],'r','Linewidth',2)
plot([1.2 max(T)],peakd1*[0.1 0.1],'k','Linewidth',2)
plot([1.2 max(T)],-peakd1*[0.1 0.1],'k','Linewidth',2)
title('1/A output disturance Response')


% Specification D
PASSD = abs(evalfr(Cfb,1))>1e5

% Specification E and F

Sens0  = 1/(1+P0*Cfb);
Sens05 = 1/(1+P05*Cfb);
Sens1  = 1/(1+P1*Cfb);

figure(4)
S0  = abs(squeeze(freqresp(Sens0,wv)));
S05 = abs(squeeze(freqresp(Sens05,wv)));
S1  = abs(squeeze(freqresp(Sens1,wv)));
plot(fv,20*log10(S0));
hold on;grid on
plot(fv,20*log10(S05),'r');
plot(fv,20*log10(S1),'k');
axis([0 10 -10 10])
plot([0 0.2 0.2 10],[0 0 6 6],'k','Linewidth',2)
title('Sensitivity function')

freqe0 = min(fv(find(S0>1)))
freqe05 = min(fv(find(S05>1)))
freqe1 = min(fv(find(S1>1)))
freqemin =  min([freqe0 freqe05 freqe1]);
PASSE = freqemin > 0.2

peake0 = 20*log10(max(S0))
peake05 = 20*log10(max(S05))
peake1 = 20*log10(max(S1))

peakemax = max([peake0 peake05 peake1]);
PASSF = peakemax < 6;

% Specification G

Margins0 = allmargin(P0*Cfb);
Margins05 = allmargin(P05*Cfb);
Margins1 = allmargin(P1*Cfb);
DM0 = min(Margins0.DelayMargin)
DM05 = min(Margins05.DelayMargin)
DM1 = min(Margins1.DelayMargin)

DMmin = min ([DM0 DM05 DM1]);
PASSG = DMmin>0.8

% Specification H

CSens0  = Cfb/(1+P0*Cfb);
CSens05 = Cfb/(1+P05*Cfb);
CSens1  = Cfb/(1+P1*Cfb);
CS0  = abs(squeeze(freqresp(CSens0,wv)));
CS05 = abs(squeeze(freqresp(CSens05,wv)));
CS1  = abs(squeeze(freqresp(CSens1,wv)));
figure(5)
plot(fv,20*log10(CS0))
hold on; grid on;
plot(fv,20*log10(CS05),'r')
plot(fv,20*log10(CS1),'k')
plot([8 10],[10 10],'k','Linewidth',2)
axis([0 10 -15 15])
title('CS')

ind8 = find(wv>2*pi*8);
peakh0 = 20*log10(max(CS0(ind8)))
peakh05 = 20*log10(max(CS05(ind8)))
peakh1 = 20*log10(max(CS1(ind8)))
peakhmax = max([peakh0 peakh05 peakh1]);
PASSH = peakhmax < 10


PASSvector =[PASSA PASSB PASSC PASSD PASSE PASSF PASSG PASSH]
% And lets check stability also !

Allstable = Margins0.Stable & Margins05.Stable & Margins1.Stable 

  Scorev = round(100*min(1,max(0,[2-risetimemax  (1.2-peakmax)/0.1  (1.4-timedmax)/0.2  PASSD (freqemin-0.1)/0.1 (12-peakemax)/6 (DMmin-0.4)/0.4 (20-peakhmax)/10]  )))
  SCORE = Allstable*mean(Scorev)
