grid;

  

% --------------------------------------------------------------------
function Menu_Save_Data(hObject, eventdata, handles)
% hObject    handle to Menu_Export (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

[fn,pn]=uiputfile('*.mat');
if fn
   cnfdb = guidata(gcf);
   cnfdb.Simulink=[];
   save([pn,fn],'cnfdb');
end   


% --------------------------------------------------------------------
function Menu_Export_Model(hObject, eventdata, handles)
% hObject    handle to Menu_Export (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

 cnfdb = guidata(gcf);
%  plant = cnfdb.System;
 
 plant = struct ( 'Name', '',...
     'A',[],...
     'B',[],...
     'C1',[],...
     'C2',[],...
     'E',[],...
     'w',0,...
     'Umax',[],...
     'N',[],...
     'Fcn',[],...
     'x0',[],...
     'RM',zpk(1));

 plant.Name=cnfdb.System.Name;
 plant.A=cnfdb.System.A;
 plant.B=cnfdb.System.B;
 plant.C1=cnfdb.System.C1;
 plant.C2=cnfdb.System.C2;
 
 if cnfdb.System.CS_in
     plant.Umax=cnfdb.System.CS_Umax;
 end
 
 if cnfdb.System.DT_in
     plant.E=cnfdb.System.DT_E;
     plant.w=cnfdb.System.DT_w;
 end
 
 if cnfdb.System.NL_in
     plant.N=cnfdb.System.NL_En;
     plant.Fcn=cnfdb.System.NL_Fcn;
 end
 
 if cnfdb.System.RM_in
     plant.RM=cnfdb.System.RM;
 end

 export_to_workspace(plant, 'Export the plant model to');



% --------------------------------------------------------------------
function Menu_Export_Controller(hObject, eventdata, handles)
% hObject    handle to Menu_Export (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

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

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

cnfdb=guidata(ud.Figure);

%  cnfdb = guidata(gcf);
controller=struct ( 'zdot','',...
    'vdot','',...
    'xhat','xhat = x',...
    'Acmp',[],...
    'Bycmp',[],...
    'Bucmp',[],...
    'Ccmp',[],...
    'Dcmp',[],...
    'u_bar','',...
    'u_pre','',...
    'u','',...
    'F',[],...
    'G',[],...
    'Fn',[],...
    'Ge',[],...
    'K',[],...
    'P',[],...
    'W',[],...
    'alpha',0,...
    'beta',0,...
    'rho','');

%     'xbar','',...

if cnfdb.Controller.OV_type>0
    if cnfdb.Controller.NC_in >0  | cnfdb.Controller.FT_in >0
        controller.vdot='vdot = Acmp v  + Bycmp y + Bucmp sat(u_bar)';    
    else
        controller.vdot='vdot = Acmp v  + Bycmp y + Bucmp sat(u)';    
    end    
    controller.xhat='xhat = Ccmp v + Dcmp y';    
    controller.Acmp=cnfdb.Controller.OV_Ao;
    controller.Bucmp=cnfdb.Controller.OV_Bo(:,1);
    controller.Bycmp=cnfdb.Controller.OV_Bo(:,2:end);
    controller.Ccmp=cnfdb.Controller.OV_Co;
    controller.Dcmp=cnfdb.Controller.OV_Do(:,2:end);
    controller.K=cnfdb.Controller.OV_K;
    xhat='xhat';
else
    xhat='x';
end    

if cnfdb.Controller.IC_in>0
    int_gain = cnfdb.Controller.FB_poles(5);
    if int_gain ==1
        controller.zdot = ['zdot = h-r'];
    else
        controller.zdot = ['zdot = ',num2str(int_gain),'*(h-r)'];
    end
    % controller.xbar = ['xbar = [z; ', xhat, '] '];
    xbar = [ '[z; ', xhat, '] '];
else
    % controller.xbar = '';
    xbar=xhat;
end    

controller.F=cnfdb.Controller.FB_F;
controller.G=cnfdb.Controller.FB_G;
controller.Ge=cnfdb.Controller.FB_Ge;
%    controller.u=[ matrix2str(controller.F), xbar,'+', matrix2str(controller.G),'*r'];
controller.u=['F * ', xbar,'+ G*r'];

if cnfdb.Controller.ctrl_type>0
    controller.Fn=cnfdb.Controller.CNF_F;
    controller.P=cnfdb.Controller.CNF_P;
    controller.W=diag(cnfdb.Controller.CNF_W);
    %    controller.u=[ controller.u, '+ rho(e)* ', matrix2str(controller.Fn),'(',xbar,'-Ge*r)' ];
    controller.u=[ controller.u, '+ rho(e)* Fn *(',xbar,'-Ge*r)' ];
    controller.beta=cnfdb.Controller.CNF_Beta;
    controller.alpha=cnfdb.Controller.CNF_Alpha;
    rho=cnfdb.Controller.CNF_Rho_Fcn;
    %     rho='exp(-Alpha*|h-r|)';
    if controller.alpha==1
        rho=strrep(rho, 'Alpha*', '');
    else
        rho=strrep(rho, 'Alpha', num2str(controller.alpha));
    end
    rho1=strrep(rho,'h','h0');
    controller.rho=['rho(e) = -',num2str(controller.beta),'*abs(',rho,'-',rho1,')'];
end   

if cnfdb.Controller.NC_in>0 | cnfdb.Controller.FT_in>0
    controller.u_bar = ['u_bar = ', controller.u];
    u_bar = 'u_bar'; 
    if cnfdb.Controller.FT_in>0
        u_bar = ['H(s)*(', u_bar, ')'];
        controller.FilterH = eval(cnfdb.Controller.FT_Fcn,'[]');   
    end
    if cnfdb.Controller.NC_in>0
        controller.u_pre = ['u_pre = ', cnfdb.Controller.NC_Fcn];
        u_bar = [u_bar, ' + u_pre'];
    end
    controller.u = ['u = ', u_bar];
    
else
    controller.u = ['u = ', controller.u];
end    

% if cnfdb.Controller.FT_in==1
%    controller.Filter=eval(cnfdb.Controller.FT_Fcn,'[]');   
% end   

export_to_workspace(controller,'Export the controller to');



% --------------------------------------------------------------------
function Menu_Print_Callback(hObject, eventdata, handles)
% hObject    handle to PrintMenuItem (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
printdlg(gcf);

% --------------------------------------------------------------------
function Menu_Close_Callback(hObject, eventdata, handles)
% hObject    handle to CloseMenuItem (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
selection = questdlg('Close this toolkit ?', get(gcf, 'Name') , 'Yes','No','Yes');
if strcmp(selection,'No')
    return;
end

cnfdb=guidata(gcf);
set_param(cnfdb.Simulink.Model,'SimulationCommand','Stop');
bdclose(cnfdb.Simulink.Model);
delete(gcf);


% --------------------------------------------------------------------
function Menu_Extract_Callback(hObject, eventdata, handles)
% hObject    handle to PrintMenuItem (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

choices=get(gcbo,'Label');
ud=get(gcbo,'UserData');
ud=get(ud.Display.SysDiagramHandle,'UserData');

cnfdb = guidata(ud.Figure);
[nh nx]=size(cnfdb.System.C2);

ref_val = fliplr(ud.Display.ref_val) ;    % Reference Input 
out_val = fliplr(ud.Display.out_val) ;    % Plant Output 
con_val = fliplr(ud.Display.con_val) ;    % Control Signal 
time_val = fliplr(ud.Display.time_val) ;  % Simulation Time
state_val= fliplr(ud.Display.xdata);

% % Read in the stored data values from the lines already drawn
% ref_val = fliplr(get(ud.Display.LineHandles(1),'YData'));
% out_val  = fliplr(get(ud.Display.LineHandles(2),'YData'));
% con_val  = fliplr(get(ud.Display.LineHandles(3),'YData'));    
% time_val  = fliplr(get(ud.Display.LineHandles(1),'XData'));   

StyleSet = {'-'; '--'; '-.'; ':'};
ColorSet = 'mbrcykg';

switch choices
    case '  System output'
        figure;  % (1)
        plot(time_val,out_val,'m-',time_val,ref_val,'b--');
        grid;
        xlabel('Time (seconds)');
        ylabel('System Output');
        
    case '  Control signal'
        figure;
        plot(time_val,con_val,'r');
        grid;
        ylabel('Control Signal');
        xlabel('Time (seconds)');
        
    case '  Output and control'
        figure;
        subplot(2,1,1);
        plot(time_val,out_val,'m-',time_val,ref_val,'b--');
        grid;
        xlabel('Time (seconds)');
        ylabel('System Output');
        subplot(2,1,2);
        plot(time_val,con_val,'r');
        grid;
        ylabel('Control Signal');
        xlabel('Time (seconds)');
        
    case '  State variables'
        figure;
        %  plot(time_val,state_val);
        nxq=size(state_val,1);
%         lc=ColorSet(1);
%         ls=StyleSet(1);
%         plot(time_val, state_val(1,:), [lc,char(ls)]); 
        for i=1:nxq
            lc=ColorSet(mod(i-1,7)+1);
            ls=StyleSet(mod(i-1,4)+1);
            plot(time_val, state_val(i,:), [lc,char(ls)]); 
            hold on;
        end
        grid;
%        title('State trajectory');   
        ylabel('State Variables');
        xlabel('Time (seconds)');
        if nxq >=1
            nq=nxq-nx;
            legstr='''x1''';
            for i=2:nx
                legstr=[legstr,',''x',num2str(i),'''' ];
            end
            for i=1:nq
                legstr=[legstr,',''q',num2str(i),'''' ];
            end
            
            eval( ['legend(',legstr,')'] );    
            
        end    
    case '  State and control'
        figure;
        subplot(2,1,1);
        %  plot(time_val,state_val);
        nxq=size(state_val,1);
%         lc=ColorSet(1);
%         ls=StyleSet(1);
%         plot(time_val, state_val(1,:), [lc,char(ls)]); 
        for i=1:nxq
            lc=ColorSet(mod(i-1,7)+1);
            ls=StyleSet(mod(i-1,4)+1);
            plot(time_val, state_val(i,:), [lc,char(ls)]); 
            hold on;
        end
        grid;
%        title('State trajectory');   
        ylabel('State Variables');
        xlabel('Time (seconds)');
        if nxq >= 1
            nq=nxq-nx;
            legstr='''x1''';
            for i=2:nx
                legstr=[legstr,',''x',num2str(i),'''' ];
            end
            for i=1:nq
                legstr=[legstr,',''q',num2str(i),'''' ];
            end
            eval( ['legend(',legstr,')'] );    
        end
        %
        subplot(2,1,2);
        plot(time_val,con_val,'r');
        grid;
        ylabel('Control Signal');
        xlabel('Time (seconds)');

        
    otherwise  % workspace data
        
        cnfdata=struct('output',out_val, ...
            'control',con_val, ...
            'reference',ref_val, ...
            'state',state_val, ...
            'time', time_val);
        
        export_to_workspace(cnfdata,'Extract Simulation Results to Workspace');

end

    
%*************************************************************************
function localCloseButton(eventSrc,eventData)
switch get(eventSrc,'Type')
case 'figure'
    figHdl = eventSrc;
otherwise
    figHdl = get(eventSrc,'Parent');
end
cnfdb = get(figHdl,'UserData');
set_param(cnfdb.Simulink.Model,'SimulationCommand','Stop');
bdclose(cnfdb.Simulink.Model);



%*************************************************************************
function cnf=update_controller(ud)

cnf=0;
handles=ud.Handles2;
cdb=guidata(ud.Figure);

if cdb.sys_ok==0 
    warndlg({'The plant model is not ready !','Please define the plant model first.'}, 'Warning','modal');
    return;
end

% determine the model
cnfdb = cnf_update_model(cdb);
if cnfdb.sys_ok==0 
    return;
end
% cnfdb = cdb;

A=cnfdb.System.A;
B=cnfdb.System.B;
C2=cnfdb.System.C2;
D2=cnfdb.System.D2;
[nh,nx]=size(C2);

IC=cnfdb.Controller.IC_in;
% check for integral augmentation
if IC==1
    %     A=[0 C2; zeros(nx,1) A];
    %     B=[0;B];
    %     C2=[0 C2];
    
    %   dot(z)) =  ki* (h-r) =  ki*[(C2 x + D2 u) -r]
    int_gain = get(handles.slider_int_gain, 'Value');
    if int_gain ==0
        warndlg('The specified integration gain is not appropriate.','Warning ','modal');
        cnf=0;
        return;
    end    
    cnfdb.Controller.FB_poles(5) = int_gain;
    A=[zeros(nh,nh) C2*int_gain; zeros(nx,nh) A];
    B=[D2*int_gain; B];
    C2=[zeros(nh,nh) C2];
end   

switch cnfdb.Controller.FB_choice
    case 1
    
        if isempty(cnfdb.Entry.F)
            F=[];
        else
            F=evalin('base',cnfdb.Entry.F,'[]');
        end    
        
        if isequal(size(F),[1,nx+IC])
            if max(real(eig(A+B*F))) > 0
                warndlg('The feedback gain matrix F will result in some RHP poles.','Warning ','modal');
                return;
            end