%% Scan parameters
clear all; 

global pixelx acc xcut scale dz line
% Scan parameters
pixelx = 1024;                      %pixels of 3d-scan files in x-direction
scansize = 8e-9;                    %length of the scan in x-direction in meters
dz = 5e-12;                         %height difference between each scan in the 2D current map
scale = scansize/pixelx;            %resolution of 3d-scan files in x-direction

%% Fitting
% Load the 2D current map without oscillation in the global variable i_proc
% we recommend to smooth the data
global i_proc;
i_proc = your2Dcurrentmap;

% Load the current line with oscillation in the global variable currSmline2
% we recommend to smooth the data
global currSmline2
currSmline2 = yourcurrentlinewithoutoscillation;

%Which linescan? Give starting parameters, the closer to real the better
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% starting values - only these values have to be changed
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A = 0.9e-9;                    %assumed amplitude (in meters)
theta = -2.0 / 360 * (2*pi);   %assumed tip tilt (negative value in radians)
linediff = 10.0;                %assumed starting linescan in the 2D current map
offset = -170;                   %starting offset (negative value, integer)
error_lim = 0.5;           %max. fitting error in Percent, start with 0.5
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% end of starting values
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
acc = 250;                          %number of points to calculate the integral
itterations = 2;            % this value determines the number of iterations and precison, 2 is fine for the most cases and should not be changed
dzpmax = A * cos((2 * pi) / acc) * sin(theta) / dz;
global scancnt 
scancnt = ceil(abs(dzpmax));   %closest possible startscan, to stay in bounds of 3D scan during oscillation
%xcut = 175; %175
xcut = round(pixelx * 0.171); % for integral calculation

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fitting
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use 'FittingGlob3' to fit in a global minimum
% Or either use 'Fitting3' to fit in a local minimum 
% the current curve with oscillation will be accessed by the global variable
% 'currSmline2', the 2D current map will be access by the global variable
% 'i_proc'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%params = Fitting3(A, theta, linediff,itterations, offset);                           
params = FittingGlob3(A, theta, linediff,itterations, offset, error_lim); 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

integral = IntegralFunktion(params(1), params(2), params(3));       %create the final simulated curve, using the parameters from Fitting above
params2d = params(2)*360/2/pi;                        % convert the tilt angle from radians to degrees

error = Error(integral,currSmline2,params(4));                  %calculate the error
errorpercent = ErrorProzent(integral,currSmline2,params(4))      %calculate the error in percent
global fitgrob
fitgrob = pixelx-2*xcut;

disp('Fitted amplitude (pm):');
params(1)*1e12
disp('Fitted theta (degree):');
abs(params2d)

%% Make a plot of fitted curve and current line with oscillation
x = linspace(0-params(4),size(integral,2)-params(4), size(integral,2));
figure;
plot(x,integral);
%title({['Linescan = ', num2str(scancnt), '+', num2str(params(3))], ['theta = ' num2str(params(2))] , ['Osc.Amp =' num2str(params(1))]});
hold on;
plot(currSmline2);
title({['Line No. = ' num2str(params(3))], ['Theta (tilt angle) = ' num2str(params2d)], ['Amplitude = ' num2str(params(1))], ['Offset = ' num2str(params(4))]})
xlabel('Pixel')
ylabel('I(A)')
legend('fitted curve', 'measured curve','Location','southeast')

% figure;
% liney = 2;
% fntsize = 20;
% lnwidth = 2.5;
% plot(x,integral*1e12, 'r','LineWidth',2);
% hold on;
% plot(currSmline2*1e12, 'b','LineWidth',2);
% %axis([0 4 120 270])
% set(gca,'fontsize',fntsize,'linewidth',lnwidth);
% set(gca,'fontsize',fntsize,'linewidth',lnwidth);
% %set(gca,'Ytick',[1,96,191],'YTickLabel',{'0', '0.56','1.12'});
% %set(gca,'Xtick',[1,96,191],'XTickLabel',{'0', '0.56','1.12'});
% xlabel('(nm)'); ylabel('(pA)');

%% Calculate fitting uncertainty
% fitted values
amp = params(1);
tilt = params(2); % in rad
lineno = params(3);
offs = params(4);
d_tilt = -2*pi/360*0.01; % 0.1 of a degree
d_amp = 1e-12;

integral0 = IntegralFunktion(amp, tilt, lineno);
error0 = ErrorProzent(integral0,currSmline2,offs); 

%vary tilt
integral1 = IntegralFunktion(amp, tilt+d_tilt, lineno);
error1 = ErrorProzent(integral1,currSmline2,offs); 

integral2 = IntegralFunktion(amp, tilt-d_tilt, lineno);
error2 = ErrorProzent(integral2,currSmline2,offs);

integral3 = IntegralFunktion(amp, tilt+2*d_tilt, lineno);
error3 = ErrorProzent(integral3,currSmline2,offs);

integral4 = IntegralFunktion(amp, tilt-2*d_tilt, lineno);
error4 = ErrorProzent(integral4,currSmline2,offs);

integral5 = IntegralFunktion(amp, tilt+3*d_tilt, lineno);
error5 = ErrorProzent(integral5,currSmline2,offs);

integral6 = IntegralFunktion(amp, tilt-3*d_tilt, lineno);
error6 = ErrorProzent(integral6,currSmline2,offs);

integral7 = IntegralFunktion(amp, tilt+4*d_tilt, lineno);
error7 = ErrorProzent(integral7,currSmline2,offs);

integral8 = IntegralFunktion(amp, tilt-4*d_tilt, lineno);
error8 = ErrorProzent(integral8,currSmline2,offs);


error_array = [error7,error5,error3,error1,error0,error2,error4,error6,error8];
theta_array = [tilt+4*d_tilt,tilt+3*d_tilt,tilt+2*d_tilt,tilt+d_tilt,tilt,tilt-d_tilt,tilt-2*d_tilt,tilt-3*d_tilt,tilt-4*d_tilt];


% fit a quadratic function
px = linspace(tilt+4*d_tilt,tilt-4*d_tilt,20);

% fit using fitting tool
myfittype = fittype('a*(x-b).^2+c', 'dependent',{'y'},'independent',{'x'},'coefficients',{'a','b', 'c'});
cft = fit(theta_array', error_array', myfittype, 'StartPoint', [4.673e+05, tilt, 0.1706]);

%cf = fit((amp_array*1e12)', error_array_amp', 'poly2');
cft_coeff = coeffvalues(cft);
cft_confint = confint(cft);
cfty = cft(px);

%calculate the uncertainty of the fitted theta (degree):
disp('Theta uncertainty (degree):');
theta_var = abs(cft_confint(1,2) - cft_confint(2,2))*180/pi

% plot error variation
% fntsize = 15;
% lnwidth = 2.0;
% figure;
% plot(-flip(theta_array)*180/pi,flip(error_array),'bo','linewidth',lnwidth);
% hold on;
% plot(-flip(px)*180/pi,flip(cfty),'r','linewidth',lnwidth);
% xlabel('\theta in degree');
% ylabel('Error in %');
% legend('calculated error', 'parabolic fit','Location','north');
% set(gca,'fontsize',fntsize,'linewidth',lnwidth);
% set(gca,'fontsize',fntsize,'linewidth',lnwidth);

% vari_theta_fit = sqrt((p2.ParameterStd(2)/(2*p2.Coefficients(1)))^2+(p2.Coefficients(2)/(2*p2.Coefficients(1)^2)*p2.ParameterStd(1))^2);
% vari_theta_fitd = 360/(2*pi)*vari_theta_fit
% theta_fit = -p2.Coefficients(2)/(2*p2.Coefficients(1));
% theta_fitd = 360/(2*pi)*theta_fit

%vary amp
integral_a1 = IntegralFunktion(amp-d_amp, tilt, lineno);
error_a1 = ErrorProzent(integral_a1,currSmline2,offs);

integral_a2 = IntegralFunktion(amp+d_amp, tilt, lineno);
error_a2 = ErrorProzent(integral_a2,currSmline2,offs);

integral_a3 = IntegralFunktion(amp-2*d_amp, tilt, lineno);
error_a3 = ErrorProzent(integral_a3,currSmline2,offs);

integral_a4 = IntegralFunktion(amp+2*d_amp, tilt, lineno);
error_a4 = ErrorProzent(integral_a4,currSmline2,offs);

integral_a5 = IntegralFunktion(amp-3*d_amp, tilt, lineno);
error_a5 = ErrorProzent(integral_a5,currSmline2,offs);

integral_a6 = IntegralFunktion(amp+3*d_amp, tilt, lineno);
error_a6 = ErrorProzent(integral_a6,currSmline2,offs);

integral_a7 = IntegralFunktion(amp+4*d_amp, tilt, lineno);
error_a7 = ErrorProzent(integral_a7,currSmline2,offs);

integral_a8 = IntegralFunktion(amp-4*d_amp, tilt, lineno);
error_a8 = ErrorProzent(integral_a8,currSmline2,offs);

error_array_amp = [error_a8,error_a5,error_a3,error_a1,error0,error_a2,error_a4,error_a6,error_a7];
amp_array = [amp-4*d_amp,amp-3*d_amp,amp-2*d_amp,amp-d_amp,amp,amp+d_amp,amp+2*d_amp,amp+3*d_amp,amp+4*d_amp];

% figure;
% plot(amp_array,error_array);

% fit a quadratic function
pxa = linspace(amp-4*d_amp,amp+4*d_amp,20);

% using fitting tool
myfittype = fittype('a*(x-b).^2+c', 'dependent',{'y'},'independent',{'x'},'coefficients',{'a','b', 'c'});
cf = fit((amp_array*1e12)', error_array_amp', myfittype, 'StartPoint', [0.01, amp*1e12, 0.1]);

%cf = fit((amp_array*1e12)', error_array_amp', 'poly2');
cf_coeff = coeffvalues(cf);
cf_confint = confint(cf);
cfy = cf(pxa*1e12);

%calculate the uncertainty of the fitted amplitude (pm):
disp('Ampltiude uncertainty (pm):');
amp_var = abs(cf_confint(1,2) - cf_confint(2,2))

% plot error variation
% fntsize = 15;
% lnwidth = 2.0;
% figure;
% plot(amp_array*1e12,error_array_amp,'bo','linewidth',lnwidth);
% hold on;
% plot(pxa*1e12,cfy,'r','linewidth',lnwidth);
% xlabel('Amplitude in pm');
% ylabel('Error in %');
% legend('calculated error', 'parabolic fit','Location','north');
% set(gca,'fontsize',fntsize,'linewidth',lnwidth);
% set(gca,'fontsize',fntsize,'linewidth',lnwidth);

%% Functions
function integral = IntegralFunktion(A, theta, linediff)        %simulates a curve using the parameters
global pixelx acc xcut scale dz line i_proc scancnt
X = zeros(acc, pixelx-2*xcut);
for i = xcut:pixelx-xcut
    hilfx = zeros(acc,1);                                                   %go through every pixel, then through every used point during oscillation
    for t = 1:acc
        dxp = A * cos(t * ((2 * pi) / acc)) * cos(theta) / scale;           %calculated distance between pixel and actual position during oscillation     
        dzp = A * cos(t * ((2 * pi) / acc)) * sin(theta) / dz + linediff;
        restx = dxp - floor(dxp);                                           %non integer part of distances, used to give each edge neighbouring pixel a weight
        restz = dzp - floor(dzp);
        B1 = (1 - restx) * i_proc(i + floor(dxp), scancnt + floor(dzp)) + restx * i_proc(i + ceil(dxp), scancnt + floor(dzp));   %calculate weighted average of neighbouring pixels, this allows for non-integer linescans
        B2 = (1 - restx) * i_proc(i + floor(dxp), scancnt + ceil(dzp)) + restx * i_proc(i + ceil(dxp), scancnt + ceil(dzp));
        B = (1-restz) * B1 + restz * B2;
        hilfx(t) = B;                                                       %write up the current for each point during oscillation
    end
    X(:,i-xcut+1) = hilfx';                                                 %write up oscilation values for each pixel
end
integral = imgaussfilt((trapz(X)/acc),2);                                   %actually integrate over the oscillation points for each pixel and smooth
end

function fitparams = Fitting3(A0, theta0, line0, steps, offset)                     %fitting the parameters, using initial ones. steps defines the number of decimal points of the parameters
%global OffsetX 
global currSmline2
params = [A0, theta0, line0, offset];                                               
paramalt = 0;                                                               
for order = 1 : steps
    paramsalt = zeros(1, 4);                                                %first linescan, then amplitude, then tip tilt
    while params ~= paramsalt
        paramsalt = params;
        while params(3) ~= paramalt
            paramalt = params(3);                                           %go until this parameter doesnt change anymore
            if Error(IntegralFunktion(params(1), params(2), params(3) + 1/10^order), currSmline2, params(4)) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))            %change if higher parameter's curve fits better
                params(3) = params(3) + 1/10^order;
            elseif Error(IntegralFunktion(params(1), params(2), params(3) - 1/10^order), currSmline2, params(4)) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))        %change if lower parameter's curve fits better
                params(3) = params(3) - 1/10^order;
            end
        end
        while params(1) ~= paramalt
            paramalt = params(1);
            if Error(IntegralFunktion(params(1) + 1e-10/10^order, params(2) , params(3)), currSmline2, params(4)) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                params(1) = params(1) + 1e-10/10^order;
            elseif Error(IntegralFunktion(params(1) - 1e-10/10^order, params(2) , params(3)), currSmline2, params(4)) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                params(1) = params(1) - 1e-10/10^order;
            end
        end
        while params(2) ~= paramalt
            paramalt = params(2);
            if Error(IntegralFunktion(params(1), params(2) + 1/360 * (2*pi)/10^order , params(3)), currSmline2, params(4)) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                params(2) = params(2) + 1/360 * (2*pi)/10^order;
            elseif Error(IntegralFunktion(params(1), params(2) - 1/360 * (2*pi)/10^order , params(3)), currSmline2, params(4)) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                params(2) = params(2) - 1/360 * (2*pi)/10^order;
            end
        end
        % Fit offset
        while params(4) ~= paramalt
            paramalt = params(4);
            if Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4) + 1) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                params(4) = params(4) + 1;
            elseif Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4) - 1) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                params(4) = params(4) - 1;
            end
        end
    end
end
fitparams = params;
end

% Fitting function with random starting values 
function fitparams = FittingGlob3(A0, theta0, line0, steps, offset, error_lim)          %difference here is, that Fitting2 gets done multiple times for different initial parameters
%global OffsetX 
global currSmline2

%Variable variations for global minimum checking
%d_A = 1e-10;
d_A = 0.3*abs(A0);
d_theta = 1.2/360*2*pi;
d_line = 3;
%d_offset = round(0.5*abs(offset));

paramalt = 0;
params = zeros(1,4);
error = 2*error_lim;
while error > error_lim
    params(1) = A0 - d_A + 2*d_A*rand;
    params(2) = theta0 - d_theta + 2*d_theta*rand;
    params(3) = line0 - d_line + 2*d_line*rand;
    params(4) = round(1.3*offset*rand);
    for order = 1 : steps
        %order = 1;
        paramsalt = zeros(1, 4);
        while params ~= paramsalt
            paramsalt = params;
            % Optimze line
            while params(3) ~= paramalt
                paramalt = params(3);
                if Error(IntegralFunktion(params(1), params(2), params(3) + 1/10^order), currSmline2, params(4)) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                    params(3) = params(3) + 1/10^order;
                elseif Error(IntegralFunktion(params(1), params(2), params(3) - 1/10^order), currSmline2, params(4)) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                    params(3) = params(3) - 1/10^order;
                end
            end
            % Optimze amplitude
            while params(1) ~= paramalt
                paramalt = params(1);
                if Error(IntegralFunktion(params(1) + 1e-10/10^order, params(2) , params(3)), currSmline2, params(4)) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                    params(1) = params(1) + 1e-10/10^order; % order = 1 -> change amplitude by 10 pm
                elseif Error(IntegralFunktion(params(1) - 1e-10/10^order, params(2) , params(3)), currSmline2, params(4)) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                    params(1) = params(1) - 1e-10/10^order;
                end
            end
            % Optimize theta
            while params(2) ~= paramalt
                paramalt = params(2);
                if Error(IntegralFunktion(params(1), params(2) + 1/360 * (2*pi)/10^order , params(3)), currSmline2, params(4)) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                    params(2) = params(2) + 1/360 * (2*pi)/10^order;
                elseif Error(IntegralFunktion(params(1), params(2) - 1/360 * (2*pi)/10^order , params(3)), currSmline2, params(4)) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                    params(2) = params(2) - 1/360 * (2*pi)/10^order;
                end
            end
            % Optimize offset
            while params(4) ~= paramalt
                paramalt = params(4);
                if Error(IntegralFunktion(params(1), params(2), params(3)), currSmline2, params(4) + 1) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                    params(4) = params(4) + 1;
                elseif Error(IntegralFunktion(params(1), params(2), params(3)), currSmline2, params(4) - 1) < Error(IntegralFunktion(params(1), params(2) , params(3)), currSmline2, params(4))
                    params(4) = params(4) - 1;
                end
                
            end
        end
    end
    %error = Error(IntegralFunktion(params(1),params(2),params(3)),currSmline2,params(4))      %error for each starting set of parameters
    error = ErrorProzent(IntegralFunktion(params(1),params(2),params(3)),currSmline2,params(4))
end % end of while loop: (error>error_lim)
%[~,I] = min(errors);                                                                            %find set of best initial parameters
%fitparams = Fitting3(paramsglob(I,1), paramsglob(I,2), paramsglob(I,3), steps, paramsglob(I,4));                 %find actual parameters to initial ones
%fitparams = Fitting3(params(1), params(2), params(3), steps+1, params(4));
fitparams = params;
end % end of function


function error = Error(curve1,curve2, offset)      %curve1 = integral
error = 0;
    % Catch the case where the fitted offset parameter causes an out of
    % range
    if offset > 0
        offset = 0;
    end
    for i = 1:size(curve1,2)
          error = error + abs(curve1(i)-curve2(i-offset))^2;
    end
end

function errorprozent = ErrorProzent(curve1,curve2, offset)      %curve1 = integral
error = zeros(1,size(curve1,2));
for i = 1:size(curve1,2)
      error(i) = abs(1-curve1(i)/curve2(i-offset));
end
errorprozent = mean(error)*100;
end
