close all; clear; clc;

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

phi = @(a,b) sqrt(sum( ( y-m(x,a,b) ).^2 )); % Potential function
%phi = @(a,b) sum( abs(y-m(x,a,b)) ); % Potential function
%phi = @(a,b) max( abs(y-m(x,a,b)) ); % Potential function
loss = @(a,b) log( phi(a,b) ); % Loss function

% Evaluate loss function for a grid of possible values of the parameters
ws = linspace(0,2,1e2); hs = linspace(0,10,1e2);
L = zeros(numel(ws),numel(hs)); % Preallocate
for ai = 1:numel(ws)
    for bi = 1:numel(hs)
        L(ai,bi) = loss(ws(ai),hs(bi));
    end
end

[~,I] = min(L(:)); [aI,bI] = ind2sub(size(L),I); % Find minimum
pf = [ws(aI) hs(bI)]; % Fitted parameters

%% Plot

%p1 = [2 -2];
p1 = [.5 8.5];
p2 = [1.8 3.5];
x_dense = linspace(-0.99,0.99,1e3);

figure(1); clf;
cm = lines;
subplot(1,2,1); hold on;
plot(x,y,'k.','MarkerSize',8);
plot(x_dense,m(x_dense,pf(1),pf(2)),'-','color',cm(1,:),'LineWidth',1.5);
plot(x_dense,m(x_dense,p1(1),p1(2)),'--','color',cm(2,:),'LineWidth',1);
plot(x_dense,m(x_dense,p2(1),p2(2)),'--','color',cm(3,:),'LineWidth',1);
grid on; box on; axis square;
ylim([-9 1]);
xlabel('x'); ylabel('y')

s = subplot(1,2,2); hold on;
colormap(viridis);
contour(ws,hs,L',10);
plot(pf(1),pf(2),'o','color',cm(1,:));
plot(p1(1),p1(2),'o','color',cm(2,:));
plot(p2(1),p2(2),'o','color',cm(3,:));
grid on; box on; axis tight square;
xlabel('width'); ylabel('height');
s.YDir = 'normal'; s.XDir = 'normal';