function [f,p] = cfolrp(beta,A,B,Q,R,W,m1)
%CFOLRP calculates f of the feedback law:  u = -fx  that maximizes 
%
%          sum {beta^t [x[t]'Qx[t] + u[t]'Ru[t] +2x[t]'Wu[t]] }
%       
%  subject to 
%		x1[t+1] = A11 x1[t] + A12 x2[t+1] + B1 u[t]
%               x2[t+1] =             A22 x2[t+1]  
%
%  or            x[t+1] = A x[t] + B u[t]
%
%  where it is assumed that the system is in controllability canonical 
%  form and (A11,B1) is controllable.  x1 is a (m1 x 1) vector of states,
%  x2 is a (m2 x 1) vector of states, u is a (k x 1) vector of controls,
%  A is (m x m), B is (m x k), Q is (m x m), R is (k x k), W is (m x k), 
%  and m=m1+m2.
%                
%  INPUTS: beta, A, B, Q, R, W, m1.  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.  This program is the most general of the 
%         three programs.  It runs DOUBLE if R is invertible.  Otherwise,
%         it is the same as DRICC.
%
m=max(size(A));
A11=A(1:m1,1:m1); A12=A(1:m1,m1+1:m);
                  A22=A(m1+1:m,m1+1:m);
Q11=Q(1:m1,1:m1); Q12=Q(1:m1,m1+1:m);
Q21=Q12;          Q22=Q(m1+1:m,m1+1:m);
W1 =W(1:m1,:);    W2 =W(m1+1:m,:);
B1=B(1:m1,:);
maxit=1000;
tol=1e-8;              % reset tol if necessary
if rank(R)>=length(R);
  rw=R\W1';
  a11=sqrt(beta)*(A11-B1*rw);
  b1=sqrt(beta)*B1;
  q11=Q11-W1*rw
%  [k,s]=dlqr(a11,b1,-q11,-R);
%  f1=k+rw;
%  p11=-s;
  [k,s]=double(a11',b1',q11,R);
  f1=k'+rw;
  p11=s;
else;
  p11=zeros(m);
  f0=(R+beta*B1'*p11*B1)\(beta*B1'*p11*A11+W1');
  dd=1;
  it=1;
  while (dd>tol & it<=maxit);
    p11 =beta*A11'*p11*(A11-B1*f0) + Q11 -W1*f0;
    f1=(R+beta*B1'*p11*B1)\(beta*B1'*p11*A11+W1');
    dd=max(max(abs( (f1-f0)./(1+abs(f0)) )));
    it=it+1;
    f0=f1;
  end;
  if it>=maxit; disp('WARNING: Iteration limit of 1000 reached in OLRP'); end;
end;
rb=R+beta*B1'*p11*B1;
rbi=inv(rb);
rbi1=rbi*(beta*B1'*p11*A12+W2');
bafb=beta*(A11'-f1'*B1');
p0=Q12+bafb*p11*A12-f1'*W2';
c1=A22;
it1=1;
diff=1;
diff1=1;
tol1=100*tol;
while (diff>tol & tol1>1e-15);
  while (diff1>tol1 & it1<=maxit);
    p1=p0+bafb*p0*c1;
    diff1=max(max(abs( (p1-p0)./(1+abs(p0)) )));
    p0=p1;
    bafb=bafb*bafb;
    c1=c1*c1;
    it1=it1+1;
  end;
  f20=rbi1+rbi*beta*B1'*p1*A22;
  f2 =rbi1+rbi*beta*B1'*(p1+bafb*p1*c1)*A22;
  diff=max(max(abs( (f2-f20)./(1+abs(f20)) )));
  tol1=.1*tol1;
end;
p12=p1;
f=[f1, f2];
if it1>=maxit; 
  disp('WARNING: Iteration limit of 1000 reached finding p12');
end;
if nargout>1;
  p0=Q22+beta*(A12'*(p11*A12+p12*A22)+A22'*p12'*A12)-f2'*rb*f2;
  c1=sqrt(beta)*A22;
  it1=1;
  diff=1;
  while (diff>tol & it1<=maxit);
    p22=p0+c1'*p0*c1;
    diff=max(max(abs( (p22-p0)./(1+abs(p0)) )));
    p0=p22;
    c1=c1*c1;
    it1=it1+1;
  end;
  p=[p11 p12;p12' p22];
end;
