% % Double-Gyre with Path Planning Setup

% Forcing Function
Fu = @(time,p) ...
             -1000/2/pi*cos(2.*pi.*p(:,2))+0.*time+0.*p(:,1);
Fv = @(time,p) 0;

% Coriolis Coefficient f=f0+b0*y
Fcor = @(time,p)...
            0+1000.*p(:,2)+0.*p(:,2)+0.*time;

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

%Setup Boundary conditions
bcsDrho= [-1 1 -1 1];       bcsNrho= [];        
bcsDP= [];         bcsNP= [0 0 0 0];        
bcsDu= [0 0]  ;    bcsNu= [0 0];
bcsDv= [0 0 0 0]  ;bcsNv= [];        

Nt   = ceil(app.T/app.dt);

%% create geometry mask
Nbcs = length(bcsDP) + length(bcsNP);
bnd = [1 1 1 1];
Mask = zeros(app.Ny, app.Nx);

%create Node matrices
[NodeP, Nodeu, Nodev, idsP, idsu, idsv] = NodePad(Mask,bnd,1);

%% 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);
P = [bcsDP';bcsNP'; 0*idsP];
u = [bcsDu';bcsNu';idsu*0];
v = [bcsDv';bcsNv'; 0*idsv];
ucentid = Nodeu(:,floor(app.Nx/2+1));
vcentid = Nodev(:,floor(app.Nx/2+1));

rho = [bcsDrho';bcsNrho'; inf*idsP];
Nbcs = length(bcsDrho) + length(bcsNrho);

%Initialize the level sets for the different assets
pathout.N = size(app.pathin.p,1);
for i = 1:size(app.pathin.p,1)
%     x =  app.pathin.p(i,1);
%     y =  app.pathin.p(i,2);
    x =  XP(find(abs(app.pathin.p(i,1)-XP)<(dx/2+eps),1));
    y =  YP(find(abs(app.pathin.p(i,2)-YP)<(dy/2+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)-sqrt((dx^2+dy^2)) / 2;
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;

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

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