clear all;

a=0.1;
N=[1];
D=conv(conv(conv([1 1],[a 1]),[a^2 1]),[a^3 1]);


linewidth=1;

gama=1.2;
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;

r=gama;

N_s=poly2sym(N,'x');
D_s=poly2sym(D,'x');
N_w=subs(N_s,{x},{j*w}); %將N_s中x變量換為j*w
D_w=subs(D_s,{x},{j*w});

s=j*w;

a=1/r*cos(t);
b=1/r*sin(t);
c=a+b*j;

    
temp_NN=s*D_w*(1+c); % 靈敏度函數
temp_DD=N_w;

  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sys=simplify(temp_NN/temp_DD);

Re_w_t=simplify(real(sys));
Im_w_t=simplify(imag(sys));
kd=0
x=-Im_w_t/w;  %kp表達式
y=kd*w^2-Re_w_t;  %ki表達式
z=diff(y,w)*diff(x,t)-diff(y,t)*diff(x,w);%% 包絡線方程和奇點軌跡線方程

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for k=1:length(v) %% method 2
    fc=subs(z,{w},{v(k)}); %將Z中的w變量換為V(k)
    
    ff=inline(char(fc)); %% 求解方程fc
    
    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

    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);
    

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