function demo_lsctrpm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function presents a demonstration of the LSCTRPM routine which computes 
% LS-estimates of Continuous-Time MIMO RPM (reinitialized partial moment)
% models.
% The demo is a game for a training and a good using of this CT RPM model
% in SISO case
%
%	Author  : Régis Ouvrard, University of Poitiers
%	e-mail  : regis.ouvrard@univ-poitiers.fr
%	Last Revised : 30 March 2010
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%   This routine has been tested with MATLAB Version 7.3 and updates 

%% Construct the figure
f = figure('Visible','off','Position',[360,500,800,500]);

%%% Construct the components to generate the system
% Text "1- Generate the system"
htext = uicontrol('Style','text','String','1- Generate the system','BackgroundColor',[0.753 0.753 0.753],...
                  'Position',[20,470,220,22],'FontSize',14);
% Popup for the system structure
hpopup = uicontrol('Style','popupmenu',...
                   'String',{'1st order', '2nd order', '2nd order with 1 zero', '3rd order', '3rd order with 1 zero', '3rd order with 2 zeros'},...
                   'Position',[20,436,140,25],'Callback',{@popup_menu_Callback});
% Pushbutton for a new system
NewSys = uicontrol('Style','pushbutton','String','New system',...
                   'Position',[20,410,90,25],'Callback',{@NewSysButton_Callback});

%%% Construct the components to generate data
% Text "2- Generate data"
htext = uicontrol('Style','text','String','2- Generate data','BackgroundColor',[0.753 0.753 0.753],...
                  'Position',[20,380,170,22],'FontSize',14);
% Text "Select the signal to noise ratio (dB)"
htext = uicontrol('Style','text','String','Select the signal to noise ratio (dB)','BackgroundColor',[0.753 0.753 0.753],...
                  'Position',[20,352,210,18],'FontSize',10);
% Edit field to select the signal to noise ratio
htext = uicontrol('Style','edit','String','20','BackgroundColor','white',...
                  'Position',[240,350,80,22],'Callback',{@SnrEdit_Callback});
% Pushbutton for a new simulation
NewSim = uicontrol('Style','pushbutton','String','New simulation','Position',[20,320,90,25],...
                   'Callback',{@NewSimButton_Callback});

%%% Construct the components for the RPM model
% Text "3- Select the design parameter T_hat"
htext = uicontrol('Style','text','String','3- Select the design parameter T_hat','BackgroundColor',[0.753 0.753 0.753],...
                  'Position',[380,470,350,22],'FontSize',14);
% Edit field to select the design parameter
htext = uicontrol('Style','edit','String','2','BackgroundColor','white',...
                  'Position',[380,441,60,22],'Callback',{@DesignPara_Callback});
% Pushbutton for the help window 
PB = uicontrol('Style','pushbutton','String','Help',...
               'Position',[450,440,90,25],'Callback',{@HelpButton_Callback});
% Text "4- Estimate the CT RPM model"
htext = uicontrol('Style','text','String','4- Estimate the CT RPM model','BackgroundColor',[0.753 0.753 0.753],...
                  'Position',[380,410,300,22],'FontSize',14);
% Pushbutton to estimate the RPM model
PB = uicontrol('Style','pushbutton','String','Estimate',...
               'Position',[380,377,90,25],'Callback',{@EstimateButton_Callback});

% Pushbutton to exit
hQuit = uicontrol('Style','pushbutton','String','Exit','Position',[680,320,90,25],...
                  'Callback',{@Exitbutton_Callback});
% Create axes graphic
ha = axes('Units','Pixels','Position',[40,40,720,270]);
% Toolbar and name 
set(f,'Toolbar','figure','Name','Demo of the CT RPM model estimation')
% Move the figure to the center of the screen
movegui(f,'center')
% Make the figure visible
set(f,'Visible','on');

%% Define the default system
MySystem.na = 1;             % First order
MySystem.nb = 0;             % Without zero
MySystem.sysc = tf(1,[1 1]); % Time constant = 1s; Gain = 1
setappdata(gcf,'SystemData', MySystem);

%% Define the simulation protocol
MySimulationData.SNR = 20;  % Signal to noise ratio (dB)
MySimulationData.ts = 0.1;  % Sampling time t_s
MySimulationData.N = 500;   % Data length
setappdata(gcf,'SimulationData', MySimulationData);

%% Define the default design parameter T_hat
MyRPMmodel.Kr = 20;         % T_hat = Kr * t_s
setappdata(gcf,'MyRPMmodel', MyRPMmodel);

%% Select the system structure
function popup_menu_Callback(source,eventdata)
   val = get(source,'Value');
   switch val;
    case 1
        MySystem.na = 1; % Transfer function orders
        MySystem.nb = 0; % Numbers of zero
    case 2
        MySystem.na = 2;
        MySystem.nb = 0; 
    case 3
        MySystem.na = 2;
        MySystem.nb = 1; 
    case 4
        MySystem.na = 3;
        MySystem.nb = 0;
    case 5
        MySystem.na = 3; 
        MySystem.nb = 1;
    case 6
        MySystem.na = 3; 
        MySystem.nb = 2;
   end
   setappdata(gcf,'SystemData', MySystem);
end

%% Generate a new system
function NewSysButton_Callback(source,eventdata)
    na = MySystem.na;
    nb = MySystem.nb;
    % Get random poles
    den = sqrt(-1);
    while (isreal(den)==0)      % While a parameter is complex
        % Get random poles < -0.01 and with a ratio of 20
        pole = eig(rss(na));    
        while (max(real(pole))>-0.01)||(min(real(pole))/max(real(pole))>20)
            pole = eig(rss(na));
        end
        % Build the denominator
        den = [1];
        for j = 1:na
            den = conv(den,[1/-pole(j) 1]);
        end
    end
    % Build the numerator
    num = [1];
    for j = 1:nb
        % Get random zeros inside [-1.5;1.5]
        zero = 3*rand(1)-1.5;
        num= conv(num,[1/zero 1]);
    end
    num=num/den(1);
    den=den/den(1); % Get unity gain
    MySystem.sysc= tf(num,den);   
    % Compute the sampling time 
    MySimulationData.ts = 1/min(abs(pole))/5; 
    setappdata(gcf,'SystemData', MySystem);
    % Display the transfer function
    % Numerator
    polystring='';
    for i=1:length(num)-1
        if (i>1)&&(num(i)>0)
            polystring=[polystring '+'];
        end
        polystring=[polystring num2str(num(i),'%0.2g') 's^' num2str(length(num)-i)];
    end
    if (num(length(num))>0)&&(length(num)>1)
        polystring=[polystring '+'];
    end
    polystring=[polystring num2str(num(length(num)),'%0.2g')];
    htext    = uicontrol('Style','text','string',polystring,'BackgroundColor',[0.753 0.753 0.753],...
        'Position',[165,450,210,16],'FontSize',10);
    % Line
    polystring='';
    for i=1:length(den)
        polystring=[polystring '-------'];
    end
    htext    = uicontrol('Style','text','string',polystring,'BackgroundColor',[0.753 0.753 0.753],...
        'Position',[165,442,210,10],'FontSize',10);
    % Denominator
    polystring=['s^' num2str(length(den)-1) '+'];
    for i=2:length(den)-1
        polystring=[polystring num2str(den(i),'%0.2g') 's^' num2str(length(den)-i) '+'];
    end
    polystring=[polystring num2str(den(length(den)),'%0.2g')];
    htext    = uicontrol('Style','text','string',polystring,'BackgroundColor',[0.753 0.753 0.753],...
        'Position',[165,422,210,20],'FontSize',10);
    % Edit field to select the design parameter
    Kr=MyRPMmodel.Kr;
    ts=MySimulationData.ts;
    htext    = uicontrol('Style','edit','String',num2str(Kr*ts,'%0.2g'),'BackgroundColor','white',...
                     'Position',[380,441,60,22],'Callback',{@DesignPara_Callback});
end

%% Edit field for the signal to noise ratio choice
function SnrEdit_Callback(source,eventdata)
    MySimulationData.SNR = str2double(get(source,'string'));
    setappdata(gcf,'SimulationData', MySimulationData);
end

%% Pushbutton for a new simulation
function NewSimButton_Callback(source,eventdata)
    %%  Simulate of the continuous-time system with an output-error noise
    N=MySimulationData.N;
    ts=MySimulationData.ts;
    SNR=10^(MySimulationData.SNR/10);
    sysc=MySystem.sysc;
    na=MySystem.na;
    % The time vector
    t=[0:ts:(N-1)*ts];
    % The square input
    u=square(2*pi/(200*ts)*t);
    % The output
    y = lsim(sysc,u,t,zeros(max(na),1),'zoh');
    % The noise sequence
    e = sqrt(var(y)/SNR)*randn(N,1);
    y = y + e; % Add noise to the output
    MySimulationData.DATA = iddata(y,u',ts);
    setappdata(gcf,'SimulationData', MySimulationData);
    % Plot input-output signals and +/- 5% band around the steady states
    plot(t,u,'b',t,y,'g',t,[1.05*ones(N,1) 0.95*ones(N,1) -0.95*ones(N,1) -1.05*ones(N,1)],'--r')
    xlabel('Time (s)');
    legend('Input','Measured output',4);grid;
end

%% Edit field for the design parameter
function DesignPara_Callback(source,eventdata)
    ts=MySimulationData.ts;
    T_hat = str2double(get(source,'string'));
    MyRPMmodel.Kr = (round (T_hat/ts/2))*2;
    setappdata(gcf,'MyRPMmodel', MyRPMmodel);
end

%% Pushbutton to help the user
function HelpButton_Callback(source,eventdata)
    fhelp = figure('Visible','off','Position',[360,400,500,120]);
    set(fhelp,'Name','How to choose T_hat? ')
    set(fhelp,'Visible','on');
    text_help='The design parameter T_hat (reinitialization interval) of the RPM model should be selected such that T_hat is near the double of the main time constant. With the plot, you can compute the main time by using the 5% settling time which is equivalent to three times the main time constant approximately ';
    htext    = uicontrol('Style','text','String',text_help,'BackgroundColor',[0.753 0.753 0.753],...
                     'Position',[30,10,450,100],'FontSize',10);
end
    
%% Pushbutton to estimate the RPM model
function EstimateButton_Callback(source,eventdata)
    N=MySimulationData.N;
    ts=MySimulationData.ts;
    na=MySystem.na;
    nb=MySystem.nb;
    Kr=MyRPMmodel.Kr;
    DATA=MySimulationData.DATA;
    u=DATA.u;
    y=DATA.y;
    % Least square estimate of the CT RPM model
    M_RPM = lsctrpm(DATA,[na nb 0],Kr,'rect');
    % Model simulation
    t=[0:ts:(N-1)*ts];
    y_hat = lsim(M_RPM,u,t,zeros(max(na),1),'zoh');
    % Plot input-output signals
    plot(t,u,'b',t,y,'g',t,y_hat,'r',t,[1.05*ones(N,1) 0.95*ones(N,1) -0.95*ones(N,1) -1.05*ones(N,1)],'--r')
    legend('Input','Measured output','Simualted output',4);grid;
    % Display the transfer function
    num=M_RPM.num{1}(na-nb+1:na+1);
    den=M_RPM.den{1};
    % Numerator
    polystring='';
    for i=1:length(num)-1
        if (i>1)&&(num(i)>0)
            polystring=[polystring '+'];
        end
        polystring=[polystring num2str(num(i),'%0.2g') 's^' num2str(length(num)-i)];
    end
    if (num(length(num))>0)&&(length(num)>1)
        polystring=[polystring '+'];
    end
    polystring=[polystring num2str(num(length(num)),'%0.2g')];
    htext    = uicontrol('Style','text','string',polystring,'BackgroundColor',[0.753 0.753 0.753],...
        'Position',[580,385,210,16],'FontSize',10);
    % Line
    polystring='';
    for i=1:length(den)
        polystring=[polystring '-------'];
    end
    htext    = uicontrol('Style','text','string',polystring,'BackgroundColor',[0.753 0.753 0.753],...
        'Position',[580,377,210,10],'FontSize',10);
    % Denominator
    polystring=['s^' num2str(length(den)-1) '+'];
    for i=2:length(den)-1
        polystring=[polystring num2str(den(i),'%0.2g') 's^' num2str(length(den)-i) '+'];
    end
    polystring=[polystring num2str(den(length(den)),'%0.2g')];
    htext    = uicontrol('Style','text','string',polystring,'BackgroundColor',[0.753 0.753 0.753],...
        'Position',[580,357,210,20],'FontSize',10);
    % Compute the fit
    FIT=100*(1 - norm(y_hat - y)/norm(y-mean(y)));
    fitting = ['Fit=',num2str(FIT,'%0.4g'),'%'];
    htext    = uicontrol('Style','text','string',fitting,'BackgroundColor',[0.753 0.753 0.753],...
        'Position',[480,380,90,18],'FontSize',10);
    MyRPMmodel.fit = FIT;
    setappdata(gcf,'MyRPMmodel', MyRPMmodel);
    % Text "The best choice"
    htext    = uicontrol('Style','text','String','The best choice','BackgroundColor',[0.753 0.753 0.753],...
                     'Position',[380,350,150,22],'FontSize',14);
    % Pushbutton to know the best choice 
    PB    = uicontrol('Style','pushbutton','String','What is it?',...
                     'Position',[380,320,90,25],'Callback',{@BestChoiceButton_Callback});
end

%% Pushbutton to test various design parameters T_hat and to search the best one
function BestChoiceButton_Callback(source,eventdata)
    N=MySimulationData.N;
    ts=MySimulationData.ts;
    na=MySystem.na;
    nb=MySystem.nb;
    DATA=MySimulationData.DATA;
    u=DATA.u;
    y=DATA.y;
    FIT=MyRPMmodel.fit;
    t=[0:ts:(N-1)*ts];
    % Test for different Kr
    FIT_max=FIT;    % Initialization of the best fit
    Kr_best=20;     % Initialization of the corresponding Kr
    Kr(1)=8;        % Start with the value 8
    for i=1:50      % Test the range: Kr is inside [8,108] ; T_hat = Kr * t_s
        % Least square estimate of the CT RPM model
        M_RPM = lsctrpm(DATA,[na nb 0],Kr(i),'rect');
        % Model simulation
        y_hat = lsim(M_RPM,u,t,zeros(max(na),1),'zoh');
        % Compute the fit
        FIT(i)=100*(1 - norm(y_hat - y)/norm(y-mean(y)));
        if ((FIT(i)<5)&&(i>4))
            break   % Stop the loop if fit < 5% for i > 4
        end
        if (FIT(i)>=FIT_max)    % Save the best fit and the best Kr
            FIT_max=FIT(i);
            Kr_best=Kr(i);
        end
        % Display the current fit and T_hat
        Best = ['Fit = ',num2str(FIT(i),'%0.4g'),'% for T_hat = ',num2str(Kr(i)*ts,'%0.3g')];
        htext    = uicontrol('Style','text','string',Best,'BackgroundColor',[0.753 0.753 0.753],...
            'Position',[480,320,190,18],'FontSize',10);
        pause(0.01);
        % Increment of Kr
        Kr(i+1)=Kr(i)+2;
    end
    % Display the best T_hat
    Best = ['The best T_hat is ',num2str(Kr_best*ts,'%0.2g')];
    htext    = uicontrol('Style','text','string',Best,'BackgroundColor',[0.753 0.753 0.753],...
        'Position',[480,320,190,18],'FontSize',10);
    % Plot the fit for various T_hat
    fFIT = figure('Visible','off','Position',[360,400,520,400]);
    movegui(fFIT,'center')
    set(fFIT,'Name','The fitting for various T_hat ')
    set(fFIT,'Visible','on');
    ha       = axes('Units','Pixels','Position',[60,40,440,340]);
    plot(Kr(1:length(FIT))*ts,FIT);grid
    xlabel('T_{hat}')
    ylabel('Fit (%)')
end

%% Pushbutton to exit
function Exitbutton_Callback(source,eventdata)
close(gcf)
end
end

