function [f,g,h] = prefilter(f0,g0,h0,S)
%PREFILTER Adds an input filter to a linear-analytic system
%
% [F,G,H]=PREFILTER(F0,G0,H0,S) adds the state-space filter (S.A,S.b,S.c) to the
% input of the literal symbolic linear-analytic state-space model
% x'= f0(x) + g0(x)u
% y = h*x

%   CBVK Toolbox
%   JHMG, PMSB 2016

 
% Filter in symbolic form
A = sym(S.A,'d');
b = sym(S.b,'d');
c = sym(S.c,'d');

% Filter, system and overall orders
N = size(A,1);
m = size(f0,1);
M = N+m;

% Symbolic state vector
x = sym(zeros(M,1));
for k=1:min(M,9)
    x(k) = sym(['x0' num2str(k)]);
end;    

% M<100
if M>9
  for k=10:M
    x(k)= sym(['x' num2str(k)]);
  end
end


% Overall state-space description
%
% [x1:N  ]'   [A*x1:N                        ]   [b]
% [xN+1:M]' = [f0(xN+1:M) + g0(xN+1:M)*c*x1:N] + [0]u
%
%           [x1:N  ]
% y = [0 h0][xN+1:M]

f = sym(zeros(M,1));
g = sym(zeros(M,1));

% Filter part
f(1:N) = A*x(1:N);
g(1:N) = b;

% Linear-analytic part
for k=m:-1:1
    f0 = subs(f0,x(k),x(k+N));
    g0 = subs(g0,x(k),x(k+N));
end;    
f0 = f0 + g0*(c*x(1:N));
f(N+1:end) = f0;
h=zeros(1,M);
h(N+1:end)=h0;
