
clc, clf,  close all
%% Specify domain size and integration time
app.Nx = 150;
app.Ny = 150;
app.dt = 1e-2/4;
app.T = 10;

% Flow-field
app.V = @(x,y,t) (x>0.5) .* (x<1.9)*0;   %Zero Flow in y direction;
app.U = @(x,y,t) (y>1.4) .* (y<1.5)*1.2; % Highway flow in x direction;

app.PlotIntrvl = 1000;
app.reinitLS =100;

app.Targets = [1.8 1.8];
app.targets = app.Targets;

app.pathin.p = [1 1];
app.pathin.v = [0 1];
app.pathin.mag=sqrt(sum(app.pathin.v.^2));


SetupScript = 'HighwayPathPlanSetup';
PlotScript =  '';

[pathout NodeP Nodeu Nodev] = Solver_pathplan_mod(app,SetupScript,PlotScript);


%%
appold = app;
%% Backward Pathfinding
Targets = app.targets;
% Create the new pathin
pathin2.p = repmat(pathout.p(1,1:2),size(Targets,1)*2,1);
pathin2.mag = app.pathin.mag;
pathpts = nan(length(pathout.C),2*size(app.targets,1));
for i = 1 : size(Targets,1)
    figure(2)
    hold on;
    plot(app.pathin.p(1),app.pathin.p(2),'ok','markerfacecolor','k','markersize',12)
    tmppath.C = pathout.C(1:pathout.crosscount(i));
    tmppath.p = pathout.p(1:pathout.crosscount(i),:);
    tmppath.t = pathout.t(1:pathout.crosscount(i));
    tmppath.v = pathout.v(1:pathout.crosscount(i));
    tmppath.c = pathout.c(1:pathout.crosscount(i));
    tmppath.mag = pathout.mag;
    tmp = findpath(tmppath,Targets(i,:),10);
    pathpts(1:size(tmp.P,1),2*i-1:2*i) = tmp.P;
end
for i = size(Targets,1) + 1: size(Targets,1) * 2
    heading = Targets(i - size(Targets,1),:) - pathout.p(1,1:2);
end
%%
