function [P u v rho NodeP Nodeu Nodev] =...
    NavierStokesInc_DO_rho_tm2(app,SetupScript,PlotScript) 
% function [P u v rho NodeP Nodeu Nodev] =
% NavierStokesInc_DO_rho(app,SetupScript,PlotScript)
% Solves the Boussinesq equations with coriolis forces using a simple Finite 
% Volume Incremental Pressure Correction in Rotational form projection method.
% This function uses a first-order implicit Euler integration scheme, and
% uses LU factorization to solve the matrices.The advection uses the quick 
% scheme when the mean is involved, Central difference when only the modes are
% invovled. Advection routines are mexed.
%
%INPUTS:
%       app:	    The application data structured. At a minimum it should 
%		     contain the following fields (which may be added through
%                    the SetupScript).
%          app.Nx:         Number of interior nodes in x-direction (integer number)
%          app.Ny:         Number of interior nodes in y-direction (integer number)
%          app.T:          Final time
%          app.dt:         Timestep size
%          app.nu:         Viscosity
%          app.kappa:      Diffusivity of density (optional)
%          app.S:          Size of the stochastic dimension. If length(app.S)==1.
%                           If length(app.S)==2, then we have adaptive number of 
%                           stochastic dimensions:
%             app.S(1):       Minimum number of stochastic dimensions
%             app.S(2):       Maximum number of stochastic dimensions
%          app.var           Variance of largest mode at initialization
%                             (Optional) for DO solvers
%          app.addvar        If smallest variance mode has this variance,
%                              a new mode is added for adaptive DO solvers    
%                             (Optional) 
%          app.subbvar       If smallest variance mode has this variance, and
%                              second smallest variance is less than twice
%                              this variance, then the smallest variance mode
%                              is subtraced for adaptive DO solvers (Optional)
%          app.MC:         Number of monte-carlo samples for stochastic 
%                           coefficients (Default = 10,000)
%          app.DOAdapt:    Number of time integration intervals between checking
%                           if the number of stochastic modes should be adjusted
%                           for adaptive stochastic dimension case 
%                           (length(app.S)==2) Default value is 10.
%          app.PlotIntrvl: Number of time integration intervals between calling
%                           "plotscript."
%       PlotScript: (String input) of filename used to plot specific problem.
%                   Can also be used to save outputs and perform other tasks.
%                       Note: Variables u, v, and P, will be available for
%                       plotting.
%       SetupScript: String input of filename used to setup the problem.
%           This file needs to correctly create the following variables:
%	    Nt:     Number of timesteps (integer number)
%           dx:     Size of control volume in x-direction
%           dy:     Size of control volume in y-direction
%           bcsDrho:Density Dirichlet boundary conditions (optional)
%           bcsDP:  Pressure Dirichlet boundary conditions
%           bcsDu:  U-velocity Dirichlet boundary conditions
%           bcsDv:  V-velocity Dirichlet boundary conditions
%           bcsNrho:Density Neumann boundary conditions (optional)
%           bcsNP:  Pressure Neumann boundary conditions
%           bcsNu:  U-velocity Neumann boundary conditions
%           bcsNv:  V-velocity Neumann boundary conditions
%           NodeP:  Pressure node-numbering matrix (see NodePad.m)
%           Nodeu:  U-velocity node-numbering matrix (see NodePad.m)
%           Nodev:  V-velocity node-numbering matrix (see NodePad.m)
%           idsP:   Interior Pressure node id's (see NodePad.m)
%           idsu:   Interior U-velocity node id's (see NodePad.m)
%           idsv:   Interior V-velocity node id's (see NodePad.m)
%           Fu(time,p):   Forcing function for U-velocity
%                         p =[ XU(idsu),YU(idsu) ]
%           Fv(time,p):   Forcing function for V-velocity
%                         p =[ XV(idsv),YV(idsv) ]
%           XU:     Matrix of x-coordinates corresponding to U-velocity CV
%                    centers. XU(idsu) used as 'x' input to Fu. (see meshgrid)
%           XV:     Matrix of x-coordinates corresponding to V-velocity CV
%                    centers. YU(idsu) used as 'y' input to Fv. (see meshgrid)
%           YU:     Matrix of y-coordinates corresponding to U-velocity CV
%                    centers. XV(idsv) used as 'x' input to Fu. (see meshgrid)
%           YV:     Matrix of y-coordinates corresponding to V-velocity CV
%                    centers. YV(idsv) used as 'y' input to Fv. (see meshgrid)
%           P:      Initial Pressure vector. The first #Nbc terms contain
%                   the boundary condition values.
%           u:      Initial U-velocity vector. The first #Nbc terms contain
%                   the boundary condition values.
%           v:      Initial V-velocity vector. The first #Nbc terms contain
%                   the boundary condition values.
%
%OUTPUTS:
%        P:      Final Pressure vector, including the boundary
%                 conditions.
%        u:      Final U-velocity vector, including the boundary
%                 conditions.
%        v:      Final V-velocity vector, including the boundary
%                 conditions.
%        rho:    Final density vector, including the boundary
%                 conditions.
%        NodeP:  Pressure node-numbering matrix (useful for plotting)
%        Nodeu:  U-velocity node-numbering matrix (useful for plotting)
%        Nodev:  V-velocity node-numbering matrix (useful for plotting)
%
%   Uses Matlab functions: symamd, lu
%   See also: mk_DiffusionOperator, FixDiffuvDbcs, Grad2D, Div2D
%
% Authors:   Matt Ueckermann, Themis Sapsis,
%            Pierre Lermusiaux, Pat Haley for MIT course 2.29

%Run setup script
eval(SetupScript)
app
if ~exist('bcsDrho','var'),bcsDrho=zeros(size(bcsDP));end
if ~exist('bcsNrho','var'),bcsNrho=zeros(size(bcsNP));end
if ~exist('rho','var'),rho=zeros(size(P));end
if ~isfield(app,'kappa'),app.kappa=0;end
if ~exist('bcsOP','var'),bcsOP=[];end
if ~exist('bcsOu','var'),bcsOu=[];end
if ~exist('bcsOv','var'),bcsOv=[];end
if ~isfield(app,'S'),    app.S=0;end
if ~exist('YYt','var'), YYt=0;end
if ~isfield(app,'DOAdapt')
    if length(app.S)==2
        app.DOAdapt=10;
    else
        app.DOAdapt=0;
    end
end
if ~exist('Fcor'), Fcor = @(t,p) zeros(size(p,1),1);end
%Find number of boundary conditions
Nbcs = length(bcsDP) + length(bcsNP);

%Set tolerances etc. for GMRES solvers
TOL=sqrt(eps);
MAXIT=20;
RESTARTS=10;


%% Constructing A and RHS
[Arho rhsrho] =  mk_DiffusionOperator(app.Nx, app.Ny, dx, dy, bcsDrho,bcsNrho, NodeP,'P');
[AP rhsPbc] =  mk_DiffusionOperator(app.Nx, app.Ny, dx, dy, bcsDP,bcsNP, NodeP,'P');
[Au rhsubc] =  mk_DiffusionOperator(app.Nx, app.Ny, dx, dy, bcsDu,bcsNu, Nodeu,'u');
[Av rhsvbc] =  mk_DiffusionOperator(app.Nx, app.Ny, dx, dy, bcsDv,bcsNv, Nodev,'v');
[AP rhsPbc] = FixDiffPObcs(AP,bcsOP,Nbcs,...
    rhsPbc,NodeP,dx,dy);bcsDP(bcsOP)=[];
[Au rhsubc] = FixDiffPObcs(Au,bcsOu,Nbcs,...
    rhsubc,Nodeu,dx,dy);bcsDu(bcsOu)=[];
[Av rhsvbc] = FixDiffPObcs(Av,bcsOv,Nbcs,...
    rhsvbc,Nodev,dx,dy);bcsDv(bcsOv)=[];
[Au Av rhsubc rhsvbc] = FixDiffuvDbcs(Au,Av,bcsDu,bcsDv,...
    rhsubc,rhsvbc,Nodeu, Nodev, dx,dy,Nbcs);

if (sum(abs(Fcor(0, [XV(idsv) YV(idsv)])))>0)
    [Au2v Av2u] = CoriolisAVG(app.Nx, app.Ny, Nodeu, Nodev, idsu, idsv, Nbcs);
    Au2v = sparse(diag(Fcor(0 + app.dt,[XV(idsv) YV(idsv)])))*Au2v;
    Av2u = sparse(diag(Fcor(0 + app.dt,[XU(idsu) YU(idsu)])))*Av2u;
else
    Au2v = spalloc((app.Ny-1)*app.Nx,(app.Nx-1)*app.Ny+Nbcs,0);
    Av2u = spalloc((app.Nx-1)*app.Ny,(app.Ny-1)*app.Nx+Nbcs,0);
end

AU=-app.nu*Au;
AV=-app.nu*Av;
ARHO = -app.kappa*Arho;

Au = app.nu*Au + 1/app.dt*speye(size(Au));
Av = app.nu*Av + 1/app.dt*speye(size(Av));
Arho = app.kappa*Arho + 1/app.dt*speye(size(Arho));
%% LU factorization of matrices
imposemean = false;
if isempty(bcsDP)
    imposemean = true;
end
%First order matrix to give best spartsity pattern
rhoperm = symamd(Arho);
Pperm = symamd(AP);
uperm = symamd(Au);
vperm = symamd(Av);

AP=AP(Pperm,Pperm);

% % LU factorization
[Lrho Urho] = lu(Arho(rhoperm,rhoperm));
[LP UP] = lu(AP);
[Lu Uu] = lu(Au(uperm,uperm));
[Lv Uv] = lu(Av(vperm,vperm));

% Get interior ids
pid = Nbcs+1:length(P);
uid = Nbcs+1:length(u);
vid = Nbcs+1:length(v);
srcid     = 2:app.Nx+1;
srcTopid  = 2:app.Ny;
srcBotid  = 3:app.Ny+1;
Neu = [length(bcsDu)+1,Nbcs];
Nev = [length(bcsDv)+1,Nbcs];
NbcsDrho = length(bcsDrho); %Needed for advection
NbcsDu = length(bcsDu); %Needed for advection
NbcsDv = length(bcsDv); %Needed for advection

%% Initialize variables and sizes
frho = zeros(length(pid),1);
fu = zeros(length(uid),1);
fv = zeros(length(vid),1);

if app.S(1)>0 || length(app.S)>1
    frhoi = zeros(length(pid),app.S(1));
    fui = zeros(length(uid),app.S(1));
    fvi = zeros(length(vid),app.S(1));
    frhoij = zeros(length(pid),app.S(1)^2);
    fuij= zeros(length(uid),app.S(1)^2);
    fvij= zeros(length(vid),app.S(1)^2);
else
    frhoi = 0;
    frhoij = 0;
    fui = 0;
    fvi = 0;
    fuij= 0;
    fvij= 0;
end

%If stochastic modes were not initialized in Setup script, 
%initialize automatically.
DOInit;
if ~exist('rhoi','var') && (app.S(1)>0 || length(app.S)>1)
   rhoi = zeros(length(rho),app.S(1)); 
end

%% Solving system
q = zeros(size(P));
CYY=cov(YYt);
T=0;k=0;  eval(PlotScript);
if app.DOAdapt > 0
    DOAdapt_mex;
end
display('Paused at initial condition. Hit any key to continue');drawnow;pause
if isfield(app,'k');
    kstart = app.k+1;
else
    kstart = 1;
end
for k = kstart:Nt
    T=app.dt*k;
%     %Calculate statistics
%     MY = mean(YYt);
%     for i=1:app.S(1)
%         YYt(:,i) = YYt(:,i) - MY(i);
%         u(uid) = u(uid) + MY(i) * ui(uid,i);
%         v(vid) = v(vid) + MY(i) * vi(vid,i);
%         rho(pid) = rho(pid) + MY(i) * rhoi(pid,i);
%         P(pid) = P(pid) + MY(i) * Pi(pid,i);
%     end
%     CYY=cov(YYt);
%     CYYINV = pinv(CYY, max(CYY(:)) * sqrt(eps));    
%     MYYY=mom_thrd(YYt);
%     if app.S(1)>0
%         MYYY=reshape(MYYY,app.S(1)*app.S(1),app.S(1));
%     end
%     if app.S(1) ==0
%        CYY=0; 
%     end
    %Solve mean field
    [dpdx dpdy]   = Grad2D(P,NodeP,dx,dy,idsu,idsv);
    % Calculate RHS vectors for mean u, v, and rho
    [fu fv] = UVadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt, u, v, u, v,Nodeu, Nodev, idsu, idsv, Nbcs, NbcsDu, NbcsDv);
    fu = fu + Av2u*v;
    src = 0.5*( rho(NodeP(srcTopid,srcid)) + rho(NodeP(srcBotid,srcid)) );
    src = src(idsv);
    fv = fv - Au2v*u -src;
    frho = SCAadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt, rho, u, v, NodeP, Nodeu, Nodev, idsP,Nbcs,NbcsDrho,NbcsDu,NbcsDv);
        
    %calculate fui and fvi forcings, as well as fuij, and fvij
    for i=1:app.S(1)
        [fui(:,i) fvi(:,i)] = UVadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt,...
            ui(:,i), vi(:,i),u      , v      , ...
            Nodeu, Nodev, idsu, idsv, Nbcs, NbcsDu, NbcsDv);
        [tmp tmp1]          = UVadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt,...
            u      , v      ,ui(:,i), vi(:,i),... 
            Nodeu, Nodev, idsu, idsv, Nbcs, NbcsDu, NbcsDv);         
        [tmp2 tmp3]          = UVadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt,...
            u      , v      ,-ui(:,i), -vi(:,i),... 
            Nodeu, Nodev, idsu, idsv, Nbcs, NbcsDu, NbcsDv);         
        
        src = 0.5*( rhoi(NodeP(srcTopid,srcid),i) + rhoi(NodeP(srcBotid,srcid),i));
        src = src(idsv);
        fui(:,i) = fui(:,i) + 0.5*(tmp - tmp2)  + Av2u*vi(:,i);
        fvi(:,i) = fvi(:,i) + 0.5*(tmp1 - tmp3) - Au2v*ui(:,i) - src;

        frhoi(:,i) = SCAadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt, rhoi(:,i),...
            u, v, NodeP, Nodeu, Nodev, idsP,Nbcs,NbcsDrho,NbcsDu,NbcsDv) ...
                    + 0.5 * SCAadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt, rho,...
            ui(:,i), vi(:,i), NodeP, Nodeu, Nodev, idsP,Nbcs,NbcsDrho,NbcsDu,NbcsDv) ...
                    - 0.5 * SCAadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt, rho,...
            -ui(:,i), -vi(:,i), NodeP, Nodeu, Nodev, idsP,Nbcs,NbcsDrho,NbcsDu,NbcsDv);
        %For stochastic boundary forcing, need to change above two lines
        for j=1:app.S(1)
            [fuij(:,i+(j-1)*app.S(1)) fvij(:,i+(j-1)*app.S(1))] = ...
                UVadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt,...
                ui(:,i),vi(:,i),ui(:,j),vi(:,j), ...
                Nodeu, Nodev, idsu, idsv, Nbcs, NbcsDu, NbcsDv);
            [tmp tmp1] = ...
                UVadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt,...
                ui(:,i),vi(:,i),-ui(:,j),-vi(:,j), ...
                Nodeu, Nodev, idsu, idsv, Nbcs, NbcsDu, NbcsDv);
            frhoij(:,i+(j-1)*app.S(1)) = 0.5 * SCAadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt, rhoi(:,i),...
                ui(:,j), vi(:,j), NodeP, Nodeu, Nodev, idsP,Nbcs,NbcsDrho,NbcsDu,NbcsDv)...
                - 0.5 * SCAadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt, rhoi(:,i),...
                -ui(:,j), -vi(:,j), NodeP, Nodeu, Nodev, idsP,Nbcs,NbcsDrho,NbcsDu,NbcsDv);
            %%To be consistent with the notation in the paper, let us take
            %%the negative sign of the above expression:
            frhoij(:,i+(j-1)*app.S(1)) = -frhoij(:,i+(j-1)*app.S(1));
            fuij(:,i+(j-1)*app.S(1)) = 0.5*(tmp - fuij(:,i+(j-1)*app.S(1)));
            fvij(:,i+(j-1)*app.S(1)) =  0.5*(tmp1 - fvij(:,i+(j-1)*app.S(1)));
        end
    end
    
    %Advance Yi's using RK4
    if app.S(1)>0
        fui_diffusion = fui+AU*ui(uid,:);
        fvi_diffusion = fvi+AV*vi(vid,:);
        frhoi_diffusion = frhoi + ARHO*rhoi(pid,:);
    end
    RHSY=zeros(size(YYt));
    YYt0 = YYt;
    
    for irk=4:-1:1
        
        %Calculate YiYj's
        for i=1:app.S(1)
            for j=1:app.S(1)
                YiYj(:,i+(j-1)*app.S(1))=YYt(:,i).*YYt(:,j);
            end
        end
        for i=1:app.S(1)
            A1j =  dx * dy * ((fui_diffusion)' * ui(uid,i) + ...
                (fvi_diffusion)' * vi(vid,i) + ...
                (frhoi_diffusion)' * rhoi(pid,i)); %column vector
            A2j =  dx * dy * (fuij' * ui(uid,i) + ...
                              fvij' * vi(vid,i) + ...
                              frhoij'*rhoi(pid,i));
            RHSY(:,i) = YYt * A1j + CYY(:)' * A2j(:) - YiYj * A2j(:);
        end
        %RK time stepping
        YYt = YYt0 + app.dt/irk*RHSY;
        %     %Calculate statistics
        MY = mean(YYt);
        for i=1:app.S(1)
            YYt(:,i) = YYt(:,i) - MY(i);
            u(uid) = u(uid) + MY(i) * ui(uid,i);
            v(vid) = v(vid) + MY(i) * vi(vid,i);
            rho(pid) = rho(pid) + MY(i) * rhoi(pid,i);
            P(pid) = P(pid) + MY(i) * Pi(pid,i);
        end
        CYY=cov(YYt);
        CYYINV = pinv(CYY, max(CYY(:)) * sqrt(eps));
        MYYY=mom_thrd(YYt);
        if app.S(1)>0
            MYYY=reshape(MYYY,app.S(1)*app.S(1),app.S(1));
        end
        if app.S(1) == 0
            CYY=0;
        end 
    end
    %Remove any non-zero mean that resulted from evolution of Yi's.
    MY = mean(YYt);
    for i=1:app.S(1)
        YYt(:,i) = YYt(:,i) - MY(i);
        u(uid) = u(uid) + MY(i) * ui(uid,i);
        v(vid) = v(vid) + MY(i) * vi(vid,i);
        rho(pid) = rho(pid) + MY(i) * rhoi(pid,i);
        P(pid) = P(pid) + MY(i) * Pi(pid,i);
    end
    %Calculate YiYj's
%     for i=1:app.S(1)
%         for j=1:app.S(1)
%             YiYj(:,i+(j-1)*app.S(1))=YYt(:,i).*YYt(:,j);
%         end
%     end
%     for i=1:app.S(1)
%         A1j =  dx * dy * ((fui_diffusion)' * ui(uid,i) + ...
%             (fvi_diffusion)' * vi(vid,i) + ...
%             (frhoi_diffusion)' * rhoi(pid,i)); %column vector
%         A2j =  dx * dy * (fuij' * ui(uid,i) + ...
%             fvij' * vi(vid,i) + ...
%             frhoij'*rhoi(pid,i));
%         RHSY(:,i) = YYt * A1j + CYY(:)' * A2j(:) - YiYj * A2j(:);
%     end
%     %Euler time stepping
%     YYt = YYt0 + app.dt * RHSY;

    
    
    % Advance mean fields
    RHSq=1/app.dt*rho(pid) + frho - frhoij*CYY(:) + app.kappa*rhsrho;
    RHSu = 1/app.dt*u(uid) + fu - fuij*CYY(:) + rhsubc*app.nu - dpdx + ...
        Fu(T + app.dt,[XU(idsu) YU(idsu)]);
    RHSv = 1/app.dt*v(vid) + fv - fvij*CYY(:) + rhsvbc*app.nu - dpdy +...
        Fv(T + app.dt,[XV(idsv) YV(idsv)]);
    
    rho(pid(rhoperm)) = Urho\(Lrho\RHSq(rhoperm));
    u(uid(uperm))=Uu\(Lu\RHSu(uperm));
    v(vid(vperm))=Uv\(Lv\RHSv(vperm));
    
    frhoijmyyy = frhoij*MYYY;
    fuijmyyy = fuij*MYYY;
    fvijmyyy = fvij*MYYY;
    %Advance ui's, vi's, and rhoi's
    for i=1:app.S(1)
        f1rho = frhoi(:,i) - frhoijmyyy*CYYINV(:,i);
        f1u = fui(:,i) - fuijmyyy*CYYINV(:,i);
        f1v = fvi(:,i) - fvijmyyy*CYYINV(:,i);
        f2rho = f1rho + ARHO*rhoi(pid,i);
        f2u = f1u + AU*ui(uid,i);
        f2v = f1v + AV*vi(vid,i);
        %Calculate inner product terms <ffiUj> -- there has to be app.S of them
        InProdi=zeros(app.S(1),1);
        for j=1:app.S(1)
            InProdi(j) = (f2u' * ui(uid,j) + ...
                f2v' * vi(vid,j) + ...
                f2rho'*rhoi(pid,j)) * dx * dy;
        end
        
        %We're going to treat the Laplacian for THIS mode implicitly.
        %but the inner product treats the Laplcian EXplicitly
        [dpdx dpdy]   = Grad2D(Pi(:,i),NodeP,dx,dy,idsu,idsv);
        RHSu = 1/app.dt*ui(uid,i) + f1u - dpdx...
            -ui(uid,:)*InProdi;
        %NOTE! I am assuming that ui's are zero on the boundary. To
        %introduce stochastic boundary forcings, add to the above line.
        RHSv = 1/app.dt*vi(vid,i) + f1v - dpdy...
            -vi(vid,:)*InProdi;
        %NOTE! I am assuming that vi's are zero on the boundary. To
        %introduce stochastic boundary forcings, add to the above line.
        ui(uid(uperm),i)=Uu\(Lu\RHSu(uperm));
        vi(vid(vperm),i)=Uv\(Lv\RHSv(vperm));

        %And now the tracers
        RHSq=1/app.dt*rhoi(pid,i) + f1rho - rhoi(pid,:)*InProdi;
        rhoi(pid(rhoperm),i) = Urho\(Lrho\RHSq(rhoperm));
    end
    
    %Calculate RHS vector for P
    divU = Div2D(u,v,Nodeu,Nodev,dx,dy,idsP,Neu,Nev);
    RHSq = rhsPbc - 1/app.dt*divU;
    %     q(pid(Pperm)) = gmres(AP,RHSq(Pperm),RESTARTS,TOL,MAXIT,UP,LP);
    q(pid(Pperm))=UP\(LP\RHSq(Pperm));
    
    %Correct Velocity
    [dqdx dqdy]   = Grad2D(q,NodeP,dx,dy,idsu,idsv);
    u(uid) = u(uid) - app.dt*dqdx;
    v(vid) = v(vid) - app.dt*dqdy;
    
    %correct pressure
    P(pid) = P(pid) + q(pid) - app.nu*divU;
    %remove mean pressure
    if imposemean
        P(pid)=P(pid)-mean(P(pid));
    else
        P([1:length(bcsDP),pid])=...
            P([1:length(bcsDP),pid])-mean(P(pid));
    end
    
    %Correct pressure for ui, vi's
    for i=1:app.S(1)
        %Calculate RHS vector for P
        divU = Div2D(ui(:,i),vi(:,i),Nodeu,Nodev,dx,dy,idsP,Neu,Nev);
        RHSq = rhsPbc - 1/app.dt*divU;
        %             q(pid(Pperm)) = gmres(AP,RHSq(Pperm),RESTARTS,TOL,MAXIT,LP,UP);
        q(pid(Pperm))=UP\(LP\RHSq(Pperm));
        
        %Correct Velocity
        [dqdx dqdy]   = Grad2D(q,NodeP,dx,dy,idsu,idsv);
        ui(uid,i) = ui(uid,i) - app.dt*dqdx;
        vi(vid,i) = vi(vid,i) - app.dt*dqdy;
        
        %correct pressure
        Pi(pid,i) = Pi(pid,i) + q(pid) - app.nu*divU;
        %remove mean pressure
        if imposemean
            Pi(pid,i)=Pi(pid,i)-mean(Pi(pid,i));
        else
            Pi([1:length(bcsDP),pid],i)=...
                Pi([1:length(bcsDP),pid],i)-mean(Pi(pid,i));
        end
    end
    %%% Ortho-Normalization of basis fields to avoid round-off errors
    for i=1:app.S(1)
        for j=1:app.S(1)
            coef = zeros(app.S(1),1);
            if i~=j
                 %Commented above to agree with THemis' code (for now). 
                 coef(j) =InProd(ui(:,i),ui(:,j),dx,dy,uid) + ...
                     InProd(vi(:,i),vi(:,j),dx,dy,vid) + ...
                     InProd(rhoi(:,i),rhoi(:,j),dx,dy,pid);
                 ui(:,i) = ui(:,i) - coef(j) * ui(:,j);
                 vi(:,i) = vi(:,i) - coef(j) * vi(:,j);
                 rhoi(:,i) = rhoi(:,i) - coef(j) * rhoi(:,j);
             end
        end
        nrm=sqrt(InProd(ui(:,i),ui(:,i),dx,dy,uid) + ...
            InProd(vi(:,i),vi(:,i),dx,dy,vid) + ...
            InProd(rhoi(:,i),rhoi(:,i),dx,dy,pid));
        ui(:,i) = ui(:,i)/nrm;
        vi(:,i) = vi(:,i)/nrm;
        rhoi(:,i) = rhoi(:,i)/nrm;
        YYt(:,i) = YYt(:,i) * nrm;
        %Add the subtracted directions back into the samples. 
        %Do this such that Yiui = Yi*ui*
        for j=1:app.S(1)
            if i~=j   
                YYt(:,i) = YYt(:,i) + coef(j) / nrm;
%                  u(uid) = u(uid) + coef(j) / nrm * ui(uid,j);
%                  v(vid) = v(vid) + coef(j) / nrm * vi(vid,j);
%                  rho(pid) = rho(pid) + coef(j) / nrm * rhoi(pid,j);
             end
        end
    end       
    %Remove any non-zero mean that resulted from the orthonormalization
    MY = mean(YYt);
    for i=1:app.S(1)
        YYt(:,i) = YYt(:,i) - MY(i);
        u(uid) = u(uid) + MY(i) * ui(uid,i);
        v(vid) = v(vid) + MY(i) * vi(vid,i);
        rho(pid) = rho(pid) + MY(i) * rhoi(pid,i);
        P(pid) = P(pid) + MY(i) * Pi(pid,i);
    end
    %Adapt stochastic dimension if required
    if ~mod(k,app.DOAdapt)
        DOAdapt_mex;
    end

    %Plot solution
    if ~mod(k,app.PlotIntrvl(1))
        eval(PlotScript);drawnow
    end
end
eval(PlotScript);drawnow
%Clean up any temporary files
if exist('existbasis','var')
    delete([d0,sprintf('/Save/DOAdaptSave/Basis/orth_basis%0.3d.mat',existbasis)]);
end
