%% 繪制H∞范數約束邊界：通過繪制曲線族（參數方程）的判別曲線，包括包絡線和奇點軌跡線，來求解
%% 范數約束邊界線

clear all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                      Define the system
% N1=[1];a=0.5;
% D1=conv(conv(conv(conv([1 1],[a 1]),[a^2 1]),[a^3 1]),[[a^4 1]]);
N1=[-1.5 1];
D1=[1 3 3 1];

a=0.3;
N1=[1];
D1=conv(conv(conv([1 1],[a 1]),[a^2 1]),[a^3 1]);

L=0.; % the system delay
tao=0; % PID controller filter constant
pade_order=0; % pade approximation oder
[num_L,den_L]=pade(L,pade_order);
N=conv(N1,num_L);
D=[conv(conv(D1,den_L),[tao,1])];
if pade_order~=0
    L=0;
end

flag=3;
linewidth=1;
gama_KD=0.0; % for a fixed kd values
gama=1.3;
sita=0:0.1:2*pi;
v=0.01:0.1:2*pi;
v=v;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%   define the syms: w=frequency, r=gama and t=sita.
syms w real;
syms t positive;
syms x;
kd=gama_KD;
r=gama;

N_s=poly2sym(N,'x');
D_s=poly2sym(D,'x');
N_w=subs(N_s,{x},{j*w})*(cos(w*L)-j*sin(w*L));%exp(-j*w*L);
D_w=subs(D_s,{x},{j*w});

s=j*w;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%    H∞范數形式
a=1/r*cos(t);
b=1/r*sin(t);
c=a+b*j;
switch flag
    case 1
    temp_NN=s*D_w+N_w*c; % Jv=1/s*Gp/(1+CGp)
    temp_DD=N_w;
    
    case 2
    temp_NN=s*D_w+s*N_w*c; % Jv=Gp/(1+CGp)
    temp_DD=N_w;    
    
    case 3
    temp_NN=s*D_w*(1+c); % Ms=1/(1+CGp)
    temp_DD=N_w;

    case 4
    temp_NN=s*D_w; % Mt=CGp/(1+CGp)
    temp_DD=N_w*(1+c);

    case 5
    temp_NN=s*D_w; % Ju=C/(1+CGp)
    temp_DD=(N_w+D_w*c);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sys=simplify(temp_NN/temp_DD);

Re_w_t=simplify(real(sys));
Im_w_t=simplify(imag(sys));

x=-Im_w_t/w;
y=kd*w^2-Re_w_t;
z=diff(y,w)*diff(x,t)-diff(y,t)*diff(x,w);%% 包絡線方程和奇點軌跡線方程

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  plot the figure in the kp-ki plane
% w0=1;   %% method 1
% for k=1:length(sita)
%     fc=subs(z,{t},{sita(k)*pi});
%     ff=inline(char(fc));
%     tp_w=fzero(ff,w0);
% 
%     kp(k)=subs(x,{w,t},{tp_w,sita(k)*pi});
%     ki(k)=subs(y,{w,t},{tp_w,sita(k)*pi});
% end
  
for k=1:length(v) %% method 2
    fc=subs(z,{w},{v(k)});
    
    ff=inline(char(fc)); %% method 2
    
    tp_t(k)=fzero(ff,[0 pi]);
    kp1(k)=subs(x,{w,t},{v(k),tp_t(k)});
    ki1(k)=subs(y,{w,t},{v(k),tp_t(k)});
    
    tp_t1(k)=fzero(ff,[pi 2*pi]);
    kp2(k)=subs(x,{w,t},{v(k),tp_t1(k)});
    ki2(k)=subs(y,{w,t},{v(k),tp_t1(k)});    
end

kp=[kp1 kp2];
ki=[ki1 ki2];

figure(1);
hold on;
plot(kp1,ki1,'b.');
hold on;
plot(kp2,ki2,'r.');
xlabel('K_p');
ylabel('K_i');
grid on;
hold on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  plot the inter curves
if flag==1
    no=length(ki1);
    for i=1:no-1
        error(i)=abs(ki1(i+1)-ki1(i));
        stand=ki1(2)-ki1(1);
        if error(i)>1.
            KK=i+1;
            break;
        end
    end
    figure(13);
    kp_in=[kp2(find(kp2<kp2(KK))) kp1(find(kp1>kp2(KK-1) & ki1>=ki2(1)-0.15))];
    ki_in=[ki2(find(kp2<kp2(KK))) ki1(find(kp1>kp2(KK-1) & ki1>=ki2(1)-0.15))];
    tpx=kp_in(1):0.0001:max(kp_in);
    tpy=interp1(kp_in(find(kp_in>kp_in(1))),ki_in(find(kp_in>kp_in(1))),tpx,'spline');
    plot(tpx(find(tpy>=ki_in(1))),tpy(find(tpy>=ki_in(1))),'b-','LineWidth',linewidth);
    
    hold on
    plot(kp_in(find(kp_in<=kp_in(1)+0.05)),ki_in(find(kp_in<=kp_in(1)+0.05)),'b-','LineWidth',linewidth);
    
    kix=1/gama.*ones(size(tpx(find(tpy>=ki_in(1)))));
    hold on;
    plot(tpx(find(tpy>=ki_in(1))),kix,'b-','LineWidth',linewidth);
end
if flag==2 || flag==4 || flag==5
    figure(13);
    hold on;
    
    tpx=kp2(1):0.0001:max(kp2);
    tpy=interp1(kp2(find(ki2>=0)),ki2(find(ki2>=0)),tpx,'spline');
    plot(tpx(find(tpy>=0)),tpy(find(tpy>=0)),'b-','LineWidth',linewidth);
    kix=zeros(size(tpx(find(tpy>=0))));
    hold on;
    plot(tpx(find(tpy>=0)),kix,'b-','LineWidth',linewidth);
    
%     tpx=kp2; %% method 2
%     tpy=ki2;
%     plot(tpx(find(tpy>=0)),tpy(find(tpy>=0)),'b-','LineWidth',linewidth);
%     kix=zeros(size(tpx(find(tpy>=0))));
%     hold on;
%     plot(tpx(find(tpy>=0)),kix,'b-','LineWidth',linewidth);
end
if flag==3
    figure(13);
    hold on;
    
    tpx=kp1(1):0.0001:max(kp1);%% method 1
    tpy=interp1(kp1(find(ki1>=0)),ki1(find(ki1>=0)),tpx,'spline');
    plot(tpx(find(tpy>=0)),tpy(find(tpy>=0)),'b-','LineWidth',linewidth);
    kix=zeros(size(tpx(find(tpy>=0))));
    hold on;
    plot(tpx(find(tpy>=0)),kix,'b-','LineWidth',linewidth);
    
%     [tpnum,M1]=max(kp1);%% method 2
%     [tpnum,M2]=max(ki1);
%     tpx1=kp1(1):0.0001:kp1(M1);%% method 2
%     tpy1=interp1(kp1(1:M1),ki1(1:M1),tpx1,'spline');
%     plot(tpx1,tpy1,'b-','LineWidth',linewidth);
%     hold on;
%     tpx2=kp1(M1):-0.0001:kp1(M2);%% method 2
%     tpy2=interp1(kp1(M1:M2),ki1(M1:M2),tpx2);%,'spline');
%     plot(tpx2,tpy2,'b-','LineWidth',linewidth);
%     hold on;
%     tpx3=kp1(M2):-0.0001:kp1(end);%% method 2
%     tpy3=interp1(kp1(M2:end),ki1(M2:end),tpx3);%,'spline');
%     plot(tpx3(find(tpy3>=0)),tpy3(find(tpy3>=0)),'b-','LineWidth',linewidth);
%     kix=zeros(size(tpx1(find(tpy1>=0))));
%     hold on;
%     plot(tpx1(find(tpy1>=0)),kix,'b-','LineWidth',linewidth);
    
%     tpx=kp1; %% method 3
%     tpy=ki1;
%     plot(tpx(find(tpy>=-0.02)),tpy(find(tpy>=-0.02)),'b-','LineWidth',linewidth);
%     kix=zeros(size(tpx(find(tpy>=-0.02))));
%     hold on;
%     plot(tpx(find(tpy>=-0.02)),kix,'b-','LineWidth',linewidth);
end
xlabel('K_p');
ylabel('K_i');
box on;
hold on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  start guess point
KP_start=kp2(1)
KI_start=ki2(1)