
w_space = [logspace(-3,3,500)];

%w_space=[0 2 3 4 5];
k=[];
ki=[];
Tf=4;
kd_space = linspace(0,-200,500);
p=tf('s');
    Gp =-0.0098*(p+0.25)/(p^2-0.040*p+0.025);
%kd_space = logspace(0.001,1.5,2000);
for j= 1:length(kd_space)
    kd=kd_space(j);
    
    for i=1:length(w_space)
        
        w=w_space(i);
        s=I*w;
        G =-0.0098*(s+0.25)/(s^2-0.040*s+0.025);
        imG = imag(G);
        reG = real(G);
        
        
        %par
        ki(i,j)= (-imG*Tf^2*w^2-Tf*w*imG^2-reG^2*Tf*w-imG+reG^2*kd*w+kd*w*imG^2)*w/((Tf^2*w^2+1)*(imG^2+reG^2));
        k(i,j) = -(w^2*reG^2*Tf*kd+reG*Tf^2*w^2+w^2*imG^2*Tf*kd+imG^2+reG+reG^2)/(w^2*reG^2*Tf^2+w^2*imG^2*Tf^2+imG^2+reG^2);
        %cas
        
        %ki(i,j) = reG*(Tf^2*w^2+1)/(w*(-reG*w*Tf+reG*w*kd+imG*Tf^2*w^2+imG*w^2*Tf*kd+2*imG));
        %k(i,j) = -reG*(Tf^2*w^2+1)/(reG^2*Tf^2*w^2+imG^2*Tf^2*w^2+reG^2*w^2*Tf*kd+imG^2*w^2*Tf*kd+2*reG^2+2*imG^2);
    end
    
    K=k(i,j)*(1+1/ki(i,j)/s+(kd*s+1)/(Tf*s+1));
    if isstable(K*Gp)==0;
        
        ki(i,j) = NaN;
        k(i,j) = NaN;
    end
    
end


%%
s=tf('s');
n=50;
m=50;
%Kc= k(m,n);
%Ki = ki(m,n);
%Kd=td_space(n);

Kc = 8.9;
Ki = -0.79;
Kd = -145.5;
%K = Kc*(1 + 1/Ki/s+(Kd*s+1)/(Tf*s+1));
K = Kc + Ki/s+ (Kd*s+1)/(Tf*s+1);
G =-0.0098*(s+0.25)/(s^2-0.040*s+0.025);

T=minreal(G*K/(1+G*K));
pole(T)
figure(3)
step(T)
grid on

% 
% figure(2)
% plot(ki(:,70),k(:,70))
% grid on
% hold on
% plot(Ki,Kc,'d')
% xlim([-100 0])
% hold off

[x_grid,y_grid]=meshgrid(kd_space,w_space);
figure(1);
mesh(x_grid,ki,k)
hold on
plot3(-145.15,-0.79,-5.65,'*')
plot3(Kd,Ki,Kc,'d')
hold off
xlabel('kd')
ylabel('ki')
zlabel('k')
ylim([-100 0])


%%
w_space = logspace(-.673,3,500);

%w_space=[0 2 3 4 5];
k=[];
ki=[];
Tf=4;
kd_space = linspace(0,30,500);
p=tf('s');
    Gp =-0.0098*(p+0.25)/(p^2-0.040*p+0.025);
I=1i;
for j= 1:length(kd_space)
    kd=kd_space(j);
    
    for i=1:length(w_space)
        
        w=w_space(i);
        s=I*w;
        G =-0.0098*(s+0.25)/(s^2-0.040*s+0.025);
        imG = imag(G);
        reG = real(G);
        
        
        %par
        %ki(i,j)= (-imG*Tf^2*w^2-Tf*w*imG^2-reG^2*Tf*w-imG+reG^2*kd*w+kd*w*imG^2)*w/((Tf^2*w^2+1)*(imG^2+reG^2));
        %k(i,j) = -(w^2*reG^2*Tf*kd+reG*Tf^2*w^2+w^2*imG^2*Tf*kd+imG^2+reG+reG^2)/(w^2*reG^2*Tf^2+w^2*imG^2*Tf^2+imG^2+reG^2);
        %cas
        
        ki(i,j) = reG*(Tf^2*w^2+1)/(w*(-reG*w*Tf+reG*w*kd+imG*Tf^2*w^2+imG*w^2*Tf*kd+2*imG));
        k(i,j) = -reG*(Tf^2*w^2+1)/(reG^2*Tf^2*w^2+imG^2*Tf^2*w^2+reG^2*w^2*Tf*kd+imG^2*w^2*Tf*kd+2*reG^2+2*imG^2);
    end
    
    K=k(i,j)*(1+1/ki(i,j)/s+(kd*s+1)/(Tf*s+1));
    if isstable(K*Gp)==0;
        
        ki(i,j) = NaN;
        k(i,j) = NaN;
    end
    
end



s=tf('s');
n=50;
m=50;
%Kc= k(m,n);
%Ki = ki(m,n);
%Kd=td_space(n);

Kc = -8.9;
Ki = 0.79;
Kd = 20;
%K = Kc*(1 + 1/Ki/s+(Kd*s+1)/(Tf*s+1));
K = Kc*(1 + 1/Ki/s+ (Kd*s+1)/(Tf*s+1));
G =-0.0098*(s+0.25)/(s^2-0.040*s+0.025);

T=minreal(G*K/(1+G*K));
pole(T)
figure(3)
step(T)
grid on


% figure(3)
% plot(ki(:,70),k(:,70))
% grid on
% hold on
% plot(Ki,Kc,'d')
% xlim([0 30])
% hold off

[x_grid,y_grid]=meshgrid(kd_space,w_space);
figure(5);
mesh(x_grid,ki,k)
hold on
plot3(-145.15/-5.65,1/(-0.79/-5.65),-5.65,'*')
plot3(Kd,Ki,Kc,'d')
hold off
xlabel('kd')
ylabel('ki')
zlabel('k')
ylim([0 30])

%%
s=tf('s');
n=50;
m=50;
%Kc= k(m,n);
%Ki = ki(m,n);
%Kd=td_space(n);

Kc = -8.9;
Ki = 0.79;
Kd = 20;
%K = Kc*(1 + 1/Ki/s+(Kd*s+1)/(Tf*s+1));
K = Kc*(1 + 1/Ki/s+ (Kd*s+1)/(Tf*s+1));
G =-0.0098*(s+0.25)/(s^2-0.040*s+0.025);

T=minreal(G*K/(1+G*K));
pole(T)
figure(6)
step(T)
grid on


% figure(3)
% plot(ki(:,70),k(:,70))
% grid on
% hold on
% plot(Ki,Kc,'d')
% xlim([0 30])
% hold off

[x_grid,y_grid]=meshgrid(kd_space,w_space);
figure(7);
mesh(x_grid,1./ki,k)
hold on
plot3(-145.15/-5.65,(-0.79/-5.65),-5.65,'*')
plot3(Kd,Ki,Kc,'d')
hold off
xlabel('kd')
ylabel('ki')
zlabel('k')
ylim([0 30])






