%% Special Controller C = P(1-I)+PI/s

s=tf('s');

gp = 1/(s+1)*exp(-s);


I_domain = 0:.02:1;
ms_con = 1.5;
K_solutions = [];
w_solutions = [];
K_init = 4;

for i = I_domain
    disp(i)
    K_solutions(end+1) = fsolve(@(K) ms_con-ms(gp,K*(1-i)+K*i/s),K_init)  
    ms(gp,K_solutions(end)*(1-i)+K_solutions(end)*i/s)
    w_solutions(end+1) = ms2(gp,K_solutions(end)*(1-i)+K_solutions(end)*i/s);
    K_init=0.9*K_solutions(end);
    figure(1)
    bode(1/(1+gp*(K_solutions(end)+i/s)))
    %pause
end

sol.ctrl_special.K = K_solutions;
sol.ctrl_special.I = I_domain;
sol.ctrl_special.w = w_solutions;


I = I_domain(1:length(K_solutions));
gc = K_solutions'.*(1-I') + K_solutions'.*I'./(1i.*w_solutions');
gp = 1./(1i.* w_solutions'+1).*exp(-1i.*w_solutions');
S = 1./(1+gp.*gc);
S=S

real_S = real(S);
imag_S = imag(S);

sol.ctrl_special.real_s = real_S;
sol.ctrl_special.imag_s = imag_S;
sol.ctrl_special.ms = sqrt(real_S.^2+imag_S.^2);

%% Normal Controller C = P+PI/s

s=tf('s');

gp = 1/(s+1)*exp(-s);


I_domain = 0:1:50;
ms_con = 1.7;
K_solutions = [];
w_solutions = [];
K_init = 0.;

for i = I_domain
    disp(i)
    K_solutions(end+1) = fsolve(@(K) ms_con-ms(gp,K+K*i/s),K_init)  
    ms(gp,K_solutions(end)+K_solutions(end)*i/s)
    w_solutions(end+1) = ms2(gp,K_solutions(end)+K_solutions(end)*i/s);
    K_init=0.9*K_solutions(end);
end

sol.ctrl.K = K_solutions;
sol.ctrl.I = I_domain;
sol.ctrl.w = w_solutions;


I = I_domain(1:length(K_solutions));
gc = K_solutions' + K_solutions'.*I'./(1i.*w_solutions');
gp = 1./(1i.*w_solutions'+1).*exp(-1i.*w_solutions');
S = 1./(1+gp.*gc);
S=S

real_S = real(S);
imag_S = imag(S);

sol.ctrl.real_s = real_S;
sol.ctrl.imag_s = imag_S;
sol.ctrl.ms = sqrt(real_S.^2+imag_S.^2);

%% Normal Controller C = P+I/s

s=tf('s');

gp = 1/(s+1)*exp(-s);


I_domain = 0:.02:.6;
ms_con = 1.3;
K_solutions = [];
w_solutions = [];
K_init = 0.4;

for i = I_domain
    disp(i)
    K_solutions(end+1) = fsolve(@(K) ms_con-ms(gp,K+i/s),K_init)  
    ms_sol = ms(gp,K_solutions(end)+i/s)
    w_solutions(end+1) = ms2(gp,K_solutions(end)+i/s);
    K_init=1.2*K_solutions(end);

end

sol.ctrl_std.K = K_solutions;
sol.ctrl_std.I = I_domain;
sol.ctrl_std.w = w_solutions;


I = I_domain(1:length(K_solutions));
gc = K_solutions' + I'./(1i.*w_solutions');
gp = 1./(1i.*w_solutions'+1).*exp(-1i.*w_solutions');
S = 1./(1+gp.*gc);
S=S

real_S = real(S);
imag_S = imag(S);

sol.ctrl_std.real_s = real_S;
sol.ctrl_std.imag_s = imag_S;
sol.ctrl_std.ms = sqrt(real_S.^2+imag_S.^2);

%% Nyquist
s=tf('s');
gp = 1/(s+1)*exp(-s);
tuning = [sol.ctrl_special.K' sol.ctrl_special.I'];
%tuning = [sol.ctrl_std.K' sol.ctrl_std.I'];

L=[];
hold on
for n = 1:4:length(tuning)
    p = tuning(n,1);
    i = tuning(n,2);
    
    nyquist(gp * (p*(1-i)+p*i/s)+1);
    
    w = sol.ctrl_special.w(n);
    
    
    gc_w = p*(1-i) + p*i/(1i*w);
    gp_w = 1/(1i*w+1)*exp(-1i*w);
    
    l_w = gc_w*gp_w+1;
    plot(real(l_w),imag(l_w),'o')
    axis([0 1 -1 0])
end
hold off
%% Plots

figure(1)
%plot(sol.ctrl.real_s,sol.ctrl.imag_s,'o-')
%hold on
plot(sol.ctrl_special.real_s,sol.ctrl_special.imag_s,'ro-')
%plot(sol.ctrl_std.real_s,sol.ctrl_std.imag_s,'blacko-')
xlabel('real(S)')
ylabel('imag(S)')
axis square

figure(2)
plot(K_solutions,I,'o-')
xlabel('K')
ylabel('I')
axis square


%% Cost Function

global gp iaeWeights manWeights iaeTuning

gp = 1/(s+1)*exp(-s);

iaeWeights = [res.minSp.iae, res.minD.iae]';
manWeights = [.5, .5];

tuning = [sol.ctrl_special.K' sol.ctrl_special.I'];


for i = 1:length(tuning)
   J(i) = costFun(tuning(i,:));
end
[j_min, j_min_indx] = min(J);


figure(3)
plot(1:length(tuning),J,j_min_indx,j_min,'o')

tuning(j_min_indx,:)




