% Saving, Adaptation, and Plots path planning using Level Set Methods
r = [1 0 0];       %# start
w = [.9 .9 .9];    %# middle
b = [0 0 1];       %# end

%# colormap of size 64-by-3, ranging from red -> white -> blue
c1 = zeros(32,3); c2 = zeros(32,3);
for i=1:3
    c1(:,i) = linspace(r(i), w(i), 32);
    c2(:,i) = linspace(w(i), b(i), 32);
end
c = [c1(1:end-1,:);c2];

%% At first timestep do preliminaries
if k==0
    % Initialize the number of outputs for zero contour level an vehicle
    % path
    auvplotcol={'b-','r-','y-','w-','m-','g-','k-','c-',...
                'b-','r-','y-','w-','m-','g-','k-','c-'};
    close all
    % Make limits for plotting
    XLIM = [min(XP(:)),max(XP(:))];
    YLIM = [min(YP(:)),max(YP(:))];
    colormap jet
    % How many std dev's away to plot vorticity
    STDVORT = 3;
end
if  ~isfield(app,'noplot') && k>0
    %keyboard;
    %Plotting Routines
    NPLOTSx=1;
    NPLOTSy=1;
  
%     PLTNUM=1;
%     %subplot(NPLOTSx,NPLOTSy,PLTNUM),cla
    tmp1 = 0.5*(u(Nodeu(2:app.Ny+1,1:app.Nx))+u(Nodeu(2:app.Ny+1,2:app.Nx+1)));
    tmp2 = 0.5*(v(Nodev(2:app.Ny+1,2:app.Nx+1))+v(Nodev(1:app.Ny,2:app.Nx+1)));
    hold on
    dudy = (u(Nodeu(2:end-2,2:end-1))-u(Nodeu(3:end-1,2:end-1)))/dy;
    dvdx = (v(Nodev(2:end-1,3:end-1))-v(Nodev(2:end-1,2:end-2)))/dx;
    
    vort = dvdx-dudy;
    %keyboard;
    figure(2); 
    clf; 
    set(gcf,'PaperUnits','centimeters')
    set(gcf,'position',[100 100 600 600])
    %keyboard;
    subplot(121)
    axes('position',[0.1,0.1,0.8,0.8])
    hold on
    [dum1,dum2] = contourf(XU(2:end,:),YV(:,2:end),vort,20);
%     caxis([-3000 3000]);
     set(dum2,'EdgeColor','none')
    %caxis([min(-std(vort(:)),-eps),max(std(vort(:)),0)]*STDVORT+mean(vort(:)));
    h=streamslice(XP,YP,tmp1,tmp2);
    colormap(hot)
    colorbar
    set(h,'color','k');
    axis([0 1 0 1]) 
%     if isfield(app,'targets')
%          plot(app.targets(:,1)',app.targets(:,2)','xk','MarkerSize',10)
%     end  
%     
%     
% %     
% %     subplot(NPLOTSx,NPLOTSy,PLTNUM),cla
% %     figure(2) 
% %     clf
% %     pcolor(XP,YP,rho(NodeP(2:app.Ny+1,2:app.Nx+1)))
% %     caxis([-1,5]);
% %     shading interp
% %     hold on

%       contour(XP,YP,rho(NodeP(2:app.Ny+1,2:app.Nx+1))...
%                                   ,[0.0 0.0],'k','linewidth',2);
%       plot(app.pathin.p(1),app.pathin.p(2),'ok','MarkerSize',15,'MarkerFaceColor','k')
%       plot(app.targets(:,1),app.targets(:,2),'pk','MarkerSize',20,'MarkerFaceColor','k')
    
      subplot(122)
      
      
% %     colorbar
% %     hold off

figure(2)
%print('-dpng','-r400',[app.plotsavedir,sprintf('%d',k)]

end
% keyboard
if isfield(app,'thesis')
      figure(3);clf;
      axes('position',[0.1,0.1,0.8,0.8])
      
      M=abs(max(max(rho(NodeP))));
      m=abs(min(min(rho(NodeP))));
      if M-m>=0      
        clim=[-m m];
      else
        clim=[-M M];
      end
      %[dum3,dum4]=contourf(flipud(rho(NodeP)),20,clim);
      %imagesc(flipud(rho(NodeP)),clim)
      %set(dum4,'edgecolor','none');
      %colormap(othercolor('RdBu7'))
      %colormap('bluewhitered')
      %colormap(c)
      %LB=flipud(lbmap(256,'BrownBlue'));
      %colormap(LB);
      %colorbar
      %shading interp;
      %hold on
     
      
     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')
    tfin(k+1)=toc;
    title(sprintf('Time = %2.4f',T))
    hold off
    axis equal
    xlim([0 app.Nx*dx])
    ylim([0 app.Ny*dy])
    grid on
     set(gcf, 'PaperPositionMode', 'manual');
     set(gcf, 'PaperUnits', 'inches');
     set(gcf, 'PaperPosition', [0.5 0.5 5 5]);
     %set(gca,'Ytick',[0 0.5 1])
     %set(gca,'Xtick',[0 0.5 1])
     set(gca,'position',[0.05,0.04,0.885,0.885])
     %set(gca, 'Position', get(gca, 'OuterPosition') - ...
     %get(gca, 'TightInset') * 2*[-1 0 1 0; 0 -1 0 1; 0 0 1 0; 0 0 0 1]);
     set(gca,'box','on')
     drawnow;
     savedir = '/home/ltapovan/FV_Matlab_Framework/trunk/Cases/PathPlanIdeal/JCP/Highway/';
end
