close all; clear; clc;

%% Make data and fit
rng(42);
x = linspace(-1,1,30+1); dx = mean(diff(x));
w = 1; h = 5; % Ideal parameters
m = @(x,w,h) ((1./(1+(x/w*2).^2))-1) .* h; % Generalised Lorentzian
y = m(x,w,h) + 0.5*randnd(-1,size(x)); % Data (model + noise)

lossfun = @(p) log( sqrt(sum( ( y-m(x,p(1),p(2)) ).^2 )) ); % Loss function

% Fit and pull out Hessian from fminunc
[pf,~,~,~,~,H] = fminunc(lossfun,[1 -1]);
fit = m(x,pf(1),pf(2));

figure(1); clf; hold on;
plot([-1 1],[0 0],'k--',0,0,'k+')
plot(x,y,'o',x,fit);
grid on; box on; axis square;
xlabel('x'); ylabel('y');
title('Fitted model')

%% Hessian error estimate
dp = sqrt(diag( var(y-fit)*inv(H) ))';

%% Bootstrap error estimate
fprintf('Bootstrapping fit ...  \n');
PLT = false;
x2 = linspace(-3,3,5e2); % Denser x-axis

clear ts
options = optimset('Display','none');
fit = m(x,pf(1),pf(2));
r = y - fit;

Nb = 1e3; % Number of Bootstraps
ps = NaN(Nb,2); boot_grad = NaN(Nb,numel(x2)); % Preallocate
for j = 1:Nb
    nr = datasample(r,numel(r),'Replace',true); % Resample residuals
    nr = nr.*sign((rand(size(r))>0.5)-0.5); % Randomise sign
    ny = fit + nr; % Fit + bootstrapped residuals
    lossfun = @(p) log( sqrt(sum( ( ny-m(x,p(1),p(2)) ).^2 )) );
    ps(j,:) = fminunc(lossfun,pf,options);
    
    if PLT
        figure(2); clf;
        plot([-1 1]*2,[0 0],'k--'); hold on;
        plot(x,ny,'.',x,fit,'-');
        plot(x2,m(x2,ps(j,1),ps(j,2))');
        ylim([-6 1]); xlim([-1 1]*2);
        grid on; box on; axis square;
        xlabel('x'); ylabel('y');
        title(sprintf('Bootstrap fit | ps = (%.2f,%.2f)',ps(j,:)));
        drawnow;
    end
    
    boot_grad(j,:) = gradient(m(x2,ps(j,1),ps(j,2)))./dx;
    if ~mod(j,round(Nb/20)), fprintf('%.2f%% done\n',j/Nb*100); end
end
dp2 = std(ps);

%% Print statistics
fprintf('Hessian error estimate\n');
fprintf('pf = %.3f %.3f\n',pf);
fprintf(' +/- %.3f %.3f\n',dp);
fprintf('\n');

fprintf('Bootstrap error estimate\n');
fprintf('pf = %.3f %.3f\n',pf);
fprintf(' +/- %.3f %.3f\n',dp2);
fprintf('\n');

%% Fit p2 distribution
gaussian = @(x,c,w,h) exp( -4.*log(2).*((x-c)./w).^2 ) .* h;
asymmetric_gaussian = @(x,c,wl,wr,h) gaussian(x,c,wl,h).*(x<c) + gaussian(x,c,wl,h).*(x>=c);

d = ps(:,2);
[yh,xh] = histcounts(d,30,'Normalization','probability');
xh = xh(1:end-1) + (xh(2)-xh(1))/2;

w = ones(size(xh)); w(12:end) = 0;
lossfun2 = @(p) mean( (yh - gaussian(xh,p(1),p(2),p(3))).^2.*w );
p0 = [xh(yh==max(yh)),mean(xh)/3,max(yh)];
pf2 = fminsearch(lossfun2,p0);

lossfun3 = @(p) mean( (yh - asymmetric_gaussian(xh,p(1),p(2),p(3),p(4))).^2 );
p0 = [xh(yh==max(yh)),mean(xh)/3,mean(xh)/3,max(yh)];
pf3 = fminsearch(lossfun3,p0);

%% Plot
cm = lines;
figure(2); clf;
s = subplot(1,2,1); cla; hold on;
plot(xh,yh,'k.');
histogram(d,30,'edgecolor','none','Normalization','probability','FaceColor',cm(3,:));
plot(xh,gaussian(xh,pf2(1),pf2(2),pf2(3)),':','LineWidth',1,'Color',cm(1,:));
plot(xh,asymmetric_gaussian(xh,pf3(1),pf3(2),pf3(3),pf3(4)),'LineWidth',1,'Color',cm(2,:));
grid on; box on;
axis square;
s.YTickLabel = '';
xlabel('p_2'); ylabel('');

% Plot bootstrapped gradient
for j = 1:size(boot_grad,2)
    bg = boot_grad(:,j);
    bg = bg-median(bg); % Shift to zero median
    pos = bg>=0; % Upper half
    
    % Upper and lower uncertainty estimate
    t = bg; t(~pos) = [];
    u(j) = sqrt(sum(t.^2)./(numel(t)-1));
    t = bg; t(pos) = [];
    l(j) = sqrt(sum(t.^2)./(numel(t)-1));
end

cm = lines;
s = subplot(1,2,2); cla; hold on
plot(x,0.1*y,'k.');
plot(x2,0.1*m(x2,pf(1),pf(2)),'-','color',cm(1,:),'Linewidth',1);
plot(x2,2*ones(size(x2)),'k--');
fit_grad = gradient(m(x2,pf(1),pf(2)))./dx;
for j = 1:3
    patch([x2 fliplr(x2)],2+[fit_grad fliplr(fit_grad)]+j*[u -l],cm(2,:),...
        'EdgeColor','none','FaceAlpha',0.2);
end
plot(x2,2+fit_grad,'k-','Linewidth',1)
xlim([-1 1]);
axis square; grid on; box on;
s.YTickLabel = '';
xlabel('x'); ylabel('');
