function [F,G,b] = carlbidir(F1,G1,P)
%CARLBIDIR computes the direct Carleman Bilinearization of a linear-analytic
%system
%
% [F,G,B] = CARLBIDIR(F1,G1,P) computes the Pth order direct Carleman
% Bilinearization v' = Fv + Gvu + bu of the linear-analytic system x'= f(x)
% + g(x)u from its Taylor expansion coeficients F1 and G1.

%   CBVK Toolbox
%   JHMG, PMSB 2016

warning('cbvk:carlbidir','Consider the use of carlbilin, which is possibly faster');

m = size(F1,1);  
allterms = m.^(1:P);
MP = sum(allterms);

F = zeros(MP,MP);
G = zeros(MP,MP);

numtermsF = cumsum([1 allterms]);
numtermsG = cumsum([1 1 allterms]);


indf = 0;
for p=1:P
    indi = indf+1;
    indf = indi+allterms(p)-1;
    Tkrn = dkronF(F1,P,p,m,MP,allterms,numtermsF,numtermsG);
    F(indi:indf,:) = Tkrn;
    Tkrn = dkronG(G1,P,p,m,MP,allterms,numtermsF,numtermsG);
    G(indi:indf,:) = Tkrn;
end;
b = zeros(MP,1);
b(1:m) = G1(1:m,1);

function Fkrn = dkronF(Ftylr,P,p,m,MP,...
    allterms,numtermsF,numtermsG)

Ftylr = Ftylr(:,2:end);

if (p==1) 
    Fkrn = Ftylr;
else
    Fkrn = zeros(allterms(p),MP);

    
    for i=1:P-p+1
        Fk = Ftylr(1:m,numtermsF(i):numtermsF(i+1)-1);
        Fk = eyekronP(Fk,p);        
        inds = numtermsF(i+p-1):numtermsF(i+p)-1;
        Fkrn(:,inds) = Fk;
    end;
    
end;


function Gkrn = dkronG(Gtylr,P,p,m,MP,...
    allterms,numtermsF,numtermsG)



if (p==1) 

    Gkrn = [Gtylr(:,2:end) zeros(m,allterms(end))];
else

    Gkrn = zeros(allterms(p),MP);
    
    
    for i=1:P-p+1
        Gk = Gtylr(1:m,numtermsG(i):numtermsG(i+1)-1);
        Gk = eyekronP(Gk,p);
        inds = numtermsF(i+p-2):numtermsF(i+p-1)-1;        
        Gkrn(:,inds) = Gk;
    end;
end;



function M=eyekronP(L,p)
m = size(L,1);
col = size(L,2);
M = zeros(m^p,col*m^(p-1));
for pc1=1:p
    Lk = prodkronI(L,m^(p-pc1));
    M = M + diagblock(Lk,m^(pc1-1));
end;

% Function that constructs a block diagonal matrix where L 
% appears N times
function M = diagblock(L, N)
Rows = size(L,1);
Cols = size(L,2);
M = zeros(N*Rows,N*Cols);
for n=1:N
    M(1+(n-1)*Rows:n*Rows,1+(n-1)*Cols:n*Cols) = L;
end;       

% Function that computes kron(L,eye(dim)) without
% actual multiplications
function M = prodkronI(L,dim)
Rows = size(L,1);
Cols = size(L,2);
M = zeros(Rows*dim,Cols*dim);
for d=1:dim
	M(d:dim:dim*Rows,d:dim:dim*Cols) = L;
end

