%
%  This routine determines the controller to place the closed
%  loop poles at the desired locations by solving the 
%  Diophantine equations
%
%  Inputs: Gp = plant transfer function
%           p = an array containing the desired closed loop pole locations
%           m = order of desired controller
%  Outputs: Gc = desired controller transfer function or [] for errors
%
%   Notes:  n = degree of denominator of proper transfer function Gp
%           the number of poles must equal n+m
%           if m = n, a type 1 controller is designed (A0=0)
%           if m = n-1, a type 0 controller is designed (A0 not
%           deliberately set to 0)
%
function Gc = solve_diophantine(Gp,p,m)
%  
%  get the numerator (N) and denominator (D) of the plant
%
  [N,D] = tfdata(Gp,'v');
  n = length(D)-1;
%
%  we need n+m poles
%
  if (n+m ~= length(p))
      fprintf('ERROR! We need n (%d) + m (%d) >= #closed loop poles (%d) \n', n, m , length(p));
      Gc = [];
      return;
  end;
%
  if ( (m > n) || (m < n-1))
      fprintf('ERROR!  m = %d, n = %d : We need m = n-1 or m = n \n',m,n);
      Gc = [];
      return;
  end;
%
%  Construct the closed loop polynomial
%
  F = poly(p); 
%
%  For convenience, first make sure N and D are the same length
%
  nN = length(N);
  nn = n-nN;
  if (nn > 0) N = [zeros(1,nn) N]; end;
%
%  Now construct the S matrix
%
  Nt = fliplr(N)';
  Dt = fliplr(D)';
  start = 1;
  S = [];
  for i = 1:m+1
      index = 2*i-1;
      S(start:start+n,index) = Dt;
      S(start:start+n,index+1) = Nt;
      start = start+1;
  end;
% 
%  what we do next depends on whether or not we want a type 1
%  system or not.  If m = n, we want a type 1 system
%
  if (n == m)
%
     fprintf(' Making a type 1 system ... \n');
%
%    eliminate the first (A0) column from S
%
     S = S(:,2:end);
%
%    solve for the coefficients
%
     x = S\fliplr(F)';
%
%    get the A and B components
%
     A = [0];
     B = [x(1)];
     for i = 2:2:length(x)-1
       A = [x(i) A];
       B = [x(i+1) B];
     end;
%
  else  % do the type 0 system
%
%    solve for the coefficients
%
     x = S\fliplr(F)';
%
%    get out the A and B components
%
     A = [];
     B = [];
     for i = 1:2:length(x)
       A = [x(i) A];
       B = [x(i+1) B];
     end;
 end;
%
%  construct the controller transfer function
%
  Gc = tf(B,A);
%
%  find the closed loop transfer function
%
  Go = feedback(Gc*Gp,1);
  Go = minreal(Go);
%
%  Find the poles of the closed loop transfer function
%
  disp('The poles of the closed loop transfer function are : ');
  pole(Go)'
  return;