close all; clear; clc;

%%
rng(42);
N = 100;
k = linspace(0,2,N)'; dk = k(2)-k(1);
P = randn(1,3);
y = 0.6*cos(2*pi*k.^(2.3) + pi*rand);

D = spdiags([ones(N,1) -ones(N,1)]/2,[1,-1],N,N);
D(1,1) = -1; D(1,2) = 1; D(end,end) = 1; D(end,end-1) = -1;
D = D./dk;

e = 0.1*randn(size(y));
ye = y + e;

%
A = spdiags([ones(N,1) 2*ones(N,N-1)],[0 -1:-1:-(N-1)],N,N);
A(:,1) = ones(N,1); A(1,:) = zeros(1,N);
A = A.*dk/2;

% Identity
alpha = 0.04;
T = alpha*speye(N);

% Differential operator
alpha = 0.002;
T = alpha*(D);

fun = @(x) norm(A*x(2:end)+x(1) - ye)^2 + norm(T*x(2:end))^2;

tic
options = optimset('MaxFunEvals',1e5,'MaxIter',1e5);
x0 = [0; randn(size(y))];
xf = fminunc(fun,x0,options);
sf = xf(1); xf = xf(2:end);
toc

%% Plot
cm = lines;

figure(1); clf;
a = axes; hold on;
plot(k,D*y+120,'k','LineWidth',1);
plot(k,D*ye+80,'-','LineWidth',1,'color',cm(1,:));
plot(k,xf+40,'-','LineWidth',1,'color',cm(2,:));
grid on; box on;
axis square;
ylim([-30 30+120]);

stem(k,2*(D*ye-D*y),'.','LineWidth',1,'Marker','none','color',cm(1,:));
stem(k,2*(xf-D*y),'.','LineWidth',1,'Marker','none','color',cm(2,:))
grid on; box on;
axis square
a.YTickLabel = ''; a.XTickLabel = '';
xlabel('x');

