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.
% where C and A are the polynomials according to your variable (for
% example s in laplace domain or z in z-transform domain) and n is the
% order E plus one (order{E} = n-1).
% please contact me for any question or suggestion.
%
%          **************************************************
%            *      Author : Davood (David) Shaghaghi     *
%            *      Email  : [e-mail]   *
%            *         KNT University of Technology       *
%            *               19 December 2012             *
%          **************************************************

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