function [f,p] = dlqrge(beta,A,B,Q,R,W)
%DLQRGE calculates f of the feedback law:  u = -fx  that maximizes 
%
%          sum {beta^t [x'Qx + u'Ru +2x'Wu] }
%       
%  subject to 
%		x[t+1] = Ax[t] + Bu[t] 
%
%  where x is the mx1 vector of states, u is the kx1 vector of controls,
%  A is mxm, B is mxk, Q is mxm, R is kxk, W is mxk.
%                
%  INPUTS: beta, A, B, Q, R, W  (or just beta, A, B, Q, R if there are no
%          cross products, W=0).  Set beta=1, if there is no discounting.
%
%  OUTPUTS: 
%    (i) f= (R+beta*B'*p*B)\(beta*B'*p*A+W'), the decision rule.
%   (ii) p= beta*A'*p*A + Q -(beta*A'*p*B+W)*f, the solution to the matrix
%           Riccati equation
%
%  NOTES: see also DOUBLE, DRICC, OLRP.  
%
m=max(size(A));
m2=2*m;
[rb,cb]=size(B);
if nargin==5; W=zeros(m,cb); end;
if rank(R)==cb;
  A=sqrt(beta)*(A-B*(R\W'));
  B=sqrt(beta)*B;
  Q=Q-W*(R\W');
  if rank(A)==m;
    [V,D] = eig([A+B/R*B'/A'*Q  -B/R*B'/A'; -A'\Q  inv(A)']);
    d = diag(D);
    [d,index] = sort(abs(d));	 % sort on magnitude of eigenvalues
    if (~((d(m) < 1) & (d(m+1)>1)));
	error('Can''t order eigenvalues, (A,B) may be uncontrollable.')
    end;
    % select vectors with eigenvalues inside unit circle
    chi = V(1:m,index(1:m));
    lambda = V((m+1):(2*m),index(1:m));
    p = lambda/chi;
  else;
    M=[A zeros(m);-Q eye(m)];
    L=[eye(m) B*(R\B');zeros(m) A'];
    [V,D]=eig(M,L);
    Dd=diag(D);
    rdd=find(real(Dd)>=inf);
    if ~isempty(rdd);
      Dd(rdd)=real(Dd(rdd));
    end;
    sorteig=sort(Dd);
    eigv1=sorteig(1:m2-1);
    eigv2=sorteig(2:m2);
    if max(eigv1==eigv2);         % use Schur Method
      [MM,LL,QQ,Z]=qz(M,L);
      d=(abs(Dd)>ones(m2,1));
      if sum(d)~=m; error('wrong number of stable eigenvalues'); end;
      done=0;
      while ~done;
        switch=0;
        it=1;
        while ~switch & it<=m2-1;
          it1=it+1;
          if d(it)>d(it1);
            tpos=d(it);
            d(it)=d(it1);
            d(it1)=tpos;
            adbc=MM(it,it)*LL(it1,it1)-MM(it1,it1)*LL(it,it);
            S=(LL(it,it1)*MM(it1,it1)-MM(it,it1)*LL(it1,it1))/adbc;
            T=(MM(it,it1)*LL(it,it)-LL(it,it1)*MM(it,it))/adbc;
            S1=1/sqrt(1+S*S);
            T1=1/sqrt(1+T*T);
            Q2=[T*T1 -T1; T1 T*T1];
            Z2=[S*S1  S1; S1 -S*S1];
            tmatq=eye(m2);
            tmatz=tmatq;
            tmatq(it:it1,it:it1)=Q2;
            tmatz(it:it1,it:it1)=Z2;
            Q2=tmatq;
            Z2=tmatz;
            MM=Q2*MM*Z2;
            LL=Q2*LL*Z2;
            Z=Z*Z2;
            switch=1;
          end;
          it=it1;
        end;
        done=~max(d(1:m));
      end;
      U=Z(:,1:m);
    else;
      U=V(:,find(abs(Dd)<ones(m2,1) & ~(diag(D)==NaN)) );
    end;
    p=U(m+1:m2,:)/U(1:m,:);
  end;
  f=(R+B'*p*B)\(B'*p*A)+R\W';
else;
  p=-.01*eye(m);
  f0=(R+beta*B'*p*B)\(beta*B'*p*A+W');
  dd=1;
  it=1;
  maxit=5000;
  tol=1e-8;       % reset tolerance if necessary 
  while (dd> tol & it<= maxit);
    p =beta*A'*p*A + Q -(beta*A'*p*B+W)*f0;
    f1=(R+beta*B'*p*B)\(beta*B'*p*A+W');
    dd=max(max(abs( (f1-f0)./(1+abs(f0)) )));
    it=it+1;
    f0=f1;
  end;
  if it>=maxit; disp('WARNING: Iteration limit of 5000 reached in DLQRGE'); end;
end;
