%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% L.048.27024/92039: Advanced Control Methods for Mechatronics
%
% Lecturer: Dr. Burak Demirel
% 
%
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% initializing the program
clear
clc
close all

%% define symbolic variables
syms s w r theta x1 x2 real

%% D-stability requirements
disp('% --------------------------------------------------------------- %')
disp('%                    D-Stability Constraints                      %')
disp('% --------------------------------------------------------------- %')
% constraints
prompt = 'Choose the settling time constraint [sigma]: ';
answer = input(prompt,'s');
%
sigma = str2double(answer);
while isnan(sigma) ~= 0
    answer = input(prompt,'s');
    sigma  = str2double(answer);
end

prompt = 'Choose the damping constraint (degree) [alpha]: ';
answer = input(prompt,'s');
%
alpha = str2double(answer);
while isnan(alpha) ~= 0
    answer = input(prompt,'s');
    alpha  = str2double(answer);
end

prompt = 'Choose the bandwidth constraint [R]: ';
answer = input(prompt,'s');
%
R = str2double(answer);
while isnan(R) ~= 0
    answer = input(prompt,'s');
    R      = str2double(answer);
end

% gridding 
settling_grid  = 100;
damping_grid   = 100;
bandwidth_grid = 100;

%% System model
disp('% --------------------------------------------------------------- %')
disp('%                  Plant Transfer Function G(s)                   %')
disp('% --------------------------------------------------------------- %')
% Plant transfer function
prompt = 'Numerator of plant transfer function: ';
answer = input(prompt,'s');
%
num = str2num(answer);
while isempty(num) ~= 0
    answer = input(prompt,'s');
    num  = str2num(answer);
end
%
prompt = 'Denumerator of plant transfer function: ';
answer = input(prompt,'s');
%
den = str2num(answer);
while isempty(den) ~= 0
    answer = input(prompt,'s');
    den  = str2num(answer);
end
%
G = poly2sym(num,s)/poly2sym(den,s);

disp('% --------------------------------------------------------------- %')
disp('%                 Sensor Transfer Function H(s)                   %')
disp('% --------------------------------------------------------------- %')
% Sensor transfer function
prompt = 'Numerator of sensor transfer function: ';
answer = input(prompt,'s');
%
num = str2num(answer);
while isempty(num) ~= 0
    answer = input(prompt,'s');
    num  = str2num(answer);
end
%
prompt = 'Denumerator of sensor transfer function: ';
answer = input(prompt,'s');
%
den = str2num(answer);
while isempty(den) ~= 0
    answer = input(prompt,'s');
    den  = str2num(answer);
end
%
H = poly2sym(num,s)/poly2sym(den,s);

disp('% --------------------------------------------------------------- %')
disp('%                Controller Transfer Function C(s)                %')
disp('% --------------------------------------------------------------- %')
% Controller transfer function
prompt = 'Numerator of controller transfer function: ';
answer = input(prompt,'s');
%
num = str2num(answer);
% while isempty(num) ~= 0
%     answer = input(prompt,'s');
%     num  = str2num(answer);
% end
%
prompt = 'Denumerator of controller transfer function: ';
answer = input(prompt,'s');
%
den = str2num(answer);
% while isempty(den) ~= 0
%     answer = input(prompt,'s');
%     den  = str2num(answer);
% end
%
C = poly2sym(num,s)/poly2sym(den,s);

% Loop transfer function 
L = G*C;

% Closed-loop transfer function (i.e., complementary transfer function)
T = collect( simplify( expand( L/(1+L*H) ) ) );

% characteristic polynomial
[N,D] = numden(T); 

% convert angle (i.e., alpha) from degree to radian
alpha = alpha*pi/180;


% Settling Time Constraints (CRB and RRB exist) 
% Damping Constraint (Only CRB exists)
% Bandwidth Constraints (CRB and RRB exist)

% Settling Time Constraint CRB
d    = subs(D,'s',-sigma+j*w);
eqn1 = simplify( real(d) );
eqn2 = simplify( imag(d) );

sol2 = solve(eqn1, eqn2, x1, x2);

sol2_x1_w = subs(sol2.x1,'w',linspace(0,tan(pi-alpha)*sigma,settling_grid));
sol2_x2_w = subs(sol2.x2,'w',linspace(0,tan(pi-alpha)*sigma,settling_grid));

% Damping Constraint CRB
d    = subs(D,'s',r*exp(j*alpha));
eqn1 = simplify( real(d) );
eqn2 = simplify( imag(d) );

sol3 = solve(eqn1, eqn2, x1, x2);

sol3_x1_w = subs(sol3.x1,'r',linspace(sigma/cos(pi-alpha),R,damping_grid));

sol3_x2_w = subs(sol3.x2,'r',linspace(sigma/cos(pi-alpha),R,damping_grid));


% Bandwidth Constraint CRB
d    = subs(D,'s',R*exp(j*theta));
eqn1 = simplify( real(d) );
eqn2 = simplify( imag(d) );

sol4 = solve(eqn1, eqn2, x1, x2);
sol4.x1 = simplify(sol4.x1);
sol4.x2 = simplify(sol4.x2);
%try
sol4_x1_w=subs(sol4.x1,'theta',linspace(alpha,pi,bandwidth_grid));
%catch
%   sol4_x1_w = Inf; 
%end

%try
sol4_x2_w=subs(sol4.x2,'theta',linspace(alpha,pi,bandwidth_grid));
%catch
%    sol4_x2_w = Inf;
%end

% Settling Time Constraint RRB
d=subs(D,'s',-sigma);

sol5 = solve(d,x2);

% Bandwidth Constraint RRB
d=subs(D,'s', -1.0*R);

sol6=solve(d,x2);

min_y = min( double([ min(sol2_x1_w) min(sol3_x1_w) min(sol4_x1_w) ]) );
max_y = max( double([ max(sol2_x1_w) max(sol3_x1_w) max(sol4_x1_w) ]) );

sol5_x2_w=subs(sol5,'x1',linspace(min_y,max_y,10000));%100

sol5_x1_w=linspace(min_y,max_y,10000);%100

sol6_x2_w=subs(sol6,'x1',linspace(min_y,max_y,1000));%20 12 alim, 4

sol6_x1_w=linspace(min_y,max_y,1000);


bubu = 0;

if bubu == 1
    
min_x = min( double([ min(sol2_x2_w) min(sol3_x2_w) min(sol4_x2_w) ]) );
max_x = max( double([ max(sol2_x2_w) max(sol3_x2_w) max(sol4_x2_w) ]) );

sol5_x1_w=subs(sol5,'x2',linspace(min_x,max_x,10000));%100

sol5_x2_w=linspace(min_x,max_x,10000);%100

sol6_x1_w=subs(sol6,'x2',linspace(min_x,max_x,1000));%20 12 alim, 4

sol6_x2_w=linspace(min_x,max_x,1000);


ulim=double(solve(sol5-sol6));
alim=double(subs(sol2.x2,'w',0));
%try
ulim2=double(abs(subs(sol4.x2,'theta',pi)));
%catch
%    ulim2 = Inf;
%end
%alim = -10;
% Caution for upper limit! -> here 60 but tuneable, see the true
% intersection.
sol5_x1_w=subs(sol5,'x2',linspace(alim,ulim,10000));%100

sol5_x2_w=linspace(alim,ulim,10000);%100

sol6_x1_w=subs(sol6,'x2',linspace(ulim,ulim2,1000));%20 12 alim, 4

sol6_x2_w=linspace(ulim,ulim2,1000);%20 12

end

%% Generate a plot for the controller parameter space
disp('% --------------------------------------------------------------- %')
disp('%                      D-Stability Region                         %')
disp('% --------------------------------------------------------------- %')
figure(1)
plot(sol2_x1_w,sol2_x2_w,'linewidth',3)
hold on
plot(sol3_x1_w,sol3_x2_w,'g','linewidth',3)%g
plot(sol4_x1_w,sol4_x2_w,'r','linewidth',3)%r
plot(sol6_x1_w,sol6_x2_w,'c','linewidth',3)%c
plot(sol5_x1_w,sol5_x2_w,'m','linewidth',3)%m

xlabel('k_p','FontName','Arial','FontSize',20)
ylabel('k_d','FontName','Arial','FontSize',20)
grid on
set(gca,'FontName','Arial','FontSize',20)

%% Pick a point 
[x1_num,x2_num] = PlotDStabilityRegion(sigma,alpha,R,C,G,H);

for ii = 1:1e10
    
prompt = 'Do you want more? Y/N [Y]: ';
reply = input(prompt,'s');

if isempty(reply)
    continue
elseif reply == 'Y' 
    try
        close(2)
        figure(1)
        [x1_num,x2_num] = PlotDStabilityRegion(sigma,alpha,R,C,G,H);
    catch
        figure(1)
        [x1_num,x2_num] = PlotDStabilityRegion(sigma,alpha,R,C,G,H);
    end
elseif reply == 'y'
    try
        close(2)
        figure(1)
        [x1_num,x2_num] = PlotDStabilityRegion(sigma,alpha,R,C,G,H);
    catch
        figure(1)
        [x1_num,x2_num] = PlotDStabilityRegion(sigma,alpha,R,C,G,H);
    end
elseif reply == 'N' 
    break
elseif reply == 'n'
    break 
else
    continue
end
%
end
    
%% End of the program