function [ fplanes,cplanes,info ] = tfjigsawsep( f, varargin )
%TFJIGSAWSEP Time frequency jigsaw puzzle tonal-transient separation
%   Usage:  fplanes = tfjigsawsep(f);
%           fplanes = tfjigsawsep(f,r1,r2);
%           fplanes = tfjigsawsep(f,r1,r2,p);
%           [fplanes, cplanes, info] = tfjigsawsep(...);
%
%   Input parameters:
%            f        : Input signal
%            r1       : Significance level of the tonal layer refered to
%                       a white noise reference
%            r2       : Same for the transient layer
%            p        : Proportionfactor of the supertilesizes relative
%                       to the time-, and frequency stepsize
%
%   Output parameters:
%           fplanes   : signallength-by-3 array containing the 3 produced
%                       layers, tonal in fplanes(:,1), transient in
%                       fplanes(:,2) and the noisy residual in fplanes(:,3).
%           cplanes   : 3-by-1 cellarray containing the Gabor coefficients
%                       for the individual layers
%
%   `tfjigsawsep(f)` applies the separation algorithm on the input signal *f*
%   and returns the tonal, the transient and the residual parts.
%   The following default values are used, *r1=r2=0.95*, *p=4* and for the
%   3 Gabor systems:
%
%       "Tonal" system:     g1 = {'hann',4096}; a1 = 512; M1 = 4096;
%
%       "Transient" system: g2 = {'hann',256};  a2 = 32;  M2 = 256;
%
%       "Residual" system:  g3 = {'hann',2048}; a3 = 512; M3 = 2048;
%
%   `tfjigsawsep(f,r1,r2)` works as before, but allows changing threshold
%   parameters *r1* and *r2*. Good values are in range [0.85,0.95].
%   *t2* sometimes has to be chosen larger (~ 1.05), eg. for
%   percussions in music signals.
%
%   `tfjigsawsep(f,r1,r2,p)` (recommended) additionally allows changing the
%   size of the supertiles to a1*p in timesamples and b2*p in
%   frequencysamples. The choice of this particular proportion is
%   reasonable since it provides equal numbers of coefficients of the two
%   Gabor systems in each supertile. Good values are in the range of [1,10],
%   but it depends very much on the type of signal.
%   E.g. for speech signals, higher values yield better results.
%
%   `[fplanes, cplanes, info] = tfjigsawsep(...)` additionally returns
%   a 3 element cell array *cplanes* containing |dgtreal| coefficients
%   of the respective separated layers and a structure *info*, with the
%   parameters used in the algorithm. The relationship between *fplanes*
%   and *cplanes* is the following:

%   Additional parameters:
%   ----------------------
%
%   The function accepts the following flags:
%
%       'ver2' Uses the second version of the algorithm.
%
%       'plot' Plots the separated waveforms and the coefficients.
%
%       'verbose' Information about the boundary condition.
%
%
%   and the following key-value pairs:
%
%       'wintype' Requested windowtype
%
%       'winsize1' and 'winsize2' Windowlengths
%
%       'a1' and 'a2' Time stepsizes
%
%       'M1' and 'M2' Number of frequency channels
%
%       'T' and 'F' Supertile sizes in samples manually - alternative to p!
%
%       'maxit' Maximum number of iterations. The default value is 15.
%
%   Algorithm:
%   ----------
%
%   The algorithm is based on [1]. It transforms a signal into a two-windowed
%   Gabor expansion such that one wide window shall lead to a high frequency
%   resolution (tonal layer is represented well) and a narrow one to a high
%   time resolution (transient layer is repr. well). The resulting Gabor
%   coefficients are respectively considered within rectangular 'supertiles'
%   in the time-frequency plane. An entropy criterion chooses those tiles
%   respectively, where tonal and transient parts of the signal are
%   represented better and are below a estimated threshold. The rest is set
%   to zero. The leftover Gabor coefficients are transformed back and
%   subtracted from the original signal. By applying this procedure
%   iteratively on the residual, tonal and transient layers emerge.
%
%   Examples:
%   ---------
%
%   The following example shows the basic usage of the function:::
%
%     % Load the glockenspiel test signal and add some noise
%     [f,fs] = gspi; f = f + 0.001*randn(size(f));
%     % Setup the parameters
%     p = 2; r1 = 0.92; r2 = 0.93;
%     [fplanes,cplanes,info] = tfjigsawsep(f,r1,r2,p,'plot','ver2','fs',fs);
%
%   See also: dgtreal plottfjigsawsep
%
%   References: jato07

%AUTHOR: Daniel Haider, 2017

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Remarks %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   The residual condition is computed very heuristically, designing it   %
%   more flexible would be a nice improvement of the algorithm.           %
%   It would also be useful to provide parameter settings for specific    %
%   types of signals (ie. speech, music,...) .                            %
%                                                                         %
%   Version 1 of the algorithm (default) works particularly well for      %
%   speech signals, but also for depicting the transient layer            %
%   (eg. percussive elements) nicely in musical signals.                  %
%   Version 2 works particularly well for depicting a nice tonal layer.   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[f,Ls,W,wasrow,remembershape]=comp_sigreshape_pre(f,upper(mfilename),0);

if W>1
    error('%s: Multichannel inputs are not supported.',upper(mfilename));
end

definput.keyvals.r1 = [];
definput.keyvals.r2 = [];
definput.keyvals.p = [];
definput.keyvals.T = [];
definput.keyvals.F = [];
definput.keyvals.wintype = 'hann';
definput.keyvals.winsize1 = 4096;
definput.keyvals.a1 = 512;
definput.keyvals.M1 = 4096;
definput.keyvals.winsize2 = 512;
definput.keyvals.a2 = 64;
definput.keyvals.M2 = 512;
definput.keyvals.maxit = 15;
definput.keyvals.fs = [];
definput.flags.algver = {'ver1','ver2'};
definput.flags.plot = {'noplot','plot'};
definput.flags.verbose = {'noverbose','verbose'};
[flags,kv] = ltfatarghelper({'r1','r2','p','T','F','wintype','winsize1','a1','M1','winsize2','a2','M2'},definput,varargin);

% significance level
r1 = kv.r1;
r2 = kv.r2;
% Gabor system settings
wintype = kv.wintype;
winsize1 = kv.winsize1;
a1 = kv.a1;
M1 = kv.M1;
winsize2 = kv.winsize2;
a2 = kv.a2;
M2 = kv.M2;
% supertile sizes
if isempty(kv.T) && isempty(kv.F)
    if isempty(kv.p)
        p = 4;
    else
        p = kv.p;
    end
    T = a1*p;
    F = dgtlength(Ls,a2,M2)/M2*p;
elseif isempty(kv.p)
    T = kv.T;
    F = kv.F;
else
    error('%s: Use EITHER the proportional setting OR set T and F manually.',upper(mfilename));
end

if winsize1 < winsize2
    error('%s: The tonal system uses a shorter window than the transient system!',upper(mfilename));
end

% why this and not setting the kv on top?
if xor(isempty(r1),isempty(r2))
    error('%s: Both r1 and r2 must be defined.',upper(mfilename));
else
    if isempty(r1), r1 = 0.95; end
    if isempty(r2), r2 = 0.95; end
end

% windows
g1 = {wintype,winsize1};
g2 = {wintype,winsize2};

L1 = dgtlength(Ls,a1,M1);
L2 = dgtlength(Ls,a2,M2);
b1 = L1/M1;
b2 = L2/M2;

complainif_notposint(T,'T',mfilename);
if ~( T < min([L1,L2]) )
    error('%s: Supertile length must be smaller than the signal length [%i]',...
          upper(mfilename),min([L1,L2]));
elseif ~( F < min([L1/2+1,L2/2+1]) )
    error('%s: Supertile height must be smaller than the frequency range [%i]',...
      upper(mfilename),min([L1/2+1,L2/2+1]));
elseif ~( T > max([a1,a1]) )
    error('%s: Supertile length must be larger than the time stepsizes [%i]',...
          upper(mfilename),max([a1,a1]));
elseif ~( F > max([b1,b2]) )
    error('%s: Supertile height must be larger than the frequency stepsizes [%i]',...
          upper(mfilename),max([b1,b2]));
end

% entropy reference from noise signal
% thresholds tau1,tau2 for tonal resp. transient layer are chosen to have
% a certain significance with respect to the estimated reference
[ref1,ref2] = noisest(Ls,g1,g2,a1,a2,M1,M2,T,F);
tau1 = ref1*r1;
tau2 = ref2*r2;

% initialization of residual, layers, min and max number of iterations
% and epsilon, the upper limit for the residual condition, which is
% computed at the kmin-th iteration
% R = postpad(f,L);
R = f;
l1 = zeros(Ls,1);
l2 = zeros(Ls,1);
k = 1;
kmin = 5;
kmax = kv.maxit;
epsilon = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%% main loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% runs until all values in R are below epsilon to exclude single peaks
while all(abs(R) < epsilon) ~= 1

    switch flags.algver
       case 'ver1'
        [x1,x2]=jigsaw1(R,g1,g2,a1,a2,M1,M2,T,F,tau1,tau2);
       case 'ver2'
        [x1,x2]=jigsaw2(R,g1,g2,a1,a2,M1,M2,T,F,tau1,tau2);
    end

    % residual parts of the signal
    R = R-x1-x2;
    l1 = l1+x1;
    l2 = l2+x2;

    if k == kmax
        break
    end

    if k == kmin
        % epsilon is computed as upper limit, corresponding to
        % an empirical p-quantile
        epsilon = sort(abs(R),'ascend');
        epsilon = 5/2*epsilon(round(0.998*numel(epsilon)));
        if flags.do_verbose
            disp(['The current maximum in the residual is ',num2str(max(abs(R)))])
            disp(['An upper bound is estimated to be ',num2str(epsilon)])
        end
    end

    k = k+1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if flags.do_verbose
    disp('max. number of iteration reached: condition for residual is not fulfilled!')
    disp('Try to increase t2 slightly or change the size of the supertiles.')
end

% fplanes as Ls-by-3 array containing the layers
fplanes = [l1,l2,R];
fplanes = postpad(fplanes,Ls);

% info structure
info.g1 = {wintype, winsize1};
info.g2 = {wintype, winsize2};
info.g3 = {wintype, 2048};
info.M1 = M1;
info.M2 = M2;
info.M3 = 2048;
info.a1 = a1;
info.a2 = a2;
info.a3 = 512;
info.supertilesizes = [F,T];
info.noiseentropy_tonal = ref1;
info.threshold_tonal = tau1;
info.noiseentropy_transient = ref2;
info.threshold_transient = tau2;

% cplanes as 3-by-1 cell array containing
% the gabor coefficients corresp. to the layers
if nargout > 1 || flags.do_plot
    cplanes = cell(3,1);
    cplanes{1} = dgtreal(l1,g1,a1,M1);
    cplanes{2} = dgtreal(l2,g2,a2,M2);
    cplanes{3} = dgtreal(R,info.g3,info.a3,info.M3);
end

% option for plots
if flags.do_plot
    plottfjigsawsep(fplanes,cplanes,info,'fs',kv.fs,'showbuttons','equalyrange');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%% compiling functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [ r ] = renyi( t,alpha )

% computes the Renyi entropy for an array
% yields high values for peaky data and
% low values for almost constant

if isempty(t)
    r = inf;
elseif norm(t) == 0
    r = inf;
else
    switch nargin
        case 1
            alpha = 2.4;
            r = (1/(1-alpha))*log2(sum(sum(abs(t).^(2*alpha)))*(sum(sum(abs(t).^2)).^(-alpha)));
        case 2
            if alpha <= 0 || alpha == 1
                error('alpha must be chosen positive and unequal to 1.');
            else
                r = (1/(1-alpha))*log2(sum(sum(abs(t).^(2*alpha))).*norm(t(:),2)^(-2*alpha));
            end
        otherwise
            error('This function takes at least one input argument.');
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [ref1,ref2] = noisest (Ls,g1,g2,a1,a2,M1,M2,T,F)

% computes the entropy for a white noise signal within one supertile
% as estimation reference for the tresholds tau1,tau2

n = noise(Ls,'white');
n1 = dgtreal(n,g1,a1,M1);
n2 = dgtreal(n,g2,a2,M2);
r1 = n1(1:floor(F*M1/Ls),1:floor(T/a1));
r2 = n2(1:floor(F*M2/Ls),1:floor(T/a2));
ref1 = renyi(r1);
ref2 = renyi(r2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% Version 1 of the jigsaw puzzle algorithm %%%%%%%%%%%%%%%%%%

function [x1,x2] = jigsaw1(R,g1,g2,a1,a2,M1,M2,T,F,tau1,tau2)

% error check?

% signal lengths
Ls = length(R);
L1 = dgtlength(Ls,a1,M1);
L2 = dgtlength(Ls,a2,M2);

% gabor transformations
c1 = dgtreal(R,g1,a1,M1); % M1/2+1-by-L/a1
c2 = dgtreal(R,g2,a2,M2); % M2/2+1-by-L/a2

% frequency hop sizes
b1 = L1/M1;
b2 = L2/M2;

% indices on the TF plane (L/2+1 x L), where the coefficients belong to
aa1 = 1:a1:L1;
aa2 = 1:a2:L2;
bb1 = 1:b1:L1/2+1;
bb2 = 1:b2:L2/2+1;

L = max([L1,L2]);

% find the indices for the coefficients in every single TF-supertile
for m=0:floor((L/2+1)/F)-1
    for n=0:floor(L/T)-1
        f1 = find(bb1<=(m+1)*F & bb1>m*F);
        f2 = find(bb2<=(m+1)*F & bb2>m*F);
        t1 = find(aa1<=(n+1)*T & aa1>n*T);
        t2 = find(aa2<=(n+1)*T & aa2>n*T);
        % look for parts of c1,c2 where tonal/transient parts are repr well
        [c1,c2] = decision1(c1,c2,f1,f2,t1,t2,tau1,tau2);
    end
end

% last column of remaining supertiles
for m=0:floor((L/2+1)/F-1)
    f1 = find(bb1<=(m+1)*F & bb1>m*F);
    f2 = find(bb2<=(m+1)*F & bb2>m*F);
    t1 = find(aa1<=L & aa1>floor(L/T)*T);
    t2 = find(aa2<=L & aa2>floor(L/T)*T);

    [c1,c2] = decision1(c1,c2,f1,f2,t1,t2,tau1,tau2);
end

% upper row of remaining supertiles
for n=0:floor(L/T-1)
    f1 = find(bb1<=L/2+1 & bb1>floor((L/2+1)/F)*F);
    f2 = find(bb2<=L/2+1 & bb2>floor((L/2+1)/F)*F);
    t1 = find(aa1<=(n+1)*T & aa1>n*T);
    t2 = find(aa2<=(n+1)*T & aa2>n*T);

    [c1,c2] = decision1(c1,c2,f1,f2,t1,t2,tau1,tau2);
end

% right upper remaining supertiles
f1 = find(bb1<=L/2+1 & bb1>floor((L/2+1)/F)*F);
f2 = find(bb2<=L/2+1 & bb2>floor((L/2+1)/F)*F);
t1 = find(aa1<=L & aa1>floor(L/T)*T);
t2 = find(aa2<=L & aa2>floor(L/T)*T);

[c1,c2] = decision1(c1,c2,f1,f2,t1,t2,tau1,tau2);

% synthesis with the canonical dual windows
x1 = idgtreal(c1,{'dual',g1},a1,M1,Ls);
x2 = idgtreal(c2,{'dual',g2},a2,M2,Ls);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [c1,c2] = decision1(c1,c2,f1,f2,t1,t2,tau1,tau2)

% decision procedure for version 1
%   case 1: both entropies are above their thresholds -> set them to zero
%   case 2: g1 has a better repr. and the entropy is below its
%           threshold -> set the tile corr. to g2 to zero
%   case 3: vice versa
%   case 4: g1 has a better repr. and its entropy is above its
%           threshold tau1 but despite of that, the entropy corr. to
%           g2 is below its threshold tau2 -> set the tile corr. to
%           g1 to zero
%   case 5: vice versa

E1 = renyi(c1(f1,t1));
E2 = renyi(c2(f2,t2));

if E1 > tau1 && E2 > tau2
    c1(f1,t1) = 0;
    c2(f2,t2) = 0;
else
    if (min([E1,E2]) == E1 && E1 < tau1) || (min([E1,E2]) == E2 && E2 > tau2 && E1 < tau1)
        c2(f2,t2) = 0;
    elseif (min([E1,E2]) == E2 && E2 < tau2) || (min([E1,E2]) == E1 && E1 > tau1 && E2 < tau2)
        c1(f1,t1) = 0;
    else
        warning('something went wrong with the decision criteria!');
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% Version 2 of the jigsaw puzzle algorithm %%%%%%%%%%%%%%%%%%

function [x1,x2] = jigsaw2(R,g1,g2,a1,a2,M1,M2,T,F,tau1,tau2)

% signal lengths
Ls = length(R);
L1 = dgtlength(Ls,a1,M1);
L2 = dgtlength(Ls,a2,M2);

% frequency hop sizes
b1 = L1/M1;
b2 = L2/M2;

% gabor transformation
c1 = dgtreal(R,g1,a1,M1); % M1/2+1-by-L/a1
c2 = dgtreal(R,g2,a2,M2); % M2/2+1-by-L/a2

% indices on the TF plane (L/2+1 x L), where the coefficients belong to
aa1 = 1:a1:L1;
aa2 = 1:a2:L2;
bb1 = 1:b1:L1/2+1;
bb2 = 1:b2:L2/2+1;

L = max([L1,L2]);

for m=0:floor((L/2+1)/F)-1
    for n=0:floor(L/T)-1
        % find the indices for the coefficients in each TF-supertile
        f1 = find(bb1<=(m+1)*F & bb1>m*F);
        f2 = find(bb2<=(m+1)*F & bb2>m*F);
        t1 = find(aa1<=(n+1)*T & aa1>n*T);
        t2 = find(aa2<=(n+1)*T & aa2>n*T);
        % keep the tonals
        [c1,c2] = decision2ton(c1,c2,f1,f2,t1,t2,tau1);
    end
end

% last column of remaining supertiles
for m=0:floor((L/2+1)/F-1)
    f1 = find(bb1<=(m+1)*F & bb1>m*F);
    f2 = find(bb2<=(m+1)*F & bb2>m*F);
    t1 = find(aa1<=L & aa1>floor(L/T)*T);
    t2 = find(aa2<=L & aa2>floor(L/T)*T);
    % keep the tonals
    [c1,c2] = decision2ton(c1,c2,f1,f2,t1,t2,tau1);
end

% upper row of remaining supertiles
for n=0:floor(L/T-1)
    f1 = find(bb1<=L/2+1 & bb1>floor((L/2+1)/F)*F);
    f2 = find(bb2<=L/2+1 & bb2>floor((L/2+1)/F)*F);
    t1 = find(aa1<=(n+1)*T & aa1>n*T);
    t2 = find(aa2<=(n+1)*T & aa2>n*T);
    % keep the tonals
    [c1,c2] = decision2ton(c1,c2,f1,f2,t1,t2,tau1);
end

% right upper remaining supertile
f1 = find(bb1<=L/2+1 & bb1>floor((L/2+1)/F)*F);
f2 = find(bb2<=L/2+1 & bb2>floor((L/2+1)/F)*F);
t1 = find(aa1<=L & aa1>floor(L/T)*T);
t2 = find(aa2<=L & aa2>floor(L/T)*T);
% keep the tonals
[c1,~] = decision2ton(c1,c2,f1,f2,t1,t2,tau1);

% synthesis of the tonal parts with the canonical dual window of g1
x1 = idgtreal(c1,{'dual',g1},a1,M1,Ls);
RR = R-x1;

% the same procedure is now applied with respect to the narrow window g2

% gabor transformation
c1 = dgtreal(RR,g1,a1,M1); % M1/2+1-by-L/a1
c2 = dgtreal(RR,g2,a2,M2); % M2/2+1-by-L/a2

for m=0:floor((L/2+1)/F)-1
    for n=0:floor(L/T)-1
        % find the indices for the coefficients in each TF-supertile
        f1 = find(bb1<=(m+1)*F & bb1>m*F);
        f2 = find(bb2<=(m+1)*F & bb2>m*F);
        t1 = find(aa1<=(n+1)*T & aa1>n*T);
        t2 = find(aa2<=(n+1)*T & aa2>n*T);
        % keep the transients
        [~,c2] = decision2trans(c1,c2,f1,f2,t1,t2,tau2);
    end
end

% last column of remaining supertiles
for m=0:floor((L/2+1)/F-1)
    f1 = find(bb1<=(m+1)*F & bb1>m*F);
    f2 = find(bb2<=(m+1)*F & bb2>m*F);
    t1 = find(aa1<=L & aa1>floor(L/T)*T);
    t2 = find(aa2<=L & aa2>floor(L/T)*T);
    % keep the transients
    [~,c2] = decision2trans(c1,c2,f1,f2,t1,t2,tau2);
end

% upper row of remaining supertiles
for n=0:floor(L/T-1)
    f1 = find(bb1<=L/2+1 & bb1>floor((L/2+1)/F)*F);
    f2 = find(bb2<=L/2+1 & bb2>floor((L/2+1)/F)*F);
    t1 = find(aa1<=(n+1)*T & aa1>n*T);
    t2 = find(aa2<=(n+1)*T & aa2>n*T);
    % keep the transients
    [~,c2] = decision2trans(c1,c2,f1,f2,t1,t2,tau2);
end

% right upper remaining supertile
f1 = find(bb1<=L/2+1 & bb1>floor((L/2+1)/F)*F);
f2 = find(bb2<=L/2+1 & bb2>floor((L/2+1)/F)*F);
t1 = find(aa1<=L & aa1>floor(L/T)*T);
t2 = find(aa2<=L & aa2>floor(L/T)*T);
% keep the transients
[~,c2] = decision2trans(c1,c2,f1,f2,t1,t2,tau2);

% synthesis of the transient parts with the canonical dual window of g2
x2 = idgtreal(c2,{'dual',g2},a2,M2,Ls);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [c1,c2] = decision2ton(c1,c2,f1,f2,t1,t2,tau1)
% decision procedure for tonals

E1 = renyi(c1(f1,t1));
E2 = renyi(c2(f2,t2));

if min([E1,E2]) == E2 || E1 > tau1
    c1(f1,t1) = 0;
end

function [c1,c2] = decision2trans(c1,c2,f1,f2,t1,t2,tau2)
% decision procedure for transients

E1 = renyi(c1(f1,t1));
E2 = renyi(c2(f2,t2));

if min([E1,E2]) == E1 || E2 > tau2
    c2(f2,t2) = 0;
end


function obj=stat_ellipse(obj,varargin)
%stat_ellipse() Create confidence ellipses around 2D groups of
% points
%
% Parameters:
% 'type': The default '95percentile' displays an ellipse that
% contains 95% of the points (assuming a bivariate normal
% distribution). The option 'ci' will first compute boostrapped
% 2D means and plot the 95% ellipse around these means
% 'geom': Sets how to display the result 'area' for a shaded
% area or 'line' for a simple contour line.
% 'patch_opts': Provide additional patch properties as name-value
% pairs in a cell array (as if those were options for Matlab's
% built in patch() function)

p=inputParser;
my_addParameter(p,'type','95percentile'); %ci
my_addParameter(p,'geom','area'); %line
my_addParameter(p,'patch_opts',{});
parse(p,varargin{:});

obj.geom=vertcat(obj.geom,{@(dobj,dd)my_ellipse(dobj,dd,p.Results)});
obj.results.stat_ellipse={};
end

function hndl=my_ellipse(obj,draw_data,params)

persistent elpoints;
persistent sphpoints;

%Cache unity ellipse points
if isempty(elpoints)
    res=30;
    ang=0:pi/(0.5*res):2*pi;
    elpoints=[cos(ang); sin(ang)];
    [x,y,z]=sphere(10);
    sphpoints=surf2patch(x,y,z);
end

combx=shiftdim(comb(draw_data.x));
comby=shiftdim(comb(draw_data.y));
combz=shiftdim(comb(draw_data.z));

%If we have "enough" points
if sum(~isnan(combx))>2 && sum(~isnan(comby))>2

    if isempty(draw_data.z)

        r=[combx comby];
        %Using a chi square with 2 degrees of freedom is proper
        %here (tested: generated ellipse do contain 1-alpha of the
        %points)
        k=@(alpha) sqrt(chi2inv(1-alpha,2));

        %If a CI on the mean is requested, we replace the original points
        %with bootstrapped mean samples
        if strcmp(params.type,'ci')
            r=bootstrp(obj.stat_options.nboot,@nanmean,r);
        end

        %Extract mean and covariance
        m=nanmean(r);
        cv=nancov(r);

        %Compute ellipse points
        conf_elpoints=sqrtm(cv)*elpoints*k(obj.stat_options.alpha);

        %Compute ellipse axes
        [evec,eval]=eig(cv);
        if eval(2,2)>eval(1,1) %Reorder
            evec=fliplr(evec);
            eval=fliplr(flipud(eval));
        end
        elaxes=sqrtm(cv)*evec*k(obj.stat_options.alpha);

        obj.results.stat_ellipse{obj.result_ind,1}.mean=m;
        obj.results.stat_ellipse{obj.result_ind,1}.cv=cv;
        obj.results.stat_ellipse{obj.result_ind,1}.major_axis=elaxes(:,1)';
        obj.results.stat_ellipse{obj.result_ind,1}.minor_axis=elaxes(:,2)';

        %plot([0 elaxes(1,1)]+m(1),[0 elaxes(2,1)]+m(2),'k')
        %plot([0 elaxes(1,2)]+m(1),[0 elaxes(2,2)]+m(2),'k')

        switch params.geom
            case 'area'
                hndl=patch(conf_elpoints(1,:)+m(1),conf_elpoints(2,:)+m(2),draw_data.color,'FaceColor',draw_data.color,'EdgeColor',draw_data.color,'LineWidth',2,'FaceAlpha',0.2);

            case 'line'
                hndl=patch(conf_elpoints(1,:)+m(1),conf_elpoints(2,:)+m(2),draw_data.color,'FaceColor','none','EdgeColor',draw_data.color,'LineWidth',2);
        end
        set(hndl,params.patch_opts{:});
        %One matlab version displayed stuff if no output value was set (but
        %crashes 2014a and earlier versions)
        %tmp = set(hndl,params.patch_opts{:});

        center_hndl=plot(m(1),m(2),'+','MarkerFaceColor',draw_data.color,'MarkerEdgeColor',draw_data.color,'MarkerSize',10);
    else

        r=[combx comby combz];
        k=@(alpha) sqrt(chi2inv(1-alpha,3));

        %If a CI on the mean is requested, we replace the original points
        %with bootstrapped mean samples
        if strcmp(params.type,'ci')
            r=bootstrp(obj.stat_options.nboot,@nanmean,r);
        end

        %Extract mean and covariance
        m=nanmean(r);
        cv=nancov(r);

        obj.results.stat_ellipse{obj.result_ind,1}.mean=m;
        obj.results.stat_ellipse{obj.result_ind,1}.cv=cv;
        obj.results.stat_ellipse{obj.result_ind,1}.major_axis=[];
        obj.results.stat_ellipse{obj.result_ind,1}.minor_axis=[];

        conf_sphpoints=sphpoints;
        conf_sphpoints.vertices=bsxfun(@plus,sqrtm(cv)*conf_sphpoints.vertices'*k(obj.stat_options.alpha),m')';
        hndl=patch(conf_sphpoints,'FaceColor',draw_data.color,'EdgeColor','none','LineWidth',2,'FaceAlpha',0.2);

        center_hndl=plot3(m(1),m(2),m(3),'+','MarkerFaceColor',draw_data.color,'MarkerEdgeColor',draw_data.color,'MarkerSize',10);
    end
    obj.results.stat_ellipse{obj.result_ind,1}.ellipse_handle=hndl;
    obj.results.stat_ellipse{obj.result_ind,1}.center_handle=center_hndl;
else
    warning('Not enough points for ellipse')

    obj.results.stat_ellipse{obj.result_ind,1}.mean=NaN;
    obj.results.stat_ellipse{obj.result_ind,1}.cv=NaN;
    obj.results.stat_ellipse{obj.result_ind,1}.major_axis=[];
    obj.results.stat_ellipse{obj.result_ind,1}.minor_axis=[];
    obj.results.stat_ellipse{obj.result_ind,1}.ellipse_handle=[];
    obj.results.stat_ellipse{obj.result_ind,1}.center_handle=[];
end
end


function [exp_apx]=exp_approx3(dboun, exp_par,show_res)

%convert i
nind=find(exp_par(:,2)<=0);
exp_par(nind,2)=-1*exp_par(nind,2);

dind1=dboun(1,1):dboun(1,2);
nd1=dboun(1,2)-dboun(1,1)+1;
dind2=dboun(2,1):dboun(2,2);
nd2=dboun(2,2)-dboun(2,1)+1;
dind3=dboun(3,1):dboun(3,2);
nd3=dboun(3,2)-dboun(3,1)+1;
dind4=dboun(4,1):dboun(4,2);
nd4=dboun(4,2)-dboun(4,1)+1;
nd=nd1+nd2+nd3+nd4;

H=zeros(nd,5);
Y=zeros(nd,1);

H(1:nd1,1)=dind1;
H(1:nd1,3)=1;
H((nd1+1):(nd1+nd2),2)=dind2;
H((nd1+1):(nd1+nd2),4)=1;
H((nd1+nd2+1):(nd1+nd2+nd3),1)=dind3;
H((nd1+nd2+1):(nd1+nd2+nd3),5)=1;
H((nd1+nd2+nd3+1):nd,2)=dind4;
H((nd1+nd2+nd3+1):nd,5)=1;

Rinv1=1./(linspace(dboun(1,1),dboun(1,2),length(dind1)));
Rinv2=1./(linspace(dboun(2,1),dboun(2,2),length(dind2)));
Rinv3=1./(linspace(dboun(3,1),dboun(3,2)^2,length(dind3)));
Rinv4=1./(linspace(dboun(4,1),dboun(4,2)^2,length(dind4)));
HR(1:nd1,1)=dind1.*Rinv1;
HR(1:nd1,3)=Rinv1;
HR((nd1+1):(nd1+nd2),2)=dind2.*Rinv2;
HR((nd1+1):(nd1+nd2),4)=Rinv2;
HR((nd1+nd2+1):(nd1+nd2+nd3),1)=dind3.*Rinv3;
HR((nd1+nd2+1):(nd1+nd2+nd3),5)=Rinv3;
HR((nd1+nd2+nd3+1):nd,2)=dind4.*Rinv4;
HR((nd1+nd2+nd3+1):nd,5)=Rinv4;

Y(1:nd1)=log(exp_par(1,2)*exp_par(1,1).^dind1);
Y((nd1+1):(nd1+nd2))=log(exp_par(2,2)*exp_par(2,1).^dind2);
Y((nd1+nd2+1):(nd1+nd2+nd3))=log(exp_par(3,2)*exp_par(3,1).^dind3);
Y((nd1+nd2+nd3+1):nd)=log(exp_par(4,2)*exp_par(4,1).^dind4);

res=inv(HR'*H)*HR'*Y;
exp_apx=[exp(res(1)) exp(res(3));exp(res(2)) exp(res(4));exp(res(1)) exp(res(5));exp(res(2)) exp(res(5))];
exp_apx(nind,2)=-1*exp_apx(nind,2);

if (show_res==1)
    exp_par(nind,2)=-1*exp_par(nind,2);
    figure;
    for i=1:4
        ind=0:dboun(i,2);
        plot(ind,exp_par(i,1).^ind*exp_par(i,2))
        hold on
        plot(ind,exp_apx(i,1).^ind*exp_apx(i,2),'r')
    end
    grid;
end


function [ abd, info ] = zpbfa ( abd, lda, n, m )

%*****************************************************************************80
%
%% ZPBFA factors a complex hermitian positive definite band matrix.
%
%  Discussion:
%
%    ZPBFA is usually called by ZPBCO, but it can be called
%    directly with a saving in time if RCOND is not needed.
%
%  Band storage:
%
%    If A is a hermitian positive definite band matrix,
%    the following program segment will set up the input.
%
%      m = (band width above diagonal)
%      do j = 1, n
%        i1 = max ( 1, j-m )
%        do i = i1, j
%          k = i-j+m+1
%          abd(k,j) = a(i,j)
%        end do
%      end do
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex ABD(LDA,N); the matrix to be factored.
%    The columns of the upper triangle are stored in the columns of ABD
%    and the diagonals of the upper triangle are stored in the rows of ABD.
%
%    Input, integer LDA, the leading dimension of ABD.
%    LDA must be at least M+1.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer M, the number of diagonals above the main diagonal.
%    0 <= M < N.
%
%    Output, integer INFO.
%    0, for normal return.
%    K, if the leading minor of order K is not positive definite.
%
%    Output, complex ABD(LDA,N); an upper triangular matrix R, stored in
%    band form, so that A = hermitian(R)*R.
%
  info = 0;

  for j = 1 : n

    s = 0.0;
    ik = m + 1;
    jk = max ( j - m, 1 );
    mu = max ( m + 2 - j, 1 );

    for k = mu : m
      t = abd(k,j) - abd(ik:ik+k-mu-1,jk)' * abd(mu:mu+k-mu-1,j);
      t = t / abd(m+1,jk);
      abd(k,j) = t;
      s = s + real ( t * conj ( t ) );
      ik = ik - 1;
      jk = jk + 1;
    end

    s = real ( abd(m+1,j) ) - s;

    if ( s <= 0.0 | imag ( abd(m+1,j) ) ~= 0.0 )
      info = j;
      break
    end

    abd(m+1,j) = sqrt ( s );

  end

  return
end


function [AF,BF]=gabframebounds(g,a,M,varargin)
%GABFRAMEBOUNDS  Calculate frame bounds of Gabor frame
%   Usage:  fcond=gabframebounds(g,a,M);
%           [A,B]=gabframebounds(g,a,M);
%           [A,B]=gabframebounds(g,a,M,L);
%           [A,B]=gabframebounds(g,a,M,'lt',lt);
%
%   Input parameters:
%           g     : The window function.
%           a     : Length of time shift.
%           M     : Number of channels.
%           L     : Length of transform to consider.
%           lt    : Lattice type (for non-separable lattices).
%   Output parameters:
%           fcond : Frame condition number (B/A)
%           A,B   : Frame bounds.
%
%   `gabframebounds(g,a,M)` calculates the ratio $B/A$ of the frame bounds
%   of the Gabor system with window *g*, and parameters *a*, *M*.
%
%   `[A,B]=gabframebounds(...)` returns the frame bounds *A* and *B*
%   instead of just the ratio.
%
%   The window *g* may be a vector of numerical values, a text string or a
%   cell array. See the help of |gabwin| for more details.
%
%   `gabframebounds(g,a,M,L)` will cut or zero-extend the window to length
%   *L*.
%
%   `gabframebounds(g,a,M,'lt',lt)` does the same for a non-separable
%   lattice specified by *lt*. Please see the help of |matrix2latticetype|
%   for a precise description of the parameter *lt*.
%
%   See also: gabrieszbounds, gabwin

%% ---------- Assert correct input.

if nargin<3
  error('%s: Too few input parameters.',upper(mfilename));
end;

definput.keyvals.L=[];
definput.keyvals.lt=[0 1];
[flags,kv,L]=ltfatarghelper({'L'},definput,varargin);

%% ------ step 2: Verify a, M and L
if isempty(L)
    if isnumeric(g)
        % Use the window length
        Ls=length(g);
    else
        % Use the smallest possible length
        Ls=1;
    end;

    % ----- step 2b : Verify a, M and get L from the window length ----------
    L=dgtlength(Ls,a,M,kv.lt);

else

    % ----- step 2a : Verify a, M and get L

    Luser=dgtlength(L,a,M,kv.lt);
    if Luser~=L
        error(['%s: Incorrect transform length L=%i specified. Next valid length ' ...
               'is L=%i. See the help of DGTLENGTH for the requirements.'],...
              upper(mfilename),L,Luser);
    end;

end;

%% ----- step 3 : Determine the window

[g,info]=gabwin(g,a,M,L,kv.lt,'callfun',upper(mfilename));

if L<info.gl
  error('%s: Window is too long.',upper(mfilename));
end;

%% ----- actual computation ------------

g=fir2long(g,L);
R=size(g,2);

if kv.lt(2)==1
    % Rectangular case
    % Get the factorization of the window.
    gf=comp_wfac(g,a,M);

    % Compute all eigenvalues.
    lambdas=comp_gfeigs(gf,L,a,M);
    s=size(lambdas,1);

else

    % Convert to multi-window
    mwin=comp_nonsepwin2multi(g,a,M,kv.lt,L);

    % Get the factorization of the window.
    gf=comp_wfac(mwin,a*kv.lt(2),M);

    % Compute all eigenvalues.
    lambdas=comp_gfeigs(gf,L,a*kv.lt(2),M);
    s=size(lambdas,1);

end;

% Min and max eigenvalue.
if a>M*R
    % This can is not a frame, so A is identically 0.
    AF=0;
else
    AF=lambdas(1);
end;

BF=lambdas(s);

if nargout<2
    % Avoid the potential warning about division by zero.
    if AF==0
        AF=Inf;
    else
        AF=BF/AF;
    end;
end;


%% Brightness and contrast adjustments
%
% In this demo we show how to perform the operation
% $g(i,j) = \alpha \cdot f(i,j) + \beta$.
%
% Sources:
%
% * <https://docs.opencv.org/3.3.0/d3/dc1/tutorial_basic_linear_transform.html>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/tutorial_code/ImgProc/BasicLinearTransforms.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/tutorial_code/HighGUI/BasicLinearTransformsTrackbar.cpp>
% * <https://github.com/opencv/opencv/blob/3.3.0/samples/cpp/tutorial_code/ImgProc/changing_contrast_brightness_image/changing_contrast_brightness_image.cpp>
%

%% Theory
%
% A general image processing operator is a function that takes one or more
% input images and produces an output image. Image transforms can be seen as:
%
% * Point operators (pixel transforms)
% * Neighborhood (area-based) operators
%
% In pixel transforms, each output pixel's value depends on only the
% corresponding input pixel value (plus, potentially, some globally collected
% information or parameters). Examples of such operators include
% *brightness and contrast adjustments* as well as color correction and
% transformations.
%
% For brightness and contrast adjustments, two commonly used point processes
% are *multiplication* and *addition* with a constant:
%
% $$g(x) = \alpha f(x) + \beta$$
%
% The parameters $\alpha > 0$ and $\beta$ are often called the *gain* and
% *bias* parameters; sometimes these parameters are said to control *contrast*
% and *brightness* respectively. You can think of $f(x)$ as the source image
% pixels and $g(x)$ as the output image pixels. Then, more conveniently we can
% write the expression as:
%
% $$g(i,j) = \alpha \cdot f(i,j) + \beta$$
%
% where $i$ and $j$ indicates that the pixel is located in the *i-th* row and
% *j-th* column.
%
% In the implementation below, instead of using a for-loop to access each
% pixel, we simply use the function |cv.convertTo| which effectively performs
% *new_image = saturate(a*image + beta)*. This is more optimized than
% accessing each pixel and works a lot faster. Also notice that the operation
% can give values out of range or not integers (if $\alpha$ is float), in
% which case |cv.convertTo| makes sure the values are valid.
%

%% Example
%
% We will put into practice what we have learned to correct an underexposed
% image by adjusting the brightness and the contrast of the image. We will
% also see another technique to correct the brightness of an image called
% gamma correction.
%
%% Brightness and contrast adjustments
%
% Increasing/decreasing the $\beta$ value will add/subtract a constant value
% to every pixel. Pixel values outside of the |[0 ; 255]| range will be
% saturated (i.e. a pixel value higher/lesser than 255/0 will be clamp to
% 255/0).
%
% <<https://docs.opencv.org/3.3.0/Basic_Linear_Transform_Tutorial_hist_beta.png>>
%
% _In light gray, histogram of the original image, in dark gray when
% |brightness = 80| in Gimp_
%
% The histogram represents for each color level the number of pixels with that
% color level. A dark image will have many pixels with low color value and
% thus the histogram will present a peak in his left part. When adding a
% constant bias, the histogram is shifted to the right as we have added a
% constant bias to all the pixels.
%
% The $\alpha$ parameter will modify how the levels spread. If $\alpha < 1$,
% the color levels will be compressed and the result will be an image with
% less contrast.
%
% <<https://docs.opencv.org/3.3.0/Basic_Linear_Transform_Tutorial_hist_alpha.png>>
%
% _In light gray, histogram of the original image, in dark gray when
% |contrast < 0| in Gimp_
%
% Note that these histograms have been obtained using the Brightness-Contrast
% tool in the Gimp software. The brightness tool should be identical to the
% $\beta$ bias parameters but the contrast tool seems to differ to the
% $\alpha$ gain where the output range seems to be centered with Gimp (as you
% can notice in the previous histogram).
%
% It can occur that playing with the $\beta$ bias will improve the brightness
% but in the same time the image will appear with a slight veil as the
% contrast will be reduced. The $\alpha$ gain can be used to diminue this
% effect but due to the saturation, we will lose some details in the original
% bright regions.
%
%% Gamma correction
%
% <https://en.wikipedia.org/wiki/Gamma_correction Gamma correction> can be
% used to correct the brightness of an image by using a non linear
% transformation between the input values and the mapped output values:
%
% $$O = \left( \frac{I}{255} \right)^{\gamma} \times 255$$
%
% As this relation is non linear, the effect will not be the same for all the
% pixels and will depend to their original value.
%
% <<https://docs.opencv.org/3.3.0/Basic_Linear_Transform_Tutorial_gamma.png>>
%
% When $\gamma < 1$, the original dark regions will be brighter and the
% histogram will be shifted to the right whereas it will be the opposite with
% $ gamma > 1$.
%
% For the gamma correction, a look-up table can be used to improve the
% performance of the computation as only 256 values needs to be calculated
% once.
%
% Let's an example of how to correct an underexposed image.
%
% The following image has been corrected with: $\alpha = 1.3$ and $\beta = 40$.
%
% <<https://docs.opencv.org/3.3.0/Basic_Linear_Transform_Tutorial_linear_transform_correction.jpg>>
%
% The overall brightness has been improved but you can notice that the clouds
% are now greatly saturated due to the numerical saturation of the
% implementation used
% (<https://en.wikipedia.org/wiki/Clipping_(photography) highlight clipping>
% in photography).
%
% The following image has been corrected with: $\gamma = 0.4$.
%
% <<https://docs.opencv.org/3.3.0/Basic_Linear_Transform_Tutorial_gamma_correction.jpg>>
%
% The gamma correction should tend to add less saturation effect as the
% mapping is non linear and there is no numerical saturation possible as in
% the previous method.
%
% <<https://docs.opencv.org/3.3.0/Basic_Linear_Transform_Tutorial_histogram_compare.png>>
%
% * Left: histogram after alpha, beta correction
% * Center: histogram of the original image
% * Right: histogram after the gamma correction
%
% The previous figure compares the histograms for the three images (the
% y-ranges are not the same between the three histograms). You can notice that
% most of the pixel values are in the lower part of the histogram for the
% original image. After $\alpha$, $\beta$ correction, we can observe a big
% peak at 255 due to the saturation as well as a shift in the right. After
% gamma correction, the histogram is shifted to the right but the pixels in
% the dark regions are more shifted (see the gamma curves figure) than those
% in the bright regions.
%
% In this tutorial, you have seen two simple methods to adjust the contrast
% and the brightness of an image. They are basic techniques and are not
% intended to be used as a replacement of a raster graphics editor!
%
%% Additional resources
%
% * <https://learnopengl.com/#!Advanced-Lighting/Gamma-Correction
%   Gamma correction in graphics rendering>
% * <http://web.archive.org/web/20170106081601/http://www.graphics.cornell.edu/~westin/gamma/gamma.html
%   Gamma correction and images displayed on CRT monitors>
% * <http://www.cambridgeincolour.com/tutorials/digital-exposure-techniques.htm
%   Digital exposure techniques>
%

%% Code

function varargout = linear_transform_demo_gui(im)
    % load source image
    if nargin < 1
        im = fullfile(mexopencv.root(),'test','lena.jpg');
        img = cv.imread(im);
    elseif ischar(im)
        img = cv.imread(im, 'Color',true);
    else
        img = im;
    end

    % create the UI
    h = buildGUI(img);
    if nargout > 0, varargout{1} = h; end
end

function onLinearTransform(~,~,h)
    %ONLINEARTRANSFORM  Event handler for UI controls

    % retrieve current values from UI controls
    a = get(h.slid(1), 'Value');
    b = round(get(h.slid(2), 'Value'));
    set(h.txt(1), 'String',sprintf('Contrast: %.2f',a));
    set(h.txt(2), 'String',sprintf('Brightness: %2d',b));

    % linear transformation
    out = cv.convertTo(h.src, 'Alpha',a, 'Beta',b);

    % show result
    out = cv.putText(out, 'Brightness/Contrast Adjustment', [10 20], ...
        'FontScale',0.5, 'Color',[0 255 0], 'LineType','AA');
    set(h.img(1), 'CData',out);
    drawnow;
end

function onGammaCorrection(~,~,h)
    %ONGAMMACORRECTION  Event handler for UI controls

    % retrieve current values from UI controls
    g = get(h.slid(3), 'Value');
    set(h.txt(3), 'String',sprintf('Gamma: %.2f',g));

    % gamma correction
    lookUpTable = uint8((((0:255)/255) .^ g) * 255);
    if true
        out = cv.LUT(h.src, lookUpTable);
    elseif mexopencv.require('images')
        out = intlut(h.src, lookUpTable);
    else
        out = lookUpTable(double(h.src) + 1);
    end

    % show result
    out = cv.putText(out, 'Gamma Correction', [10 20], ...
        'FontScale',0.5, 'Color',[0 255 0], 'LineType','AA');
    set(h.img(2), 'CData',out);
    drawnow;
end

function h = buildGUI(img)
    %BUILDGUI  Creates the UI

    % parameters
    a = 1.0;  % alpha gain (contrast)
    b = 0;    % beta bias (brightness)
    g = 1.0;  % gamma correction
    sz = size(img);
    sz(2) = max(sz(2), 300);  % minimum figure width

    % build the user interface (no resizing to keep it simple)
    h = struct();
    h.src = img;
    h.fig = figure('Name','Linear Transform Demo', ...
        'NumberTitle','off', 'Menubar','none', 'Resize','off', ...
        'Position',[200 200 sz(2)*2 sz(1)+80-1]);
    if ~mexopencv.isOctave()
        %HACK: not implemented in Octave
        movegui(h.fig, 'center');
    end
    h.ax(1) = axes('Parent',h.fig, 'Units','pixels', 'Position',[1 80 sz(2) sz(1)]);
    h.ax(2) = axes('Parent',h.fig, 'Units','pixels', 'Position',[sz(2)+1 80 sz(2) sz(1)]);
    if ~mexopencv.isOctave()
        h.img(1) = imshow(img, 'Parent',h.ax(1));
        h.img(2) = imshow(img, 'Parent',h.ax(2));
    else
        %HACK: https://savannah.gnu.org/bugs/index.php?45473
        axes(h.ax(1)); h.img(1) = imshow(img);
        axes(h.ax(2)); h.img(2) = imshow(img);
    end
    h.txt(1) = uicontrol('Parent',h.fig, 'Style','text', 'FontSize',11, ...
        'Position',[5 5 130 20], 'String',sprintf('Contrast: %.2f',a));
    h.txt(2) = uicontrol('Parent',h.fig, 'Style','text', 'FontSize',11, ...
        'Position',[5 30 130 20], 'String',sprintf('Brightness: %2d',b));
    h.txt(3) = uicontrol('Parent',h.fig, 'Style','text', 'FontSize',11, ...
        'Position',[5 55 130 20], 'String',sprintf('Gamma: %.2f',g));
    h.slid(1) = uicontrol('Parent',h.fig, 'Style','slider', 'Value',a, ...
        'Min',0.1, 'Max',3, 'SliderStep',[0.01 0.2]./(3-0.1), ...
        'Position',[135 5 sz(2)-135-5 20]);
    h.slid(2) = uicontrol('Parent',h.fig, 'Style','slider', 'Value',b, ...
        'Min',-100, 'Max',100, 'SliderStep',[1 20]./(100+100), ...
        'Position',[135 30 sz(2)-135-5 20]);
    h.slid(3) = uicontrol('Parent',h.fig, 'Style','slider', 'Value',g, ...
        'Min',0.1, 'Max',3, 'SliderStep',[0.01 0.2]./(3-0.1), ...
        'Position',[135 55 sz(2)-135-5 20]);

    % hook event handlers, and trigger default start
    opts = {'Interruptible','off', 'BusyAction','cancel'};
    set(h.slid(1:2), 'Callback',{@onLinearTransform,h}, opts{:});
    set(h.slid(3), 'Callback',{@onGammaCorrection,h}, opts{:});
    onLinearTransform([],[],h);
    onGammaCorrection([],[],h);
end


function partition_count_values_test ( )

%*****************************************************************************80
%
%% PARTITION_COUNT_VALUES_TEST demonstrates the use of PARTITION_COUNT_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2005
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PARTITION_COUNT_VALUES_TEST:\n' );
  fprintf ( 1, '  PARTITION_COUNT_VALUES returns values of \n' );
  fprintf ( 1, '  the integer partition count function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N         P(N)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, fn ] = partition_count_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %4d  %10d\n', n, fn );

  end

  return
end


classdef MOEADM2M_F3 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing MOEA/D-M2M

%------------------------------- Reference --------------------------------
% H. Liu, F. Gu, and Q. Zhang, Decomposition of a multiobjective
% optimization problem into a number of simple multiobjective subproblems,
% IEEE Transactions on Evolutionary Computation, 2014, 18(3): 450-455.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 10; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            t = X(:,2:end) - repmat(sin(pi/2*X(:,1)),1,size(X,2)-1);
            g = 10*sin(pi/2*X(:,1)).*sum(abs(t)./(1+exp(5*abs(t))),2);
            PopObj(:,1) = (1+g).*cos(pi/2*X(:,1));
            PopObj(:,2) = (1+g).*sin(pi/2*X(:,1));
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = sqrt(1-R(:,1).^2);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function gX = ardKernGradX(kern, X, X2)

% ARDKERNGRADX Gradient of ARD kernel with respect to a point x.
% FORMAT
% DESC computes the gradient of the pre-built RBF and linear ARD
% kernel with respect to the input positions.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG x : locations against which gradients are being computed.
% RETURN g : the returned gradients. The gradients are returned in
% a matrix which is numData x numInputs x numData. Where numData is
% the number of data points and numInputs is the number of input
% dimensions in X.
%
% FORMAT
% DESC computes the gradident of the pre-built RBF and linear ARD
% kernel with respect to the input positions where both the row
% positions and column positions are provided separately.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG x1 : row locations against which gradients are being computed.
% ARG x2 : column locations against which gradients are being computed.
% RETURN g : the returned gradients. The gradients are returned in
% a matrix which is numData2 x numInputs x numData1. Where numData1 is
% the number of data points in X1, numData2 is the number of data
% points in X2 and numInputs is the number of input
% dimensions in X.
%
% SEEALSO ardKernParamInit, kernGradX, ardKernDiagGradX
%
% COPYRIGHT : Neil D. Lawrence, 2004

% KERN

gX = zeros(size(X2, 1), size(X2, 2), size(X, 1));
for i = 1:size(X, 1);
  gX(:, :, i) = ardKernGradXpoint(kern, X(i, :), X2);
end

function gX = ardKernGradXpoint(kern, x, X2)

% ARDKERNGRADXPOINT Gradient with respect to one point of x.

scales = sparse(diag(kern.inputScales));

gX = kern.linearVariance.*X2*scales;

scales = sqrt(scales);

n2 = dist2(X2*scales, x*scales);
wi2 = (.5 .* kern.inverseWidth);
rbfPart = kern.rbfVariance*exp(-n2*wi2);
for i = 1:size(x, 2)
  gX(:, i) = gX(:, i) + kern.inverseWidth*kern.inputScales(i)*(X2(:, i) - x(i)).*rbfPart;
end


%DEMO_SVI_CLASSIFIC  Classification problem demonstration for SVI GP
%
%  Description
%    The demonstration program is based on synthetic two class data
%    used by B.D. Ripley (Pattern Recognition and Neural Networks,
%    1996}. The data consists of 2-dimensional vectors that are
%    divided into two classes, labeled 0 or 1. Each class has a
%    bimodal distribution generated from equal mixtures of Gaussian
%    distributions with identical covariance matrices. A Bayesian
%    approach is used to find the decision line and predict the
%    classes of new data points. The result can be compared to the
%    ones from the DEMO_CLASSIFIC.
%
%    The probability of y being one is assumed to be
%
%      p(y=1|f) = normcdf(f)
%
%    The latent values f are given a zero mean Gaussian process
%    prior. This implies that at the observed input locations
%    latent values have prior
%
%      f ~ N(0, K),
%
%    where K is the covariance matrix, whose elements are given as
%    K_ij = k(x_i, x_j | th). The function k(x_i, x_j | th) is
%    covariance function and th its parameters.
%
%    Here we demonstarte use of stochastic variational inference
%    methods to find the posterior of the latent values and parameters.
%    With these we can make predictions on the class probability of
%    future observations. See Hensman et. al. (2013) for the
%    detailed treatment.
%
%  See also
%    DEMO_SVI_REGRESSION, DEMO_CLASSIFIC
%
%
%  References:
%    Hensman, J., Fusi, N. and Lawrence, N. D. (2013). Gaussian
%    processes for big data. arXiv preprint arXiv:1309.6835.

% Copyright (c) 2014 Tuomas Sivula

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% This demonstration is based on the dataset used in the book Pattern
% Recognition and Neural Networks by B.D. Ripley (1996), Cambridge
% University Press.

% Training data
S = which('demo_classific');
L = strrep(S,'demo_classific.m','demodata/synth.tr');
x=load(L);
y=x(:,end);
y = 2.*y-1;
x(:,end)=[];
[n, nin] = size(x);

% Test data
xt1=repmat(linspace(min(x(:,1)),max(x(:,1)),20)',1,20);
xt2=repmat(linspace(min(x(:,2)),max(x(:,2)),20)',1,20)';
xt=[xt1(:) xt2(:)];

% Create likelihood function
lik = lik_probit();
%lik = lik_logit();

% Create covariance functions
gpcf = gpcf_sexp('lengthScale', [0.9 0.9], 'magnSigma2', 10);
% Set the prior for the parameters of covariance functions
pl = prior_t();
pm = prior_sqrtt('s2',0.5);
gpcf = gpcf_sexp(gpcf, 'lengthScale_prior', pl,'magnSigma2_prior', pm);

% Create the GP structure (type is by default FULL)
fprintf('SVI GP classification model with probit likelihood\n')
gp = gp_set('lik', lik, 'cf', gpcf, ...
  'latent_method', 'SVI', 'jitterSigma2', 1e-6);

% Select 20 inducing inputs by clustering 10 from both training class
% inputs
fprintf(['Select 20 inducing inputs by clustering 10 points ', ...
  'from both training classes ...'])
Sw = warning('off','stats:kmeans:EmptyCluster');
[~,X_u1] = kmeans(x(y==1,:), 10,'Start','uniform',...
    'EmptyAction','singleton');
[~,X_u2] = kmeans(x(y==-1,:), 10,'Start','uniform',...
    'EmptyAction','singleton');
warning(Sw);
X_u = [X_u1 ; X_u2];
fprintf(' done\n')

% Optimise
maxi = 1000; % The maximum number of iteration rounds
gp = svigp(gp,x,y,'X_u',X_u,'maxiter',maxi,'mu2',1e-7);
% Make predictions
[Eft, Varft, lpyt, Eyt, Varyt] = ...
    gpsvi_pred(gp, x, y, xt, 'yt', ones(size(xt,1),1) );

% Visualise predictive probability p(ystar = 1) with grayscale
figure, hold on;
n_pred=size(xt,1);
h1=pcolor(reshape(xt(:,1),20,20),reshape(xt(:,2),20,20),reshape(exp(lpyt),20,20));
set(h1, 'edgealpha', 0), set(h1, 'facecolor', 'interp')
colormap(repmat(linspace(1,0,64)', 1, 3).*repmat(ones(1,3), 64,1))
axis([-inf inf -inf inf]), %axis off
plot(x(y==-1,1),x(y==-1,2),'o', 'markersize', 8, 'linewidth', 2);
plot(x(y==1,1),x(y==1,2),'rx', 'markersize', 8, 'linewidth', 2);
plot(gp.X_u(:,1),gp.X_u(:,2),'gs', 'markersize', 8, 'linewidth', 1);
set(gcf, 'color', 'w'), title('predictive probability, training cases and inducing inputs with SVIGP', 'fontsize', 14)


function y=scInv(z, LinCol)
%scInv : Inverts a given impedance (admitance) by mirroring it about the origin
%
%  SYNOPSIS:
%     This function inverts the given impedance (admittance) by mirroring it about
%     the origin of the smith chart.
%
%     See also scDraw, scMove, scConCirc, scMatchCirc
%
%  SYNTAX:
%     [y] = scInv(z)
%
%  INPUT ARGUMENTS:
%     z     : Impedance (or admittance)
%
%  OUTPUT ARGUMENT:
%     y : Inverted admittance (or impedance)
%
%  EXAMPLE:
%        y = scInv([2 3])
%        y =
%        0.1538   -0.2308
%
%
%     Mohammad Ashfaq - (31-05-2000)
%     Mohammad Ashfaq - (13-04-2006) Modified (example included)
%

 if nargin < 1
    error('scInv.m: One input argument required...You may give LinCol as second argument as well');
 end

 if nargin == 1
    LinCol = 'm';
 end

 if (size(z)==[1, 1])
    if conj(z) == z
       r = z;
       x = 0;
    else
       r = real (z);
       x = imag (z);
    end
 elseif max(size(z)) == 2 && min(size(z)) == 1
    r = z(1);
    x = z(2);
 else
    error('scInv.m: Input must either be a complex number or a 1x2 vector  [r x]');
 end

 if (r+j*x)~=0
    y1   = 1/(r+j*x);
    y(1) = real(y1);
    y(2) = imag(y1);

 else
    y = inf;
 end


%% TOMLAB PROPT Parameter Estimation Problems
% http://tomdyn.com/parameter_estimation_dynamic_systems.html
clc
clear

%% http://tomdyn.com/examples/catalyticCracking.html
clc
ode = @(t,z,p) [-(p(1)+p(3))*z(1).^2
                p(1)*z(1).^2-p(2)*z(2)];
z0 = [1;0]; %ic

% Various constants and expressions
y1meas = [1.0;0.8105;0.6208;0.5258;0.4345;0.3903;...
    0.3342;0.3034;0.2735;0.2405;0.2283;0.2071;0.1669;...
    0.153;0.1339;0.1265;0.12;0.099;0.087;0.077;0.069];
y2meas = [0;0.2;0.2886;0.301;0.3215;0.3123;0.2716;...
    0.2551;0.2258;0.1959;0.1789;0.1457;0.1198;0.0909...
    ;0.0719;0.0561;0.046;0.028;0.019;0.014;0.010];
tmeas = [0;0.025;0.05;0.075;0.1;0.125;...
    0.15;0.175;0.2;0.225;0.25;0.3;0.35;0.4;...
    0.45;0.5;0.55;0.65;0.75;0.85;0.95];

%Build OPTI Object
theta0 = [10;8;1]; %inital parameter guess
opts = optiset('display','iter');
Opt = opti('ode',ode,'data',tmeas,[y1meas' y2meas'],'theta0',theta0,'z0',z0,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% http://tomdyn.com/examples/isometrizationAlpha.html
clc
ode = @(t,z,p) [-(p(1)+p(2))*z(1)
                p(1)*z(1)
                p(2)*z(1)-(p(3)+p(4))*z(3)+p(5)*z(5)
                p(3)*z(3)
                p(4)*z(3)-p(5)*z(5)];
z0 = [100;0;0;0;0]; %ic

y1meas = [88.35; 76.4; 65.1; 50.4; 37.5; 25.9; 14.0; 4.5];
y2meas = [7.3; 15.6; 23.1; 32.9; 42.7; 49.1; 57.4; 63.1];
y3meas = [2.3; 4.5; 5.3; 6.0; 6.0; 5.9; 5.1; 3.8];
y4meas = [0.4; 0.7; 1.1; 1.5; 1.9; 2.2; 2.6; 2.9];
y5meas = [1.75; 2.8; 5.8; 9.3; 12.0; 17.0; 21.0; 25.7];
tmeas  = [1230; 3060; 4920; 7800; 10680; 15030; 22620; 36420];

%Build OPTI Object
theta0 = [0;0;0;0;0]; %inital parameter guess
opts = optiset('display','iter','dynamicOpts',optidynset('sensitivity','cs','initialT',0));
Opt = opti('ode',ode,'data',tmeas,[y1meas' y2meas' y3meas' y4meas' y5meas'],'theta0',theta0,'z0',z0,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% http://tomdyn.com/examples/marinePopulation.html
clc
ode = @(t,z,p) [[0; p(9:end)].*[0; z(1:7)] - (p(1:8)+[p(9:end);0]).*z];

ymeas = [20000 17000 10000 15000 12000 9000 7000 3000
    12445 15411 13040 13338 13484 8426 6615 4022
     7705 13074 14623 11976 12453 9272 6891 5020
     4664  8579 12434 12603 11738 9710 6821 5722
     2977  7053 11219 11340 13665 8534 6242 5695
     1769  5054 10065 11232 12112 9600 6647 7034
      943  3907  9473 10334 11115 8826 6842 7348
      581  2624  7421 10297 12427 8747 7199 7684
      355  1744  5369  7748 10057 8698 6542 7410
      223  1272  4713  6869  9564 8766 6810 6961
      137   821  3451  6050  8671 8291 6827 7525
       87   577  2649  5454  8430 7411 6423 8388
       49   337  2058  4115  7435 7627 6268 7189
       32   228  1440  3790  6474 6658 5859 7467
       17   168  1178  3087  6524 5880 5562 7144
       11    99   919  2596  5360 5762 4480 7256
        7    65   647  1873  4556 5058 4944 7538
        4    44   509  1571  4009 4527 4233 6649
        2    27   345  1227  3677 4229 3805 6378
        1    20   231   934  3197 3695 3159 6454
        1    12   198   707  2562 3163 3232 5566];
tmeas  = 0:0.5:10;

z0 = ymeas(1,:); %ic

%Build OPTI Object
theta0 = [zeros(8,1);zeros(7,1)]; %inital parameter guess
opts = optiset('display','iter','dynamicOpts',optidynset('sensitivity','cs'));
Opt = opti('ode',ode,'data',tmeas,ymeas,'theta0',theta0,'z0',z0,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% http://tomdyn.com/examples/methanolToHydrocarbons.html
clc
ode = @(t,z,p) [-(2*p(2)-(p(1)*z(2))./((p(2)+p(5))*z(1)+z(2))+p(3)+p(4)).*z(1)
                (p(1)*z(1).*(p(2)*z(1)-z(2)))./((p(2)+p(5))*z(1)+z(2))+p(3)*z(1)
                (p(1)*z(1).*(z(2)+p(5)*z(1)))./((p(2)+p(5))*z(1)+z(2))+p(4)*z(1)];

y1meas = [0.7085;0.5971;0.5537;0.3684;0.1712;...
    0.1198;0.0747;0.0529;0.0415;0.0261;0.0208;...
    0.0085;0.0053;0.0019;0.0018];
y2meas = [0.1621;0.1855;0.1989;0.2845;0.3491;...
    0.3098;0.3576;0.3347;0.3388;0.3557;0.3483;...
    0.3836;0.3611;0.3609;0.3485];
y3meas = [0.0811;0.0965;0.1198;0.1535;0.2097;...
    0.2628;0.2467;0.2884;0.2757;0.3167;0.2954;...
    0.295;0.2937;0.2831;0.2846];
tmeas = [0.05;0.065;0.08;0.123;0.233;0.273;...
    0.354;0.397;0.418;0.502;0.553;...
    0.681;0.75;0.916;0.937];

z0 = [1;0;0]; %ic
lb = ones(5,1)*sqrt(eps);
ub = 10*ones(5,1);

%Build OPTI Object
theta0 = [1;1;1;1;1]; %inital parameter guess
opts = optiset('display','iter','solver','nl2sol','dynamicOpts',optidynset('initialT',0,'sensitivity','cs'));
Opt = opti('ode',ode,'bounds',lb,ub,'data',tmeas,[y1meas y2meas y3meas],'theta0',theta0,'z0',z0,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% http://tomdyn.com/examples/parameterEstimation.html
clc
ode = @(t,z,p) [p(1)*z(2); 1-2*z(2)-z(1)];

y1meas = [0.264;0.594;0.801;0.959];
tmeas = [1;2;3;5];

z0 = [NaN;NaN]; %ic

lb = -1.5*ones(3,1); lb(1)=1; %not actually interested in p1, but needed for OPTI
ub = 1.5*ones(3,1); ub(1)=1;

%Build OPTI Object
theta0 = [1;0;0]; %inital parameter guess
opts = optiset('display','iter','solver','nl2sol','dynamicOpts',optidynset('stateIndex',1,'initialT',0,'sensitivity','cs'));
Opt = opti('ode',ode,'bounds',lb,ub,'data',tmeas,y1meas,'theta0',theta0,'z0',z0,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)


function [px,py,pz,tx,ty,tz,bx,by,bz,ierr] = nd2pt(wanx,wany,wanz,wdx,wdy,wdz)

%% c     compute Cartesian component of P, T and B axes from outward normal
%% c     and slip vectors
%% c
%% c     usage:
%% c     call nd2pt(anx,any,anz,dx,dy,dz,px,py,pz,tx,ty,tz,bx,by,bz,ierr)
%% c
%% c     arguments:
%% c     anx,any,anz    components of fault plane outward normal vector in the
%% c                    Aki-Richards Cartesian coordinate system (INPUT)
%% c     dx,dy,dz       components of slip vector in the Aki-Richards
%% c                    Cartesian coordinate system (INPUT)
%% c     px,py,pz       components of downward P (maximum dilatation) axis versor
%% c                    in the Aki-Richards Cartesian coordinate system (OUTPUT)
%% c     tx,ty,tz       components of downward T (maximum tension) axis versor
%% c                    in the Aki-Richards Cartesian coordinate system (OUTPUT)
%% c     bx,by,bz       components of downward B (neutral) axis versor in the
%% c                    Aki-Richards Cartesian coordinate system (OUTPUT)
%% c     ierr           error indicator (OUTPUT)
%% c
%% c     errors:
%% c     1              input vectors not perpendicular among each other

%% c
%% c      implicit none
%% c-------------------------------------------------------------------------------
%%       integer io
%%       real amistr,amastr,amidip,amadip,amirak,amarak,amitre,amatre
%%      1,amiplu,amaplu,orttol,ovrtol,tentol,dtor,c360,c90,c0,c1,c2,c3
%%       common /fpscom/amistr,amastr,amidip,amadip,amirak,amarak,amitre
%%      1,amatre,amiplu,amaplu,orttol,ovrtol,tentol,dtor,c360,c90,c0,c1,c2
%%      2,c3,io
%% c-------------------------------------------------------------------------------
%%       real wanx,wany,wanz,amn,anx,any,anz,wdx,wdy,wdz,amd,dx,dy,dz
%%      1,ang,px,py,pz,tx,ty,tz,bx,by,bz,amp
%%       integer ierr
%% c

%%       call fpsset
         amistr=-360.;
         amastr=360.;
         amidip=0.;
         amadip=90.;
         amirak=-360.;
         amarak=360.;
         amitre=-360.;
         amatre=360.;
         amiplu=0.;
         amaplu=90.;
         orttol=2.;
         ovrtol=0.001;
         tentol=0.0001;
         dtor=0.017453292519943296;
         c360=360.;
         c90=90.;
         c0=0.;
         c1=1.;
         c2=2.;
         c3=3.;
         io=6;

      ierr=0;
      [amn,anx,any,anz] = focal_norm(wanx,wany,wanz);
      [amd,dx,dy,dz] = focal_norm(wdx,wdy,wdz);
      [ang] = focal_angle(anx,any,anz,dx,dy,dz);
      if (abs(ang-c90) > orttol)
        disp(['ND2PT: input vectors not perpendicular, angle=' num2str(ang)]);
        ierr=1;
      end
      px=anx-dx;
      py=any-dy;
      pz=anz-dz;
      [amp,px,py,pz] = focal_norm(px,py,pz);
      if (pz < c0)
        [px,py,pz] = focal_invert(px,py,pz);
      end
      tx=anx+dx;
      ty=any+dy;
      tz=anz+dz;
      [amp,tx,ty,tz] = focal_norm(tx,ty,tz);
      if (tz < c0)
        [tx,ty,tz] = focal_invert(tx,ty,tz);
      end
      [bx,by,bz] = focal_vecpro(px,py,pz,tx,ty,tz);
      if(bz < c0)
        [bx,by,bz] = focal_invert(bx,by,bz);
      end


function [f] = elec_sphere_fit_optim(r, X, Y, Z, xo, yo, zo)

% elec_sphere_fit_optim - Optimization for elec_sphere_fit.m
%
% Called from elec_sphere_fit.m
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:55 $

% Licence:  GNU GPL, no implied or express warranties
% History:  02/2002, Darren.Weber_at_radiology.ucsf.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% with center (Xo,Yo,Zo) and radius r, the equation of a sphere is:
%
% r^2 = (x-xo)^2  +  (y-yo)^2  +  (z-zo)^2
%
% This function below creates a scalar value to
% return to the fminsearch function in elec_sphere_fit.

S = (X-xo).^2  +  (Y-yo).^2  +  (Z-zo).^2  -  r^2;

f = sum( S.^2 );


function [NABF]=analysis_nabf(f,I1,I2)
% function [QABF,LABF,NABF,NABF1]=objective_fusion_perform_fn(f,I1,I2)
%
%%% objective_fusion_perform_fn: Computes the Objective Fusion Performance Parameters proposed by Petrovic
%%% and modified Fusion Artifacts (NABF) measure proposed by B. K. Shreyamsha Kumar
%%%
%%% Inputs:
%%% xrcw -> fused image
%%% x -> source images, x{1}, x{2}
%%%
%%% Outputs:
%%% QABF -> Total information transferred from source images to fused image measure proposed by Petrovic
%%% LABF -> Total loss of information measure proposed by Petrovic
%%% NABF1 -> Fusion Artifacts measure proposed by Petrovic
%%% NABF -> Modified Fusion Artifacts measure proposed by B. K. Shreyamsha Kumar
%%%
%%% Author : B. K. SHREYAMSHA KUMAR
%%% Created on 28-10-2011.
%%% Updated on 08-11-2011.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Petrovic Metrics %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% Parameters for Petrovic Metrics Computation.
Td=2;
wt_min=0.001;
P=1;
Lg=1.5;
Nrg=0.9999;
kg=19;
sigmag=0.5;
Nra=0.9995;
ka=22;
sigmaa=0.5;

xrcw = (f);
x1 = (I1);
x2 = (I2);

%%% Edge Strength & Orientation.
[gvA,ghA]=sobel_fn(x1);
gA=sqrt(ghA.^2+gvA.^2);

[gvB,ghB]=sobel_fn(x2);
gB=sqrt(ghB.^2+gvB.^2);

[gvF,ghF]=sobel_fn(xrcw);
gF=sqrt(ghF.^2+gvF.^2);

%%% Relative Edge Strength & Orientation.
[p,q]=size(xrcw);
for ii=1:p
   for jj=1:q
      if(gA(ii,jj)==0 | gF(ii,jj)==0)
         gAF(ii,jj)=0;
      elseif(gA(ii,jj)>gF(ii,jj))
         gAF(ii,jj)=gF(ii,jj)/gA(ii,jj);
      else
         gAF(ii,jj)=gA(ii,jj)/gF(ii,jj);
      end
      if(gB(ii,jj)==0 | gF(ii,jj)==0)
         gBF(ii,jj)=0;
      elseif(gB(ii,jj)>gF(ii,jj))
         gBF(ii,jj)=gF(ii,jj)/gB(ii,jj);
      else
         gBF(ii,jj)=gB(ii,jj)/gF(ii,jj);
      end
      if(gvA(ii,jj)==0 & ghA(ii,jj)==0)
         aA(ii,jj)=0;
      else
         aA(ii,jj)=atan(gvA(ii,jj)/ghA(ii,jj));
      end
      if(gvB(ii,jj)==0 & ghB(ii,jj)==0)
         aB(ii,jj)=0;
      else
         aB(ii,jj)=atan(gvB(ii,jj)/ghB(ii,jj));
      end
      if(gvF(ii,jj)==0 & ghF(ii,jj)==0)
         aF(ii,jj)=0;
      else
         aF(ii,jj)=atan(gvF(ii,jj)/ghF(ii,jj));
      end
   end
end
aAF=abs(abs(aA-aF)-pi/2)*2/pi;
aBF=abs(abs(aB-aF)-pi/2)*2/pi;

%%% Edge Preservation Coefficient.
QgAF=Nrg./(1+exp(-kg*(gAF-sigmag)));
QaAF=Nra./(1+exp(-ka*(aAF-sigmaa)));
QAF=sqrt(QgAF.*QaAF);
QgBF=Nrg./(1+exp(-kg*(gBF-sigmag)));
QaBF=Nra./(1+exp(-ka*(aBF-sigmaa)));
QBF=sqrt(QgBF.*QaBF);

%%% Total Fusion Performance (QABF).
wtA=wt_min*ones(p,q);
wtB=wt_min*ones(p,q);
cA=ones(p,q); cB=ones(p,q);
for ii=1:p
   for jj=1:q
      if(gA(ii,jj)>=Td)
         wtA(ii,jj)=cA(ii,jj)*gA(ii,jj)^Lg;
      end
      if(gB(ii,jj)>=Td)
         wtB(ii,jj)=cB(ii,jj)*gB(ii,jj)^Lg;
      end
   end
end
wt_sum=sum(sum(wtA+wtB));
QAF_wtsum=sum(sum(QAF.*wtA))/wt_sum;  %% Information Contributions of A.
QBF_wtsum=sum(sum(QBF.*wtB))/wt_sum;  %% Information Contributions of B.
QABF=QAF_wtsum+QBF_wtsum;   %% QABF=sum(sum(QAF.*wtA+QBF.*wtB))/wt_sum -> Total Fusion Performance.

%%% Fusion Gain (QdeltaABF).
Qdelta=abs(QAF-QBF);
QCinfo=(QAF+QBF-Qdelta)/2;
QdeltaAF=QAF-QCinfo;
QdeltaBF=QBF-QCinfo;
QdeltaAF_wtsum=sum(sum(QdeltaAF.*wtA))/wt_sum;
QdeltaBF_wtsum=sum(sum(QdeltaBF.*wtB))/wt_sum;
QdeltaABF=QdeltaAF_wtsum+QdeltaBF_wtsum;   %% Total Fusion Gain.
QCinfo_wtsum=sum(sum(QCinfo.*(wtA+wtB)))/wt_sum;
QABF11=QdeltaABF+QCinfo_wtsum;              %% Total Fusion Performance.

%%% Fusion Loss (LABF).
rr=zeros(p,q);
for ii=1:p
   for jj=1:q
      if(gF(ii,jj)<=gA(ii,jj) | gF(ii,jj)<=gB(ii,jj))
         rr(ii,jj)=1;
      else
         rr(ii,jj)=0;
      end
   end
end
LABF=sum(sum(rr.*((1-QAF).*wtA+(1-QBF).*wtB)))/wt_sum;

%%% Fusion Artifacts (NABF) by Petrovic.
for ii=1:p
   for jj=1:q
      if(gF(ii,jj)>gA(ii,jj) & gF(ii,jj)>gB(ii,jj))
         na1(ii,jj)=2-QAF(ii,jj)-QBF(ii,jj);
      else
         na1(ii,jj)=0;
      end
   end
end
NABF1=sum(sum(na1.*(wtA+wtB)))/wt_sum;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Fusion Artifacts (NABF) changed by B. K. Shreyamsha Kumar .
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for ii=1:p
   for jj=1:q
      if(gF(ii,jj)>gA(ii,jj) & gF(ii,jj)>gB(ii,jj))
         na(ii,jj)=1;
      else
         na(ii,jj)=0;
      end
   end
end
NABF=sum(sum(na.*((1-QAF).*wtA+(1-QBF).*wtB)))/wt_sum;

QABF+LABF+NABF1;
QABF+LABF+NABF;


function d=meansqtf(b,a)
%AVEPSPEC calculates the mean square transfer function for a filter D=(B,A)
%
% Inputs: B,A         Numerator and denominator filter coefficients.
%
% Output: D           The mean square transfer function of the filter B/A. This equals
%                     the average otuput power when the filter is fed with unit variance
%                     white noise.
%
%                     D may be obtained approximately by:
%                         N=1024; D=sum(filter(B,A,[1 zeros(1,N)]).^2)

% Since the power spectrum is the fourier transform of the autocorrelation, we can calculate
% the average value of pb/pa by taking the 0'th order term of the convolution of the autocorrelation
% functions associated with b and 1/a. Since b is an FIR filter, this convolution is
% a finite sum even though the autocorrelation function of 1/a is infinite in extent.

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: meansqtf.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if length(a)==1
   d=(b(:)')*b(:);
else
   m=lpcar2ra(b(:)');
   m(1)=m(1)*0.5;
   d=2*lpcar2rr(a(:)',length(m)-1)*m';
end


function [X, R, S] = snapshot_gen_sto(design, doas, wavelength, t, ncov, scov)
%SNAPSHOT_GEN_STO Generates snapshots for the stochastic model.
%Syntax:
%   X = STO_SNAPSHOT_GEN(design, doas, wavelength[, t, ncov, scov]);
%   [X, R] = STO_SNAPSHOT_GEN(design, doas, wavelength[, t, ncov, scov]);
%   [X, R, S] = STO_SNAPSHOT_GEN(design, doas, wavelength[, t, ncov, scov]);
%Inputs:
%   design - Array design.
%   doas - DOA vector. For 2D DOAs, each column represents a DOA pair.
%   wavelength - Wavelength.
%   t - Number of snapshots.
%   ncov - Covariance matrix of the additive complex circular-symmetric
%          Gaussian noise. Can be a scalar, vector (for uncorrelated noise
%          with different powers), or a matrix.
%   scov - Covariance matrix of the source signals. Can be a scalar, vector
%          (for uncorrelated sources with different powers), or a matrix.
%Outputs:
%   X - Snapshots, where each columns is a single snapshot.
%   R - Sample covariance matrix (averaged by the number of snapshots).
%   S - A source_count x snapshot_count matrix consists of source signal
%       vectors.
if nargin <= 5
    scov = 1;
end
if nargin <= 4
    ncov = 1;
end
if nargin <= 3
    t = 1;
end
A = steering_matrix(design, wavelength, doas);
[m, k] = size(A);
S_internal = gen_ccsg(k, t, scov);
X = A * S_internal + gen_ccsg(m, t, ncov);
if nargout >= 2
    R = (X*X')/t;
    if nargout == 3
        S = S_internal;
    end
end
end

function X = gen_ccsg(m, n, cov)
X0 = randn(m, n) + 1j * randn(m, n);
if isscalar(cov)
    X = sqrt(cov/2) * X0;
elseif isvector(cov)
    X = bsxfun(@times, X0, cov(:));
else
    C = sqrtm(cov/2);
    X = C*X0;
end
end


function [GAmp,GTime]=GySpiral(p)

global VCtl;
global VObj;
global VVar;

tStart=p.tStart;
dt = p.dt;

% 2D spiral encoding
FOV = VCtl.FOVFreq; % choose FOVFreq as FOV
Res = VCtl.ResFreq; % choose ResFreq as effective resolution

KMax  = Res/ (2*FOV);
ThetaMax = KMax / VCtl.S_Lamda;
Beta = (VObj.Gyro/(2*pi))*(VCtl.S_SlewRate / VCtl.S_Lamda);
a2 = (9*Beta/4)^(1/3);
CLamda = VCtl.S_SlewRate/VCtl.S_SlewRate0;
Ts = ((3*VObj.Gyro*VCtl.S_Gradient)/(4*pi*VCtl.S_Lamda*a2^2))^3;

ThetaTs = (0.5 * Beta * Ts^2)/(CLamda + (Beta/(2 * a2)) * Ts^(4/3));
if ThetaTs < ThetaMax
    t1 = 0:dt:Ts;
    Theta1 = (0.5 * Beta * t1.^2)./(CLamda + (Beta/(2 * a2)) * t1.^(4/3));

    Tacq = t1(end) + ((pi*VCtl.S_Lamda)/(VObj.Gyro * VCtl.S_Gradient))*(ThetaMax^2 - Theta1(end)^2);
    t2 = t1(end):dt:Tacq;
    Theta2 = sqrt(Theta1(end)^2 + (VObj.Gyro / (pi * VCtl.S_Lamda))*VCtl.S_Gradient*(t2 - t1(end)));

    Theta = [Theta1(1:end-1) Theta2];
    t     = [t1(1:end-1)     t2];

else
    Tacq = ((2*pi*FOV)/(3*VCtl.S_ShotNum))*sqrt(pi/(VObj.Gyro*VCtl.S_SlewRate*VCtl.RFreq^3));
    t1 = 0:dt:Tacq;
    Theta1 = (0.5 * Beta * t1.^2)./(CLamda + (Beta/(2 * a2)) * t1.^(4/3));

    Theta = Theta1;
    t     = t1;
end

DTheta = [0 diff(Theta)./diff(t)];

GAmp = ((2*pi)/(VObj.Gyro))*VCtl.S_Lamda*DTheta.*(sin(Theta + (VVar.PhaseCount-1)*(2*pi/VCtl.S_ShotNum)) ...
                                         + Theta.*cos(Theta + (VVar.PhaseCount-1)*(2*pi/VCtl.S_ShotNum)));
GTime = tStart + VCtl.TEAnchorTime + t;

GTime = [GTime GTime(end) + max(VCtl.MinUpdRate,abs(GAmp(end)/VCtl.S_SlewRate))];
GAmp = [GAmp 0];

[GTime,m,n]=unique(GTime);
GAmp=GAmp(m);

end


function R = AngleAxis2RotationMatrix(angle_axis)

theta2 = dot(angle_axis,angle_axis);
if (theta2 > 0.0)
    % We want to be careful to only evaluate the square root if the
    % norm of the angle_axis vector is greater than zero. Otherwise
    % we get a division by zero.

    theta = sqrt(theta2);
    wx = angle_axis(1) / theta;
    wy = angle_axis(2) / theta;
    wz = angle_axis(3) / theta;

    costheta = cos(theta);
    sintheta = sin(theta);

    R(1+0, 1+0) =     costheta   + wx*wx*(1 -    costheta);
    R(1+1, 1+0) =  wz*sintheta   + wx*wy*(1 -    costheta);
    R(1+2, 1+0) = -wy*sintheta   + wx*wz*(1 -    costheta);
    R(1+0, 1+1) =  wx*wy*(1 - costheta)     - wz*sintheta;
    R(1+1, 1+1) =     costheta   + wy*wy*(1 -    costheta);
    R(1+2, 1+1) =  wx*sintheta   + wy*wz*(1 -    costheta);
    R(1+0, 1+2) =  wy*sintheta   + wx*wz*(1 -    costheta);
    R(1+1, 1+2) = -wx*sintheta   + wy*wz*(1 -    costheta);
    R(1+2, 1+2) =     costheta   + wz*wz*(1 -    costheta);
else
    % At zero, we switch to using the first order Taylor expansion.
    R(1+0, 1+0) =  1;
    R(1+1, 1+0) = -angle_axis(3);
    R(1+2, 1+0) =  angle_axis(2);
    R(1+0, 1+1) =  angle_axis(3);
    R(1+1, 1+1) =  1;
    R(1+2, 1+1) = -angle_axis(1);
    R(1+0, 1+2) = -angle_axis(2);
    R(1+1, 1+2) =  angle_axis(1);
    R(1+2, 1+2) = 1;
end


function basis_11_t6_test ( )

%*****************************************************************************80
%
%% BASIS_11_T6_TEST verifies BASIS_11_T6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    None
%
  node_num = 6;

  t = [ ...
    2.0, 0.0; ...
    4.0, 3.0; ...
    0.0, 4.0; ...
    3.0, 1.5; ...
    2.0, 3.5; ...
    1.0, 2.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BASIS_11_T6_TEST:\n' );
  fprintf ( 1, '  Verify basis functions for element T6.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes = %d\n', node_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Physical Nodes:\n' );
  fprintf ( 1, '\n' );
  for j = 1 : node_num
    fprintf ( 1, '  %8d  %7f  %7f\n', j, t(1:2,j) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The basis function values at basis nodes\n' );
  fprintf ( 1, '  should form the identity matrix.\n' );
  fprintf ( 1, '\n' );

  for i = 1 : node_num
    for j = 1 : node_num
      [ phi(i,j), dphidx(i,j), dphidy(i,j) ] = basis_11_t6 ( t, i, t(1:2,j) );
    end
  end

  for i = 1 : node_num
    for ( j = 1 : node_num )
      fprintf ( 1, '  %7f', phi(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The X and Y derivatives should sum to 0.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      dPhidX sum    dPhidY sum\n' );
  fprintf ( 1, '\n' );
  for j = 1 : node_num
    sum_x = sum ( dphidx(1:node_num,j) );
    sum_y = sum ( dphidy(1:node_num,j) );
    fprintf ( 1, '  %14f  %14f\n', sum_x, sum_y );
  end

  return
end


function nz_num = wathen_st_size ( nx, ny )

%*****************************************************************************80
%
%% WATHEN_ST_SIZE: Size of Wathen matrix stored in sparse triplet format.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 June 2014
%
%  Author:
%
%    John Burkardt.
%
%  Reference:
%
%    Nicholas Higham,
%    Algorithm 694: A Collection of Test Matrices in MATLAB,
%    ACM Transactions on Mathematical Software,
%    Volume 17, Number 3, September 1991, pages 289-305.
%
%    Andrew Wathen,
%    Realistic eigenvalue bounds for the Galerkin mass matrix,
%    IMA Journal of Numerical Analysis,
%    Volume 7, Number 4, October 1987, pages 449-457.
%
%  Parameters:
%
%    Input, integer NX, NY, values which determine the size of the matrix.
%
%    Output, integer NZ_NUM, the number of items of data used to describe
%    the matrix.
%
  nz_num = nx * ny * 64;

  return
end


function log_L = Kalman_Estimation(y, psi, matur, dt, a0, P0, N, nobs, locked_parameters)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Extracting initial parameter values from initial psi
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k = psi(1,1);
sigmax = psi(2,1);
lambdax = psi(3,1);
mu = psi(4,1);
sigmae = psi(5,1);
rnmu = psi(6,1);
pxe = psi(7,1);

if sum(locked_parameters) == 0
    k = psi(1,1);
    sigmax = psi(2,1);
    lambdax = psi(3,1);
    mu = psi(4,1);
    sigmae = psi(5,1);
    rnmu = psi(6,1);
    pxe = psi(7,1);

    s = zeros(1, size(psi,1)-7);
    for i = 1:size(s,2)
        s(1, i) = psi(i+7,1);
    end
end

if sum(locked_parameters) ~= 0
    s = zeros(1, size(psi,1)-7+size(locked_parameters,1));
    j = 1;
    for i = 1:size(s,2)
        if all(abs(i-(locked_parameters))) == 1
             s(1, i) = psi(7+j,1);
             j = j+1;
        end
    end
end

% m = Number of state variables (number of rows in a0)
m = size(a0,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% THE TRANSITION EQUATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% S&S NOTATION: x(t)=c+G*x(t-1)+w(t)        w~N(0,W)    Equation (14)
% NEW NOTATION: a(t)=c+T*a(t-1)+R(t)*n(t)   n~N(0,Q)

% c is a {m x 1} Vector
% T is a {m x m} Matrix
c=[0;mu*dt];
T=[exp(-k*dt),0;0,1];

% Defining Q = var[n(t)] and R
xx=(1-exp(-2*k*dt))*(sigmax)^2/(2*k);
xy=(1-exp(-k*dt))*pxe*sigmax*sigmae/k;
yx=(1-exp(-k*dt))*pxe*sigmax*sigmae/k;
yy=(sigmae)^2*dt;
Q=[xx,xy;yx,yy];
R=eye(size(Q,1));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% THE MEASUREMENT EQUATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% S&S NOTATION: y(t)=d(t)+F(t)'x(t)+v(t)    v~N(0,V) Equation (15)
% NEW NOTATION: y(t)=d(t)+Z(t)a(t)+e(t)     e~N(0,H)

% d is a {N x 1} Vector
% Z is a {N x m} Matrix
    for i=1:N
        p1=(1-exp(-2*k*matur(i)))*(sigmax)^2/(2*k);
        p2=(sigmae)^2*matur(i);
        p3=2*(1-exp(-k*matur(i)))*pxe*sigmax*sigmae/k;
        d(i,1)=rnmu*matur(i)-(1-exp(-k*matur(i)))*lambdax/k+.5*(p1+p2+p3);
        Z(i,1)=exp(-k*matur(i));
        Z(i,2)=1;
    end

% Measurment errors Var-Cov Matrix: Cov[e(t)]=H
H=diag(s);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% RUNNING THE KALMAN FILTER
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Creating placeholder vectors/matrices for variables to be stored in
global save_vt save_att save_dFtt_1 save_vFv save_vtt save_Ptt_1 save_Ftt_1 save_Ptt

save_ytt_1 = zeros(nobs,N);
save_vtt = zeros(nobs,N);
save_vt    = zeros(nobs,N);
save_att_1 = zeros(nobs,m);
save_att   = zeros(nobs,m);
save_Ptt_1 = zeros(nobs,m*m);
save_Ptt   = zeros(nobs,m*m);
save_Ftt_1 = zeros(nobs,N*N);
save_dFtt_1 = zeros(nobs,1);
save_vFv    = zeros(nobs,1);
%save_log_Lt   = zeros(nobs,1);

Ptt = P0;
att = a0;

% Running the kalman filter for t = 1,...,nobs
    for t = 1:nobs
        Ptt_1   = T*Ptt*T'+R*Q*R';
        Ftt_1   = Z*Ptt_1*Z'+H;
        dFtt_1  = det(Ftt_1);

        %Ptt_1_test = [Ptt_1(1,1) 0; 0 Ptt_1(2,2)];
        %Ftt_1_test   = Z*Ptt_1_test*Z'+H;
        %dFtt_1_test  = det(Ftt_1_test);

        att_1   = T*att + c;
        yt      = y(t,:)';
        ytt_1   = Z*att_1+d;
        vt      = yt-ytt_1;

        att = att_1 + Ptt_1*Z'*inv(Ftt_1)*(vt);
        Ptt = Ptt_1 - Ptt_1*Z'*inv(Ftt_1)*Z*Ptt_1;

        ytt = Z*att+d;
        vtt  = yt-ytt;

        % save_ytt_1(t,:) = ytt_1';
        save_vtt(t,:) = vtt';
        save_vt(t,:)    = (vt)';
        % save_att_1(t,:) = att_1';
        save_att(t,:)   = att';
        save_Ptt_1(t,:) = [Ptt_1(1,1), Ptt_1(1,2), Ptt_1(2,1), Ptt_1(2,2)];
        save_Ptt(t,:)   = [Ptt(1,1), Ptt(1,2), Ptt(2,1), Ptt(2,2)];
        % save_Ftt_1(t,:) = [Ftt_1(1,1), Ftt_1(1,2), Ftt_1(1,3), Ftt_1(1,4), Ftt_1(1,5), Ftt_1(2,1), Ftt_1(2,2), Ftt_1(2,3), Ftt_1(2,4), Ftt_1(2,5), Ftt_1(3,1), Ftt_1(3,2), Ftt_1(3,3), Ftt_1(3,4), Ftt_1(3,5), Ftt_1(4,1), Ftt_1(4,2), Ftt_1(4,3), Ftt_1(4,4), Ftt_1(5,5), Ftt_1(5,1), Ftt_1(5,2), Ftt_1(5,3), Ftt_1(5,4), Ftt_1(5,5)];

        %save_dFtt_1(t,:)= dFtt_1_test;
        %save_vFv(t,:)   = vt'*inv(Ftt_1_test)*vt;
        save_dFtt_1(t,:)= dFtt_1;
        save_vFv(t,:)   = vt'*inv(Ftt_1)*vt;

    end

logL = -(N*nobs/2)*log(2*pi)-0.5*sum(log(save_dFtt_1))-0.5*sum(save_vFv);
% logL = -(N*nobs/2)*log(2*pi)-0.5*sum(save_vFv);
% logL = sum(diag(save_vt'*save_vt));
log_L = -logL;


function [out] = soilmoisture_1(S1,S1max,S2,S2max)
%soilmoisture_1

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Water rebalance to equal relative storage (2 stores)
% Constraints:  -
% @(Inputs):    S1    - current storage in S1 [mm]
%               S1max - maximum storage in S1 [mm]
%               S2    - current storage in S2 [mm]
%               S2max - maximum storage in S2 [mm]

out = ((S2.*S1max-S1.*S2max)/(S1max+S2max)).*smoothThreshold_storage_logistic(S1./S1max,S2./S2max);

end


function [f,G,H] = self_collision_barrier(V,E,tol)
  % SELF_COLLISION_BARRIER Compute the a barrier function (and its derivatives)
  % for point-edge collisions of a given *strictly feasible** line-complex in
  % 2D. See "Bijective Parameterization with Free Boundaries" [Smith and
  % Schaefer 2015] or for a similar barrier "Incremental Potential Contact:
  % Intersection- and Inversion-free, Large-Deformation Dynamics" [Li et al.
  % 2020].
  %
  % [f,G,H] = self_collision_barrier(V,E,tol)
  %
  % Inputs:
  %   V  #V by 2 list of input vertex positions
  %   E  #E by 2 list of edge indices into rows of V
  %   tol  distance at which barrier term becomes positive {1e-3}
  % Outputs:
  %   f  scalar total objective value
  %   G  #V*2 gradient
  %   H  #V*2 by #V*2 sparse Hessian matrix
  %
  % Note: this function uses a special symbolic library trick which generates
  % files self_collision_barrier_cap_sym.m and self_collision_barrier_line_sym.m
  % if they don't already exist. If your change the symbolic-math part of this
  % file, you must delete these automatically generated files.
  %

  function [sqrD,T] = point_segment_squared_distance(P,A,B)
    PA = P-A;
    BA = B-A;
    T = min(max(sum(PA.*BA,2)./sum(BA.^2,2),0),1);
    % vector to closest point
    PC = PA-BA.*T;
    sqrD = sum(PC.^2,2);
  end

  function [f,G,H] = self_collision_barrier_cap(V,E,IJ)
    % vertex i colliding with edge jk

    n = size(V,1);
    % [x x x y y y]
    IJ = [IJ  n+IJ];
    X = V(IJ);

    %% Sanity check
    %p = X(:,1:2:end);
    %c = X(:,2:2:end);
    %pc = p-c;
    %d = sqrt(sum(pc.^2,2));
    %if ~isempty(d) && max(d) > tol && nargout > 1
    %  error
    %end

    % Writing the symbol
    path = mfilename('fullpath');
    aux = [path '_cap_sym.m'];
    % should also check date...
    if ~exist(aux,'file')
      % From here, only touch X
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      sX = sym('X',[1 4]);
      stol = sym('tol',[1 1]);
      sp = sX(1:2:end);
      sc = sX(2:2:end);
      spc = sp-sc;
      sd = sqrt(sum(spc.^2,2));
      sf = barrier(sd,stol);

      hess = @(sf,sX) cell2sym(arrayfun(@(g) gradient(g,sX),gradient(sf,sX),'UniformOutput',false));

      aux_handle = ...
        matlabFunction(sf,gradient(sf,sX),hess(sf,sX),'vars',{sX,stol},'File',aux);
    else
      aux_name = [mfilename('func') '_cap_sym'];
      aux_handle = str2func(aux_name);
    end

    faux=[];gaux = [];Haux = [];
    switch nargout
    case {0,1}
      [faux] = aux_handle(X,tol);
    case 2
      [faux,gaux] = aux_handle(X,tol);
    case 3
      [faux,gaux,Haux] = aux_handle(X,tol);
    end

    % unnecessary indirection?
    f_fun = @(X) faux;
    dfdX_fun = @(X) gaux;
    d2fdX2_fun = @(X) Haux;
    f = sum(f_fun(X));
    if nargout<=1
      return;
    end
    dfdX = dfdX_fun(X);
    G = full_sparse(IJ,ones(size(IJ)),reshape(dfdX,size(IJ)),2*n,1);
    if nargout<=2
      return;
    end
    d2fdX2 = double(d2fdX2_fun(X));

    d2fdX2 = reshape(d2fdX2,[],4*4);
    if psd_project
      d2fdX2 = psd_project_rows(d2fdX2);
    end

    HI = repmat(IJ,[1 1 size(IJ,2)]);
    HJ = permute(repmat(IJ,[1 1 size(IJ,2)]),[1 3 2]);
    H = fast_sparse(HI(:),HJ(:),d2fdX2(:),2*n,2*n);
  end

  function [f,G,H] = self_collision_barrier_line(V,E,IJK)

    n = size(V,1);
    % [x x x y y y]
    IJK = [IJK n+[IJK]];
    X = V(IJK);

    %% Sanity check
    %p = X(:,1:3:end);
    %a = X(:,2:3:end);
    %b = X(:,3:3:end);
    %pa = p-a;
    %ba = b-a;
    %t = sum(pa.*ba,2)./sum(ba.^2,2);
    %pc = pa - ba.*t;
    %d = sqrt(sum(pc.^2,2));
    %if ~isempty(d) && max(d) > tol && nargout > 1
    %  error
    %end

    % Writing the symbol
    path = mfilename('fullpath');
    aux = [path '_line_sym.m'];
    % should also check date...
    if ~exist(aux,'file')
      % From here, only touch X
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      sX = sym('X',[1 6]);
      stol = sym('tol',[1 1]);
      sp = sX(1:3:end);
      sa = sX(2:3:end);
      sb = sX(3:3:end);
      spa = sp-sa;
      sba = sb-sa;
      st = sum(spa.*sba,2)./sum(sba.^2,2);
      spc = spa - sba.*st;
      sd = sqrt(sum(spc.^2,2));
      sf = barrier(sd,stol);

      hess = @(sf,sX) cell2sym(arrayfun(@(g) gradient(g,sX),gradient(sf,sX),'UniformOutput',false));
      aux_handle = ...
        matlabFunction(sf,gradient(sf,sX),hess(sf,sX),'vars',{sX,stol},'File',aux);
    else
      aux_name = [mfilename('func') '_line_sym'];
      aux_handle = str2func(aux_name);
    end

    faux=[];gaux = [];Haux = [];
    switch nargout
    case {0,1}
      [faux] = aux_handle(X,tol);
    case 2
      [faux,gaux] = aux_handle(X,tol);
    case 3
      [faux,gaux,Haux] = aux_handle(X,tol);
    end

    % unnecessary indirection?
    f_fun = @(X) faux;
    dfdX_fun = @(X) gaux;
    d2fdX2_fun = @(X) Haux;
    f = sum(f_fun(X));
    if nargout<=1
      return;
    end
    dfdX = dfdX_fun(X);
    G = full_sparse(IJK,ones(size(IJK)),reshape(dfdX,size(IJK)),2*n,1);
    if nargout<=2
      return;
    end
    d2fdX2 = double(d2fdX2_fun(X));

    d2fdX2 = reshape(d2fdX2,[],6*6);
    if psd_project
      d2fdX2 = psd_project_rows(d2fdX2);
    end

    HI = repmat(IJK,[1 1 size(IJK,2)]);
    HJ = permute(repmat(IJK,[1 1 size(IJK,2)]),[1 3 2]);
    H = fast_sparse(HI(:),HJ(:),d2fdX2(:),2*n,2*n);

  end

  psd_project = true;
  % The max is not needed because we're handling that explicitly
  %
  % Smith and Schaefer
  %barrier = @(d,tol) (tol./d - 1).^2;
  % IPC
  barrier = @(D,tol) -(D-tol).^2.*log(D./tol);

  b = unique(E);
  [b1,b2] = box_each_element(V,b);
  [E1,E2] = box_each_element(V,E);
  I = box_intersect(b1-tol,b2+tol,E1-tol,E2+tol);
  I = I(~any(b(I(:,1))==E(I(:,2),:),2),:);
  BC = barycenter(V,E);

  [sqrD,T] = point_segment_squared_distance(V(b(I(:,1)),:),V(E(I(:,2),1),:),V(E(I(:,2),2),:));

  keep = find(sqrD<tol^2);

  I = I(keep,:);
  T = T(keep);

  %BC = barycenter(V,E);
  %plot_edges(V,E,'-ok');
  %hold on;
  %sct(V(b(I(:,1)),:),'or','LineWidth',2);
  %I(:,2)
  %size(E)
  %sct(BC(I(:,2),:),'og','LineWidth',2);
  %hold off;

  cap = T==0 | T==1;
  IJKline = [b(I(~cap,1)) E(I(~cap,2),:)];

  sqrD = sqrD(keep);

  Icap = I(cap,:);
  Tcap = T(cap);
  Jcap = E(Icap(:,2),1);
  Jcap(Tcap==1) = E(Icap(Tcap==1,2),2);
  IJcap = unique(sort([b(Icap(:,1)) Jcap],2),'rows');

  switch nargout
  case {0,1}
    [fline] = self_collision_barrier_line(V,E,IJKline);
    [fcap] = self_collision_barrier_cap(V,E,IJcap);
    f = fline + fcap;
  case 2
    [fline,Gline] = self_collision_barrier_line(V,E,IJKline);
    [fcap,Gcap] = self_collision_barrier_cap(V,E,IJcap);
    f = fline + fcap;
    G = Gline + Gcap;
  case 3
    [fline,Gline,Hline] = self_collision_barrier_line(V,E,IJKline);
    [fcap,Gcap,Hcap] = self_collision_barrier_cap(V,E,IJcap);
    f = fline + fcap;
    G = Gline + Gcap;
    H = Hline + Hcap;
  end

end


function [Psi, Astats, Kstats] = sampleHMMhypers( Psi, algParams )
% Sample hyperparameters (alpha,kappa) that control HMM transition distr.
% Uses a Metropolis-Hastings random walk proposal centered on current state
% INPUT:
%   Psi : input Markov Chain state
% OUTPUT
%   Psi : new Markov chain state
% DETAILS:
%  Given hyperparams alpha (non-sticky) and kappa (sticky) params,
%   each sequence draws transition probabilities as follows:
%     eta(jj,:) ~ Gamma( alpha, alpha, ... alpha+kappa, alpha, alpha )
%     pi(jj,:) = eta(jj,:)/sum( eta(jj,:)

%------------------------ Unpack
F = Psi.F;
[N,K] = size(F);
alpha0 = Psi.TransM.prior.alpha;
kappa0 = Psi.TransM.prior.kappa;
TransM   = Psi.TransM;

% Hyperparameters for prior on kappa:
a_kappa = TransM.prior.a_kappa;
b_kappa = TransM.prior.b_kappa;
% Variance of gamma proposal:
var_kappa = algParams.HMM.var_kappa;

% Hyperparameters for prior on alpha:
a_alpha = TransM.prior.a_alpha;
b_alpha = TransM.prior.b_alpha;
% Variance of gamma proposal:
var_alpha = algParams.HMM.var_alpha;

% Build sufficient stats
%    Ki( ii ) : # total features on for object ii
%    Skk( ii ) : sum( log(   diag entries of Pz(ii)  )  )
%    Sall( ii )    : sum( log(   all entries of Pz(ii)  ) )
Ki = zeros(1,N);
Skk = zeros(1,N);
Sall = zeros(1,N);
for ii=1:N
    Ki(ii) = sum(F(ii,:));
    pi_ii = TransM.pi( ii );

    Skk( ii ) = sum( log( diag( pi_ii ) ) );
    Sall( ii ) = sum( log(  pi_ii(:) ) );
end

Astats.nAccept = 0;
Astats.nTotal   = algParams.HMM.Niter;

Kstats.nAccept = 0;
Kstats.nTotal   = algParams.HMM.Niter;

for nn=1:algParams.HMM.Niter

    %%%%%%% Sample kappa given alpha %%%%%%%

    % (a,b) hyperparameters of gamma prior based on fixed variance and setting
    % mean equal to previous kappa value:
    aa_kappa0 = (kappa0^2)/var_kappa;
    bb_kappa0 = kappa0/var_kappa;

    % Sample a proposed kappa:
    kappaP = randgamma(aa_kappa0) / bb_kappa0;

    % Determine log-likelihood of transition distributions given previous kappa
    % value and proposed kappa value:
    log_diff_Z = sum( ...
              Ki .* ( gammaln(alpha0*Ki+kappaP) - gammaln(alpha0*Ki+kappa0) )...
            - Ki .* ( gammaln(alpha0+kappaP) - gammaln(alpha0+kappa0)) ...
                            + (kappaP-kappa0)*Skk ...
                    );
    % Add in prior probability of previous and proposed kappa values:
    log_diff_Prior = (a_kappa-1)*(log(kappaP)-log(kappa0))-(kappaP-kappa0)*b_kappa;

    % (a,b) hyperparameters of gamma prior based on fixed variance and setting
    % mean equal to proposed kappa value:
    aa_kappaP = (kappaP^2)/var_kappa;

    log_diff_Q  = (gammaln(aa_kappa0) - gammaln(aa_kappaP))...
                  + (aa_kappaP-aa_kappa0-1)*log(kappa0) - (aa_kappa0-aa_kappaP-1)*log(kappaP)...
                  + (aa_kappa0-aa_kappaP)*log(var_kappa);

    % Log accept-reject ratio:
    log_rho = log_diff_Z + log_diff_Prior + log_diff_Q;

    if isinf(log_rho)
        log_rho = -Inf;
    end
    rho = exp(log_rho);

    if rand < rho
        kappa0 = kappaP;
        Kstats.nAccept = Kstats.nAccept + 1;
    end
    % otherwise, just keep kappa0 to current value

    %%%%%%% Sample alpha given kappa %%%%%%%

    % (a,b) hyperparameters of gamma prior based on fixed variance and setting
    % mean equal to previous alpha value:
    aa_alpha0 = (alpha0^2)/var_alpha;
    bb_alpha0 = alpha0/var_alpha;

    % Sample a proposed alpha:
    alphaP = randgamma(aa_alpha0) / bb_alpha0;

    % Determine log-likelihood of transition distributions given previous alpha
    % value and proposed alpha value:
    log_diff_Z = sum( ...
                          Ki .* ( gammaln( alphaP*Ki + kappa0 ) - gammaln( alpha0*Ki + kappa0 )) ...
                        - Ki .* ( gammaln( alphaP+kappa0 )      - gammaln( alpha0+kappa0 ) ) ...
                        - Ki .* (Ki-1) .* ( gammaln( alphaP ) - gammaln( alpha0 )  ) ...
                        + ( alphaP - alpha0 ) .* Sall ...
                      );

    % Add in prior probability of previous and proposed alpha values:
    log_diff_Prior = (a_alpha-1)*(log(alphaP)-log(alpha0))-(alphaP-alpha0)*b_alpha;

    % (a,b) hyperparameters of gamma prior based on fixed variance and setting
    % mean equal to proposed kappa value:
    aa_alphaP = (alphaP^2)/var_alpha;
    %bb_alpha = alphaP/var_alpha; % seems to be unused

    log_diff_Q = (gammaln(aa_alpha0) - gammaln(aa_alphaP))...
        + (aa_alphaP-aa_alpha0-1)*log(alpha0) - (aa_alpha0-aa_alphaP-1)*log(alphaP)...
        + (aa_alpha0-aa_alphaP)*log(var_alpha);

    % Log accept-reject ratio:
    log_rho = log_diff_Z + log_diff_Prior + log_diff_Q;

    if isinf(log_rho)
        log_rho = -Inf;
    end
    rho = exp(log_rho);

    if rand < rho
        alpha0 = alphaP;
        Astats.nAccept = Astats.nAccept + 1;
    end
    % otherwise, just keep previous value of alpha0

end % end iterative loop over params

% -------------------------------  Repack
Psi.TransM.prior.alpha = alpha0;
Psi.TransM.prior.kappa = kappa0;


function C = mrdivide(A,B)
%MRDIVIDE     Long division A / B
%

% written  12/30/98     S.M. Rump
% modified 10/22/99     S.M. Rump  improvement of error if 1/B exact in few digits
% modfied  02/09/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 01/29/10     S.M. Rump  midpoint corrected, thanks to Nozomu Matsuda and Nobito Yamamoto
% modified 06/09/10     S.M. Rump  division w/o error term, thanks to Nozomu Matsuda
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  INTLAB_LONG_BETA = getappdata(0,'INTLAB_LONG_BETA');
  INTLAB_LONG_LOGBETA = getappdata(0,'INTLAB_LONG_LOGBETA');
  INTLAB_LONG_ERROR = getappdata(0,'INTLAB_LONG_ERROR');
  INTLAB_LONG_PRECISION = getappdata(0,'INTLAB_LONG_PRECISION');

  A = long(A);

  if isa(B,'double') & all( B==round(B) ) & all( abs(B)<INTLAB_LONG_BETA )

    % check zero denominator
    if any( B==0 )
      error('long division by zero')
    end

    % Division by integer
    C = A;
    C.sign = C.sign .* sign(B);
    B = abs(B);

    % Compute mantissa
    precA = size(A.mantissa,2);
    C.mantissa(:,1) = floor( A.mantissa(:,1)./B );
    for i=2:precA
      A.mantissa(:,i) = A.mantissa(:,i) + ...
         ( A.mantissa(:,i-1) - B.*C.mantissa(:,i-1) )*INTLAB_LONG_BETA ;
      C.mantissa(:,i) = floor( A.mantissa(:,i)./B );
    end

    % If remainder nonzero, continue division
    rem = A.mantissa(:,precA) - B.*C.mantissa(:,precA);
    if any( rem )
      C.mantissa = ...
         [ C.mantissa zeros(size(C.mantissa,1),INTLAB_LONG_PRECISION-precA+1) ];
      i = precA;
      while any( rem ) & ( i<INTLAB_LONG_PRECISION+1 )
        i = i+1;
        num = rem*INTLAB_LONG_BETA;
        C.mantissa(:,i) = floor( num./B );
        rem = num - B.*C.mantissa(:,i);
      end
    end

    % Omit trailing zeros
    if i<INTLAB_LONG_PRECISION
      C.mantissa = C.mantissa(:,1:i);
    end

    % Compute error
    if INTLAB_LONG_ERROR
      precC = size(C.mantissa,2);
      C.error = errorupdate( -B , C.error , 0 , 1 , rem~=0 , C.exponent-precC );
    end

    % Omit leading zeros
    C = omitleadingzeros(C);

    % Normalize result
    C = normalize(C);
    if INTLAB_LONG_ERROR
      C.error = errornormalize(C.error);
    end

  else   % long division

    % denominator with one extra digit precision
    INTLAB_LONG_PRECISION = INTLAB_LONG_PRECISION + 1;
    B = long(B);
    if INTLAB_LONG_ERROR
      Bmid = mid(B);
      % denominator offset rad(B)^2/mid(B) in double
      N = long2intval(rad(B)).^2./long2intval(Bmid);
      C = addlongerror(Bmid - mid(N),rad(N));     % to be inverted
    else
      Bmid = B;
      C = Bmid;
    end
    precC = size(C.mantissa,2);

    % check zero denominators
    if any( C.mantissa(:,1)==0 )
      error('long division by zero')
    end

    % approximation of inverse for Newton iteration, approx. 52 bits accuracy
    Cinv = C;
    Cinv.exponent = 0;
    Cinv = long(1./long2dble(Cinv));    % inverse of mid(C)
    Cinv.exponent = Cinv.exponent - C.exponent;

    % approximate inverse without error term
    err = longinit('errorterm',0);
    longinit('WithoutErrorTerm',0);

    while 1
      Res = 2 - Cinv*C;
      % check Res==1
      if all( Res.mantissa(:,1)==1 ) & all(all( Res.mantissa(:,2:end)==0 ))
        break
      end
      Cinv = Cinv * Res;
    end

    % reset precision and error term, Cinv~1/C
    longinit(err,0);
    INTLAB_LONG_PRECISION = INTLAB_LONG_PRECISION - 1;

    if INTLAB_LONG_ERROR
      % calculate error
      % error by inversion:  Res_/C  with
      %   Res_ < beta^(1-precRes),   1/C >= 1/C_1*beta^(1-C.exponent)
      % even Res < beta^(-INTLAB_LONG_PRECISION) because Newton iteration
      %   stopped with Res==1 in precision INTLAB_LONG_PRECISION+1; this
      %   is important if 1/C is exactly representable in fewer than
      %   INTLAB_LONG_PRECISION+1 beta-digits (thanks to Kurt Zehetleitner)
      precCinv = size(Cinv.mantissa,2);
      Cmant = C.mantissa(:,1);
      if precC>1
        Cmant = Cmant + C.mantissa(:,2)/INTLAB_LONG_BETA;
      end
      Cinv.error = ...
           errorupdate( -Cmant , 1 , 1-C.exponent-INTLAB_LONG_PRECISION );

      % error introduced by B:  B.error/(sqr(B)-sqr(B.error))
      Bmant = B.mantissa(:,1);
      if size(B.mantissa,2)>1
        Bmant = Bmant + B.mantissa(:,2)/INTLAB_LONG_BETA;
      end
      setround(1)
      N = ( B.error.mant .* INTLAB_LONG_BETA.^(B.error.exp-B.exponent) ) .^ 2;
      setround(-1)
      N = ( Bmant/INTLAB_LONG_BETA ) .^ 2 - N;
      % true denominator  N*beta^(2*B.exponent)

      % check zero denominator (including error)
      if any( N<=0 )
        error('long division by zero')
      end

      % first part:   B.error/(sqr(B)-sqr(B.error))
      Cinv.error = errorupdate( 1 , Cinv.error , 0 , ...
                               -N , B.error , -2*B.exponent );
      Cinv.error = errornormalize(Cinv.error);
    end

    C = A*Cinv;

  end

  setround(rndold)