function x0new = CNDiffusion(D,InVec,p,dt,nsteps)
%----------------------------------------------------------------------------------- 
% Time marching the diffusion step using the Crank-Nicholson rule (unstable
% for large dt)
% @ Sohan Dharmaraja MIT JUN2007

m=p^2;
dx = 1/(p+1);
DiffCoeff = D*dt/(2*(dx^2));
Ab=zeros(m,p+1); Ab(:,1)=4; Ab(:,2)=-1; Ab(p:p:m,2)=0; Ab(:,p+1)=-1;
A=spdiags(Ab(:,[1,2,p+1]),-[0,1,p],m,m); A=A+tril(A,-1)';
x0 = InVec;

% Plot initial condition
I = sparse(1:m,1:m,1);
A = DiffCoeff*A;
MMatrix = (I + A)\(I - A);

for i = 1:nsteps
    x0new  = MMatrix*x0;
    x0 = x0new;
end