% Saving, Adaptation, and Plots for Adaptive DO solvers
Targets = [0.35,0.68;
    0.06,0.066];
%At first timestep do preliminaries
if k==0
    %Initialize the number of outputs for zero contour level an vehicle
    %path
    SIGN(1:app.S) = 1;
    pathcount=0;
    %     auvplotcol={'bs','rs','ys','ws','ms','ks','cs','gs'};
    auvplotcol={'b-','r-','y-','w-','m-','g-','k-','c-'};   
    close all
    EIGPLOT=[];
    MEAN_ENER=[];
    %Will also require second smallest mode to be less
    %than twice this value
    %Make limits for plotting
    XLIM = [min(XP(:)),max(XP(:))];
    YLIM = [min(YP(:)),max(YP(:))];
    %Select the colormap
    %colormap jet
    colormap(mk_colmap)
    
    %How many std dev's away to plot vorticity
    STDVORT = 3;
    if length(app.S)>1 %Adaptation!
        Smin=app.S(1);
        Smax=app.S(2);
    else %No adaptation
        Smin=app.S;
        Smax=app.S;
    end
end

% First Step is to diagonalize the modes
[VC,DC] = eig(CYY);  %Diagonal Covariance, and Diagonal Vectors
DC=diag(DC);
EIGPLOT=[ EIGPLOT, [flipud(DC);NaN(Smax-length(DC),1)] ];
MEAN_ENER=[ MEAN_ENER, InProd(u(:),u(:),dx,dy,uid)+InProd(v(:),v(:),dx,dy,vid)];
%VC = diag(sign(sum(VC)))*VC;
ui(uid,:)=ui(uid,:)*fliplr(VC);
vi(vid,:)=vi(vid,:)*fliplr(VC);
Pi(pid,:)=Pi(pid,:)*fliplr(VC);
YYt=YYt*fliplr(VC);

%Plotting Routines
NPLOTSx=2;
NPLOTSy=3;
figure(1)
%     clf
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;
if k==0
    for i = 1:app.S
        vortold(:,:,i) = vort;
    end
    vortold(:)=1;
end

pcolor(XU(2:end,:),YV(:,2:end),vort),
shading interp
caxis([min(-std(vort(:)),-eps),max(std(vort(:)),0)]*STDVORT+mean(vort(:)));
h=streamslice(XP,YP,tmp1,tmp2);
set(h,'color','k');
plot(APath(:,1)',APath(:,2)','ok')
plot(APath(:,1)',APath(:,2)','.k','MarkerSize',10)
%Plot endpoints
%     plot([0.0768, .3146],[0.06077,0.8574 ],'xk','LineWidth',1.5)
%     plot([0.0768, .3146],[0.06077,0.8574 ],'.k','MarkerSize',10)
%     plot([0.0768, .3146],[0.06077,0.8574 ],'oK')
plot(Targets(:,1),Targets(:,2),'xk','LineWidth',1.5)
plot(Targets(:,1),Targets(:,2),'.k','MarkerSize',10)
plot(Targets(:,1),Targets(:,2),'oK')

hold off

xlim(XLIM);ylim(YLIM)
TT(1)=xlabel('x');TT(2)=ylabel('y');
TT(3)=title(sprintf('Time =%03.03g \nu=[%0.2g,%0.2g]',T,...
    min(sqrt(tmp1(:).^2+tmp2(:).^2)),max(sqrt(tmp1(:).^2+tmp2(:).^2))));

PLTNUM=PLTNUM+1;
subplot(NPLOTSx,NPLOTSy,PLTNUM),cla
pcolor(XP,YP,rho(NodeP(2:app.Nx+1,2:app.Ny+1)))
caxis([-0.2,0.2]);
%     colorbar
shading interp
hold on
pathcount=pathcount+1;
pathout.C{pathcount}=contour(XP,YP,rho(NodeP(2:app.Nx+1,2:app.Ny+1)),[0.0 0.0],'k','linewidth',2);
for i = 1:pathout.N
    pathout.p(pathcount,:,i) = [APath(i,:) T];
    plot(pathout.p(1:pathcount,1,i)',pathout.p(1:pathcount,2,i)',auvplotcol{i},'linewidth',2)
    plot(pathout.p(pathcount,1,i)',pathout.p(pathcount,2,i)',sprintf('.%s',auvplotcol{i}))
end
pathout.t(pathcount)     = T;
%Figure out how many close contours we have:
lev1=pathout.C{pathcount}(1);
num=pathout.C{pathcount}(2);
numS = 1;
numC=0;
while numS < size(pathout.C{pathcount},2)
    num = pathout.C{pathcount}(2,numS);
    numS = num+numS+1;
    numC=numC+1;
end

%Plot endpoints
plot(Targets(:,1),Targets(:,2),'xk','LineWidth',1.5)
plot(Targets(:,1),Targets(:,2),'.k','MarkerSize',10)
plot(Targets(:,1),Targets(:,2),'oK')
title('Level Set with Zero-level highlighted')

PLTNUM=PLTNUM+1;
subplot(NPLOTSx,NPLOTSy,PLTNUM),hold on
numS=1;
CC=[];
for i = 1:numC
    num = pathout.C{pathcount}(2,numS);
    ids = numS+1:numS+num;
    CC = [CC,pathout.C{pathcount}(1:2,ids)];
    %         plot3(pathout.C{pathcount}(1,ids),pathout.C{pathcount}(2,ids),ids*0+T)
    
    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,:))]));
pathout.vi{pathcount}=full(double([interp2(XU,YU,ui(Nodeu(2:end-1,2:end-1)),...
    CC(1,:),CC(2,:));...
    interp2(XV,YV,vi(Nodev(2:end-1,2:end-1)),...
    CC(1,:),CC(2,:))]))*max(DC);
c=sqrt(sum(pathout.vi{pathcount}.^2));
pathout.c{pathcount}=c;
plot3k([CC;CC(1,:)*0+ T]',c,'o');
axis([0 1 0 1])
view(3)
hold off
title('Zero-Level Set, colored with uncertainty')

offu=size(ui,1);
offv=size(vi,1);
offP=size(Pi,1);

if 0 && k>0
    PLTNUM=PLTNUM+1;
    subplot(NPLOTSx,NPLOTSy,PLTNUM),cla
    imagesc(dpdx(Nodeu(2:end-1,2:end-1)));
    PLTNUM=PLTNUM+1;
    subplot(NPLOTSx,NPLOTSy,PLTNUM),cla
    imagesc(dpdy(Nodev(2:end-1,2:end-1)));
    
    %     colorbar
    shading flat
    
else
    
    for i=1:app.S
        PLTNUM=PLTNUM+1;
        subplot(NPLOTSx,NPLOTSy,PLTNUM),cla
        tmp1 = 0.5*(ui((i-1)*offu+Nodeu(2:app.Ny+1,1:app.Nx))+ui((i-1)*offu+Nodeu(2:app.Ny+1,2:app.Nx+1)));
        tmp2 = 0.5*(vi((i-1)*offv+Nodev(2:app.Ny+1,2:app.Nx+1))+vi((i-1)*offv+Nodev(1:app.Ny,2:app.Nx+1)));
        %     quiver(XP(:),YP(:),(tmp1(:)),(tmp2(:)),'g')
        hold on
        dudy = (ui((i-1)*offu+Nodeu(2:end-2,2:end-1))-ui((i-1)*offu+Nodeu(3:end-1,2:end-1)))/dy;
        dvdx = (vi((i-1)*offv+Nodev(2:end-1,3:end-1))-vi((i-1)*offv+Nodev(2:end-1,2:end-2)))/dx;
        %surf(XU(2:end,:),YV(:,2:end),dvdx-dudy,'FaceAlpha',0.5),shading interp
        vort = dvdx-dudy;
        
        SIGN(i) = sign(sum(sum(vortold(:,:,i).*vort)));
        vortold(:,:,i) = SIGN(i)*vort;
        pcolor(XU(2:end,:),YV(:,2:end),SIGN(i)*vort),shading interp,
        caxis([-std(vort(:)),std(vort(:))]*STDVORT+mean(SIGN(i)*vort(:)));
        h=streamslice(XP,YP,SIGN(i)*tmp1,SIGN(i)*tmp2);
        set(h,'color','k');
        hold off
        xlim(XLIM);ylim(YLIM)
        TT(1)=xlabel('x');TT(2)=ylabel('y');
        TT(3)=title(sprintf('Mode =%g \nu=[%0.2g,%0.2g]',i,...
            min(sqrt(tmp1(:).^2+tmp2(:).^2)),max(sqrt(tmp1(:).^2+tmp2(:).^2))));
        
        
    end
    PLTNUM=PLTNUM+1;
    subplot(NPLOTSx,NPLOTSy,PLTNUM),cla
    [n x] = ksdensity(SIGN(1)*YYt(:,1));
    % %         if SIGN(1) < 0
    % %             n=flipud(fliplr(n));
    % %         end
    plot(x/(max(x)-min(x))*2,n/max(n),':.');
    xlim([-1,1]);
    [n2 x2] = ksdensity(SIGN(2)*YYt(:,2));hold on
    %          if SIGN(2) < 0
    %             n2=flipud(fliplr(n2));
    %         end
    plot(x2/(max(x)-min(x))*2,n2/max(n),':.g');
    xlim([-1,1]);
    ylim([0 2.5]);
    title(sprintf('Marginal PDF'));
    legend('Mode 1','Mode 2')
end

if k>= Nt
   figure
   path1 = findpath(pathout,Targets(1,:), 1);
   figure
   path2 = findpath(pathout,Targets(2,:), 1);
end

