%%
addpath('../../StandardTestImages/');

im = double(imread('Cameraman256','png'));


[gx,gy] = gradient(im);
gim = sqrt(gx.^2 + gy.^2);


%% some image functionals.

TK1 = @(s) (s.^2)/2;
TV =  @(s) (abs(s));
sTV = @(s,T) (T*sqrt(s.^2 + T^2) - T^2);
PM1 = @(s,T) (T^2/2*(log(1 + (s/T).^2) ));
%Huber = @(s,T) (if (s < T) s/^2/2; else T*s -T^2/2; end  );

dTK1 = @(s) (s);
dTV =  @(s) (1);
dsTV = @(s,T) ((s*T)./sqrt(s.^2 + T^2));
dPM1 = @(s,T) ((s*T^2)./(s.^2 + T^2));

kapTK1 = @(s) (1);
kapTV =  @(s) (0/s);
kapsTV = @(s,T) ((T)./sqrt(s.^2 + T^2));
kapPM1 = @(s,T) ((T^2)./(s.^2 + T^2));
%%
% for qualtitative picture, just use an arbitray function
IT = @(s,T) log(s + T);

T = 0.1;

figure(1);clf; colormap('bone');
subplot(1,3,1); imagesc(im);colorbar;
subplot(1,3,2); imagesc(gim);colorbar;
subplot(1,3,3); imagesc(IT(gim,T));colorbar;


% As seperate images
figure(11);clf; colormap('bone');
imagesc(im);colorbar;
figure(12);clf; colormap('bone');
imagesc(gim);colorbar;
figure(13);clf; colormap('bone');
imagesc( IT(gim,T  ));colorbar;



%% simple test images and their resulting functional values
imt = zeros(256);
imt(32:131,32:131) = 1;
imt(64:113,64:113) = 4;
imt(148:197,32:131)  = 2;
imt(32:231,148:247)  = 0.5;

[gxt,gyt] = gradient(imt);
gimt = sqrt(gxt.^2 + gyt.^2);

T = 0.1; % set threshold

figure(2);clf; colormap('bone');
subplot(2,4,1); imagesc(imt);colorbar;title('test image : f');
subplot(2,4,3); imagesc(gimt);colorbar;title('gradient : |\nabla f|');
subplot(2,4,5); imagesc( TK1(gimt)  );colorbar;title('TK1');
subplot(2,4,6); imagesc( sTV(gimt,T)  );colorbar;title(['Smoothed TV, T = ',num2str(T)]);
subplot(2,4,7); imagesc( PM1(gimt,T)  );colorbar;title(['Perona-Malik, T = ',num2str(T)]);
subplot(2,4,8); imagesc( Huber(gimt,T)  );colorbar;title(['Huber, T = ',num2str(T)]);

%% now add in the derivatives and kappa
figure(3);clf; colormap('bone');
subplot(4,4,1); imagesc(imt);colorbar;title('test image : f');
subplot(4,4,3); imagesc(gimt);colorbar;title('gradient : |\nabla f|');
subplot(4,4,5); imagesc( TK1(gimt)  );colorbar;title('TK1');
subplot(4,4,6); imagesc( sTV(gimt,T)  );colorbar;title(['Smoothed TV, T = ',num2str(T)]);
subplot(4,4,7); imagesc( PM1(gimt,T)  );colorbar;title(['Perona-Malik, T = ',num2str(T)]);
[Hub,dHub,kapHub] = Huber(gimt,T) ;
subplot(4,4,8); imagesc( Hub  );colorbar;title(['Huber, T = ',num2str(T)]);
% derivatives
subplot(4,4,9); imagesc( dTK1(gimt)  );colorbar;title('\partial TK1');
subplot(4,4,10); imagesc( dsTV(gimt,T)  );colorbar;title(['\partial Smoothed TV, T = ',num2str(T)]);
subplot(4,4,11); imagesc( dPM1(gimt,T)  );colorbar;title(['\partial Perona-Malik, T = ',num2str(T)]);
subplot(4,4,12); imagesc( dHub  );colorbar;title(['\partial Huber, T = ',num2str(T)]);

% diffusivity
subplot(4,4,13); imagesc( kapTK1(gimt)  );colorbar;title('\kappa TK1');
subplot(4,4,14); imagesc( kapsTV(gimt,T)  );colorbar;title(['\kappa Smoothed TV, T = ',num2str(T)]);
subplot(4,4,15); imagesc( kapPM1(gimt,T)  );colorbar;title(['\kappa Perona-Malik, T = ',num2str(T)]);

subplot(4,4,16); imagesc( kapHub );colorbar;title(['\kappa Huber, T = ',num2str(T)]);



%% repeat for Camerman
figure(4);clf; colormap('bone');
subplot(4,4,1); imagesc(im);colorbar;title('test image : f');
subplot(4,4,3); imagesc(gim);colorbar;title('gradient : |\nabla f|');
T = 0.3*max(gim(:));
subplot(4,4,5); imagesc( TK1(gim)  );colorbar;title('TK1');
subplot(4,4,6); imagesc( sTV(gim,T)  );colorbar;title(['Smoothed TV, T = ',num2str(T)]);
subplot(4,4,7); imagesc( PM1(gim,T)  );colorbar;title(['Perona-Malik, T = ',num2str(T)]);
[Hub,dHub,kapHub] = Huber(gim,T) ;
subplot(4,4,8); imagesc( Hub  );colorbar;title(['Huber, T = ',num2str(T)]);
% derivatives
subplot(4,4,9); imagesc( dTK1(gim)  );colorbar;title('\partial TK1');
subplot(4,4,10); imagesc( dsTV(gim,T)  );colorbar;title(['\partial Smoothed TV, T = ',num2str(T)]);
subplot(4,4,11); imagesc( dPM1(gim,T)  );colorbar;title(['\partial Perona-Malik, T = ',num2str(T)]);
subplot(4,4,12); imagesc( dHub  );colorbar;title(['\partial Huber, T = ',num2str(T)]);

% diffusivity
subplot(4,4,13); imagesc( kapTK1(gim)  );colorbar;title('\kappa TK1');
subplot(4,4,14); imagesc( kapsTV(gim,T)  );colorbar;title(['\kappa Smoothed TV, T = ',num2str(T)]);
subplot(4,4,15); imagesc( kapPM1(gim,T)  );colorbar;title(['\kappa Perona-Malik, T = ',num2str(T)]);

subplot(4,4,16); imagesc( kapHub );colorbar;title(['\kappa Huber, T = ',num2str(T)]);


%%
[v,d,k] = Huber([0:0.1:5],0.5)
figure; plot(k);
%% ----------------------------------------------------------------------
function [val,dval,kval] = Huber(s,T)
val = zeros(size(s));
dval = val; kval = val;
for j = 1:size(s(:))
    if (s(j) < T)
        val(j) = s(j)^2/2;
        dval(j) = s(j);
        kval(j) = 1;
    else
        val(j) = T.*s(j) - T^2/2;
        dval(j) = T;
        kval(j) = T./s(j);
    end
end
end