%
%  This function computes the controller for 
%  a Diophantine sysytem. This version is only valid for
%  a second order system of the form
%
%    Gp(s) = a/(bs^2 + cs+d)
%
%  Inputs:  Amp = amplitude of the step (in cm)
%           Gp = the plant transfer function
%           p = vector of the desired closed loop poles
%           m = order of compensator (1 or 2)
%           final_time = final time to run the simulation for
%           filename = the name of the file containing the measured data
%
function diophantine(Amp,Gp,p,m,final_time,filename);
%
%  be sure the number of poles is correct
%
if (length(p) ~= (m+2))
    fprintf('Error! there must be m+2 closed loop poles');
    return;
end;
%
%  Determine the plant parameters
%
  [num,den] = tfdata(Gp,'v');
  N0 = num(end);
  D0 = den(3);
  D1 = den(2);
  D2 = den(1);
%  
%  Determine the desired closed loop transfer function
%
  DD = poly(p);
  F0 = DD(end);
  F1 = DD(end-1);
  F2 = DD(end-2);
  F3 = DD(end-3);
  if (m == 2) F4 = DD(1); end;
%
%  Now solve the equations for m = 1
%
  if (m == 1)
      A = [D0 N0 0 0; D1 0 D0 N0; D2 0 D1 0; 0 0 D2 0];
      y = [F0; F1; F2; F3];
      x = A\y;
      disp('The unscaled transfer function .... ')
      Gc = tf([x(4) x(2)],[x(3) x(1)])
  elseif (m == 2)
      A = [N0 0 0 0 0; 0 D0 N0 0 0 ; 0 D1 0 D0 N0; 0 D2 0 D1 0; 0 0 0 D2 0];
      y = [F0; F1; F2; F3; F4];
      x = A\y;
      disp('The unscaled transfer function ...');
      Gc = tf([x(5) x(3) x(1)],[x(4) x(2) 0])
  else
      fprintf('m must be 1 or 2 \n');
      return;
  end;
%
  [numGc,denGc] = tfdata(Gc,'v');
%
%  Now scale the coefficients
%
  scale = 1/max(denGc);
  num = scale*numGc;
  den = scale*denGc;
  disp('The (scaled) controller transfer function ...');
  Gc = tf(num,den)
  fprintf('\n');
  fprintf(['S = (0 to N) ' num2str(fliplr(num),5) '\n']);
  fprintf(['R = (0 to N) ' num2str(fliplr(den),5) '\n']); 
  fprintf('\n');
  G1 = feedback(Gc*Gp,1);
  disp('The closed loop poles are at ...');
  disp(pole(G1))
%
%  Now simulate the system
%
  t = linspace(0,final_time,1000);
  u = Amp*ones(1,length(t));
  y = lsim(G1,u,t);
%
%  plot the results
%
  if (~isempty(filename))
    orient landscape
    encoder = 1;
    data = importdata(filename);
    tdata = data(:,encoder+1);
    ydata = data(:,encoder+3)/2196;
    plot(t,y,':',tdata,ydata,'-'); grid;
    ymin = min(min(y),min(ydata));
    ymax = max(max(y),max(ydata));
    axis([0 final_time ymin ymax]);
    legend('Model y','Real System y',4);
    xlabel('Time (sec)');
    ylabel('Position (cm)');
    title(['Diophantine , m = ' num2str(m) ', poles = [' num2str(p) ']']);
  else    % no data to compare to
    orient landscape
    plot(t,y,'-'); grid;
    xlabel('Time (sec)');
    ylabel('Position (cm)');
    title(['Diophantine, m = ' num2str(m) ', poles = [' num2str(p) ']']);
  end;
%
%  Now spit out the coefficients for the ECP system
%
  fp = fopen('values.par','w');
  fprintf(fp,'[DYNFWD_C]\n');
%
%  get the numerator
%
  N =  length(num);
  temp = [zeros(1,8-N) num];
  S = fliplr(temp);
%
%  get the denominator
%
  N = length(den);
  temp = [zeros(1,8-N) den];
  R = fliplr(temp);
%
%  now write it out
%
  format long 
  fprintf(fp,'s0=%15.12e\n',S(1));
  fprintf(fp,'s1=%15.12e\n',S(2));
  fprintf(fp,'s2=%15.12e\n',S(3));
  fprintf(fp,'s3=%15.12e\n',S(4));
  fprintf(fp,'s4=%15.12e\n',S(5));
  fprintf(fp,'s5=%15.12e\n',S(6));
  fprintf(fp,'s6=%15.12e\n',S(7));
  fprintf(fp,'s7=%15.12e\n',S(8));
  fprintf(fp,'r0=%15.12e\n',R(1));
  fprintf(fp,'r1=%15.12e\n',R(2));
  fprintf(fp,'r2=%15.12e\n',R(3));
  fprintf(fp,'r3=%15.12e\n',R(4));
  fprintf(fp,'r4=%15.12e\n',R(5));
  fprintf(fp,'r5=%15.12e\n',R(6));
  fprintf(fp,'r6=%15.12e\n',R(7));
  fprintf(fp,'r7=%15.12e\n',R(8));
fclose(fp);
  return;