umax=max(con_val);
   if umax - umin>1e-3
      uscale=100;
      if max(abs(umin),abs(umax))<10, uscale=1000; end
      ustep=ceil(uscale*(umax-umin)/5)/uscale;
      %       umin=(round(umin/ustep)-1)*ustep;
      %       umax=(round(umax/ustep)+1)*ustep;
      umax=ceil(umax/ustep) * ustep;
      umin=(-ceil(-umin/ustep)) * ustep;
   else
      ustep=abs(umin);
      if ustep==0, ustep=0.1; end
      umin=umin-ustep;    
      umax=umax+ustep;
   end
   %  msgbox(num2str(umin), num2str(umax),'modal');
   set(ud.Display.GraphHandles(2),'YLim',[umin umax]);
   set(ud.Display.GraphHandles(2),'YTick',[umin:ustep:umax]);
   
   %  Re-Plot the lines by setting their XData and YData fields to the values from Simulink
   %  set(ud.Display.uLineHandles(1),'YData',con_val,'XData',time_val,'Color',ud.Display.LineColorU);
   %  set(cnfdb.Display.LineHandles,'EraseMode','normal');
   
   %  rescale the axis  for output signal
   ymin=min([out_val,ref_val]);
   ymax=max([out_val,ref_val]);
%    ref=abs(cnfdb.SetPoint);
   ref=max(abs(ref_val));
   
   infimum=1e-16;
   ystep=max(ref,infimum)/5;
   
   ymax=ceil(ymax/ystep) * ystep;
   ymin=(-ceil(-ymin/ystep))*ystep;
   if ymax-ymin>10*ystep
      ystep=(ymax-ymin)/5;
   end    
   if ymax-ymin < ystep
      ymax=ymin+ystep;
   end   
   cur_Lim = get(ud.Display.GraphHandles(1),'YLim');
   if ~isequal(cur_Lim, [ymin ymax])
      set(ud.Display.GraphHandles(1),'YLim',[ymin ymax]);
      set(ud.Display.GraphHandles(1),'YTick',[ymin:ystep:ymax]);
   end  
   %     Re-Plot the lines by setting their XData and YData fields to the values from Simulink
   %     set(ud.Display.rLineHandles(1),'YData',ref_val,'XData',time_val,'Color',ud.Display.LineColorR);
   %     set(ud.Display.hLineHandles(1),'YData',out_val,'XData',time_val,'Color',ud.Display.LineColorY);
   %     set(cnfdb.Display.LineHandles,'EraseMode','normal');
   
   % rescale (x,q) lines
   xqmin=min(min(xq_val));
   xqmax=max(max(xq_val));
   if  xqmin < xqmax
       set(ud.Display.GraphHandles(3),'YLim',[xqmin xqmax]);
       set(ud.Display.GraphHandles(3),'YTick',[xqmin:(xqmax-xqmin)/5:xqmax]);
   end    
 
   
end   % time_val 


% Update db structure
guidata(ud.Figure, cnfdb);


% --- Called to fill simulink model with data
function Fill_Simulink_Model(cnfdb)

% Set the kernel system
[nx,nu]=size(cnfdb.System.B);
[ny,nc]=size(cnfdb.System.C1); 

cnf_cntr.A = cnfdb.System.A;
cnf_cntr.B = cnfdb.System.B;
cnf_cntr.C1 = cnfdb.System.C1;
cnf_cntr.C2 = cnfdb.System.C2;
cnf_cntr.D2 = cnfdb.System.D2;
cnf_cntr.x0 = cnfdb.System.x0;
cnf_cntr.Um = cnfdb.System.CS_Umax;
if cnfdb.System.DT_in
    cnf_cntr.E = cnfdb.System.DT_E;
end
if cnfdb.System.RM_in
    [num,den]=tfdata(cnfdb.System.RM,'v');
    cnf_cntr.RM_num = num;
    cnf_cntr.RM_den = den;
end

cnf_cntr.Int_gain = 1;

if cnfdb.Controller.IC_in  % integral 
   eigen=cnfdb.Controller.FB_poles;
   if length(eigen)>=5
       cnf_cntr.Int_gain = eigen(5);
   end
end


% assign the controller parameters to workspace for usage in Simulink model
% To pass the consistence check-up in the following Simulink set_param command.
% execute serveral times
assignin('base','cnf_cntr',cnf_cntr);   

set_param(cnfdb.Simulink.Sys,...
 'A',  'cnf_cntr.A' ,...
 'B',  strcat('eye(',num2str(nx),')' ) ,...
 'C',  strcat('eye(',num2str(nx),')' ) ,...
 'D',  strcat('zeros(',num2str(nx), ',', num2str(nx),')' ) ,...
 'X0', 'cnf_cntr.x0' );

set_param(cnfdb.Simulink.Sys_B, 'Gain',  'cnf_cntr.B' );
set_param(cnfdb.Simulink.Sys_C1, 'Gain', 'cnf_cntr.C1' );
set_param(cnfdb.Simulink.Sys_C2, 'Gain', 'cnf_cntr.C2' );
set_param(cnfdb.Simulink.Sys_D2, 'Gain', 'cnf_cntr.D2' );

% Set the saturation
% set_param(cnfdb.Simulink.CS_Switch, 'sw',  num2str(cnfdb.System.CS_in) );
if cnfdb.System.CS_in
    set_param(cnfdb.Simulink.CS_Block, 'Upper Limit',  'cnf_cntr.Um', 'Lower Limit',  '-cnf_cntr.Um' );
    set_param(cnfdb.Simulink.CS_Block1, 'Upper Limit', 'cnf_cntr.Um', 'Lower Limit',  '-cnf_cntr.Um' );
else
    set_param(cnfdb.Simulink.CS_Block,  'Upper Limit', 'inf', 'Lower Limit',  '-inf' );
    set_param(cnfdb.Simulink.CS_Block1, 'Upper Limit', 'inf', 'Lower Limit',  '-inf' );
end

% Set the disturbance
if cnfdb.System.DT_in
    set_param(cnfdb.Simulink.DT_E, 'Gain', 'cnf_cntr.E' );
    Fcn=change_function_argument(cnfdb.System.DT_w, 't');
    % set_param(cnfdb.Simulink.DT_w, 'Expression',  Fcn );
    set_param(cnfdb.Simulink.DT_w, 'Parameters', ['''', Fcn, ''''] );
else
    set_param(cnfdb.Simulink.DT_E, 'Gain',  strcat('zeros(',num2str(nx), ',1)' ) );
    set_param(cnfdb.Simulink.DT_w, 'Parameters', '''0''' );
end

% Set the static nonLinearity
if cnfdb.System.NL_in
    Fcn=change_function_argument(cnfdb.System.NL_Fcn,'y');
    set_param(cnfdb.Simulink.NL_Fcn, 'Expression',  Fcn );
else
   set_param(cnfdb.Simulink.NL_Fcn, 'Expression',   '0' );
end

% Set the resonance modes
if cnfdb.System.RM_in
    set_param(cnfdb.Simulink.RM_Model, 'Numerator', 'cnf_cntr.RM_num', 'Denominator', 'cnf_cntr.RM_den' );
else
    set_param(cnfdb.Simulink.RM_Model, 'Numerator', '[1]', 'Denominator','[1]' );
end


% Set the NonLinearity Compensation
if cnfdb.Controller.NC_in
    Fcn=change_function_argument(cnfdb.Controller.NC_Fcn,'y');
    set_param(cnfdb.Simulink.NC_Fcn, 'Expression',  Fcn );
else
    set_param(cnfdb.Simulink.NC_Fcn, 'Expression',   '0' );
end


% Set the Parasite NonLinear Dynamics
if cnfdb.System.QD_in
    Fcn = cnfdb.System.QD_Fcn;
    set_param(cnfdb.Simulink.QD_Fcn, 'Parameters', ['''', Fcn, ''''] );
    nq=length(cnfdb.System.Q0);
    set_param(cnfdb.Simulink.QDK,  'Gain', ['eye(',num2str(nx+nq),')']  );
    set_param(cnfdb.Simulink.QD_Int, 'Initial', matrix2str(cnfdb.System.Q0) );
else
    set_param(cnfdb.Simulink.QD_Fcn, 'Parameters', ['''', matrix2str(0), ''''] );
    set_param(cnfdb.Simulink.QDK,  'Gain', [ '[eye(',num2str(nx),') zeros(',num2str(nx), ',1)]' ]  );
    set_param(cnfdb.Simulink.QD_Int, 'Initial', matrix2str(0) );
end


% the integration gain
set_param(cnfdb.Simulink.Int_gain,  'Gain', 'cnf_cntr.Int_gain' );


% Set the Observer
if cnfdb.Controller.OV_type
    if cnfdb.Controller.IC_in  % integral 
        cnfdb.Controller.OV_K1=[zeros(1,nx) 1; zeros(nx,nx+1)];
        cnfdb.Controller.OV_K2=[zeros(1,nx); eye(nx)];
    else  
        cnfdb.Controller.OV_K1=zeros(nx,nx+1);
        cnfdb.Controller.OV_K2=eye(nx);
    end
    % initial value of the observer
    Co=cnfdb.Controller.OV_Co;
    Do=cnfdb.Controller.OV_Do(:,nu+1:end);
    x0=cnfdb.System.x0;
    y0=cnfdb.System.C1*x0;
    cnf_cntr.OV_x0=inv(Co'*Co)*Co'*(x0-Do*y0);

else  
    cnfdb.Controller.OV_Ao=zeros(nx,nx);
    cnfdb.Controller.OV_Bo=zeros(nx,1+ny);
    cnfdb.Controller.OV_Co=eye(nx);
    cnfdb.Controller.OV_Do=zeros(nx,1+ny);
    cnf_cntr.OV_x0=zeros(size(cnfdb.Controller.OV_Ao,1),1);
    
    if cnfdb.Controller.IC_in  % integral 
        cnfdb.Controller.OV_K1=[zeros(1,nx) 1; eye(nx) zeros(nx,1)];
        cnfdb.Controller.OV_K2=zeros(nx+1,nx);
    else  
        cnfdb.Controller.OV_K1=[eye(nx) zeros(nx,1)];
        cnfdb.Controller.OV_K2=zeros(nx,nx);
    end
end   
set_param(cnfdb.Simulink.OV_K1, 'Gain',  matrix2str(cnfdb.Controller.OV_K1) );
set_param(cnfdb.Simulink.OV_K2, 'Gain',  matrix2str(cnfdb.Controller.OV_K2) );

cnf_cntr.K=cnfdb.Controller.OV_K;
cnf_cntr.Ao=cnfdb.Controller.OV_Ao;
cnf_cntr.Bo=cnfdb.Controller.OV_Bo;
cnf_cntr.Co=cnfdb.Controller.OV_Co;
cnf_cntr.Do=cnfdb.Controller.OV_Do;

cnf_cntr.F = cnfdb.Controller.FB_F;
cnf_cntr.H = cnfdb.Controller.FB_H;
cnf_cntr.Ge = cnfdb.Controller.FB_Ge;
cnf_cntr.Fn = cnfdb.Controller.CNF_F;
cnf_cntr.beta = cnfdb.Controller.CNF_Beta;
cnf_cntr.alpha = cnfdb.Controller.CNF_Alpha;

% filter 
if cnfdb.Controller.FT_in==1
   [N1 D1]=tfdata(evalin('base',cnfdb.Controller.FT_Fcn, 'zpk(1)'), 'v');   
   cnf_cntr.FT_num=N1;
   cnf_cntr.FT_den=D1;
else
   cnf_cntr.FT_num=1;
   cnf_cntr.FT_den=1;
end    

% assign the controller parameters to workspace for usage in Simulink model
assignin('base','cnf_cntr',cnf_cntr);   

set_param(cnfdb.Simulink.FT_Model, 'Numerator', 'cnf_cntr.FT_num', 'Denominator', 'cnf_cntr.FT_den' );
% set_param(cnfdb.Simulink.FT_Model, 'Numerator', '[1]', 'Denominator','[1]' );

set_param(cnfdb.Simulink.Observer,...
    'A',  'cnf_cntr.Ao' ,...
    'B',  'cnf_cntr.Bo' ,...
    'C',  'cnf_cntr.Co' ,...
    'D',  'cnf_cntr.Do' ,...
    'X0', 'cnf_cntr.OV_x0' );

% Set linear Feedback 
set_param(cnfdb.Simulink.FB_F,  'Gain',  'cnf_cntr.F' );
set_param(cnfdb.Simulink.FB_H,  'Gain',  'cnf_cntr.H' );
set_param(cnfdb.Simulink.FB_Ge, 'Gain',  'cnf_cntr.Ge' );

% Set CNF nonlinear feedback
if cnfdb.Controller.ctrl_type>0
    
    set_param(cnfdb.Simulink.CNF_F, 'Gain', 'cnf_cntr.Fn');  
    set_param(cnfdb.Simulink.CNF_Beta, 'Gain',  'cnf_cntr.beta' );
    
    Alpha = cnfdb.Controller.CNF_Alpha;
    Rho_Fcn = cnfdb.Controller.CNF_Rho_Fcn;
    Rho_Fcn = strrep(Rho_Fcn ,'Alpha', num2str(Alpha,'%0.8g'));
    
    t=0;
    r = eval(cnfdb.SetPoint,'[]');
%     if isempty(r) % invalid target
%         return;
%     end    
    h = cnfdb.System.C2*cnfdb.System.x0;
    rho_offset = eval(Rho_Fcn);
    
    Rho_Fcn = strrep(Rho_Fcn, 'h', 'u(1)');
    Rho_Fcn = strrep(Rho_Fcn, 'r', 'u(2)');
    
    set_param(cnfdb.Simulink.CNF_Rho_Fcn, 'Expression',  Rho_Fcn );
    set_param(cnfdb.Simulink.CNF_Rho_offset, 'Value',  num2str(rho_offset,'%0.8g') );
    
else
    %   cnf_cntr.CNF_Fn=cnfdb.Controller.FB_F * 0;
    set_param(cnfdb.Simulink.CNF_F, 'Gain',  matrix2str(cnfdb.Controller.FB_F * 0) );
    set_param(cnfdb.Simulink.CNF_Beta, 'Gain',  '0' );
    set_param(cnfdb.Simulink.CNF_Rho_offset, 'Value',  '0' );
end          

Fcn=change_function_argument(cnfdb.SetPoint,'t');   % --> f(u)
val= eval(Fcn, '[]');
if ~isempty(val)   % not dependent on t or u 
   Fcn=matrix2str(val);    
end
set_param(cnfdb.Simulink.Target, 'Expression',  Fcn );
% set_param(cnfdb.Simulink.SetPoint, 'Value',  num2str(1,'%0.8g') );

set_param(cnfdb.Simulink.Model, 'StopTime',  num2str(cnfdb.StopTime,'%0.8g') );
set_param(cnfdb.Simulink.Model, 'FixedStep', num2str(cnfdb.StepSize,'%0.8g') );


set_param(cnfdb.Simulink.Model, 'SimulationCommand',  'Start' );



% --------------------------------------------------------------------
function Menu_Help_Callback(hObject, eventdata, handles)
% hObject    handle to OpenMenuItem (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
msgbox('Not designed yet ...','CNF Toolkit','modal');



% --------------------------------------------------------------------
function Menu_Load_Data(hObject, eventdata, handles)
% hObject    handle to Menu_Import (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
[fn,pn]=uigetfile('*.mat');
if fn
   load([pn,fn]);
   if  exist('cnfdb','var')
       ud = get(gcbo,'UserData');
       cnfdb0 = guidata(ud.Figure);
%        cnfdb0.System=cnfdb.System; 
%        cnfdb0.Controller=cnfdb.Controller; 
%        cnfdb0.sys_ok=cnfdb.sys_ok; 
%        cnfdb0.ctrl_ok=cnfdb.ctrl_ok; 
%        guidata(ud.Figure,cnfdb0);
       cnfdb.Simulink=cnfdb0.Simulink; 
       cnfdb.Figure=ud.Figure;
       cnfdb.Running=0;

       Entry=eval('cnfdb.Entry','[]');
       if isempty(Entry)
           cnfdb.Entry=struct( ...
               'x0','', ...
               'A','', ...
               'B','', ...
               'C1','', ...
               'C2','', ...
               'D2','', ...
               'E','', ...
               'Um','', ...
               'RM','', ...
               'F','', ...
               'K','');
       end
       
       QD=eval('cnfdb.System.QD_in','[]');
       if isempty(QD)
           cnfdb.System.QD_in=0;
           cnfdb.System.QD_Fcn='';
           cnfdb.System.Q0=0;
       end    
       
       FT=eval('cnfdb.Controller.FT_in','[]');
       if isempty(FT)
           cnfdb.Controller.FT_in=0;
           cnfdb.Controller.FT_Fcn='zpk(1)';
       end    
              
       guidata(ud.Figure,cnfdb);
       
       cnf_update_panel(ud.Display.SysDiagramHandle);  
       msgbox('Data has been loaded successfully.','CNF Toolkit','modal');
       
   else
       warndlg('The selected file does not contain the desired data.' ,'CNF Toolkit', 'modal');      
   end  
end   
    

% --------------------------------------------------------------------
function Menu_Rlocus_Callback(hObject, eventdata, handles)

ud=get(hObject,'UserData');
ud=get(ud.Display.SysDiagramHandle,'UserData');

cnf=update_controller(ud);
if cnf==0
    return;
end    

cnfdb=guidata(ud.Figure);


A=cnfdb.System.A;
B=cnfdb.System.B;
C2=cnfdb.System.C2;
D2=cnfdb.System.D2;

IC=cnfdb.Controller.IC_in;
[nh,nx]=size(C2);

% if IC==1
%     A=[0 C2; zeros(nx,1) A];
%     B=[0; B];
%     C2=[0 C2];
% end   

if IC==1
    int_gain = cnfdb.Controller.FB_poles(5);
    A=[zeros(nh,nh) C2*int_gain; zeros(nx,nh) A];
    B=[D2*int_gain; B];
    C2=[zeros(nh,nh) C2];
end   


F=cnfdb.Controller.FB_F;
AA=A+B*F;
BB=B;
CC=cnfdb.Controller.CNF_F;
DD=0;

Beta=cnfdb.Controller.CNF_Beta * (1-exp(-300));

% zpk(ss(AA,BB,CC,DD))

figure;

% rlocus(ss(AA,BB,CC,DD));  
% hold;
% rlocus(ss(AA,BB,CC,DD), Beta);

rlocus(ss(AA,BB,CC,DD),linspace(0,Beta,1000));