%%%%%%%%%% Initialization %%%%%%%%%%
clear;clc;

%%%%%%%%%% WHAT YOU NEED TO ENTER %%%%%%%%%%
%location of file to load.
file = 'data.xlsx';
%sheet to read out
sheet = 'FigS4B';
stringtooutputfile = ['MHMC-Mesa',filesep,'FigS4B_valuesappendwithpriors.mat'];

dir = 1 %%%% dir=1 means along x-axis, dir=2 means along y-axis

appendonprev = 1 
% appendonprev             boolean, if False(0) will start a new
%                          MHMC step-chain. For True(1) will continue on an existing
%                          chain

%%%%%%%%%% Load Data %%%%%%%%%%%%%%%%%%%
opts = detectImportOptions(file); 
opts.DataRange = 'A4'; %define start of data
opts.Sheet = sheet; %select the correct sheet

T = readtable(file,opts); %read the xlsx file

positionaxis = T.Var2; %extract x values (um)
positionaxis = positionaxis(~isnan(positionaxis)); %remove any possible NaN values

ESR = T.Var1; %extract y values (um)
ESR = ESR(~isnan(ESR)); %remove any possible NaN values

%%%calculate the background ESR resonance location
L1 = mean(ESR(1:10)); %take the average of the first 10 points
Lend = mean(ESR(:,end)); %take the average of the last 10 points
bkg = (L1+Lend)/2; %take the average of these values as the background 

fields_avg = 100*(bkg - ESR)/0.0028; %convert the ESR resonance value (GHz) to stray field (uT)

%% Start parameters for Fit

%%%% Define the necessary startparameters for a fit

offset = 28.5 %% position first-edge mesa in um
w = 2.05 %% mesa width in um

if dir==1
mag = 17 %%%% uA
phi = 260; %% NV in plane angle
else
mag = -10 %%%% uA
phi = 170; %% NV in plane angle (since scan direction rotated by -90 deg)
end

mu_0 = 1.2566 % uT*um/uA
d = 50e-3; %um
x1 = 0.174; %um
theta = 57; %% NV out of plane angle
offset_addition = 0; %% constant bias field value
hd = -0.03; %%% in this special case here for FourCurrentFunc2heights. this is the height difference due to slanted pillar
mu_Ms_t = mu_0*mag; %%% uT*um ///

val = [mu_Ms_t,d,x1,theta,phi,offset,w,offset_addition,hd];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%% Upper and lower bound values %%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


active_LBUB = 1; %%% 1 = with boundary values 0= without boundary values. UB means upper bound, LB lower bound %%%%

if active_LBUB == 1   
    UB_mu_Ms_t =1.256*40; % Magnetisierung in muA*mu_0
    LB_mu_Ms_t =-1.256*40; %A
    UB_d = 180e-3; % NV height in mum
    LB_d = 20e-3; %m
    UB_x1 = 0.175; %%%% stepheight
    LB_x1 = 0.174; %m
    UB_theta = 90; %%%% Winkel in grad oop
    LB_theta = 45; %%%% 
    UB_phi = 330; %%%% Winkel inplane
    LB_phi = 10;
    UB_offset = val(6)+1; %%%% left edge micrometer
    LB_offset = val(6)-1;
    UB_w = val(7)+1; %%%%%% width in micrometer
    LB_w = val(7)-1;
    UB_offset_addition = 0.001; %%% offset on data
    LB_offset_addition = -0.001;
    UB_hd = 0.07; %%% NV height difference in micrometer
    LB_hd = -0.07;

    valub = [UB_mu_Ms_t,UB_d,UB_x1,UB_theta,UB_phi,UB_offset,UB_w,UB_offset_addition,UB_hd];
    vallb = [LB_mu_Ms_t,LB_d,LB_x1,LB_theta,LB_phi,LB_offset,LB_w,LB_offset_addition,LB_hd];
else 
    valub = [];
    vallb = [];    
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
%% Pre-Fit and reset boundaries prepare MHMC and next fits
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Definition of th Fitfunction %%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fuevals = 1000; %%% Max. No. of evaluations of the fitting function
maxiterno = 1000; %%% Max. Iterations of fitting-solver
fitfunction = @FourCurrentFit2heightsFunc_simple;
fitfunctioncost= @FourCurrentFit2heightsFunc_cost_simple;

         [ahat,fval]  =  FitFun_MultiStart(positionaxis,fields_avg,val,vallb,valub,fuevals,maxiterno,fitfunctioncost);
         [B_NV_parallel] = fitfunction(ahat,positionaxis); 
         [B_NV_parallel0] = fitfunction(val,positionaxis);
         val = ahat; %% Update the MHMC starting parameterset to the fitparameters
         
         figure('Name','Data and Fit');
         title(sprintf(sheet), 'Interpreter', 'none')
         hold on;
         p1 = plot(positionaxis,fields_avg,'LineStyle','none','marker','.');
         p2 = plot(positionaxis,B_NV_parallel);
         p3 = plot(positionaxis,B_NV_parallel0);

         ylabel('Field [µT]');
         xlabel('position [um]');
         legend([p1,p2,p3],{'meas µT','BNV fit','BNV starting paras MHMC'}, 'Location', 'north' );



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Upper and lower bound values for MHMC %%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

UB_mu_Ms_t =1.256*40; % Magnetisierung in muA*mu_0
LB_mu_Ms_t =-1.256*40; %A
UB_d = 180e-3; % NV height in mum
LB_d = 20e-3; %m
UB_x1 = 0.175; %%%% stepheight
LB_x1 = 0.174; %m
UB_theta = 70; %%%% Winkel in grad oop
LB_theta = 45; %%%% 
UB_phi = 360; %%%% Winkel inplane
LB_phi = 0;
UB_offset = val(6)+1; %%%% left edge micrometer
LB_offset = val(6)-1;
UB_w = val(7)+1; %%%%%% width in micrometer
LB_w = val(7)-1;
UB_offset_addition = 0.001; %%% offset on data
LB_offset_addition = -0.001;
UB_hd = 0.07; %%% NV height difference in micrometer
LB_hd = -0.07;

valub = [UB_mu_Ms_t,UB_d,UB_x1,UB_theta,UB_phi,UB_offset,UB_w,UB_offset_addition,UB_hd];
vallb = [LB_mu_Ms_t,LB_d,LB_x1,LB_theta,LB_phi,LB_offset,LB_w,LB_offset_addition,LB_hd];


        
%% Append on previous files
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Try loading previous values %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if appendonprev ==1
            try
                load(stringtooutputfile)
                values = valuesappend(end,:);
            catch
               sprintf(['No previous values found',stringtooutputfile])
               valuesappend = val;
               values = val;
            end
else %% If no file exists yet, start new MHMC chain.
    valuesappend = val;
    values = val;
end
%%
%%%%% Call MHMC %%%%%%%%%%

savefile=1  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Define the jump parameters %%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
proposedsigmas = abs(0.03*val); %% Defines the width of the distribution around the current parameterset to draw a new candidate set.
proposedsigmas(3) = 0.000001;
proposedsigmas(4) = 4;
proposedsigmas(5) = 4;
proposedsigmas(6) = 0.001;
proposedsigmas(7) = 0.001;
proposedsigmas(8) = 0.001;
 
for k=1:length(val)
logpriors{k} = @unifpdf; %% Sets uniform priors with the bounds supplied by valUB and valLB
end

                    % Take the priors from analysis of all Mesa-scans %
                    fitparas = [61.433,2.3544]; %% Theta values, center and width of distribution
 
                    if dir ==1
                    fitparasphi = [260.7,1.0852]; %% Phi values, center and width of distribution
                    else
                    fitparasphi = [-90,0,0,0]+[260.7,1.0852]; %%% Rotate the prior by 90 deg when scanning along y-axis
                    end
                    
                    logpriors{4} = @(x,xlb,xub) normpdf(x,fitparas(1),fitparas(2));
                    logpriors{5} = @(x,xlb,xub) normpdf(x,fitparasphi(1),fitparasphi(2));
                    
                    values(4) = fitparas(1);
                    values(5) = fitparasphi(1);
                    
                    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Initialize the MHMC  %%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

       nu = length(fields_avg); %% Length of Datast
       steps=1e5; %% Total steps to perform
       loops = 1e2; %% loops to perform
       tic %% Starttime to estimate time needed for a single loop
       
        for j=1:loops
        valnew=values(end,:);
        [values,na] = MetropolisHasting_wp(positionaxis,fields_avg,valnew,vallb,valub,steps/loops,proposedsigmas,nu,fitfunctioncost,logpriors); %% Call the Metropolis Hastings algorithm with the fitfunction and recoreded data + prior knowledge
        toc
        sprintf('time per loop %d, remain. min. %d, loop number %d, percent done %d, acceptance rate is %f',toc/j,floor((-1+loops/j)*toc/60),j,floor(j/loops*100),na*loops/steps) %%% Output to check for accpetance rate and time per loop
        valuesappend = cat(1,valuesappend,values); 
        end

        %%% Display Output and Fit Gaussian to find parameters and
        %%% uncertainties
        
        parameterstring={'µ0Mst','d','thickness','theta','phi','position','width','dataoffset','heightdiff'};
        figure;
        
         for k=1:length(valuesappend(1,:));
                    
                    subplot(3,3,k);
                    hold on;
                    
                    polo=histogram(valuesappend(:,k)); %%% Histogram the MHMC steps
                    Bin_centers = (polo.BinEdges+polo.BinWidth/2); %% Create array of Bin_centers
                    histogramxvals = Bin_centers(1:end-1); %%% Assign xvalues of the histrogram (bin centers)
                    histogramyvals = polo.Values %%% Assign yvalues of the histrogram (occurences)
                    [valmax(k),valmaxpos(k)] = max(polo.Values); %%% Extract the height and position of maximum occurences
                    valmaxposvalue(k) = polo.BinEdges(valmaxpos(k))+polo.BinWidth/2; %% Correct for edge-mid position of bin. Store the position of maximum occurence in valmaxposvaluedir1                     
                    fitparas = lsqcurvefit(@Gaussian,[valmaxposvalue(k),valmax(k),0,std(valuesappend(:,k))],histogramxvals,histogramyvals); %%% Fits the distribution by a Gaussian. Fitparameters stored in fitparas
                    valmaxposvalueff(k) = fitparas(1); %%% Store the parametervalue as from fit
                    valFWHM(k) = fitparas(4); %%% Store the Full Width Half Max uncertainty of the parametervalue from fit

                    plot(histogramxvals,Gaussian(fitparas,histogramxvals))
                    title(sprintf(parameterstring{k}));
         end
        
        if savefile==1
        save((stringtooutputfile),'valuesappend');
        else
        end
        
%% Subfunctions that are needed
function [ahat,fval]  =  FitFun_MultiStart(cutxaxis,RF_avg,val,vallb,valub,fuevals,maxiterno,fun); %% Sets up the fitting routine to find initial parameters for the MHMC algorithm
    Modelstring = 'Model:FourCurrentFitwithVzand2heightsFunc_MultiStart';
    problem = createOptimProblem('fmincon','objective',@(val)fun(val,cutxaxis,RF_avg),'lb',vallb,'ub',valub,'x0',val,'options',optimoptions('fmincon','MaxFunctionEvaluations',fuevals,'MaxIterations',maxiterno,'FunctionTolerance',1e-20,'StepTolerance',1e-20));
    ms = MultiStart;
    [ahat,fval] = run(ms,problem,50);    
end

function [B_NV_parallel] = FourCurrentFit2heightsFunc_simple(v,x,field) %% Stray field model for a mesa
   

    mu0_Ms_t = v(1); %%%% prefactor-product
    d = v(2); %%%% NV distance
    x1 = v(3); %%% thickness of film
    theta = v(4); %%% oop NV angle
    phi = v(5); %%% ip NV angle
    offset = v(6); %%%% offset on stripe position
    w = v(7); %%% width of stripe
    offset_addition = v(8); 
    hd = v(9);

                 prefac = (mu0_Ms_t/(2*pi));
                 d2 = d+x1;
                 d3 = d+hd;
                 d4 = d+hd+x1;
                 denom  = ((x-offset).^2+d.^2); %%%% top left edge
                 denom3 = ((x-offset-w).^2+d3.^2); %%%% top right edge
                 denom4 = ((x-offset-w).^2+(d4).^2); %%%% bottom right edge
                 denom2 = ((x-offset).^2+(d2).^2); %%% bottom left edge
                 
                 Bx_stripe1 = prefac*((-d)./denom + d3./denom3); %%%% top left and right edge x
                 Bx_stripe2 = prefac*((d2)./denom2 + (-d4)./denom4); %%% bottom left and right edge x
                 Bx_stripe = Bx_stripe1 + Bx_stripe2; %%% adding the field contributions to x
                 
                 Bz_stripe1 = prefac*((x-offset)./denom - (x-offset-w)./denom3); %%%% top left and right z
                 Bz_stripe2 = prefac*((x-offset)./denom2 - (x-offset-w)./denom4);%%%% bottom left and right egde
                 Bz_stripe = Bz_stripe1 - Bz_stripe2; %%% adding the field contributions to z
                 
                 %%%% calculate projection on NV axis
                 B_NV_parallel = Bx_stripe*sind(theta)*cosd(phi) +Bz_stripe*cosd(theta) + offset_addition; 
                 
end

function [B_NV_parallel_cost] = FourCurrentFit2heightsFunc_cost_simple(v,x,field) %% The cost function of the stray field model for a mesa
   

    mu0_Ms_t = v(1); %%%% prefactor-product
    d = v(2); %%%% NV distance
    x1 = v(3); %%% thickness of film
    theta = v(4); %%% oop NV angle
    phi = v(5); %%% ip NV angle
    offset = v(6); %%%% offset on stripe position
    w = v(7); %%% width of stripe
    offset_addition = v(8); 
    hd = v(9);

                 prefac = (mu0_Ms_t/(2*pi));
                 d2 = d+x1;
                 d3 = d+hd;
                 d4 = d+hd+x1;
                 denom  = ((x-offset).^2+d.^2); %%%% top left edge
                 denom3 = ((x-offset-w).^2+d3.^2); %%%% top right edge
                 denom4 = ((x-offset-w).^2+(d4).^2); %%%% bottom right edge
                 denom2 = ((x-offset).^2+(d2).^2); %%% bottom left edge
                 
                 Bx_stripe1 = prefac*((-d)./denom + d3./denom3); %%%% top left and right edge x
                 Bx_stripe2 = prefac*((d2)./denom2 + (-d4)./denom4); %%% bottom left and right edge x
                 Bx_stripe = Bx_stripe1 + Bx_stripe2; %%% adding the field contributions to x
                 
                 Bz_stripe1 = prefac*((x-offset)./denom - (x-offset-w)./denom3); %%%% top left and right z
                 Bz_stripe2 = prefac*((x-offset)./denom2 - (x-offset-w)./denom4);%%%% bottom left and right egde
                 Bz_stripe = Bz_stripe1 - Bz_stripe2; %%% adding the field contributions to z
                 
                 %%%% calculate projection on NV axis
                 B_NV_parallel = Bx_stripe*sind(theta)*cosd(phi) +Bz_stripe*cosd(theta) + offset_addition; 
                 
                 B_NV_parallel_cost = sum(sqrt((B_NV_parallel - field).^2));
                 
end

function [values,na] = MetropolisHasting_wp(cutxaxisC,fields_avg,vals,vallb,valub,steps,proposedsigmas,nu,fun,logpriors)

%%% Passed parameters are %%%%
%%% data:               consisting of xvalues and yvalues
%%% xvals:              are in array cutxaxisC
%%% yvals:              are in fields_avg
%%% vals:               are the initial starting parameters
%%% vallb:              lower bound for the parameters 
%%% valub:              upper bound for the parameters
%%% steps:              iterationsteps to perform
%%% proposedsigmas:     sigma value for performing random walk MHMC
%%%                     Draws from a normal distribution around current
%%%                     point with the proposed sigma
%%% nu:                 No of points in the dataset (yvals)
%%% fun:                handle to the model-function to 'match' the data
%%% logpriors:          prior knowledge as given by logarithm of the
%%%                     likelihood distribution. This can be uniform normal
%%%                     distributed or any other function.


na = 0; %% initialze total number of jumps to be 0
values = zeros(steps,length(vals)); %% initialize array
values(1,:)=vals; %% Set the first point to the passed initial vals

for i=2:steps
    
for k = 1:length(vals);
valsptsm(k) =values(i-1,k); %% current parameterset
uvals(k) = normrnd(valsptsm(k),proposedsigmas(k)); %% next proposed parameterset
end

%%% Calculate the logarithm of the pdf's for the current (p1) and proposed (p2)
%%% parameterset 
cost1 = fun(valsptsm,cutxaxisC,fields_avg);
p1 = -(nu+1)/2 * log(cost1);
cost2 = fun(uvals,cutxaxisC,fields_avg);
p2 = -(nu+1)/2 * log(cost2);

%%% Includes the prior knowledge into the pdf calculation
    for k = 1:length(vals)  %%% Here a chaning of a prior should be introduced. Iterative on datasets, or once?
    p1 = p1 + log(logpriors{k}(valsptsm(k),vallb(k),valub(k)));
    p2 = p2 + log(logpriors{k}(uvals(k),vallb(k),valub(k)));
    end

%%% Caculate the difference between the pdfs, denoted as r, and compare it
%%% to the logarithm of a random number between 0 and 1
u = log(rand);
r = p2-p1;
   
%%% Apply the criteria for a successful jump in the Markov-Chain 
if u <= r %% Acceptance criteria fullfilled, accept proposed parameter-candidate
    na = na+1; %% Increase number of successful jumps
    for k = 1:length(vals) 
    values(i,k) = uvals(k); %%% update current parameterset to proposed parameterset
    end
    
else %% acceptance criteria not fullfilled
     
    for k = 1:length(vals) 
    values(i,k) = valsptsm(k); %% Keep the parameters and ignore the proposed parameterset
    end

    end
end %% repeat this loop until steps are reached

end

function [B_NV_parallel] = Gaussian(v,x);  
xc = v(1);
Amp = v(2);
offset = v(3);
width = (1/2.35482)*v(4);%% (* converting sigma into FWHM *)
B_NV_parallel = Amp*exp(-0.5*((x-xc)./width).^2)+offset;              
end