function [F1,P] = lqmarkov(beta,A1,A2,B,Q,R,W,pi,S,ct)
%LQMARKOV  Linear quadratic regulator design for discrete-time systems
%          with some states governed by a Markov chain.
%
%          [F,P] = lqmarkov(beta,A1,A2,B,Q,R,W,pi,S,ct) calculates the 
%          decision functions (u[t]) that maximize:
%
%		J = Sum beta^t{x[t]'Qx[t] + u[t]'Ru[t] + 2x[t]'Wu[t]}
%
%          subject to the constraint equations: 
%		
%	        x1[t+1] = A1*x1[t] + A2*x2[t] + B*u[t]
%               pi(i,j) = probability that x2[t+1] = S(:,j) given that
%                         x2[t] = S(:,i), i,j in {1,2,..,n}, all t
%               element ct of x1 is equal to 1.
%
%          The solution is given by 
%               
%               u[t] = -Fj *x1[t]  if x2[t] = S(:,j) 
%         
%          where F = [F1;F2; ... ;Fn].   The value function, given the
%          state vector is [x1;S(:,j)], is equal to x1'*Pj*x1 and
%          P = [P1;P2; ... ;Pn]. 
%        

%	   Ellen McGrattan, 2-3-93

A      = [A1,A2];
[m1,n] = size(A1);
[m2,n] = size(S);
e      = zeros(m1,1);
e(ct)  = 1;

P      = -kron(ones(n,1),eye(m1));
p      = kron(pi,eye(m1))*P;
F0     = [ ];
for j = 1:n;
  pj  = p((j-1)*m1+1:j*m1,:);
  vj  = [eye(m1);S(:,j)*e'];
  F0  = [F0,(R+beta*B'*pj*B)\(beta*B'*pj*A+W')*vj];
end;
dd=1;
it=1;
maxit=5000;
tol=1e-5;       % reset tolerance if necessary 
while (dd> tol & it<= maxit);
  for j=1:n;
    pj  = p((j-1)*m1+1:j*m1,:);
    fj  = F0(:,(j-1)*m1+1:j*m1);
    vj  = [eye(m1);S(:,j)*e'];
    Pj  = vj'*Q*vj-vj'*W*fj-fj'*W'*vj+fj'*R*fj+beta*(A*vj-B*fj)'*pj* ..
          (A*vj-B*fj);
    P((j-1)*m1+1:j*m1,:) = Pj;
  end;
  p     = kron(pi,eye(m1))*P;
  F1    = [ ];
  for j = 1:n;
    pj  = p((j-1)*m1+1:j*m1,:);
    vj  = [eye(m1);S(:,j)*e'];
    F1  = [F1,(R+beta*B'*pj*B)\(beta*B'*pj*A+W')*vj];
  end;
  dd = norm(F1-F0);
  it = it+1;
  F0 = F1;
end;
if it>=maxit; 
  disp(sprintf('WARNING: Iteration limit of %g reached in LQMARKOV',maxit))
end;
