%% Import Peaklist
V_uc = 100; % in A^3

% Load Peak List including Peak Positions and Intensities for Oriented Component
opts = delimitedTextImportOptions("NumVariables", 14);
opts.DataLines = [2, Inf];
opts.Delimiter = "\t";
opts.VariableNames = ["h", "k", "l", "q_x", "q_y", "q_z", "q_xy", "q",...
    "realSF", "imagSF", "absSF", "absSF2", "absSF2Rotation", "d"];
opts.VariableTypes = ["string", "string", "string", "double",...
    "double", "double", "double", "double", "double", "double",...
    "double", "double", "double", "double"];
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";
tbl = readtable(".\peak_list.txt", opts);
hkl_oriented = tbl.h+tbl.k+tbl.l;
q_xy = tbl.q_xy;
q_z = tbl.q_z;
q_ref_oriented = tbl.q;
Psi_ref_oriented = zeros(size(q_z));
Psi_ref_oriented(q_z>=0) = 180./pi.*atan(q_xy(q_z>=0)./q_z(q_z>=0));        % calculate Psi from q_xy and q_z
Psi_ref_oriented(q_z<0) = 180+180./pi.*atan(q_xy(q_z<0)./q_z(q_z<0));
absSF2_oriented = tbl.absSF2Rotation./V_uc.^2.*2;

% Load Peak List including Peak Positions and Intensities for Unoriented Component Extracted from Mercury Powder Calculation
filename_hkl = "./Powder.hkl";
startRow = 2;
formatSpec = '%6f%7f%7f%10f%13f%f%[^\n\r]';
fileID = fopen(filename_hkl,'r');
dataArray = textscan(fileID, formatSpec, 'Delimiter', '', 'WhiteSpace', '', 'TextType', 'string', 'HeaderLines' ,startRow-1, 'ReturnOnError', false, 'EndOfLine', '\r\n');
fclose(fileID);
hkl_unoriented = erase(strcat(num2str(dataArray{:, 1}),num2str(dataArray{:, 2}),num2str(dataArray{:, 3}))," ");
q_ref_unoriented = 2*pi./dataArray{:, 4};                                   % twotheta calculated from lambda and d
F_2 = dataArray{:, 5};
multiplicity = dataArray{:, 6};
absSF2_unoriented = multiplicity.*F_2./V_uc.^2;
clearvars filename_hkl startRow formatSpec fileID dataArray;

clear opts tbl q_xy q_z

%% Import Line Profiles
foldername = "LineProfiles";

files = dir(fullfile(foldername+'\*.txt'));                                 % import all txt files from folder 'foldername'
files = struct2table(files);
files = sortrows(files,'date');                                             % sort data by date to ensure having the original order

Psi = cell(1,numel(files.name)-1);
I = Psi;
l = 0;
for k = 1:numel(files.name)
    if contains(files.name(k),'LineProfileSpecs')                           % Import File Containing Line Profile Extraction Specifications
        opts = delimitedTextImportOptions("NumVariables", 7);
        opts.DataLines = [2, Inf];
        opts.Delimiter = "\t";
        opts.VariableNames = ["q_pos", "q_size", "q_size_background", "Psi_pos", "Psi_size", "q_regridding", "Psi_regridding"];
        opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double"];
        opts.ExtraColumnsRule = "ignore";
        opts.EmptyLineRule = "read";
        tbl = readtable(".\"+foldername+"\"+files.name(k), opts);
        q_pos = tbl.q_pos;
        q_size = tbl.q_size;
        q_size_background = tbl.q_size_background;
        Psi_pos = tbl.Psi_pos;
        Psi_size = tbl.Psi_size;
        q_regridding = tbl.q_regridding;
        Psi_regridding = tbl.Psi_regridding;
    else
        l = l+1;                                                            % Import Files Containing Radial Line Profiles
        opts = delimitedTextImportOptions("NumVariables", 2);
        opts.DataLines = [2, Inf];
        opts.Delimiter = "\t";
        opts.VariableNames = ["Psi", "I"];
        opts.VariableTypes = ["double", "double"];
        opts.ExtraColumnsRule = "ignore";
        opts.EmptyLineRule = "read";
        tbl = readtable(".\"+foldername+"\"+files.name(k), opts);
        Psi{l} = tbl.Psi;
        I{l} = tbl.I;
        
        I{l}(Psi{l}<15) = NaN;

        Psi{l} = Psi{l}(~isnan(I{l}));
        I{l} = I{l}(~isnan(I{l}));
    end
end
clear opts tbl files k

%% Extract relevant Peaks for each Line Profile
Psi_ref_oriented = cell(numel(Psi),2);
I_ref = Psi_ref_oriented;
hkl_ref = Psi_ref_oriented;
q_lim = [q_pos-q_size+q_size_background,q_pos-q_size_background+q_size];
for k = 1:numel(Psi)
    q_lim_k = q_lim(k,:);
    
    index = NaN(numel(hkl_oriented),1);
    for l = 1:numel(hkl_oriented)                                           % find peaks of the oriented fraction which are located within the extraction limits of the radial line profile
        if q_lim_k(1) < q_ref_oriented(l) && q_ref_oriented(l) < q_lim_k(2) && Psi_ref_oriented(l) >= 0
            index(l) = 1;
        end
    end
    Psi_ref_oriented{k,1} = Psi_ref_oriented(~isnan(index));
    I_ref{k,1} = absSF2_oriented(~isnan(index));
    hkl_ref{k,1} = hkl_oriented(~isnan(index));

    index = NaN(numel(hkl_unoriented),1);
    for l = 1:numel(hkl_unoriented)                                         % find peaks of the unoriented fraction which are located within the extraction limits of the radial line profile
        if q_lim_k(1) < q_ref_unoriented(l) && q_ref_unoriented(l) < q_lim_k(2)
            index(l) = 1;
        end
    end
    I_ref{k,2} = sum(absSF2_unoriented(~isnan(index)));
    hkl_ref{k,2} = hkl_unoriented(~isnan(index));
end
clear k l index

%% Fitting Procedure
% set the parameters for the simulated annealing algorithm
k_max = 100;                                                                % number of temperature steps
L = 1000;                                                                   % number of trials per temperature step
T_0 = 4;                                                                    % start temperature

% create the temperature vector
T = linspace(T_0, T_0*0.02, k_max);                                         % Cooling strategy: T_k = T_0.*k.*q
mu = linspace(1, 0.01, k_max);                                              % width of the Gaussian -> at lower temperatures less variation is allowed

% start values
omega = 0.1;
factors = [1,1];
start = [omega,factors];

best = start;                                                               % initializing the optimization
trial = best;

% variable to store the best cost function
I_final = FitFunction(Psi,Psi_ref_oriented,I_ref,omega,factors);
cost_best = CostFunction(I, I_final, q_lim);

cost = zeros(L*k_max, 1);                                                   % vectors to store the parameters obtained at each iteration to visualize success of optimization
cost_best_history = cost;
factors_history = zeros(numel(cost),numel(start));

cost(1) = cost_best;
factors_history(1,:) = best;

change_factor = 1:numel(start);
change = zeros(L*k_max, 7);

counter = 2;

improved = zeros(k_max,1);
rejected = zeros(k_max,1);
accepted = zeros(k_max,1);

% do the actual optimization
for k = 1:k_max
    for kk = 1:L
        % PROPOSE A NEW STATE
        % choose a factor to be varied
        factor = randi(numel(change_factor), 1);
        change((k-1)*L+kk, change_factor(factor)) = normrnd(0, mu(k), 1);   % define how much it should be changed
        
        % new trial parameter set with gaussian, varying one parameter of the best result so far for l = 1:numel(best)
        switch change_factor(factor)                                        % use random number as index for change_factor
            case 1
                trial(factor) = best(factor) + change((k-1)*L+kk, change_factor(factor)).*pi./180;      % it is more useful to change omega by smaller amounts compared to the quantities of the two components
            case 2
                trial(factor) = best(factor) + change((k-1)*L+kk, change_factor(factor));
            case 3
                trial(factor) = best(factor) + change((k-1)*L+kk, change_factor(factor));
        end
        
        if any(trial < 0)                                                   % reject any new state where any of the quantities turn negative
            trial = best;
            cost(counter) = cost(counter-1);
            factors_history(counter,:) = factors_history(counter-1,:);
            cost_best_history(counter) = cost_best_history(counter-1);
            counter = counter + 1;
            rejected(k) = rejected(k) + 1;
            continue
        end
        
        % EVALUATE COST FUNCTION
        % evaluate the fit function using the new trial factors
        omega_k = trial(1);
        factors_k = trial(2:end);
        I_k = FitFunction(Psi,Psi_ref_oriented,I_ref,omega_k,factors_k);

        % calculate the cost function for the new trial factors
        CF = CostFunction(I,I_k,q_lim);
                
        delta_f = CF - cost_best;
        if exp(-delta_f/T(k)) > 1
            improved(k) = improved(k) + 1;
        end
        
        cost(counter) = CF;
        factors_history(counter,:) = trial;
        cost_best_history(counter) = min(cost(cost>0));
       
        counter = counter + 1;
        
        % ACCEPTENCE OF THE NEW STATE
        p_accept = min(1,exp(-delta_f/(T(k))));
        if rand() < p_accept
            best = trial;
            cost_best = CF;
            accepted(k) = accepted(k) + 1;
        else % new state gets rejected
            trial = best;
            rejected(k) = rejected(k) + 1;
        end
        
    end
end
clear omega_k factors_k I_k

% Best Results obtained during Simulated Annealing
index_best = find(cost == min(cost(cost>0)));
factors_best = factors_history(index_best(1),:);

omega = factors_best(1);
factors = factors_best(2:end);

% Quantification
percentage_oriented = factors(1)./sum(factors(1:2));
percentage_unoriented = factors(2)./sum(factors(1:2));
fprintf(['Quantification results:\n' ...
    ' Oriented:  %2.2f%% \n Unoriented:  %2.2f%% \n FWHM:  %2.2f° \n'],...
    percentage_oriented.*100,percentage_unoriented.*100,omega.*2*sqrt(2*log(2)).*180./pi)       % Formula for FWHM is correct if Gaussian is used, otherwise adjustment is needed

%% Plot Line Profiles with Fitted Courves
Psi_final = cell(size(Psi));
for k = 1:numel(Psi_final)
    Psi_final{k} = linspace(0,90,1000);
end
[I_final,I_ind] = FitFunction(Psi_final,Psi_ref_oriented,I_ref,omega,factors);
I_fit = FitFunction(Psi,Psi_ref_oriented,I_ref,omega,factors);

f = figure(1);
clf(f,'reset')
set(f,'Position',[1925 45 800 410])
layout = 2;
for k = 1:numel(Psi)
    p2 = plot(Psi_final{k},I_final{k},'r',LineWidth=1.5);                   % fit function
    hold on

    yline(0,'k--',LineWidth=1.5)
    line_unoriented = factors(2).*sum(I_ref{k,2});
    yline(line_unoriented,'r--',LineWidth=1.5)                              % line for unoriented  line

    a1 = patch([0,0,90,90],[0,line_unoriented,line_unoriented,0],...
        [0 0.4470 0.7410],'EdgeColor','none','FaceAlpha',.3);               % area for unoriented  line
    for l = 1:numel(I_ind)-1
        I_fit_single = I_ind{l,k}+I_ind{end,k};
        I_fit_single(1) = line_unoriented;
        a2 = patch([Psi_final{k},fliplr(Psi_final{k})],...
            [ones(size(I_fit_single)).*line_unoriented,fliplr(I_fit_single)],...
            [0.8500 0.3250 0.0980],'EdgeColor','none','FaceAlpha',.3);      % area for peaks of oriented component
    end
    
    p1 = plot(Psi{k},I{k},'ko','MarkerSize',5);                                     % measured radial line profile
    p3 = plot(Psi{k},I{k} - I_fit{k},'.','MarkerSize',10,'Color', [.5 .5 .5]);      % plot residual

    xlabel('\Psi / °')
    xlim([0,90])
    ylabel('intensity / a.u.')
    ylim([-0.2*max(I{k}),1.2*max(I{k})])
    grid on
end
lgd = legend([p1,p2,p3,a2,a1],"data","fit","residual","oriented","unoriented",Location="northwest");
grid off

set(findall(f, '-property', 'fontsize'), 'fontsize', 15)

%% Plot cost function evolution during simulated annealing
g = figure(2);
clf(g,'reset')
semilogy(1:numel(cost_best_history),cost_best_history)

xlim([0,numel(cost_best_history)])
xlabel('iteration')
ylabel('cost function')
set(findall(g, '-property', 'fontsize'), 'fontsize', 15)

%% Functions
function [f,f_k] = CostFunction(I_meas,I_calc,q_lim)                        % Calculate Cost Function based on the absolute square difference of the measured datapoints and the calculated data from the fit
    f = 0;
    f_k = zeros(1,numel(I_meas));
    for k = 1:numel(I_meas)
        q_lim_k = q_lim(k,:);
        q_mean_k = sum(q_lim_k)./2;
        f_k(k) = 1./q_mean_k.^4.*sum(abs(I_meas{k}-I_calc{k}).^2);          % 1/q^4 used to compensate for q^2 multiplication during Line Profile Extraction
        f = f+f_k(k);
    end
end

function [I,I_ind] = FitFunction(Psi,Psi_ref,I_ref,omega,factors)           % Calculation of the Fit Function
    % Peak functions
    G = @(x,x_0,omega) 1./sqrt(2.*pi.*omega.^2).*exp(-0.5.*((x-x_0)./omega).^2).*sin(x);    % Gaussian
%     L = @(x,x_0,omega) 1./(pi.*omega).*1./(1+((x-x_0)./omega).^2).*sin(x);                  % Lorentzian
%     V = @(x,x_0,omega,eta) eta.*L(x,x_0,omega)+(1-eta).*G(x,x_0,omega).*sin(x);             % Pseudo-Voigt
%     P = @(x,x_0,omega,m) 1./(sqrt(m.*omega.^2).*beta(m./2,0.5)).*(1+1./m.*((x-x_0)./omega).^2).^(-(m+1)/2).*sin(x);           % Pearson / Student t

    I = cell(1,numel(Psi));
    I_ind = cell(1,numel(Psi));
    Psi_norm = linspace(0,180,1000);
    for k = 1:numel(Psi)                                                    % loop over line profiles
        I{k} = zeros(size(Psi{k}));
        for l = 1:2                                                         % loop over oriented and unoriented contributions
            for j = 1:numel(I_ref{k,l})                                     % loop over different peaks
                if l == 1                                                   % for oriented component a Gaussian is used
                    norm = trapz(Psi_norm.*pi./180,...
                        G(Psi_norm.*pi./180,Psi_ref{k,l}(j).*pi./180,omega));               % normalization step
                    I_ind{j,k} = factors(l).*1./norm.*I_ref{k,l}(j)...
                        .*G(Psi{k}.*pi./180,Psi_ref{k,l}(j).*pi./180,omega)./sind(Psi{k});  % only required for plotting
                    I{k} = I{k} + factors(l).*1./norm.*I_ref{k,l}(j).*...
                        G(Psi{k}.*pi./180,Psi_ref{k,l}(j).*pi./180,omega)./sind(Psi{k});
                elseif l == 2                                               % for unoriented component only a constant value is used                      
                    I_ind{numel(I_ref{k,l-1})+j,k} = factors(l)...
                        .*I_ref{k,l}(j).*ones(size(Psi{k}));                % only required for plotting
                    I{k} = I{k} + factors(l).*I_ref{k,l}(j).*ones(size(Psi{k}));
                else
                    error('too many input factors')
                end
            end
        end
    end
end