% %%
clear all
close all
LoadDirectory = ('C:\Users\'); % directory for loading datqa
SaveDirectory = ('C:\Users\FORCE'); % directory for saving processsed datafile
Measurement = ('\filename'); %  3DFFM filename

DataSaveName = [SaveDirectory,Measurement,'_FORCEk']; 
DataLoadName = [LoadDirectory,Measurement];
VidDirectory = ('C:\Users\VIDEOS\'); % directory for sacing videos

SaveFigures = 0; % do you want to save the figures produced here, 0 => no, 1 => yes
load(DataLoadName)
[zSIZE,xSIZE,ySIZE] = size(PHASEphaseD);
% PHASEphaseD = medfilt3(PHASEphaseD,[3,3,3]); AMPampD = medfilt3(AMPampD,[3,3,3]);
%%
%%%%%%%%% calculate force gradient kGRAD
close all
vEXC = FreqDrive; vE = FreqRes; A0 = mean(nanmean(mean(AMPampD(5:20,:,:)))); ShiftEq = 90-mean(mean(nanmean(PHASEphaseD(5:20,:,:)))); % use all nanmean instead of mean??
F0 = kCONST/Qfactor*A0;
for n = 1:zSIZE
    for x = 1:xSIZE
        for y = 1:ySIZE
            kGRAD(n,x,y) = -F0./AMPampD(n,x,y).*cos((PHASEphaseD(n,x,y)+ShiftEq)*pi/180);
            GAMMA(n,x,y) = -kCONST/(2*pi*vEXC*Qfactor) -F0./(2*pi*vEXC.*AMPampD(n,x,y)).*sin(PHASEphaseD(n,x,y)*pi/180); % first vEXC is actually vE, but we are making the assumption that the two values are equivalent
        end
    end
end
kGRADave = [nanmean(squeeze(nanmean(kGRAD(1:5,:,:),2)),2); nanmean(squeeze(nanmean(kGRAD(6:end,:,:),2)),2)];
PHASEave = ShiftEq+[nanmean(squeeze(nanmean(PHASEphaseD(1:5,:,:),2)),2); nanmean(squeeze(nanmean(PHASEphaseD(6:end,:,:),2)),2)];
DLINEAR = Dlinear*10^9;

figure(1)
yyaxis left
plot(DLINEAR,kGRADave,'linewidth',2), hold on
set(gca,'linewidth',2,'fontsize',17)
xlabel('z (nm)','fontsize',20)
ylabel('k (N/m)','fontsize',20)
xlim([0 inf])

figure(1)
yyaxis right
plot(DLINEAR,PHASEave,'linewidth',2), 
set(gca,'linewidth',2,'fontsize',17)
xlabel('z (nm)','fontsize',20)
ylabel('\phi (deg)','fontsize',20)

if SaveFigures == 1
    FigName = [VidDirectory,'kAVE_PhaseAVE_',Measurement,'.tif'];
   saveas(figure(1),FigName)
end
%%
%%%%%%% Set up exponential long-range fit and options.
% close all
ft = fittype( 'exp1' ); 
opts = fitoptions( 'Method', 'NonlinearLeastSquares' ); opts.Display = 'Off';
IND = isfinite(kGRADave); 
[fitresult, gof] = fit( DLINEAR(IND)', kGRADave(IND), ft, opts ); % Fit model to data.
COEF = coeffvalues(fitresult);
LongRange = COEF(1)*exp(COEF(2)*DLINEAR);
figure(5)
plot(DLINEAR,kGRADave,'k','linewidth',2), hold on
hold on
plot(DLINEAR,LongRange,'--','linewidth',2), 
legend({'force gradient','exponential fit'})
minxv = find(DLINEAR<0);
set(gca,'linewidth',2,'fontsize',17)
xlabel('z (nm)','fontsize',20)
ylabel('k (N/m)','fontsize',20)
axis([0 inf kGRADave(minxv(1)-1)  inf])
if SaveFigures == 1
    FigName = [VidDirectory,'ExpFit_',Measurement,'.tif'];
   saveas(figure(5),FigName)
end

LongRange = LongRange';
figure(2)
plot(DLINEAR,kGRADave-LongRange,'linewidth',2), hold on
set(gca,'linewidth',2,'fontsize',17)
xlabel('z (nm)','fontsize',20)
ylabel('k (N/m)','fontsize',20)
axis([-inf inf -inf inf])
if SaveFigures == 1
    FigName = [VidDirectory,'k_Subtract_',Measurement,'.tif'];
   saveas(figure(2),FigName)
end

kGRADfin = [];
for x = 1:xSIZE
    for y = 1:ySIZE
        kGRADfin(:,x,y) = kGRAD(:,x,y) - LongRange;
    end
end
%%
%%%%%%%%%%%%%% plot slice of kGRADfin (kGRAD minues long range interactions)
Znm = 1; SLICEpix = 46; 
figure(10)
[~,Dheight] = min(abs(Dlinear-Znm*10^-9)); % find index of Znm
% IM1 = squeeze(kGRADfin(:,:,SLICEpix)); IM2 = squeeze(kGRADfin(:,:,SLICEpix-2)); IM3 = squeeze(kGRADfin(:,:,SLICEpix+2));
% IMk = (IM1+IM2+IM3)/3;
IMk = (kGRADfin(:,:,SLICEpix));
imagesc(IMk); colorbar,
set(gca,'linewidth',2,'fontsize',19), axis([-inf inf Dheight length(Dlinear)])
if Xnm == 3, xticks((0:Xnm)*xSIZE/Xnm), xticklabels({'0','1','2','3'})
else, xticks((0:Xnm/4:Xnm)*xSIZE/Xnm), xticklabels({'0',num2str(Xnm/4),num2str(2*Xnm/4),num2str(3*Xnm/4),num2str(Xnm)}), end
yticks(Dheight:(length(Dlinear)-Dheight)/4:length(Dlinear)), yticklabels({num2str(Znm), num2str(3*Znm/4), num2str(Znm/2) , num2str(Znm/4), num2str(0)})
xlabel('x (nm)','fontsize',23), ylabel('z (nm)','fontsize',23)
title('Force Gradient minus exponential fit')
if SaveFigures == 1
    FigName = [VidDirectory,'kSLICE_',Measurement,'.tif'];
   saveas(figure(10),FigName)
end

%%%%%%%%%%%%% plot average phase curve from this xz slice
%%%%%%%%%%%%% plot a few individual force curves
figure(20)
cmp = get(gca,'ColorOrder');
hold on
S = [20 30 40 50];
patchline(DLINEAR,kGRADfin(:,S(1),S(1)),'edgecolor',cmp(1,:),'edgealpha',0.8), 
patchline(DLINEAR,kGRADfin(:,S(2),S(2)),'edgecolor',cmp(2,:),'edgealpha',0.8), 
patchline(DLINEAR,kGRADfin(:,S(3),S(3)),'edgecolor',cmp(3,:),'edgealpha',0.8), 
patchline(DLINEAR,kGRADfin(:,S(4),S(4)),'edgecolor',cmp(4,:),'edgealpha',0.8), 
plot(DLINEAR,kGRADave-LongRange,'k','linewidth',3), 
axis([0 inf -inf inf])
set(gca,'linewidth',2,'fontsize',17)
xlabel('z (nm)','fontsize',20)
ylabel('k (N/m)','fontsize',20)
if SaveFigures == 1
    FigName = [VidDirectory,'kExample_',Measurement,'.tif'];
   saveas(figure(20),FigName)
end
%%
% % %%%%%% create animation of xy slices
% h1 = figure(30);
% Znm_slice = 0.1:0.2:1.2;
% CLIMlim = (abs(min(kGRADave-LongRange)) + abs(max(  kGRADave(1:minxv(1))  -  LongRange(1:minxv(1)) )) ) / 3; % complex way to set clims 
% clims(1) = min(kGRADave-LongRange) - CLIMlim;
% clims(2) = max(  kGRADave(1:minxv(1))  -  LongRange(1:minxv(1)) )   + CLIMlim; 
% VideoName = [VidDirectory,'XY_kGRAD_Subtract_',Measurement];
% if SaveFigures == 1
% OBJ = VideoWriter(VideoName); FrameRate=3; open(OBJ); 
% end
% for i = 1:length(Znm_slice)
%     ZnmSLICE = Znm_slice(i); [~,Dheight2] = min(abs(Dlinear-ZnmSLICE*10^-9)); % find index of Znm
%     IM1 = squeeze(kGRADfin(Dheight2,:,:)); IM2 = squeeze(kGRADfin(Dheight2-1,:,:)); IM3 = squeeze(kGRADfin(Dheight2+1,:,:));
%     IM = (IM1+IM2+IM3)/3; IM = (fliplr(IM))'; 
%     
%     imagesc(IM,clims) , colorbar
%     daspect([1 1 1])
%     set(gca,'linewidth',2,'fontsize',14), set(gca,'xtick',[],'ytick',[])
%     TITLE = [num2str(Xnm),'\times',num2str(Ynm),' nm^2, z = ',num2str(ZnmSLICE),' nm'];    
%     title(TITLE,'fontsize',18)
%     drawnow
%     pause(0.6)
%     if SaveFigures == 1
%     currFrame = getframe(h1); writeVideo(OBJ,currFrame);  
%     end
% end
% if SaveFigures == 1,  close(OBJ); end
%%
%%%%% create animation of xz slices
Znm = 1.6; [~,Dheight] = min(abs(Dlinear-Znm*10^-9)); % find index of Znm
Xnm_slice = 0.2:0.4:Xnm-0.2;
h2 = figure(31);
% CLIMlim = (abs(min(kGRADave-LongRange)) + abs(max(  kGRADave(1:minxv(1))  -  LongRange(1:minxv(1)) )) ) / 3; % complex way to set clims 
% clims(1) = min(kGRADave-LongRange) - CLIMlim;
% clims(2) = max(  kGRADave(1:minxv(1))  -  LongRange(1:minxv(1)) )   + CLIMlim; 
VideoName = [VidDirectory,'XZ_kGRAD_Subtract_',Measurement];
if SaveFigures == 1
    OBJ = VideoWriter(VideoName); OBJ.FrameRate=3; open(OBJ); 
end
for i = 1:length(Xnm_slice)
    XnmSLICE = round( Xnm_slice(i)*xSIZE/Xnm ); if rem(XnmSLICE,2) == 1, XnmSLICE=XnmSLICE+1; end
    IM1 = squeeze(kGRADfin(:,:,XnmSLICE)); %IM2 = squeeze(kGRADfin(:,:,XnmSLICE-2)); IM3 = squeeze(kGRADfin(:,:,XnmSLICE+2));
    IM = IM1;
    minxv = find(DLINEAR<0);
    imagesc(IM), %,clims), colorbar
    set(gca,'linewidth',2,'fontsize',16), axis([-inf inf Dheight length(Dlinear)])
    xticks((0:Xnm)*xSIZE/Xnm), xticklabels({'0','1','2','3'})
    yticks(Dheight:(length(Dlinear)-Dheight)/4:length(Dlinear)), yticklabels({num2str(Znm), num2str(3*Znm/4), num2str(Znm/2) , num2str(Znm/4), num2str(0)})
    xlabel('x (nm)','fontsize',20), ylabel('z (nm)','fontsize',20)
    TITLE = ['y = ',num2str(Xnm_slice(i)),' nm'];    
    title(TITLE,'fontsize',18)
    drawnow
    pause(0.2)
    kGRADfinAVEslice(:,i) = nanmean(IM,2);
    if SaveFigures == 1
    writeVideo(OBJ,currFrame);  
    end
end
if SaveFigures == 1,  close(OBJ); end
%%
% save(DataSaveName,'kGRAD','GAMMA','F0','ShiftEq','kGRADfin','kGRADave','DLINEAR','LongRange','COEF')