function M = lsctrpm(DATA,ORDERS,Kr,method)

%   LSCTRPM computes LS-estimates of Continuous-Time MIMO RPM models 
%   (RPM : reinitialized partial moment)
%   M=lsctrpm(DATA,ORDERS,Kr,OPT)
% 
%   M: returned with the estimated parameters of the RPM model in an IDPOLY
%   object format.
%   See HELP IDPOLY for all details.
%  
%   DATA: The estimation data as an IDDATA object. 
%   Data must be regularly sampled.
%   See  HELP IDDATA.
%     
%   ORDERS = [na nb nk], the orders and delays of the above model.
%   For multi-output systems, ORDERS has as many rows as there are outputs.
%   na is then an ny|1 column vector whose j:th row gives the order of the
%   polynomials (deniminators of the row transfer functions) relating 
%   the j:th output. Similarly nb and nk are ny|nu matrices. (ny:# of 
%   outputs, nu:# of inputs).
% 
%	Optional inputs:
%	Kr    : interval of reinitialization
%           Default : Kr = 20
%   method: method used for the calculation of integrations of the input
%           - 'cont' use the Simpson's method
%           - 'rect' use the rectangle method
% 	        Default: a procedure tests the input type and sets
%                    'rect' for piecewise constant input
%
%	Author  : Régis Ouvrard, University of Poitiers
%	e-mail  : regis.ouvrard@univ-poitiers.fr
%	Last Revised : 30 March 2010
%   Descritpion  : Rapport Interne N° 20100226OR
%                  http://laii.univ-poitiers.fr/ouvrard/CTRPM/

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

%% Preliminaries 
[N,ny] = size(DATA.y); 
[N,nu] = size(DATA.u);
ts=DATA.Ts;

[nr,nc]=size(ORDERS);
if nc<(nu+2)    % If the delays are not specified
    ORDERS(1:ny,nu+2:2*nu+1)=zeros(ny,nc-1);
end
na=ORDERS(1:ny,1);
nb=ORDERS(1:ny,2:nu+1);
nk=ORDERS(1:ny,nu+2:2*nu+1);
nk = round(nk/ts);  % Input delays (samples)

if ( nargin<3 )
	Kr = [];
end
if ( isempty(Kr) )
	Kr = 20*ones(1,ny);
end

if ( nargin<4 )
	method = [];
end

%% 	If no method : Test of the input type - piecewise constant or not?
if ( isempty(method) )
	udiff	=	diff(DATA.u);		
	Nudiff	=	length(udiff);		
	k	=	find(udiff==0);		
	if ( length(k) >  Nudiff / 10) % Input is piecewise constant
		method = 'rect'; 	% Rectangle method for integrations of the input
	else 					
		method = 'cont'; 	% Simpson's method for integrations of the input
	end   
end

%% Loop to estimate ny MISO models 
for counter1=1:ny
    
    %% Inputs with delays
    for counter2 = 1:nu,
        u(:,counter2) = [zeros(nk(counter1,counter2),1);DATA.u(1:N-nk(counter1,counter2),counter2)];
    end

    %% Extract the current output 
    y=DATA.y(:,counter1);

    %% Compute the fi functions used to simulate the CT RPM FIR filter m(t)
    fi=zeros(na(counter1)+1,Kr(counter1)+1);
    for L=0:Kr(counter1)
        for i=0:na(counter1)
            if (i==0)
                fi(i+1,L+1)=((L*ts)^na(counter1))*((Kr(counter1)*ts-L*ts)^(na(counter1)-1))/(factorial(na(counter1)-1)*((Kr(counter1)*ts)^na(counter1)));
            else
                for j=0:i
                    if ((na(counter1)-j-1)>=0)
                        fi(i+1,L+1)=fi(i+1,L+1)+((-1)^j)*Cij(i,j)*factorial(na(counter1)-1)*factorial(na(counter1))/(factorial(na(counter1)-1-j)*factorial(na(counter1)-i+j))*((L*ts)^(na(counter1)-i+j))*((Kr(counter1)*ts-L*ts)^(na(counter1)-j-1));
                    end
                end
                fi(i+1,L+1)=((-1)^i)*fi(i+1,L+1)/(factorial(na(counter1)-1)*((Kr(counter1)*ts)^na(counter1)));
            end
        end
    end

    %% Compute the Fi^{sq} functions used to simulate CT RPM FIR filter
    %% m(t) with a piecewise constant input
    Fi_rect=zeros(na(counter1)+1,Kr(counter1));
    for L=0:Kr(counter1)-1
        for i=0:na(counter1)
            if (i==0)
                for r=0:na(counter1)-1
                    Fi_rect(i+1,L+1)=Fi_rect(i+1,L+1)+((-1)^r)*Cij(na(counter1)-1,r)*(Kr(counter1)*ts)^(na(counter1)-1-r)*(((L+1)*ts)^(na(counter1)+r+1)-(L*ts)^(na(counter1)+r+1))/(na(counter1)+r+1)/(factorial(na(counter1)-1)*((Kr(counter1)*ts)^na(counter1)));
                end
            else
                for j=0:i
                    Fi_rect_temp=0;
                    if ((na(counter1)-j-1)>=0)
                        for r=0:na(counter1)-j-1
                            Fi_rect_temp=Fi_rect_temp+((-1)^r)*Cij(na(counter1)-j-1,r)*(Kr(counter1)*ts)^(na(counter1)-j-1-r)*(((L+1)*ts)^(na(counter1)-i+j+r+1)-(L*ts)^(na(counter1)-i+j+r+1))/(na(counter1)-i+j+r+1);
                        end
                        Fi_rect(i+1,L+1)=Fi_rect(i+1,L+1)+((-1)^j)*Cij(i,j)*factorial(na(counter1)-1)*factorial(na(counter1))/(factorial(na(counter1)-1-j)*factorial(na(counter1)-i+j))*Fi_rect_temp;
                    end
                end
                Fi_rect(i+1,L+1)=((-1)^i)*Fi_rect(i+1,L+1)/(factorial(na(counter1)-1)*((Kr(counter1)*ts)^na(counter1)));
            end
        end
    end

    %% Compute the functions beta(t) 
    beta(:,:)=zeros(N-Kr(counter1),sum(nb(counter1,:)+1));
    for counter2=1:nu,
        for k=Kr(counter1)+1:N
            if (method=='rect') % The rectangle method
                temp=u(k-Kr(counter1):k-1,counter2);
                for i=0:nb(counter1,counter2)
                    beta(k-Kr(counter1),sum(nb(counter1,1:counter2-1)+1)+i+1)=Fi_rect(i+1,:)*temp;
                end
            else                % The Simpson's method
                for L=2:2:Kr(counter1)
                    temp=u(k-Kr(counter1)+L-2:k-Kr(counter1)+L,counter2);
                    for i=0:nb(counter1,counter2)
                        beta(k-Kr(counter1),sum(nb(counter1,1:counter2-1)+1)+i+1)=beta(k-Kr(counter1),sum(nb(counter1,1:counter2-1)+1)+i+1)+ts/3*[fi(i+1,L-1) 4*fi(i+1,L) fi(i+1,L+1)]*temp;
                    end
                end
            end
        end
    end

    %% Compute the functions alpha(t) (and gamma(t)) with Simpson's method
    alpha(:,:)=zeros(N-Kr(counter1),na(counter1)+1);
    for k=Kr(counter1)+1:N
        for L=2:2:Kr(counter1)
            temp=[y(k-Kr(counter1)+L-2:k-Kr(counter1)+L)];
            for i=0:na(counter1)
                alpha(k-Kr(counter1),i+1)=alpha(k-Kr(counter1),i+1)-ts/3*[fi(i+1,L-1) 4*fi(i+1,L) fi(i+1,L+1)]*temp;
            end
        end
    end

    %% Generate regressor
    phi=[alpha(:,1:na(counter1)) beta]';

    %%% Get LS-estimate
    theta=inv(phi*phi')*phi*(y(Kr(counter1)+1:N)-alpha(:,na(counter1)+1));

    %% Extract numerator and denominator and build the MISO IDPOLY model
    num=zeros(nu,max(nb(counter1,:))+1);
    for counter2=1:nu
        num(counter2,max(nb(counter1,:))-nb(counter1,counter2)+1:max(nb(counter1,:))+1)=[flipud(theta(na(counter1)+sum(nb(counter1,1:counter2-1)+1)+1:na(counter1)+sum(nb(counter1,1:counter2-1)+1)+nb(counter1,counter2)+1))'];
    end
    M1=idpoly([1 flipud(theta(1:na(counter1)))'],num,[],[],[],0,0);
    for counter2=1:nu
        M1.InputDelay(counter2)=nk(counter1,counter2)*ts;
    end

    %% Build the MIMO model in tf form 
    M(counter1,1:nu)=tf(M1);
    set(M,'OutputName',[M.OutputName(1:counter1-1,1);'y' num2str(counter1)])

    clear alpha
    clear beta
end

%% Internal function
function [result] = Cij(i,j)
%                 i!
%   Compute   ----------
%              j! (i-j)!
result = factorial(i)/(factorial(j)*factorial(i-j));

