mimodioph(A,B,N)

% This function solve MIMO-Diofantine equation.
% this equation should be in this form
%                         I = E(j)*A_tild + z^(-j)*F(j)
% This equation is based on chapter 6 of Model Predictive Control Book by
% E.F Comacho and C. Bordons.
% [G,Gp,F,E] = mimodioph(A,B,N)
% where A and B are the polynomials are calculated from system transfer 
% function as following example.
% please contact me for any question or suggestion.
%
%          **************************************************
%            *      Author : Davood (David) Shaghaghi     *
%            *      Email  : [e-mail]   *
%            *         KNT University of Technology       *
%            *               19 December 2012             *
%            *      Last revision: 22-June-2013           * 
%          **************************************************
% 
% Example :
% A and B is 2*2*1 matrix (one-dimensional): 
% A = [1 - 1.8954*z^(-1) + 0.8981*z^(-2)                  0
%                 0                        1 - 1.8452*z^(-1) + 0.8511*z^(-2)];
% 
% B = [0.744*z^(-2) - 0.7094*z^(-3)  , -0.8789*z^(-4) + 0.8278*z^(-5)
%      0.5786*z^(-8) - 0.5398*z^(-9) , -1.302*z^(-4) + 1.1878*z^(-5)];
% 
% Now A and B are converted to a 2*2*n (3-dimensional) matrix that "n"
% depends on degree of polynomials of every arrays of A and B .
% This is done as follows :
% A (:,:,1) = [1 0 ; 0 1 ];
% A (:,:,2) = [-1.8954 0 ; 0 -1.8452 ];
% A (:,:,3) = [.8981 0 ; 0 .8511 ];
% 
% B(:,:,1) = zeros(2);
% B(:,:,2) = zeros(2);     % z^(-1)
% B(:,:,3) = [0.744 0 ; 0 0]; % z^(-2)
% B(:,:,4) = [-0.7094 0 ; 0 0]; % z^(-3)
% B(:,:,5) = [0 -0.8789 ; 0 -1.302];  % z^(-4)
% B(:,:,6) = [0 0.8278 ; 0 1.1878]; % z^(-5)
% B(:,:,7) = zeros(2); % z^(-6)
% B(:,:,8) = zeros(2); % z^(-7)
% B(:,:,9) = [0 0 ; 0.5786 0]; % z^(-8)
% B(:,:,10) = [0 0 ; -0.5398 0]; % z^(-9)
  
function [G,Gp,F,E] = mimodioph(A,B,N)

% Y_n*1 = A_n*n \ B_n*m * U_m*1
% A(z^-1) = I + A1*z^-1 + A2*z^-1 + ... + Ana*z^-na
% B(z^-1) = I + B1*z^-1 + B2*z^-1 + ... + Bnb*z^-nb

na = size(A,3); % degree of A
nb = size(B,3); % degree of B
m = size(B,2);  % num. of inputs
n = size(A,2);  % num. of outputs

I = eye(n);

for k=1:na
    if k==1
        A_tild(:,:,k) = I;
    elseif k==2
        A_tild(:,:,k) = A(:,:,k)- I;
    else
        A_tild(:,:,k) = A(:,:,k) - A(:,:,k-1);
    end
end
A_tild(:,:,k+1) = -A(:,:,k);

F = cell(N+1,1);
E = cell(N+1,1);
for k=1:N
    F{k} = zeros(n,n,na+1);
end

E{1}(:,:,1) = I;
F{1} = -A_tild(:,:,2:end);
F{1}(:,:,na+1) = zeros(n,n);

for j=1:N
    R{j} = F{j}(:,:,1);
    E{j+1} = E{j};
    E{j+1}(:,:,j+1) = R{j};
    for i=1:na
        F{j+1}(:,:,i) = F{j}(:,:,i+1) - R{j} * A_tild(:,:,i+1);
    end
end

for k=1:N
    F{k}(:,:,na+1)=[];
end


%% G:
for k = 1:N    % stage k
    for i = 1:n
        for j = 1:m
            g = 0;
            for p = 1:n    % summation
                vE = zeros(1,size(E,3));
                vB = zeros(1,size(B,3));
                for q = 1:size(E{k},3)
                    vE(q) = E{k}(i,p,q);
                end
                for r = 1:size(B,3)
                    vB(r) = B(p,j,r);
                end
                g = g + conv(vE,vB);
            end
            for f = 1:length(g)
                GG{k}(i,j,f) = g(f);
            end
        end
    end
end

for k = 1:N
    ng = size(GG{k},3);
    G{k} = GG{k}(:,:,1:k);
    Gp{k}= GG{k}(:,:,k+1:ng);
end

for k = 1:N
    snow = size(G{k},3);
    send = size(G{end},3);
    G{k}(:,:,snow+1:send) = zeros(n,m,send-snow); 
end

G = G';
Gp = Gp';

end