% Francis equations
% by Stefano Panzieri  (panzieri@uniroma3.it)
% 24-05-2007
%
%   Solves:
%   [A B]*[Pg] - [I 0]*[Pg] * [S] = [ -P ]
%   [C 0] [Ga]   [0 0] [Ga]         [ -Q ]
%

function [Gamma,Pigreco]=Francis_EQ(A,B,C,S,P,Q)

% Dimensioni 
n=size(A,1);
ns=size(S,1);
ni=size(B,2);
nu=size(C,1);

%
clear mapelmex
%
maple('with(linalg);');
%
maple('alias(Id=&*()):');

%
A_r=reshape(A',1,n^2);
stringa=['[' num2str(A_r(1))];
if n^2>1
    for i=2:n^2
        stringa=[stringa ',' num2str(A_r(i))]; 
    end
end
stringa=[stringa ']'];
maple(['A:=matrix(' num2str(n) ',' num2str(n) ',' stringa ')']);


%
B_r=reshape(B',1,n*ni);
stringa=['[' num2str(B_r(1))];
if n*ni>1
    for i=2:n*ni
        stringa=[stringa ',' num2str(B_r(i))]; 
    end
end
stringa=[stringa ']'];
maple(['B:=matrix(' num2str(n) ',' num2str(ni) ',' stringa ')']);

%
C_r=reshape(C',1,n*nu);
stringa=['[' num2str(C_r(1))];
if n*nu>1
    for i=2:n*nu
        stringa=[stringa ',' num2str(C_r(i))]; 
    end
end
stringa=[stringa ']'];
maple(['C:=matrix(' num2str(nu)  ','  num2str(n) ',' stringa ')']);


%
S_r=reshape(S',1,ns^2);
stringa=['[' num2str(S_r(1))];
if ns^2>1
    for i=2:ns^2
        stringa=[stringa ',' num2str(S_r(i))]; 
    end
end
stringa=[stringa ']'];
maple(['S:=matrix(' num2str(ns) ',' num2str(ns) ',' stringa ')']);


%
P_r=reshape(P',1,n*ns);
stringa=['[' num2str(P_r(1))];
if ns*n>1
    for i=2:ns*n
        stringa=[stringa ',' num2str(P_r(i))]; 
    end
end
stringa=[stringa ']'];
maple(['P:=matrix(' num2str(n) ',' num2str(ns) ',' stringa ')']);


%
Q_r=reshape(Q',1,nu*ns);
stringa=['[' num2str(Q_r(1))];
if nu*ns>1
    for i=2:nu*ns
        stringa=[stringa ',' num2str(Q_r(i))]; 
    end
end
stringa=[stringa ']'];
maple(['Q:=matrix(' num2str(nu) ',' num2str(ns) ',' stringa ')']);


% Null matrix of size nu x ni
stringa=['[0'];
if  nu*ni>1
    for i=2:nu*ni
        stringa=[stringa ', 0']; 
    end
end
stringa=[stringa ']'];
maple(['Z:=matrix(' num2str(nu) ',' num2str(ni) ',' stringa ')']);


%
maple('A_1:=augment(stackmatrix(A,C),stackmatrix(B,Z));');


% Check solution existence
% It is done only for constant references/disturbances 
% it should be extended to consider all the eigenvalues of S
%
d=maple('det(A_1)');
if d==0
    disp('Regulator unsolvable: null rank of [A B; C 0].')
    return
end
    
%
maple('R:=stackmatrix(-P,-Q);');

% Corretto fino a qui

%
n_id=n+nu; % rows and columns of A_2 (remember nu=ni)
A_2=zeros(n_id);
A_2(1:n,1:n)=eye(n);
A_2_r=reshape(A_2,1,n_id*n_id);
stringa=['[' num2str(A_2_r(1))];
for i=2:n_id*n_id
    stringa=[stringa ',' num2str(A_2_r(i))]; 
end
stringa=[stringa ']'];
maple(['A_2:=matrix(' num2str(n_id) ',' num2str(n_id) ',' stringa ')']);


% Pi greek and Gamma variables
incogn='';
incogn_p='';
incogn_g='';
for i=1:n_id
    for j=1:ns
        if i<=n
            incogn=[incogn 'p' num2str(i*ns+j-ns)];
            incogn_p=incogn;
        else
            incogn=[incogn 'g' num2str(i*ns+j-n*ns-ns)];
            incogn_g=[incogn_g 'g' num2str(i*ns+j-n*ns-ns)];
            if not(i==n_id & j==ns)
                incogn_g=[incogn_g ','];
            end
        end
        if not(i==n_id & j==ns)
            incogn=[incogn ','];
        end
    end
end

% % Pi greek and Gamma matrix
maple(['X:=matrix(' num2str(n_id) ',' num2str(ns) ',[' incogn '])']);

% Francis equations
maple('SS:=evalm(A_1&*X-A_2&*X&*S-R)');


% Extract equations
n_eq=0;
for i=1:n_id
    for j=1:ns
        n_eq=n_eq+1;
        str_eq=['eq' num2str(n_eq) ':=SS[' num2str(i) ',' num2str(j) ']=0'];
        maple(str_eq);
    end
end

%
str_eq='eq1';
for i=2:n_eq
    str_eq=[str_eq ', eq' num2str(i)];
end

maple(['sols:=solve({' str_eq '},{' incogn '})']);

%
maple('assign(sols);');

% 
Pigreco_s=maple(['Pigreco:=matrix(' num2str(n) ',' num2str(ns) ',[' incogn_p ']);']);
Pigreco=reshape(str2num(Pigreco_s(18:end)),ns,n)';

%
Gamma_s=maple(['Gamma:=matrix(' num2str(n_id-n) ',' num2str(ns) ',[' incogn_g '])']);
Gamma=reshape(str2num(Gamma_s(16:end)),ns,n_id-n)';


