%% PathPlan Setup script

%Set size of domain
a    = 1;          b    = 1;
dx   = a./(app.Nx);    dy   = b./(app.Ny); 

%Set boundary conditions for Level Set
bcsDrho= [1 1 1 1];       bcsNrho= [];  
%Narrow band Bc's
len = sqrt(dx*dx + dy * dy);
bcsNR = [len*5, -len*5];
bcsNR = [];

%Set number of timesteps
Nt   = ceil(app.T/app.dt);

%% create geometry mask
Nbcs = length(bcsDrho) + length(bcsNrho);
bnd = [1 1 1 1];
Mask = zeros(app.Ny, app.Nx);
%create Node matrices
[NodeP, Nodeu, Nodev, idsP, idsu, idsv] = NodePad(Mask,bnd,1);

%Create new Node Matrices to handle the normal advection of the level set. 
ids = find(Nodeu<=4);
Nubcs=length(ids);
Nodeu = Nodeu + Nubcs - 4;
Nodeu(ids) = 1:Nubcs;

Nodev = Nodev;
ids = find(Nodev<=4);
Nvbcs = length(ids);
Nodev = Nodev + Nvbcs - 4;
Nodev(ids) = 1:Nvbcs;


%% Initialize fields
xp = linspace(dx/2, a-dx/2, app.Nx  );yp = fliplr(linspace(dy/2, b-dy/2, app.Ny  ));
xu = linspace( dx   ,a-dx , app.Nx-1);yu = fliplr(linspace(dy/2, b-dy/2, app.Ny  ));
xv = linspace(dx/2, a-dx/2, app.Nx  );yv = fliplr(linspace(dy    , b-dy, app.Ny-1));
[XP YP] = meshgrid(xp,yp);[XU YU] = meshgrid(xu,yu);[XV YV] = meshgrid(xv,yv);

xxu = linspace( 0    ,a     , app.Nx+1);yyu = fliplr(linspace(-dy/2, b+dy/2, app.Ny+2  ));
xxv = linspace(-dx/2, a+dx/2, app.Nx+2);yyv = fliplr(linspace(0     , b, app.Ny+1));
[XXU YYU] = meshgrid(xxu,yyu);[XXV YYV] = meshgrid(xxv,yyv);
XXU(Nodeu) = XXU;
YYU(Nodeu) = YYU;
XXV(Nodev) = XXV;
YYV(Nodev) = YYV;

rho = [bcsDrho';bcsNrho'; inf*idsP];

%Initialize the level sets for the different assets
pathout.N = size(app.pathin.p,1);
for i = 1:size(app.pathin.p,1)
    x =  XP(find(abs(app.pathin.p(i,1)-XP) < dx/2 + 10 * eps,1));
    y =  YP(find(abs(app.pathin.p(i,2)-YP) < dy/2 + 10 * eps,1));
    app.pathin.p(i,1) = x;
    app.pathin.p(i,2) = y;
    D = (sqrt((XP-x).^2+(YP-y).^2));
   rho(Nbcs+1:end) = min(rho(Nbcs+1:end),D(idsP));
end
rho(Nbcs+1:end) = rho(Nbcs+1:end) - len;
clear D x y

%Modify input paths if a constant path vector (relative to the vehicle) is
%followed
if size(app.pathin.v,1)==1
    for i = 1:size(app.pathin.p,1)
        app.pathin.v(1:2,1:3,i) = repmat([app.pathin.v(1,1:2,i) , 0],2,1);
        app.pathin.v(2,3,i)   = app.T;
    end
end
APath = app.pathin.p;




