function [pathout NodeP Nodeu Nodev] =...
    Solver_pathplan(app,SetupScript,PlotScript) 
%function [pathout NodeP Nodeu Nodev] =...
%    Solver_pathplan(app,SetupScript,PlotScript) 
%
%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.dt:         Size of timestep (TotalTime/Nt)
%          app.U:          Function handle for u-velocity f(x,y,t)
%          app.V:          Function handle for v-velocity f(x,y,t)
%          app.pathin:     Structure containing information about the
%                           path-planning section
%          app.pathin.p:   Starting positions of assets p = [x;y]
%          app.pathin.mag: Magnitude of velocity that vehicle has
%          app.pathin.v:   Prescribed vector velocities of assets relative to the
%                     asset coordinate system. That is, the coordinate
%                     system is fixed in space, not taking into account the
%                     flowfield velocity. v = [vx;vy;t];
%       PlotIntrvl: Number of time integration intervals between of calling
%                   "plotscript."
%       PlotScript: String input of filename used to plot specific problem.
%                   Can also be used to save outputs.
%                       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:
%           dx:     Size of control volume in x-direction
%           dy:     Size of control volume in y-direction
%           Nt:         Number of timesteps (integer number)
%           bcsDrho:Density Dirichlet boundary conditions
%           bcsNrho:Density 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)
%           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)
%
%OUTPUTS:
%           pathout:Information about the path
%           PATHOUT:        Structure containing level set information
%   `        PATHOUT.MAG    Maximum magnitude relative velocity of vehicle
%            PATHOUT.N      Number of assets (vehicles)
%            PATHOUT.C      Cell array points defining level set curves (curves
%                            created using contourc.
%            PATHOUT.P      Points tracing current asset path (used for
%                            comparison and plots)
%            PATHOUT.T      Time at which each curve in pathout.c was generated
%            PATHOUT.V      Velocity vectors of flowfield at points in
%                            pathout.c
%           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)

%
%   Uses Matlab functions: 
%   See also: mk_DiffusionOperator, FixDiffuvDbcs, Grad2D, Div2D
%
%   Written by: Matt Ueckermann, Konur Yigit, Pierre Lermusiaux, Pat Haley


%Run setup script
eval(SetupScript)
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 ~exist('MASK','var'),MASK = ones(size(XP));end
if ~isfield(app,'U')
    display('***Need to specify app.U to use this solver')
    pathout =[];
    return; 
end
if ~isfield(app,'V')
    display('***Need to specify app.V to use this solver')
    pathout =[];
    return; 
end
if ~isfield(app,'pathin'), app.pathin.p = []; app.pathin.mag = 0; end
pathout.mag = app.pathin.mag;
%Find number of boundary conditions
Nbcs = length(bcsDrho) + length(bcsNrho) + length(bcsNR);

% Get interior ids
pid = Nbcs+1:length(rho);

reinitLS=0;

%Check if re-initialization was set. If not, set it to default value of 5
if ~isfield(app,'reinitLS')
    app.reinitLS=5;
end

%% Solving system
T=0;k=0;
u = app.U(XXU,YYU,T);
v = app.V(XXV,YYV,T);


eval(PlotScript);
display('Paused at initial condition. Hit any key to continue');drawnow;pause
for k=1:Nt
    T = app.dt * k;    
    %Get the velocity at mid-timestep
     u = app.U(XXU, YYU, T - app.dt/2);
     v = app.V(XXV, YYV, T - app.dt/2);    

    % Advect Tracers (re-use RHSq to save a little memory)    
    % To determine the RHS contribution of the level set expansion we solve
    % phi_t + |grad phi| = 0 using upwind differences. So basically, we're
    % looking for the upwind value of |grad phi|
    
    
    [dpdx dpdy]   = Grad2D(rho,NodeP,dx,dy,idsu,idsv); 
    dpdx = [zeros(Nubcs,1);dpdx]; % Matrix of x derivatives of rho.
    dpdy = [zeros(Nvbcs,1);dpdy]; % Matrix of y derivatives of rho.
    tmpleft = [ones(size(NodeP, 1) - 2, 1),...
               rho(NodeP(2 : end - 1, 3 : end - 1))...
               <= rho(NodeP(2 : end - 1, 2 : end - 2))];
    tmpright = [rho(NodeP(2 : end - 1, 2 : end - 2))...
                <= rho(NodeP(2 : end - 1, 3 : end - 1)),...
                ones(size(NodeP, 1)-2, 1)];
    tmp =  ((1 - tmpleft) .* abs(...
        [dpdx(Nodeu(2 : end - 1, 2)), dpdx(Nodeu(2 : end - 1, 2 : end - 1))])...
        + (1- tmpright) .* abs(...
       [dpdx(Nodeu(2 : end - 1, 2 : end - 1)), dpdx(Nodeu(2 : end - 1, end - 1))]))...
       ./ max(tmpleft + tmpright, 1);
    RHSq = tmp(idsP);

   tmptop = [ones(1, size(NodeP, 2)-2);...
        rho(NodeP(3 : end - 1, 2 : end - 1))...
        <= rho(NodeP(2 : end - 2, 2 : end - 1))];
   tmpbot = [rho(NodeP(2 : end - 2, 2 : end - 1))...
        <= rho(NodeP(3 : end - 1, 2 : end - 1));...
        ones(1, size(NodeP, 2)-2)];
       
   tmp =  ((1 - tmptop) .* abs(...
        [dpdy(Nodev(2, 2 : end - 1))' ; dpdy(Nodev(2 : end - 1, 2 : end - 1))])...
        + (1 - tmpbot) .* abs(...
       [dpdy(Nodev(2 : end - 1, 2 : end - 1)) ; dpdy(Nodev(end - 1, 2 : end - 1))']))...
       ./ max(tmptop + tmpbot, 1);
   RHSq = - sqrt(RHSq .^ 2 + (tmp(idsP)) .^ 2) * app.pathin.mag;
   %Advance the levelset normal to itself by half a timestep
   rho(pid) = rho(pid) + app.dt / 2 * RHSq .* MASK(idsP);

    
    %Now add the contributions of the velocity field.  
    RHSq = SCAadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt, rho, u, v, NodeP, Nodeu, Nodev, idsP,Nbcs,length(bcsDrho),Nbcs,Nbcs);       
%         SCAadvect_mex(app.Nx, app.Ny, dx, dy, rho, u, v, NodeP, Nodeu, Nodev, idsP,Nbcs,length(bcsDrho),NbcsDu,NbcsDv);
    rho(pid) = rho(pid) + app.dt * RHSq .* MASK(idsP);
    
    
       %Advect Tracers (re-use RHSq to save a little memory)    
    %To determine the RHS contribution of the level set expansion we solve
    %phi_t + |grad phi| = 0 using upwind differences. So basically, we're
    %looking for the upwind value of |grad phi|
   [dpdx dpdy]   = Grad2D(rho,NodeP,dx,dy,idsu,idsv);
    dpdx = [zeros(Nubcs,1);dpdx];
    dpdy = [zeros(Nvbcs,1);dpdy];
    tmpleft = [ones(size(NodeP, 1) - 2, 1),...
        rho(NodeP(2 : end - 1, 3 : end - 1))...
         <= rho(NodeP(2 : end - 1, 2 : end - 2))];
    tmpright = [rho(NodeP(2 : end - 1, 2 : end - 2))...
         <= rho(NodeP(2 : end - 1, 3 : end - 1)),...
         ones(size(NodeP, 1)-2, 1)];
    tmp =  ((1 - tmpleft) .* abs(...
        [dpdx(Nodeu(2 : end - 1, 2)), dpdx(Nodeu(2 : end - 1, 2 : end - 1))])...
        + (1- tmpright) .* abs(...
       [dpdx(Nodeu(2 : end - 1, 2 : end - 1)), dpdx(Nodeu(2 : end - 1, end - 1))]))...
       ./ max(tmpleft + tmpright, 1);
    RHSq = tmp(idsP);

    tmptop = [ones(1, size(NodeP, 2)-2);...
        rho(NodeP(3 : end - 1, 2 : end - 1))...
        <= rho(NodeP(2 : end - 2, 2 : end - 1))];
    tmpbot = [rho(NodeP(2 : end - 2, 2 : end - 1))...
        <= rho(NodeP(3 : end - 1, 2 : end - 1));...
        ones(1, size(NodeP, 2)-2)];
       
    tmp =  ((1 - tmptop) .* abs(...
        [dpdy(Nodev(2, 2 : end - 1))' ; dpdy(Nodev(2 : end - 1, 2 : end - 1))])...
        + (1 - tmpbot) .* abs(...
       [dpdy(Nodev(2 : end - 1, 2 : end - 1)) ; dpdy(Nodev(end - 1, 2 : end - 1))']))...
       ./ max(tmptop + tmpbot, 1);
   RHSq = - sqrt(RHSq .^ 2 + (tmp(idsP)) .^ 2) * app.pathin.mag ;
   %Advance the levelset normal to itself by half a timestep
   rho(pid) = rho(pid) + app.dt / 2 * RHSq .* MASK(idsP);
   
   %Need re-initialize the level set every so often... let's say every
   %time we plot
%Plot solution  
if (mod(k,app.PlotIntrvl(1))) == 0
        eval(PlotScript);drawnow
  end   
   
   reinitLS=reinitLS+1;
   if   k>0 && (reinitLS>=app.reinitLS)% || k * app.dt * app.pathin.mag < norm([dx, dy].^2))
       D=inf;
       numS = 1;
       for i = 1:numC
           num = pathout.C{pathcount}(2,numS);
           D = min(dpoly([XP(:) YP(:)],pathout.C{pathcount}(:,numS+1:numS+num)'),D);
           numS = numS+num+1;
       end
       D(NodeP(2:end-1, 2:end-1)) = D;
       D = D(pid);
       rho(pid)=D.*(1-(sign(rho(Nbcs+1:end))==-1 & sign(D)==1));
       reinitLS=0;
   end
   

   
%  vrel=[0,0];
%  Evolve the vehicle trajectory
   for i = 1:pathout.N
        try
        vrel = [interp1(app.pathin.v(:,3,i),app.pathin.v(:,1,i), T),...
            interp1(app.pathin.v(:,3,i),app.pathin.v(:,2,i), T)];
        end
        APath(i,:) = APath(i,:) + app.dt / 2 *vrel;
        try
        vrel = [interp2(XU,YU,u(Nodeu(2:end-1,2:end-1)),APath(i,1),APath(i,2)),...
             interp2(XV,YV,v(Nodev(2:end-1,2:end-1)),APath(i,1),APath(i,2))];
        end
        APath(i,:) = APath(i,:) + app.dt * vrel;
        try
        vrel = [interp1(app.pathin.v(:,3,i),app.pathin.v(:,1,i), T),...
            interp1(app.pathin.v(:,3,i),app.pathin.v(:,2,i), T)];
        end
        APath(i,:) = APath(i,:) + app.dt / 2 * vrel;
    end
    
    

    
%  Stopping Criterion for Level Sets

   tmptrglev = interp2(XP,YP,rho(NodeP(2:end-1,2:end-1)),...
    app.targets(:,1),app.targets(:,2));
   if tmptrglev < 0
      k = Nt;
      break
   end

    clear dpdx dpdy tmp
end
if (~mod(k,app.PlotIntrvl(1))) == 0
        eval(PlotScript);drawnow
end
