function [sys,x0,str,ts] = cnf_getdata(t,x,u,flag)
% This S function reads in time t, control u, state x and output h, reference r, 
% from the Simulink model cnf_sim.mdl
% The data is then passed to a graphics update function 
% The general structure of an S-function is used

switch flag,
case 0, % Initialization %
    [sys,x0,str,ts]=mdlInitializeSizes(u);
case 1,% Derivatives %
    % Do nothing not used    
    % sys=mdlDerivatives(t,x,u);
case 2,  % Update %
    [sys] = mdlUpdate(t,x,u);
case 3,  % Outputs %
    % Do nothing not used
    % sys=mdlOutputs(t,x,u);
case 4,  % GetTimeOfNextVarHit %
    % Do nothing not used
    % sys=mdlGetTimeOfNextVarHit(t,x,u);
case 9,  % Terminate %
    sys=mdlTerminate(t,x,u);
otherwise  % Unexpected flags %
    error(['Unhandled flag = ',num2str(flag)]);
end
% end sfuntmpl

%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
function [sys,x0,str,ts]=mdlInitializeSizes(u)
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
% Note that in this example, the values are hard coded.  This is not a
% recommended practice as the characteristics of the block are typically
% defined by the S-function parameters.
ud = get_param('cnf_sim/Get Data','UserData');
ud = get(ud.Display.SysDiagramHandle,'UserData');


ud.Display.time_val = [];    
ud.Display.ref_val = [];
ud.Display.out_val  = [];
ud.Display.con_val = [];
ud.Display.xdata =[];


cnfdb = guidata(ud.Figure);
if cnfdb.System.QD_in==1
    nn=length(cnfdb.System.Q0);
else
    nn=0;
end    

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

sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 0;
sizes.NumInputs      =  nh*2 + size(cnfdb.System.B, 2) +nx+nn;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;   % at least one sample time is needed
sys = simsizes(sizes);
% initialize the initial conditions
x0  = [];
% str is always an empty matrix
str = [];
% initialize the array of sample times
ts  = [0 0];

set_param('cnf_sim/Get Data','UserData', ud);

% end mdlInitializeSizes

%=============================================================================
% mdlUpdate     flag = 2
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=============================================================================
function [sys] = mdlUpdate(t,x,u)		% flag = 2

% Get the function handle which has been stored in the Userdata field of BlockData

ud = get_param('cnf_sim/Get Data','UserData');     %  LinkData;

%  to udate the display screen

cnfdb = guidata(ud.Figure);
[nh nu] = size(cnfdb.System.D2);

% Read in the values from Simulink and store them in the data storage array

ud.Display.ref_val  = [u(1:nh)  ud.Display.ref_val];    % Reference Input 
ud.Display.out_val  = [u(nh+1:2*nh) ud.Display.out_val];    % Plant Output 
ud.Display.con_val  = [u(2*nh+1:2*nh+nu) ud.Display.con_val];    % Control Signal 
ud.Display.time_val = [t(1) ud.Display.time_val];       % Simulation Time

ud.Display.xdata  = [u(2*nh+nu+1:end) ud.Display.xdata];    % state variables 


% Plot the lines by setting their XData and YData fields to the values from Simulink
for i = 1: nh,
        set(ud.Display.rLineHandles(i),'YData',ud.Display.ref_val(i,:),  'XData',ud.Display.time_val);
        set(ud.Display.hLineHandles(i),'YData',ud.Display.out_val(i,:),  'XData',ud.Display.time_val);
end    
% Re-Plot the lines for control signals by setting their XData and YData fields to the values from Simulink
for i=1: nu,
        set(ud.Display.uLineHandles(i),'YData',ud.Display.con_val(i,:), 'XData',ud.Display.time_val);
end
% for state variables
nxq=size(ud.Display.xdata,1);
for i=1: nxq,
        set(ud.Display.xLineHandles(i),'YData',ud.Display.xdata(i,:), 'XData',ud.Display.time_val);
end

% % Read in the stored data values from the lines already drawn
% ref_val = get(ud.Display.rLineHandles(1),'YData');
% out_val = get(ud.Display.hLineHandles(1),'YData');
% con_val = get(ud.Display.uLineHandles(1),'YData');    
% time    = get(ud.Display.rLineHandles(1),'XData');    
% 
% % Read in the values from Simulink and store them in the data storage array
% ref_val  = [u(1) ref_val];    % Reference Input 
% out_val  = [u(2) out_val];    % Plant Output 
% con_val  = [u(3) con_val];    % Control Signal 
% time_val = [t(1) time];       % Simulation Time
% 
% % 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(ud.Display.uLineHandles(1),'YData',con_val,'XData',time_val,'Color',ud.Display.LineColorU);
% 

set_param('cnf_sim/Get Data','UserData', ud);
% set(ud.Display.SysDiagramHandle, 'UserData',ud);
   
% Return nothing in sys
sys = [];
% end mdlUpdate



%=============================================================================
% mdlTerminate
% Perform any end of simulation tasks.
%=============================================================================
function sys=mdlTerminate(t,x,u)

% ud = get_param('cnf_sim/Get Data', 'UserData');     %  LinkData;
% set(ud.Display.SysDiagramHandle, 'UserData',ud);
% 
sys = [];

ud = get_param('cnf_sim/Get Data', 'UserData'); 
set(ud.Display.SysDiagramHandle, 'UserData',ud);

% end mdlTerminate