function C = diophanbtb(G,ps)
%
% C = DIOPHANBTB(G, ps)
% function to solve the general diophantine equations
% and thus directly design a cascade dynamic compensator
%
% Inputs G: open-loop plant model
%        ps: desired pole locations
%       NOTE ps must contain more poles than there are plant poles
%
% Works for full-order compensator BTB 08-23-04

% extract plant numerator and denominator:
% (current matlab version pads the numerator with zeros--should make life
% easier)
[nump,denp] = tfdata(G);
b = nump{:};
a = denp{:};

% check sizes:
% plant order:
na = length(a);

% compensator order:
m = length(ps) - (na - 1);

% difference:
nd = (na - 1) - m;

if m <= 0
    disp('Must have a pole set larger than the number of plant poles')
    return
end

% form the desired CL polynomial:
alfa = poly(ps);
bee = alfa'; % rhs of sylvester matrix eqn:

% form the sylvester matrix:
% break into num and denom halves

% num half:
rsly = zeros(2*m+2,m+1);

% den half:
lsly = zeros(2*m+2,m+1);

% for full-order controllers, force a type 1 system
if nd == 0
    
for n = 0:m
    
    % top "half"
    rsly(n+1,:) = [fliplr(b(1:1+n)), zeros(1,m-n)];
    lsly(n+1,:) = [fliplr(a(1:1+n)), zeros(1,m-n)];
    
    % bottom "half":
    rsly(m+1+n,:) = [zeros(1,n), fliplr(b(n+1+nd:na))];
    lsly(m+1+n,:) = [zeros(1,n), fliplr(a(n+1+nd:na))];
end
    A = [lsly(1:na+m,1:m) rsly(1:na+m,:)];
        dc = A\bee; % solve the system
d = [dc(1:m)', 0];

c = dc(m+1:2*m+1)';
C = tf(c,d);
else
    
for n = 0:m
    
    % top "half"
    rsly(n+1,:) = [fliplr(b(1:1+n)), zeros(1,m-n)];
    lsly(n+1,:) = [fliplr(a(1:1+n)), zeros(1,m-n)];
    
    % bottom "half":
    rsly(m+2+n,:) = [zeros(1,n), fliplr(b(n+1+nd:na))];
    lsly(m+2+n,:) = [zeros(1,n), fliplr(a(n+1+nd:na))];
end
    A = [lsly rsly];
    
    dc = A\bee; % solve the system
d = dc(1:m+1)';

c = dc(m+2:2*m+2)';
C = tf(c,d);
end