function [pathout NodeP Nodeu Nodev] =...
    Solver_pathplan_mod(app,SetupScript,PlotScript) 
%function [pathout NodeP Nodeu Nodev] =...
%    Solver_pathplan_mod(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 vehicle 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];
%          app.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 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,
%   Tapovan Lolla.


% Run setup script
eval(SetupScript)
%keyboard;
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;              % Counter to keep track of reinitialization

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

T=0;                    % Time elapsed    
k=0;                    % Time counter 
u = app.U(XXU,YYU,T);   % Velocity in x direction
v = app.V(XXV,YYV,T);   % Velocity in y direction
eval(PlotScript);       
display('Paused at initial condition. Hit any key to continue');
drawnow;
pause
pathcount=0;            % Counter for number of level sets stored                         
tfin = zeros(Nt,1);indic=0;
% ------------------%
% Begin Time Loop:  %
% ------------------%

for k=1:Nt                       
    T = app.dt * k;
    u = app.U(XXU, YYU, T + app.dt/2);
    v = app.V(XXV, YYV, T + app.dt/2);
    % ---------------------------
    % MOTION IN NORMAL DIRECTION
    % ---------------------------
    % Get RHS contribution due to level set normal advection
    RHSq = LS_NormalAdvect_UW(rho,NodeP,Nodeu,Nodev,dx,dy,idsu,idsv,idsP,...
                              Nubcs,Nvbcs);                      
    % Multiply RHS by magnitude of vehicle speed
    RHSq = RHSq*app.pathin.mag;
    %keyboard;
    % Advance the levelset normal to itself by half a timestep
    rho(pid) = rho(pid) + app.dt/2*RHSq.*MASK(idsP);

    
    % --------------------------------
    % ADVECTION DUE TO VELOCITY FIELD
    % --------------------------------
    % Get RHS contribution due to motion by velocity field for a timestep
    RHSq1 = SCAadvect_TVD_mex(app.Nx, app.Ny, dx, dy, app.dt, rho, u, v,...
                             NodeP, Nodeu, Nodev, idsP,Nbcs,...
                             length(bcsDrho),Nbcs,Nbcs);
                         
    % Advance the levelsets by a timestep                     
    rho(pid) = rho(pid) + app.dt *RHSq1.*MASK(idsP);
   
    % --------------------------------------------
    % ADVECTION DUE TO MOTION IN NORMAL DIRECTION
    % --------------------------------------------
    % Get RHS contribution due to level set normal advection
    RHSq2 = LS_NormalAdvect_UW(rho,NodeP,Nodeu,Nodev,dx,dy,idsu,...
                              idsv,idsP,Nubcs,Nvbcs);
                          
    % Multiply RHS by magnitude of vehicle speed
    RHSq2 = RHSq2*app.pathin.mag;
    
    % Advance the levelset normal to itself by half a timestep
    rho(pid) = rho(pid) + app.dt/2*RHSq2.*MASK(idsP);
    %rho(pid) = rho(pid) + app.dt*RHSq1.*MASK(idsP)+app.dt*RHSq2.*MASK(idsP);
   
    
    % =====================================================================
    
    % ---------------------------------------------------------------%
    % Extract, store and plot the zero level set at every time step: %
    % ---------------------------------------------------------------%
    
    figure(1); clf;
    pathcount = pathcount+1;     % Update pathcount to store level sets
    
    pathout.C{pathcount}=contour(XP,YP,rho(NodeP(2:app.Ny+1,2:app.Nx+1))...
        ,[0.0 0.0],'k','linewidth',2);
    hold on
    plot(app.pathin.p(1),app.pathin.p(2),'o','markersize',15,...
        'markeredgecolor','k','markerfacecolor','k')
    plot(app.Targets(:,1),app.Targets(:,2),'p','markersize',15,...
        'markeredgecolor','k','markerfacecolor','k')
    xlabel('x')
    ylabel('y')
   
    title('Level Set (reachability front) evolution', 'FontSize',14)
    hold off
    axis equal
    xlim([0 app.Nx*dx])
    ylim([0 app.Ny*dy])
    grid on
    
    % =====================================================================    
    %  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
    
    for i = 1:pathout.N
        pathout.p(pathcount,:,i) = [APath(i,:) T];
    end
    
  pathout.t(pathcount) = T;
    
% =========================================================================

% -----------------------------------------------
% Number of pieces in the zero level set contour:
% -----------------------------------------------
%   num = Number of discrete points in first contour
    numS = 1;                                 % Indicator variable     
    numC = 0;                                 % Number of contour pieces
    
    while numS < size(pathout.C{pathcount},2) % While indicator does not 
                                              % reach the end of array
        num = pathout.C{pathcount}(2,numS);   % Update 'num' each time a 
                                              % contour piece is accounted 
        numS = num+numS+1;                    % Update indicator position  
        numC=numC+1;                          % Update number of contours  
    end
% -------------------------------------------------------------------------    
    numS=1;                             
    CC=[];  % Stores points of the zero level set (same as pathout.C except
            % it does not have the breaks in between two pieces of the
            % contour.
    for i = 1:numC
        num = pathout.C{pathcount}(2,numS);
        ids = numS+1:numS+num;
        CC = [CC,pathout.C{pathcount}(1:2,ids)];
        numS = 1+numS+num;
    end
    
    pathout.v{pathcount}=full(double([interp2(XU,YU,...
                              u(Nodeu(2:end-1,2:end-1)),CC(1,:),CC(2,:));
                              interp2(XV,YV,v(Nodev(2:end-1,2:end-1)),...
                              CC(1,:),CC(2,:))]));
    % Flowfield velocities at zero level set position at each time
    
    c=sqrt(sum(pathout.v{pathcount}.^2));
    pathout.c{pathcount}=c;   % Magnitude of flow velocity
    % =====================================================================
    % --------------------------------------------------
    % Stop evolving level sets when they reach the goal:
    % --------------------------------------------------
    app.targets = app.Targets;
    tmptrglev = interp2(XP,YP,rho(NodeP(2:end-1,2:end-1)),...
                        app.targets(:,1),app.targets(:,2));
    % Variable indicating the level set function at each of the
    % goal locations at every timestep.
    
    if k == 1
        tmptrglevindic = zeros(size(tmptrglev));
    end
    for ii = 1:length(tmptrglev)
        if tmptrglev(ii) < 0 && tmptrglevindic(ii) == 0
            pathout.crosscount(ii) = pathcount; 
            
            % pathout.crosscount indicates, for each goal location, when
            % the zero level set has crossed it. 
            
            tmptrglevindic(ii) = 1;
            
            % Variable that indicates whether a goal point has been reached
            % (value = 1) or not (value = 0). 
        end
    end
    
    if tmptrglev < 0   % Forward computation stops when all the entries of
                       % tmptrglev are negative.
        k = Nt;
       % keyboard;
        break
    end   
    % =========================================================================
    % ------------------%
    % REINITIALIZATION: %
    % ------------------%
    
    reinitLS=reinitLS+1;                      % Reinitialization counter
    if  (reinitLS >= app.reinitLS)    % Then reinitialize
        D=inf;                                % Set to large value
        numS = 1;                             % Number of close contours
        for i = 1:numC
            num = pathout.C{pathcount}(2,numS);
            D = min(dpolyus([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));
        rho(pid) = D.*sign(rho(Nbcs+1:end));
        reinitLS=0;
    end
    
    %     %%%====================================================================
    %     % ------------------------------%
    %     % For Time Dependent Obstacles: %
    %     % ------------------------------%
    %     %
    %     % Run this before modifying setupscript:
    %     D=inf;                                % Set to large value
    %     numS = 1;                             % Number of close contours
    %     for i = 1:numC
    %         num = pathout.C{pathcount}(2,numS);
    %         D = min(dpolyus([XP(:) YP(:)],pathout.C{pathcount}(:,...
    %             numS+1:numS+num)'),D);
    %         numS = numS+num+1;
    %     end
    %
    %     signs = sign(rho(NodeP));   % Old Signs of function value.
    %     NodePold = NodeP;
    %
    %     % Run Setup Script for new boundary conditions, NodeP values.
    %     D(NodeP(2:end-1, 2:end-1)) = D;
    %     D(NodeP) = D(NodeP).*signs; % Modify signs
    %     D = D(pid);
    %     rho(pid) = D;
    %
    
    
    
% % =========================================================================    
    if (mod(k,app.PlotIntrvl(1)) == 0)
        eval(PlotScript);drawnow
    end
   % keyboard;
end
if (~mod(k,app.PlotIntrvl(1))) == 0
    eval(PlotScript);drawnow
end

