function Udt = NatHistStep(U,dt,foi,p0,p1,ps,h0,h1,w)
% function Udt = NatHistStep(U,dt,foi,p0,p1,ps,h0,h1,w)
% Natural history of HPV progression. Propagates the probability 
% density function (pdf) for one time step.
% The states in U are ordered as X = (S,R,X0,X1,X2,X3,XA,DCA)
% 
% INPUT:   size    
%     U   (xm,pm)  U(:,ip) is pdf for population ip, ip=1,...,pm
%                  xm = number of states (8), 
%                  pm = number of populations (e.g.,different age cohorts)
%     dt  (1,1)    dt>0 time step
%     foi (1,pm)   foi(ip) is the force of infection for population ip
%                  from S to X1
%     p0  (1,1)    progression rate from X0
%     p1  (1,1)    progression rate from X1-X3
%     ps  (1,1)    symptom rate from XA to DCA (detected cancer)
%     h0  (1,1)    clearance rate from X0 to R
%     h1  (1,1)    clearance rate from X1-X3 to R
%     w   (1,1)    waning natural immunity rate from R to S
% 
% OUTPUT: size     
%     Udt (xm,pm)  Udt(:,ip) is pdf for population ip after time step dt

[xm,pm] = size(U);
Udt = zeros(xm,pm);
ndQ = zeros(xm,xm); % non-diagonal part of Q

%       S  R  X0 X1 X2 X3 XA DCA
% Q = [ .  w  0  0  0  0  0  0;  S
%       0  .  h0 h1 h1 h1 0  0;  R
%      foi 0  .  0  0  0  0  0;  X0
%       0  0  p0 .  0  0  0  0;  X1
%       0  0  0  p1 .  0  0  0;  X2
%       0  0  0  0  p1 .  0  0;  X3
%       0  0  0  0  0  p1 .  0;  XA
%       0  0  0  0  0  0  ps .]; DCA

% Part which remains the same for all populations:
ndQ(1,2) = w;
ndQ(2,3:6) = [h0,h1,h1,h1];
ndQ(4,3) = p0;
ndQ(5,4) = p1;
ndQ(6,5) = p1;
ndQ(7,6) = p1;
ndQ(8,7) = ps;

for ip = 1:pm
  ndQ(3,1) = foi(ip);
  q = sum(ndQ,1); % diagonal = sum of rates from the state
  % Diagonal for Ndt
  Ndt = diag(exp(-q*dt)); 
  % non-diagonal part for Ndt
  % To avoid division by zero, concider only columns with nonzero q
  ind = find(q>0);
  q = q(ind);
  Ndt(:,ind) = Ndt(:,ind) + (ones(xm,1)*((1-exp(-q*dt))./q)).*ndQ(:,ind);
  
  %To check the Ndt matrix change 0 --> 1 on next line:
  if 0
      figure(1)
      imagesc(log(Ndt))
      keyboard
  end
  
  Udt(:,ip) = Ndt*U(:,ip);
end
