function I_out = vesselFilter(I_in, grid_spacing, scales, varargin)
%VESSELFILTER   Frangi's 3D vessel filter.
%
% DESCRIPTION:
%       vesselFilter filters a 3D image using Frangi's vessel filtering
%       algorithm [1-3]. The algorithm works by calculating the Hessian
%       matrix (containing second order gradients) at each image voxel. The
%       eigenvalues of this matrix are then ordered and used to classify
%       whether the voxel is part of a vessel. The algorithm is performed
%       over multiple scales by convolving the input image with a Gaussian
%       of the given input scales. The final output is taken as the maximum
%       of the vessel filtered image across all scales.
%
% USAGE:
%       I_out = vesselFilter(I_in, grid_spacing, scales)
%       I_out = vesselFilter(I_in, grid_spacing, scales, ...)
%
% INPUTS:
%       I_in         - 3D image data to filter
%       grid_spacing - [dx, dy, dz] in [m]
%       scales       - array of scales to use [m]
%
% OPTIONAL INPUTS:
%       Optional 'string', value pairs that may be used to modify the
%       default computational settings.
%
%       alpha        - Value of sensitivity parameter for metric that
%                      distinguishes between plate-like and other
%                      structures (vessel-like or ball-like).
%                      (default = 0.5)
%       beta         - Value of sensitivity parameter for metric that
%                      distinguishes between ball-like and other structures
%                      (vessel-like or plate-like).
%                      (default = 0.5)
%       c            - Value used to scale the sensitivity parameter for
%                      the noise metric (the sensitivity parameter itself
%                      is calculated automatically based on the magnitudes
%                      of the eigenvalues).
%                      (default = 1)
%       gamma        - normalisation factor for scale-space derivatives
%                      (default = 1)
%       DisplayUpdates - Boolean controlling whether command line updates
%                      are displayed (default = true)
%       Plot         - Boolean controlling whether a maximum intensity
%                      projection of the vessel filtered image at each
%                      scale is displayed (default = false)
%       Colormap     - colormap to use if 'Plot' is set to true (default =
%                      flipud(gray))
%
% OUTPUTS:
%       I_out        - vessel filtered image
%
% ABOUT:
%       author       - Bradley Treeby and Tanmayi Oruganti
%       date         - 9th May 2012
%       last update  - 21st August 2014
%
%       This function is based on original code by Dean Barratt and Yipeng
%       Hu, CMIC, UCL, 2006-2009
%
% REFERENCES:
%   [1] A. F. Frangi, W. J. Niessen, K. L. Vincken, M. A. Viergever (1998)
%       "Multiscale vessel enhancement filtering," MICCAI, pp. 130-137.
%   [2] R. Manniesing, M.A. Viergever, W.J. Niessen (2006) "Vessel
%       enhancing diffusion: A scale space representation of vessel
%       structures," Med. Image Anal. 10, pp. 815-25.
%   [3] T. Oruganti, J. Laufer, B. E. Treeby (2013) "Vessel filtering of
%       photoacoustic images," Proc. of SPIE, vol. 8581, p. 85811W-1.
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox
%
% See also interpftn, smooth

% This file is part of k-Wave. k-Wave is free software: you can
% redistribute it and/or modify it under the terms of the GNU Lesser
% General Public License as published by the Free Software Foundation,
% either version 3 of the License, or (at your option) any later version.
%
% k-Wave 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 Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% start timer
start_time = clock;

% set default parameters
num_req_input_variables = 3;    % minimum number of input variables
alpha = 0.5;                    % value of sensitivity parameter for plate vs vessel/ball measure
beta = 0.5;                     % value of sensitivity parameter for ball vs vessel/plate measure
c_scale = 1;                    % scale value of sensitivity parameter for noise
gamma = 0;                      % normalisation factor for scale-space derivatives
origin_smoothness_const = 1e-6; % constant for Manniesing's origin smoothness term
disp_updates = true;            % boolean controlling display of command line updates
plot_updates = false;           % boolean controlling MIP display
color_map = flipud(gray);       % colormap to use if images are displayed
cardan_tol = 0.001;             % tolerance for finding complex roots using cardanRoots

% check user defined inputs
% replace with user defined values if provided
if nargin < num_req_input_variables
    error('Incorrect number of inputs');
elseif ~isempty(varargin)
    for input_index = 1:2:length(varargin)
        switch varargin{input_index}
            case 'alpha'
                alpha = varargin{input_index + 1};
            case 'beta'
                beta = varargin{input_index + 1};
            case 'c'
                c_scale = varargin{input_index + 1};
            case 'gamma'
                gamma = varargin{input_index + 1};
            case 'Display'
                disp_updates = varargin{input_index + 1};
            case 'DisplayUpdates'
                disp_updates = varargin{input_index + 1};
            case 'Plot'
                plot_updates = varargin{input_index + 1};
            case 'Colormap'
                color_map = varargin{input_index + 1};
            otherwise
                error('Unknown optional input');
        end
    end
end

% get size of input volume
sz = size(I_in);

% create k-space grid to use for spectral derivatives
kgrid = makeGrid(sz(1), grid_spacing(1), sz(2), grid_spacing(2), sz(3), grid_spacing(3));

% force wavenumber vectors to be in the correct directions for bsxfun
kx_vec = kgrid.kx_vec;
ky_vec = kgrid.ky_vec.';
kz_vec = permute(kgrid.kz_vec, [2 3 1]);

% compute 3D fft of input image
Ik = fftn(I_in);

% preallocate empty output image
I_out = zeros(size(I_in));

% loop through the scales defined by the user
for scale_index = 1:length(scales)

    % extract the current scale
    scale = scales(scale_index);

    % update command line status
    if disp_updates
        disp(['Computing Vesselness filter for scale ' num2str(scale) ':']);
        tic, fprintf('  Calculating derivatives... ');
    end

    % create normalised frequency domain gaussian at current scale
    FG = exp( (-0.5*scale^2)*(kgrid.kx.^2 + kgrid.ky.^2 + kgrid.kz.^2) );

    % compute components of Hessian matrix convolved with Gaussian
    Hxx = reshape(real(ifftn( Ik .* ifftshift( bsxfun(@times, (1i*kx_vec).^2, FG)) )), [], 1);
    Hyy = reshape(real(ifftn( Ik .* ifftshift( bsxfun(@times, (1i*ky_vec).^2, FG)) )), [], 1);
    Hzz = reshape(real(ifftn( Ik .* ifftshift( bsxfun(@times, (1i*kz_vec).^2, FG)) )), [], 1);
    Hxy = reshape(real(ifftn( Ik .* ifftshift( bsxfun(@times, (1i*kx_vec), bsxfun(@times, (1i*ky_vec), FG))) )), [], 1);
    Hxz = reshape(real(ifftn( Ik .* ifftshift( bsxfun(@times, (1i*kx_vec), bsxfun(@times, (1i*kz_vec), FG))) )), [], 1);
    Hyz = reshape(real(ifftn( Ik .* ifftshift( bsxfun(@times, (1i*ky_vec), bsxfun(@times, (1i*kz_vec), FG))) )), [], 1);

    % update command line status
    if disp_updates
        toc, tic, fprintf('  Computing eigenvalues... ');
    end

    % compute trace of Hessian matrix
    P2 = -(Hxx + Hyy + Hzz);

    % compute principal minors of Hessian matrix
    M11 = Hyy.*Hzz - Hyz.^2;
    M22 = Hzz.*Hxx - Hxz.^2;
    M33 = Hxx.*Hyy - Hxy.^2;
    P1 = (M11 + M22 + M33);

    % compute determinant of Hessian matrix
    P0 = - Hxx.*M11 ...
         + Hxy.*(Hxy.*Hzz - Hyz.*Hxz) ...
         - Hxz.*(Hxy.*Hyz - Hyy.*Hxz);

    % find eigenvalues using roots of 3rd order polynomial
    P3 = 1;
    eigenval_mat = cardanRoots(P3, P2, P1, P0, cardan_tol).';

    % update command line status
    if disp_updates
        toc, tic, fprintf('  Sorting eigenvalues... ');
    end

    % sort eigenvalues based on their absolute values
    [~, ind] = sort(abs(eigenval_mat));
    ind = bsxfun(@plus, (0:numel(I_in)-1)*3, ind);
    eigenval_mat = eigenval_mat(ind);

    % update command line status
    if disp_updates
        toc, tic, fprintf('  Evaluating vesselness measure... ');
    end

    % calculate noise parameter dynamically if not given
    Rs = sqrt( abs(eigenval_mat(1, :)).^2 + abs(eigenval_mat(2, :)).^2 + abs(eigenval_mat(3, :)).^2 );
    c = c_scale*0.5*max(Rs(:));

    % calculate overall vesselness measure and scale by Lindeberg?s constant
    %   term 1: plate vs vessel/ball measure
    %   term 2: ball vs vessel/plate measure
    %   term 3: noise measure (Frobenius matrix norm of the Hessian)
    %   term 4: Manniesing's origin smoothness term
    V = scale.^(gamma) .* ...
        (1 - exp(-(abs(eigenval_mat(2, :))./abs(eigenval_mat(3, :))).^2./(2*alpha^2))) .* ...
        (exp(-(abs(eigenval_mat(1, :))./sqrt(abs(eigenval_mat(2, :).*eigenval_mat(3, :)))).^2./(2*beta^2))) .* ...
        (1 - exp(-Rs.^2./(2*c^2))) .* ...
        (exp(-(2*origin_smoothness_const.^2)./(abs(eigenval_mat(2, :)).*eigenval_mat(3, :).^2)));

    % eliminate structures with positive eigenvalues as these indicate
    % regions of low optical absorption on a background of high optical
    % absorption
    V(eigenval_mat(2, :) > 0 | eigenval_mat(3, :) > 0) = 0;

    % reshape to be the correct size
    V = reshape(V, sz);

    % take maximum response over multiple scales
    I_out = max(I_out, V);

    % display time elapsed
    if disp_updates
        toc
    end

    % plot steps if required
    if plot_updates

        % get suitable axis scaling
        [~, axis_scale, axis_prefix] = scaleSI(max([sz(1)*grid_spacing(1), sz(2)*grid_spacing(2)]));

        % plot figure
        figure;
        imagesc(kgrid.y_vec * axis_scale, kgrid.x_vec * axis_scale,  max(V, [], 3));
        axis image;
        colormap(color_map);
        title(['Vessel Filtered Image (MIP), Scale = ' num2str(scale)]);
        ylabel(['x [' axis_prefix 'm]']);
        xlabel(['y [' axis_prefix 'm]']);
        colorbar;
        drawnow;

        % plot final multi-scale image
        if scale_index == length(scales)
            figure;
            imagesc(kgrid.y_vec * axis_scale, kgrid.x_vec * axis_scale,  max(I_out, [], 3));
            axis image;
            colormap(color_map);
            title('Vessel Filtered Image (MIP), Multi-Scale');
            ylabel(['x [' axis_prefix 'm]']);
            xlabel(['y [' axis_prefix 'm]']);
            colorbar;
            drawnow;
        end
    end
end

% display total time elapsed
if disp_updates
    disp(['  Total computation time ' scaleTime(etime(clock, start_time))]);
end

% end of function
end

% =========================================================================
% SUB FUNCTIONS
% =========================================================================

function roots = cardanRoots(varargin)
% Find roots of third order polynomials using Cardan's formula
%
% INPUT
%   P: (n x 4) array, each row corresponds to coefficients of each
%   polynomial, P(:,1)*x^3 + P(:,2)*x^2 + P(:,3)*x + P(:,4)
% OUTPUT
%   roots: (n x 3) array, each row correspond to the roots of P
%
% To adjust the parameter below which the the discriminant is considerered
% as nil, use
%   CardanRoots(P, tol)
% Adjusting tol is useful to avoid the real roots become complex due to
% numerical accuracy. The default TOL is 0
%
% http://www.sosmath.com/algebra/factor/fac11/fac11.html
% http://mathforum.org/dr.math/faq/faq.cubic.equations.html
%
% See also: roots, ParabolaRoots, eig3
%
% Author: Bruno Luong <brunoluong@yahoo.com>
% History:
%     Original 20-May-2010

% Copyright (c) 2010, Bruno Luong
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.

% Adjustable parameter
tol = 0;

if nargin<4
    P = varargin{1};
    a = P(:,1);
    b = P(:,2);
    c = P(:,3);
    d = P(:,4);
    if nargin>=2
        tol = varargin{2};
    end
else
    [a, b, c, d] = deal(varargin{1:4});
    if nargin>=5
        tol = varargin{5};
    end
end

if ~isequal(a,1)
    b = b./a;
    c = c./a;
    d = d./a;
end

b2 = b.^2;

p = -b2/3 + c;
q = ((2/27)*b2-(1/3)*c).*b + d;
delta = q.^2 + (4/27)*p.^3;

% Three cases of discriminant sign
iscmplx = imag(p) | imag(q);
notcmplx = ~iscmplx;
deltanull = notcmplx & abs(delta)<tol; % = 0
deltaneg = notcmplx & delta<0;
deltapos = notcmplx & ~(deltanull | deltaneg);

n = size(delta,1);
roots = zeros(n, 3, class(delta));

if any(deltanull)
    idx = find(deltanull);
    roots(idx,:) = CardanNull(p(idx), q(idx));
end

if any(deltaneg)
    idx = find(deltaneg);
    roots(idx,:) = CardanNeg(q(idx), delta(idx));
end

if any(deltapos)
    idx = find(deltapos);
    roots(idx,:) = CardanPos(q(idx), delta(idx));
end

if any(iscmplx)
    idx = find(iscmplx);
    roots(idx,:) = CardanCmplx(p(idx), q(idx), delta(idx));
end

roots = bsxfun(@minus, roots, b/3);

end

function roots = CardanNull(p, q)

    S1 = 3*q./p;
    S2 = -0.5*S1;
    roots = [S1 S2 S2];  % double real solutions

end

function roots = CardanNeg(q, delta)

    alfa = -q;
    beta = sqrt(-delta);
    r2 = alfa.^2-delta;
    rho = (4^(1/3))*exp(log(r2)/6);
    theta = atan2(beta,alfa)/3;
    alfa = rho.*cos(theta);
    beta = rho.*sin(theta);
    S1 = alfa;
    x = (-0.5)*alfa;
    y = (sqrt(3)/2)*beta;
    S2 = x-y;
    S3 = x+y;
    roots = [S1 S2 S3];

end

function roots = CardanPos(q, delta)

    sqrtdelta = sqrt(delta);
    u3 = (-q+sqrtdelta)/2;
    v3 = (-q-sqrtdelta)/2;

    % Cubic roots of u3 and v3
    u = sign(u3).*exp(log(abs(u3))/3);
    v = sign(v3).*exp(log(abs(v3))/3);

    S1 = u+v;
    % Complex solutions
    j = complex(-0.5,sqrt(3)/2);
    j2 = complex(-0.5,-sqrt(3)/2);
    S2 = j*u+j2*v;
    S3 = conj(S2);
    roots = [S1 S2 S3];

end

function roots = CardanCmplx(p, q, delta)

    sqrtdelta = sqrt(delta);
    u3 = (-q+sqrtdelta)/2;
    v3 = (-q-sqrtdelta)/2;

    % we need u*v = -p/3
    p = (-1/3)*p;
    iu = abs(u3)>abs(v3);
    u = zeros(size(u3),class(u3));
    v = zeros(size(v3),class(v3));
    u(iu) = exp(log(u3(iu))/3);
    v(iu) = p(iu)./u(iu);
    v(~iu) = exp(log(v3(~iu))/3);
    u(~iu) = p(~iu)./v(~iu);

    S1 = u+v;

    j = complex(-0.5,sqrt(3)/2);
    j2 = complex(-0.5,-sqrt(3)/2);
    S2 = j*u+j2*v;
    S3 = j2*u+j*v;
    roots = [S1 S2 S3];

end


function wc = FWT2_PO(x,L,qmf)
% FWT2_PO -- 2-d MRA wavelet transform (periodized, orthogonal)
%  Usage
%    wc = FWT2_PO(x,L,qmf)
%  Inputs
%    x     2-d image (n by n array, n dyadic)
%    L     coarse level
%    qmf   quadrature mirror filter
%  Outputs
%    wc    2-d wavelet transform
%
%  Description
%    A two-dimensional Wavelet Transform is computed for the
%    array x.  To reconstruct, use IWT2_PO.
%
%  See Also
%    IWT2_PO, MakeONFilter
%
	[n,J] = quadlength(x);
	wc = x;
	nc = n;
	for jscal=J-1:-1:L, % from fine (J) to coarse (L)
		top = (nc/2+1):nc; bot = 1:(nc/2);
		for ix=1:nc,
			row = wc(ix,1:nc);
			wc(ix,bot) = DownDyadLo(row,qmf);
			wc(ix,top) = DownDyadHi(row,qmf);
		end
		for iy=1:nc,
			row = wc(1:nc,iy)';
			wc(top,iy) = DownDyadHi(row,qmf)';
			wc(bot,iy) = DownDyadLo(row,qmf)';
		 end
		nc = nc/2; % downsampling
	end

%
% Copyright (c) 1993. David L. Donoho
%

%
%  Part of Wavelab Version 850
%  Built Tue Jan  3 13:20:40 EST 2006
%  This is Copyrighted Material
%  For Copying permissions see COPYING.m
%  Comments? e-mail wavelab@stat.stanford.edu


function c=ref_fftreal(f)
%REF_FFTREAL  Reference FFTREAL
%
%  FFTREAL is computed by doing an FFT, and keeping only the positive
%  frequencies.

L=size(f,1);
c=fft(f);

c=c(1:floor(L/2)+1,:);


function BrainNet=SR(BOLD,lambda)
% FC Network Construction using Sparse Representation (SR)
%
% Inpute:
% BOLD. A cell array with size of N x 1, each cell is a matrix of BOLD signals (#time points x #ROIs) from one of N subject
%       Each subject may have different #time points but should have the same #ROIs
% lambda. parameter for sparsity
%
% Output:
% BrainNet. FC network (#ROIs x #ROIs x #Subjects)
%
% Requires SLEP toolbox: http://www.yelab.net/software/SLEP/
%
% By Yu Zhang, zhangyu0112@gmail.com
% IDEA lab https://www.med.unc.edu/bric/ideagroup
% Department of Radiology and BRIC, UNC Chapel Hill

%% Initialize parameters for SLEP toolbox
opts=[];
opts.init=2;    % starting point: starting from a zero point here
opts.tFlag=0;   % termination criterion
opts.nFlag=0;   % normalization option: 0-without normalization
opts.rFlag=1;   % regularization % the input parameter 'rho' is a ratio in (0, 1)
opts.rsL2=0;    % the squared two norm term in min  1/2 || A x - y||^2 + 1/2 rsL2 * ||x||_2^2 + z * ||x||_1
opts.mFlag=0;   % treating it as compositive function
opts.lFlag=0;   % Nemirovski's line search

%% Construct FC networks using sparse representation
[nTime,nROI]=size(BOLD{1});
nSubj=length(BOLD);
BrainNet=zeros(nROI,nROI,nSubj,'single');
for i=1:nSubj
    temp=BOLD{i};
    temp=temp-repmat(mean(temp,2),1,nROI);     % centralization
    tempNet=zeros(nROI,nROI);
    for j=1:nROI
        ndic=setdiff(1:nROI,j);
        y=temp(:,j);
        A=temp(:,ndic);
        x=LeastR(A,y,lambda,opts);
        tempNet(ndic,j)=x;
    end
    tempNet=(tempNet+tempNet')/2;              % form symmetric adjancency matrix (main diag are zeros)
    tempNet=tempNet-diag(diag(tempNet));       % ignore self connections
    BrainNet(:,:,i)=tempNet;
end


function I = mutualinfo1(A,B)
%==================================================
%This is a prog in the MutualInfo 0.9 package written by
% Hanchuan Peng.
%
% I = mutualinfo(vec1,vec2)
% calculate the mutual information of two vectors
%
% For small images this function is faster than
% function 'mutualinfo2'.
%==================================================

pA = estpa(A);
HA = estentropy(pA);

pB = estpa(B);
HB = estentropy(pB);

pAB = estpab(A,B);
HAB = estjointentropy(pAB);

I = HA + HB - HAB;
return


function test_gppde_2d

% $$$ x_g = -5:0.5:5;
% $$$ [x1_g, x2_g] = meshgrid(
T = 1;
L = 1;
%[x1_g, x2_g] = meshgrid(linspace(-0*T,3*T,60), linspace(-1*L,2*L,40));
min_L = 0.0*L;
max_L = 1.0*L;
min_T = 0.0*T;
max_T = 1.0*T;
[x1_g, x2_g] = meshgrid(linspace(min_T,max_T,40), linspace(min_L,max_L,40));
X_g = [x1_g(:), x2_g(:)];
[x1_f, x2_f] = meshgrid(linspace(0,T,45), linspace(0,L,45));
X_f = [x1_f(:), x2_f(:)];

% Define the partial differential equation
N_g = size(X_g, 1);
switch 1
 case 1
  %
  % HEAT EQUATION (d_t - d_x^2 = 0)
  %
  theta = [1 1/10]; % [scale, length scale]
  s2_y = theta(1)^2 * 1e-5;

  D = [1 0;     % differentiation
       0 2];
  alpha = [-1;   % coefficients
           0.02]; % (<- heat conduction)

  % Spatial boundary fixed to zero
  N_y = 30;
  bound1 = [linspace(min_T,max_T,N_y)', min_L*ones(N_y,1)];
  bound2 = [linspace(min_T,max_T,N_y)', max_L*ones(N_y,1)];
  X_y = [bound1; bound2];
  y = zeros(size(X_y,1),1);
  % Initial function at time T=min_T
  T_obs = min_T + 0.1*(max_T-min_T);
  bound_init = [T_obs*ones(N_y,1), linspace(min_L,max_L,N_y)'];
  X_y = [X_y; bound_init];
  y_init = 2*exp( -0.5*(bound_init(:,2)-0.5*(min_L+max_L)).^2 / 0.1^2);
  y = [y; y_init] + sqrt(s2_y)*randn(size(X_y,1),1);

  equation = 'Heat equation';

 case 2
  %
  % WAVE EQUATION (d_t^2 - d_x^2 = 0)
  %
  theta = [1 1/30]; % [scale, length scale]
  s2_y = theta(1)^2 * 1e-4;

  D = [2 0;
       0 2];
  alpha = [-1;
           16]; % speed ^ 2
  % Spatial boundary fixed to zero
  N_y = 100;
  bound1 = [linspace(min_T,max_T,N_y)', min_L*ones(N_y,1)];
  bound2 = [linspace(min_T,max_T,N_y)', max_L*ones(N_y,1)];
  X_y = [bound1; bound2];
  y = zeros(size(X_y,1),1);
% $$$   % Initial function at time T=min_T
% $$$   bound_init = [min_T*ones(N_y,1), linspace(min_L,max_L,N_y)'];
% $$$   X_y = [X_y; bound_init];
% $$$   y_init = 2*exp( -0.5*(bound_init(:,2)-0.5*(min_L+max_L)).^2 / 0.03^2);
% $$$   y = [y; y_init];
  y = y + sqrt(s2_y)*randn(size(X_y,1),1);

  equation = 'Wave equation';

 case 3
  %
  % LAPLACE EQUATION (d_x^2 + d_y^2 = 0)
  %
  theta = [1 1/20]; % [scale, length scale]
  s2_y = theta(1)^2 * 1e-5;

  D = [2 0;
       0 2];
  alpha = [1;
           1];
  X_y = zeros(0,2);
  y = zeros(0,1);

end
g = zeros(N_g,1);

% Inference: Get posterior predictive distribution
[m_f, V_f] = gppde(X_y, y, s2_y, theta, X_g, g, alpha, D, X_f);
% $$$ figure
% $$$ imagesc(V_f);

% Draw posterior predictive samples
samples = 1;
I_f = speye(length(V_f));
s2_f = 1e-8; % numerical reasons
L_f = chol(V_f + s2_f*I_f, 'lower');
f = bsxfun(@plus, L_f * randn(length(L_f),samples), m_f);
figure
for i = 1:samples
  subplot(ceil(sqrt(samples)), ceil(sqrt(samples)), i);
%  pcolor(reshape(f(:,i), size(x1_f)));
  contourf(x1_f, x2_f, reshape(f(:,i), size(x1_f)), 100);
  shading('flat')
  map_colormap();
  cl = max( abs(f(:,i)) );
  set(gca, 'clim', [-cl, cl])
  title(equation)
  xlabel('time');
  xlabel('time');
  ylabel('location');
  set(gca, 'xtick', [], 'ytick', []);
end


function [s, lambda_k] = tr_subsolver(problem, w, grad, tr_radius, sub_hess_indices, ...
                                                successful_flag, lambda_k, subproblem_solver,...
                                                exact_tol, krylov_tol)
    if strcmp(subproblem_solver, 'cauchy_point')
        %Hg = Hv_f(w, new_X, new_Y, grad,alpha);
        Hg = problem.hess_vec(w, grad, sub_hess_indices);
        gBg = grad'*Hg;
        tau = 1;
        if gBg > 0  % if model is convex quadratic the unconstrained minimizer may be inside the TR
            tau = min([(norm(grad))^3 / (tr_radius * gBg), 1]);
        end
        pc = - tau * tr_radius * (grad./norm(grad));
        s = pc;
        lamda_k = 0;
    elseif strcmp(subproblem_solver, 'dog_leg')
        %H = hessian_f(w, new_X, new_Y, alpha);
        H = problem.hess(w, sub_hess_indices);
        gBg = grad'*(H'*grad);
        if gBg <= 0
            error('dog_leg requires H to be positive definite in all steps!') ;
        end

        % Compute the Newton Point and return it if inside the TR
        L = chol(H,'lower');
        y = L\grad;
        pn = -L'\y;
%         cholesky_B = chol(H,'lower');
%         pn = - (cholesky_B\grad);
        if (norm(pn) < tr_radius)
            s = pn;
            lambda_k = 0;
            return;
        end

        % Compute the 'unconstrained Cauchy Point'
        pc = -((grad'*grad)/gBg) * grad;
        pc_norm = norm(pc);

        % if it is outside the TR, return the point where the path intersects the boundary
        if pc_norm >= tr_radius
            p_boundary = pc * (tr_radius / pc_norm);
            s = p_boundary;
            lambda_k = 0;
            return;
        end

        % else, give intersection of path from pc to pn with tr_radius.
        [t_lower, t_upper] = solve_quadratic_equation(pc, pn, tr_radius);
        p_boundary = pc + t_upper * (pn - pc);
        s = p_boundary;
        lambda_k = 0;

    elseif strcmp(subproblem_solver, 'cg')
        grad_norm = norm(grad);
        p_start = zeros(size(grad,1),size(grad,2));

        if grad_norm < min([sqrt(norm(grad)) * norm(grad), krylov_tol])
            s = p_start;
            lambda_k = 0;
            return;
        end

        % initialize
        z = p_start;
        r = grad;
        d = -r;
        k = 0;

        while true
            %Bd = Hv_f(w, new_X, new_Y, d, alpha);
            Bd = problem.hess_vec(w, d, sub_hess_indices);
            dBd = d'*Bd;
            % terminate when encountering a direction of negative curvature with lowest boundary point along current search direction
            if dBd <= 0
                [t_lower, t_upper] = solve_quadratic_equation(z, d, tr_radius);
                p_low = z + t_lower * d;
                p_up = z + t_upper * d;
                %m_p_low = loss_f(w + p_low, X, Y, alpha) + grad'*p_low + 0.5 * p_low'*(H'*p_low);
                %m_p_up = loss_f(w + p_up, X, Y, alpha) + grad'*p_up + 0.5 * p_up'*(H'*p_up);

                H = problem.hess(w, sub_hess_indices); % Added by HK (Need to be checked.)
                m_p_low = problem.cost(w+p_low) + grad'*p_low + 0.5 * p_low'*(H'*p_low);
                m_p_up = problem.cost(w+p_up) + grad'*p_up + 0.5 * p_up'*(H'*p_up);
                problem.cost(w);
                if m_p_low < m_p_up
                    s = p_low;
                    lambda_k = 0;
                    return;
                else
                    s = p_up;
                    lambda_k = 0;
                    return;
                end
            end

            alpha = (r'*r) / dBd;
            z_next = z + alpha * d;
            % terminate if z_next violates TR bound
            if norm(z_next) >= tr_radius
                % return intersect of current search direction w/ boud
                [t_lower, t_upper] = solve_quadratic_equation(z, d, tr_radius);
                s = z + t_upper * d;
                lambda_k = 0;
                return;
            end

            r_next = r + alpha * Bd;
            if norm(r_next) < min([sqrt(norm(grad)) * norm(grad),krylov_tol])
                s = z_next;
                lambda_k = 0;
                return;
            end

            beta_next = (r_next'*r_next) / (r'*r);
            d_next = -r_next + beta_next * d;
            % update iterates
            z = z_next;
            r = r_next;
            d = d_next;
            k = k + 1;
        end
    elseif strcmp(subproblem_solver, 'GLTR')
        g_norm = norm(grad);
        s = zeros(size(grad,1), size(grad,2));

        if g_norm == 0
            % escape along the direction of the leftmost eigenvector as far as tr_radius permits
            fprintf('zero gradient encountered');
            %H = hessian_f(w, new_X, new_Y, alpha);
            H = problem.hess(w, sub_hess_indices);
            [s, lambda_k] = exact_TR_suproblem_solver(grad, H, tr_radius, exact_tol, successful_flag, lambda_k);
        else
            % initialize
            g = grad;
            p = -g;
            gamma = g_norm;
            T = zeros(1, 1);
            alpha_k = [];
            beta_k = [];
            interior_flag = true;
            k = 0;

            while true
                %Hp = Hv_f(w, new_X, new_Y, p, alpha);
                Hp = problem.hess_vec(w, p, sub_hess_indices);
                pHp = p'*Hp;
                alpha = g'*g / pHp;

                alpha_k = [alpha_k; alpha];

                %Lanczos Step 1: Build up subspace
                % a) Create g_lanczos = gamma*e_1

                e_1 = zeros(k + 1, 1);
                e_1(1) = 1.0;
                g_lanczos = gamma .* e_1;

                % b) Create T for Lanczos Model
                T_new = zeros(k + 1, k + 1);
                if k == 0
                    T(k+1, k+1) = 1.0/alpha;
                    T_new(1:k+1, 1:k+1) = T;
                else
                    T_new(1:size(T,1), 1:size(T,2)) = T;
                    T_new(k+1, k+1) = 1.0 / alpha + beta_/ alpha_k(k);
                    T_new(k, k+1) = sqrt(beta_) / abs(alpha_k(k));
                    T_new(k+1, k) = sqrt(beta_) / abs(alpha_k(k));
                    T = T_new;
                end

                if (interior_flag == true && alpha < 0) || norm(s + alpha * p) >= tr_radius
                    interior_flag = false;
                end

                if interior_flag == true
                    s = s + alpha * p;
                else
                    % Lanczos Step 2: solve problem in subspace
                    [h, lambda_k] = exact_TR_suproblem_solver(g_lanczos, T, tr_radius, exact_tol, successful_flag, lambda_k);
                end
                g_next = g + alpha * Hp;

                % test for convergence
                e_k = zeros(k + 1,1);
                e_k(k+1) = 1.0;

                if interior_flag == true && norm(g_next) < min([sqrt(norm(grad)) * norm(grad), krylov_tol])
                    break;
                end
                if interior_flag == false && norm(g_next) * abs(h'*e_k) < min([sqrt(norm(grad)) * norm(grad), krylov_tol])
                    break;
                end

                if k == problem.d
                    fprintf('Krylov dimensionality reach full space! Breaking out..\n');
                    break;
                end
                beta_ = (g_next'*g_next) / (g'*g);
                beta_k = [beta_k; beta_];
                p = -g_next + beta_ * p;
                g = g_next;
                k = k + 1;
            end

            if interior_flag == false
                % Recover Q by building up the lanczos space, TBD: keep storable Qs in memory
                n = size(grad,1);
                Q1 = zeros(n, k + 1);

                g = grad;
                p = -g;

                for j = 0 : k
                    gn = norm(g);
                    if j == 0
                        sigma = 1;
                    else
                        sigma = -sign(alpha_k(j)) * sigma;
                    end
                    Q1(:, j+1) = sigma * g / gn;

                    if ~ (j == k)
                        %Hp = Hv_f(w, new_X, new_Y, p, alpha);
                        Hp = problem.hess_vec(w, p, sub_hess_indices);
                        g = g + alpha_k(j+1) * Hp;
                        p = -g + beta_k(j+1) * p;
                    end
                end

                % compute final step in R^n
                s = Q1*h;
            end
        end
    elseif strcmp(subproblem_solver, 'exact')
        %H = hessian_f(w, new_X, new_Y, alpha);
        Hw = problem.hess(w, sub_hess_indices);
        [s, lambda_k] = exact_TR_suproblem_solver(grad, H, tr_radius, exact_tol, successful_flag, lambda_k);
    else
    	error('solver unknown\n');
    end
end

function [s, lambda_j] = exact_TR_suproblem_solver(grad, H, tr_radius, exact_tol, successful_flag, lambda_k)

    s = zeros(size(grad,1),size(grad,2));
    % Step 0: initialize safeguards
    H_ii_min = min(diag(H));
    absH = abs(H);
    H_max_norm = sqrt((size(H,1))^2) * max(absH(:));
    H_fro_norm = norm(H, 'fro');
    list_l = [];
    list_u = [];
    for i = 1 : length(H)
        l = H(i, i) + sum(abs(H(i, :))) - abs(H(i, i));
        u = -H(i, i) + sum(abs(H(i, :))) - abs(H(i, i));
        list_l = [list_l l];
        list_u = [list_u u];
    end
    gerschgorin_l = max(list_l);
    gerschgorin_u = max(list_u);

    lambda_lower = max([0, -H_ii_min, norm(grad) / tr_radius - min([H_fro_norm, H_max_norm, gerschgorin_l])]);
    lambda_upper = max([0, norm(grad) / tr_radius + min([H_fro_norm, H_max_norm, gerschgorin_u])]);

    if successful_flag == false && (lambda_lower <= lambda_k) && (lambda_k <= lambda_upper) % reinitialize at previous lambda in case of unscuccesful iterations
        lambda_j = lambda_k;
    elseif lambda_lower == 0  % allow for fast convergence in case of inner solution
        lambda_j = lambda_lower;
    else
        lambda_j = lambda_lower + (lambda_upper-lambda_lower)*rand(1,1);
    end

    i = 0;
    % Root Finding
    while true
        i = i + 1;
        lambda_in_N = false;
        lambda_plus_in_N = false;
        B = H + lambda_j * eye(size(H,1), size(H,2));
        try
            % 1 Factorize B
            L = chol(B, 'lower');
            % 2 Solve LL^Ts=-g
            Li = inv(L);
            s = - (Li'*Li)*grad;
            sn = norm(s);
            % 2.1 Termination: Lambda in F, if q(s(lamda))<eps_opt q(s*) and sn<eps_tr tr_radius -> stop. By Conn: Lemma 7.3.5:
            phi_lambda = 1.0 / sn - 1.0 / tr_radius;
            %if (abs(sn - tr_radius) <= exact_tol * tr_radius):
            if (abs(phi_lambda)<=exact_tol) %
                break;
            end

            % 3 Solve Lw=s
                w = Li*s;
                wn = norm(w);

            % Step 1: Lambda in L
            if lambda_j > 0 && (phi_lambda) < 0
                % print ('lambda: ',lambda_j, ' in L')
                lambda_plus = lambda_j + ((sn - tr_radius) / tr_radius) * ((sn^2) / (wn^2));
                lambda_j = lambda_plus;

            % Step 2: Lambda in G    (sn<tr_radius)
            elseif (phi_lambda) > 0 && lambda_j > 0 && any(grad ~= 0) % TBD: remove grad
                % print ('lambda: ',lambda_j, ' in G')
                lambda_upper = lambda_j;
                lambda_plus = lambda_j + ((sn - tr_radius) / tr_radius) * ((sn^2) / (wn^2));

                % Step 2a: If factorization succeeds: lambda_plus in L
                if lambda_plus > 0
                    try
                        % 1 Factorize B
                        B_plus = H + lambda_plus * eye(size(H,1), size(H,2));
                        L = chol(B_plus, 'lower');
                        lambda_j = lambda_plus;
                        % print ('lambda+', lambda_plus, 'in L')
                    catch
                        lambda_plus_in_N = true;
                    end
                end

                % Step 2b/c: If not: Lambda_plus in N
                if lambda_plus <= 0 || lambda_plus_in_N == true
                    % 1. Check for interior convergence (H pd, phi(lambda)>=0, lambda_l=0)
                    try
                        U = chol(H, 'upper');
                        H_pd = true;
                    catch
                        H_pd = false;
                    end

                    if lambda_lower == 0 && H_pd == true && phi_lambda >= 0 %cannot happen in ARC!
                        lambda_j = 0;
                        % print ('inner solution found');
                        break;
                    % 2. Else, choose a lambda within the safeguard interval
                    else
                        % print ('lambda_plus', lambda_plus, 'in N')
                        lambda_lower = max([lambda_lower, lambda_plus]);  % reset lower safeguard
                        lambda_j = max([sqrt(lambda_lower * lambda_upper),lambda_lower + 0.01 * (lambda_upper - lambda_lower)]);
                        lambda_upper = single(lambda_upper);

                        if lambda_lower == lambda_upper
                            lambda_j = lambda_lower;
                            % Hard case
                            [ev, ew] = eig(H);
                            d = ev(:, 1);
                            dn = norm(d);
                            assert((ew == -lambda_j), 'Ackward: in hard case but lambda_j != -lambda_1');
                            [tao_lower, tao_upper] = mitternachtsformel(1, 2*(s'*d), (s'*s)-tr_radius^2);
                            s = s + tao_lower * d;
                            fprintf('hard case resolved inside');
                        end
                    end
                end

            elseif (phi_lambda) == 0
                break;
            else % TBD:  move into if lambda+ column #this only happens for Hg=0 -> s=(0,..,0)->phi=inf -> lambda_plus=nan -> hard case (e.g. at saddle)
                lambda_in_N = true;
            end
        % Step 3: Lambda in N
        catch
            lambda_in_N = true;
        end

        if lambda_in_N == true
            % print ('lambda: ',lambda_j, ' in N')
            try
                U = chol(H, 'upper');
                H_pd = true;
            catch
                H_pd = false;
            end

            % 1. Check for interior convergence (H pd, phi(lambda)>=0, lambda_l=0)
            if lambda_lower == 0 && H_pd == true && phi_lambda >= 0
                lambda_j = 0;
                %print ('inner solution found')
                break;
            % 2. Else, choose a lambda within the safeguard interval
            else
                lambda_lower = max([lambda_lower, lambda_j]);  % reset lower safeguard
                lambda_j = max([sqrt(lambda_lower * lambda_upper),lambda_lower + 0.01 * (lambda_upper - lambda_lower)]);  % eq 7.3.14
                lambda_upper = single(lambda_upper);
                % Check for Hard Case:
                if lambda_lower == lambda_upper
                    lambda_j = lambda_lower;
                    [ev, ew] = eig(H);
                    d = ev(:, 1);
                    dn = norm(d);
                    assert((ew == -lambda_j), 'Ackward: in hard case but lambda_j != -lambda_1');
                    [tao_lower, tao_upper] = mitternachtsformel(1, 2*(s'*d), (s'*s)-tr_radius^2);
                    s = s + tao_lower * d;

                    fprintf('hard case resolved outside');
                end
            end
        end
    end

    % compute final step
    B = H + lambda_j * eye(size(H,1), size(H,2));
    % 1 Factorize B
    L = chol(B, 'lower');
    % 2 Solve LL^Ts=-g
    Li = inv(L);
    s = - Li'*Li*grad;
    %print (i,' exact solver iterations')

end

% Auxiliary Functions
function [t_lower, t_upper] = mitternachtsformel(a, b, c)
    sqrt_discriminant = sqrt(b * b - 4 * a * c);
    t_lower = (-b - sqrt_discriminant) / (2 * a);
    t_upper = (-b + sqrt_discriminant) / (2 * a);
end

function [t_lower, t_upper] = solve_quadratic_equation(pc, pn, tr_radius)
    % solves ax^2+bx+c=0
    a = (pn - pc)'*(pn - pc);
    b = 2 * (pc'*(pn - pc));
    c = (pc'*pc) - tr_radius^2;
    sqrt_discriminant = sqrt(b * b - 4 * a * c);
    t_lower = (-b - sqrt_discriminant) / (2 * a);
    t_upper = (-b + sqrt_discriminant) / (2 * a);
end


function y = logmvn(X, Mu, Sigma, options)
%LOGMVN Evaluate the logarithms of normal density functions.
%The function evaluates the logarithm of Gaussian density function given
%data at X(n,:) with center at Mu(n,:) and covariance matrix Sigma(:,:,n).
%The code can handle different means and covariance matrices for each
%sample. It is much faster than the native function mvnpdf.
%
% Input:
%   X - N by D data. N is the number of samples. D is the number of
%       dimensions.
%   Mu - N by D (multiple) or 1 by D (single) mean.
%   Sigma - D by D by N (multiple) or D by D (single) covariance matrice.
%   Options - structure containing indices for transforming the Sigma to a
%       sparse matrix. Usually no need to set.
% Output:
%   y - N by 1 results that contain the logarithm of the Gaussians.
%
% Example:
%
% N = 1000;
% D = 2;
% Mu = [1 -1];
% Mu = repmat(Mu, 1000, 1) + randn(N,D);
% SN = repmat(reshape(abs(randn(N,1)), [1,1,N]), D, D) .* repmat(eye(D), [1,1,N]);
% Sigma = [.9 .4; .4 .3];
% Sigma = repmat(Sigma, [1,1,N]) + SN;
% X = mvnrnd(Mu,Sigma,1000);
%
% tic
% y = logmvn(X, Mu, Sigma);
% y = exp(y);
% toc
%
% tic
% y1 = mvnpdf(X, Mu, Sigma);
% toc
%
% norm(y-y1)
%
% Author: Yuan Zhou (zhouyuanzxcv@gmail.com)

% Copyright (C) 2015
if nargin < 4
    options = [];
end

if size(Mu,1) == 1 && ndims(Sigma) == 2 % single
    y = loggausspdf(X, Mu, Sigma);
elseif size(Mu,1) == size(X,1) && size(Sigma,3) == size(X,1) % multiple means
    y = logmvn_multiple(X, Mu, Sigma, options);
end

end

function y = logmvn_multiple(X, Mu, Sigma, options)
[N,B] = size(X);

% Transform the sigma matrix to a sparse block diagonal matrix
if ~isempty(options) && isfield(options,'sigma_update_in_logmvn')
    sparse_update_inplace(options.sigma_update_in_logmvn, Sigma(:));
    Sigma1 = options.sigma_update_in_logmvn;
else
    if ~isempty(options) && isfield(options,'Is') && isfield(options,'Js')
        Is = options.Is;
        Js = options.Js;
    else
        Is = (1:N*B);
        Is = repmat(reshape(Is, [B,1,N]), 1, B);
        Is = Is(:);

        Js = (1:N*B);
        Js = repmat(reshape(Js, [1,B,N]), B, 1);
        Js = Js(:);
    end

    Sigma1 = sparse(Is,Js,reshape(Sigma,N*B*B,1),N*B,N*B);
end

% Compute the Cholesky decomposition
[R,err] = chol(Sigma1);
if err ~= 0
    error('A Covariance matrix is not positive definite.');
end

% Final calculation
Y1 = X - Mu;

x1 = reshape(Y1', 1, N*B) / R;
x1 = sum(reshape(x1, B, N).^2, 1)';
x2 = sum(reshape(log(diag(R)),B,N), 1)';

y = -0.5 * x1 - x2 - B * log(2*pi) / 2;
end

function y = loggausspdf(X, mu, Sigma)
X = X';
mu = mu';
d = size(X,1);
X = bsxfun(@minus,X,mu);
[U,p]= cholcov(Sigma);
if p ~= 0
    error('ERROR: Sigma is not PD.');
end
Q = U'\X;
q = dot(Q,Q,1);  % quadratic term (M distance)
c = d*log(2*pi)+2*sum(log(diag(U)));   % normalization constant
y = -(c+q)/2;

y = y';
end


% fig_error_J.m
% plot maximum normalized error vs J using QR method
% for min-max NUFFT with uniform scaling factors
% This is Fig. 3 in the 2003 IEEE T-SP paper.
% Jeff Fessler, The University of Michigan

if ~isvar('emax')
%	Nlist = 1;
	Nlist = 2^10;
	Mlist = [1.5 2 2.5 3 4 5];	% over-sampling factors
%	Jlist = [2:11];
	Jlist = [2:20];	% neighborhood size

	[JJ, MM, NN] = ndgrid(Jlist, Mlist, Nlist);

	emax = zeros(size(NN));

	alpha = [1]; beta = [];
	for ii=1:numel(NN)
		N = NN(ii);
		J = JJ(ii);
		M = MM(ii);
		printf('M=%d J=%d', M, J)
		K = M * N;
		gam = 2*pi/K;
		om = gam * [0:20]'/40;
		emax(ii) = max(nufft1_err_mm(om, N, J, K, 'qr', alpha, beta));
	end
end

%
% plot min-max errors
%
if 1
	semilogy(Jlist, emax(:,1), '-o', ...
		Jlist, emax(:,2), '-d', ...
		Jlist, emax(:,3), '-^', ...
		Jlist, emax(:,4), '-s', ...
		Jlist, emax(:,5), '-p', ...
		Jlist, emax(:,6), '-+')
%		Jlist(Jlist <= 99), emax(Jlist <= 99, 2), '-d', ...
%		Jlist(Jlist <= 98), emax(Jlist <= 98, 3), '-^', ...
%		Jlist(Jlist <= 96), emax(Jlist <= 96, 4), '-s', ...
%		Jlist(Jlist <= 95), emax(Jlist <= 95, 5), '-p')
%	axis([minmax(Jlist)'+[-0.2 0.2] 1e-5 1e-1])
%	axis tight
%	axisx([minmax(Jlist)'+[-0.2 0.2]])
	axis([minmax(Jlist)'+[-0.2 0.2] 1e-10 1e-0])
	xtick([2:3:20])
	ytick(10.^[-10:2:0])
%	text(2.2, 2e-4, '(for unit-norm signal)')
	xlabel J, ylabel 'E_{max}'
	title(sprintf('Maximum error for \\alpha = (%g)', alpha))
	leg = {};
	for ii=1:length(Mlist)
		leg{ii} = sprintf('K/N=%g', Mlist(ii));
	end
	legend(leg)

%	ir_savefig c 'fig_error_J'
end


%
% Author: Marius Drulea
% http://www.cv.utcluj.ro/optical-flow.html

% References
% M. Drulea and S. Nedevschi, "Total variation regularization of
% local-global optical flow," in Intelligent Transportation Systems (ITSC),
% 2011 14th International IEEE Conference on, 2011, pp. 318-323.

% Copyright (C) 2011 Technical University of Cluj-Napoca

% 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 3 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 should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

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

function [u, v, pu, pv] = solve_clg_tv_equation(u, v, u0, v0, pu, pv, ...
    I1, Ix, Iy, It, D, settings)
%%
lambda = settings.lambda;

theta = 0.5;
tau = 0.5;
% tau = 1.0/(4.0*theta + epsilon);

der_mask = [0 -1 1];
adjoint_der_mask = [-1 1 0];

if settings.use_bilateral == 1
     r0 = It - u0.*Ix - v0.*Iy;
    % apply the bilateral filter to the data terms
    [W_Ix_2, W_Ixy, W_Iy_2, W_Ix_r0, W_Iy_r0] = ...
    applyBilateralFilterToDataTerms(I1, Ix.*Ix, Ix.*Iy, Iy.*Iy, Ix.*r0, Iy.*r0, ...
        settings.wSize, settings.sigma_d, settings.sigma_r);
else
    gauss = fspecial('gaussian', [settings.wSize settings.wSize], settings.wSize/6);

    W_Ix_2 = imfilter(Ix.^2, gauss, 'replicate');
    W_Ixy = imfilter(Ix.*Iy, gauss, 'replicate');
    W_Iy_2 = imfilter(Iy.^2, gauss, 'replicate');

    r0 = It - u0.*Ix - v0.*Iy;
    W_Ix_r0 = imfilter(Ix.*r0, gauss, 'replicate');
    W_Iy_r0 = imfilter(Iy.*r0, gauss, 'replicate');
end

a11 = 1 + 2*lambda*theta*W_Ix_2;
a12 = 2*lambda*theta*W_Ixy;
a21 = a12;
a22 = 1 + 2*lambda*theta*W_Iy_2;
l_t_2_W_Ix_r0 = 2*lambda*theta*W_Ix_r0;
l_t_2_W_Iy_r0 = 2*lambda*theta*W_Iy_r0;

delta = a11.*a22 - a21.*a12;
%%
for k = 1:settings.its
    %%
    % 1. update the coupling variable

    b1 = u - l_t_2_W_Ix_r0;
    b2 = v - l_t_2_W_Iy_r0;

    % update u_ and v_
    deltaU_ = b1.*a22 - b2.*a12;
    deltaV_ = b2.*a11 - b1.*a21;

    u_ = deltaU_./delta;
    v_ = deltaV_./delta;

    %%
  % compute the divergence of the dual variable
  % the adjoint of the nabla (derivative) operator = (- divergence) operator

  div_u = imfilter(pu(:, :, 1), adjoint_der_mask, 'replicate') + ...
          imfilter(pu(:, :, 2), adjoint_der_mask', 'replicate');
  div_v = imfilter(pv(:, :, 1), adjoint_der_mask, 'replicate') + ...
          imfilter(pv(:, :, 2), adjoint_der_mask', 'replicate');

  % update primal variable u
  u = u_ + theta*div_u;

  % update primal variable v
  v = v_ + theta*div_v;

  % compute nabla(u); the derivative operator
  ux = imfilter(u, der_mask, 'replicate');
  uy = imfilter(u, der_mask', 'replicate');
  vx = imfilter(v, der_mask, 'replicate');
  vy = imfilter(v, der_mask', 'replicate');

  % update dual variable; gradient descent
  % p = p_k + tau*nabla(u);
  pu(:, :, 1) = pu(:, :, 1) + tau * ux;
  pu(:, :, 2) = pu(:, :, 2) + tau * uy;
  pv(:, :, 1) = pv(:, :, 1) + tau * vx;
  pv(:, :, 2) = pv(:, :, 2) + tau * vy;

  % project the dual variable to ensure the inequality |p| <= D
  % p = p./max(|p|, D) .* D;
  pu(:, :, 1) = pu(:, :, 1)./max(abs(pu(:, :, 1)), D) .* D;
  pu(:, :, 2) = pu(:, :, 2)./max(abs(pu(:, :, 2)), D) .* D;
  pv(:, :, 1) = pv(:, :, 1)./max(abs(pv(:, :, 1)), D) .* D;
  pv(:, :, 2) = pv(:, :, 2)./max(abs(pv(:, :, 2)), D) .* D;
end


function [month, day, year] = gdate (jdate)

% convert Julian date to Gregorian (calendar) date

% input

%  jdate = julian day

% output

%  month = calendar month [1 - 12]
%  day   = calendar day [1 - 31]
%  year  = calendar year [yyyy]

%  note: day may include fractional part

% Orbital Mechanics with Matlab

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

jd = jdate;

z = fix(jd + .5);
fday = jd + .5 - z;

if (fday < 0)
   fday = fday + 1;
   z = z - 1;
end

if (z < 2299161)
   a = z;
else
   alpha = floor((z - 1867216.25) / 36524.25);
   a = z + 1 + alpha - floor(alpha / 4);
end

b = a + 1524;
c = fix((b - 122.1) / 365.25);
d = fix(365.25 * c);
e = fix((b - d) / 30.6001);
day = b - d - fix(30.6001 * e) + fday;

if (e < 14)
   month = e - 1;
else
   month = e - 13;
end

if (month > 2)
   year = c - 4716;
else
   year = c - 4715;
end


function i = binary_to_i4 ( s )

%*****************************************************************************80
%
%% BINARY_TO_I4 converts a binary representation into an integer value.
%
%  Example:
%
%        S        I
%
%      '101'      5
%    '-1000'     -8
%        '1'      1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the binary representation.
%
%    Output, integer I, the integer whose representation was input.
%
  nchar = s_len_trim ( s );

  i = 0;
  ichr = 1;
  istate = 0;
  isgn = 1;

  while ( ichr <= nchar )

    c = s(ichr);
%
%  Blank.
%
    if ( c == ' ' )

      if ( istate == 2 )
        istate = 3;
      end
%
%  Sign, + or -.
%
    elseif ( c == '-' )

      if ( istate == 0 )
        istate = 1;
        isgn = - 1;
      else
        istate = - 1;
      end

    elseif ( c == '+' )

      if ( istate == 0 )
        istate = 1;
      else
        istate = - 1;
      end
%
%  Digit, 0 or 1.
%
    elseif ( c == '1' )

      i = 2 * i;
      i = i + 1;
      istate = 2;

    elseif ( c == '0' )

      i = 2 * i;
      istate = 2;
%
%  Illegal or unknown sign.
%
    else

      fprintf ( 1, '\n' );
      fprintf ( 1, 'BINARY_TO_I4 - Serious error!\n' );
      fprintf ( 1, '  Illegal digit = "%c"\n', c );
      fprintf ( 1, '  Conversion halted prematurely!\n' );
      error ( 'BINARY_TO_I4 - Serious error!' );

    end

    if ( istate == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'BINARY_TO_I4 - Serious error!\n' );
      fprintf ( 1, '  Unable to decipher input!\n' );
      error ( 'BINARY_TO_I4 - Serious error!' );
    end

    if ( 3 <= istate )
      break;
    end

    ichr = ichr + 1;

  end
%
%  Apply the sign.
%
  i = isgn * i;

  return
end


function [x, infos] = als_nmf(V, rank, in_options)
% Alternative least squares (ALS) for non-negative matrix factorization (NMF).
%
% The problem of interest is defined as
%
%       min || V - WH ||_F^2,
%       where
%       {V, W, H} > 0.
%
% Given a non-negative matrix V, factorized non-negative matrices {W, H} are calculated.
%
%
% Inputs:
%       V           : (m x n) non-negative matrix to factorize
%       rank        : rank
%       in_options
%           alg     : als: Alternative least squares (ALS)
%
%                   : hals: Hierarchical alternative least squares (Hierarchical ALS)
%                       Reference:
%                           Andrzej Cichocki and PHAN Anh-Huy,
%                           "Fast local algorithms for large scale nonnegative matrix and tensor factorizations,"
%                           IEICE Transactions on Fundamentals of Electronics, Communications and Computer Sciences,
%                           vol. 92, no. 3, pp. 708-721, 2009.
%
%                   : acc_hals: Accelerated hierarchical alternative least squares (Accelerated HALS)
%                       Reference:
%                           N. Gillis and F. Glineur,
%                           "Accelerated Multiplicative Updates and hierarchical ALS Algorithms for Nonnegative
%                           Matrix Factorization,",
%                           Neural Computation 24 (4), pp. 1085-1105, 2012.
%                           See http://sites.google.com/site/nicolasgillis/.
%                           The corresponding code is originally created by the authors,
%                           Then, it is modifided by H.Kasai.
%
%
% Outputs:
%       x           : non-negative matrix solution, i.e., x.W: (m x rank), x.H: (rank x n)
%       infos       : log information
%           epoch   : iteration nuber
%           cost    : objective function value
%           optgap  : optimality gap
%           time    : elapsed time
%           grad_calc_count : number of sampled data elements (gradient calculations)
%
%
% This file is part of NMFLibrary
%
% Created by H.Kasai on Mar. 24, 2017
%
% Change log:
%
%       Oct. 27, 2017 (Hiroyuki Kasai): Fixed algorithm.
%
%       Apr. 22, 2019 (Hiroyuki Kasai): Fixed bugs.
%
%       May. 20, 2019 (Hiroyuki Kasai): Added initialization module.
%
%       Jun. 24, 2022 (Hiroyuki Kasai): Added momentum acceleration mode and mofified.
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.alg   = 'hals';
    local_options.sub_mode = 'std';
    local_options.alpha = 2;
    local_options.delta = 0.1;
    local_options.inner_max_epoch = 500;
    local_options.inner_max_epoch_parameter = 0.5;
    local_options.beta0 = 0.5;
    local_options.eta = 1.5;
    local_options.gammabeta = 1.01;
    local_options.gammabetabar = 1.005;
    local_options.momentum_h = 0;
    local_options.momentum_w = 0;
    local_options.scaling = true;
    local_options.warm_restart = false;

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % set paramters
    if ~strcmp(options.alg, 'als') && ~strcmp(options.alg, 'hals') ...
       && ~strcmp(options.alg, 'acc_hals')
        fprintf('Invalid algorithm: %s. Therfore, we use hals (i.e., Hierarchical ALS).\n', options.alg);
        options.alg = 'hals';
    end

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    W = init_factors.W;
    H = init_factors.H;

    % initialize
    method_name = sprintf('ALS (%s:%s)', options.alg, options.sub_mode);
    epoch = 0;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % intialize for als
    if strcmp(options.alg, 'acc_hals')
        eit1 = cputime;
        VHt = V*H';
        HHt = H*H';

        scaling = sum(sum(VHt.*W))/sum(sum( HHt.*(W'*W) ));
        W = W * scaling;

        options_halsupdt = [];
    end

    if options.scaling
        [W, H] = normalize_WH(V, W, H, rank, 'type1');
    end

    [options, beta, betamax] = check_momemtum_setting(options);

    if options.warm_restart
        nV = norm(V, 'fro');
        rel_error = zeros(1, options.max_epoch);
        rel_error(1) = sqrt(nV^2 - 2*sum(sum(V * H' .* W)) + sum(sum( H * H' .* (W'*W)))) / nV;
    end
    W_prev = W;
    H_prev = H;

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, W, H, [], options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('%s: Epoch = 0000, cost = %.16e, optgap = %.4e\n', method_name, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        %% update H
        VtW = V'*W;
        WtW = W'*W;
        WtV = W' * V;

        if strcmp(options.alg, 'als')

            %H = (W*pinv(W'*W))' * V;
            H = WtW \ WtV;        % H = inv(W'*W) * W' * V;
            H = H .* (H>0);

        elseif strcmp(options.alg, 'hals')

            for k=1:rank
                tmp = (VtW(:,k)' - (WtW(:,k)' * H) + (WtW(k,k) * H(k,:))) / WtW(k,k);
                tmp(tmp<=eps) = eps;
                H(k,:) = tmp;
            end

        elseif strcmp(options.alg, 'acc_hals')

            eit1 = cputime;
            options_halsupdt.max_epoch = change_inner_max_epoch(V, W, options);
            H = HALSupdt(H, WtW, WtV, eit1, options.alpha, options.delta, options_halsupdt);

        end

        % perform momentum for H
        if strcmp(options.sub_mode, 'momentum')
            [H, H_tmp1, H_tmp2] = do_momentum_h(H, H_prev, beta, epoch, options);
        end

        %% update W
        VHt = V * H';
        HHt = H * H';

        if strcmp(options.alg, 'als')

            %W = ((inv(H*H')*H)*V')';
            W = VHt / HHt;        % W = V * H' * inv(H*H');
            W = (W>0) .* W;

            % normalize columns to unit
            W = W ./ (repmat(sum(W), m, 1)+eps);

        elseif strcmp(options.alg, 'hals')

            for k=1:rank
                tmp = (VHt(:,k) - (W * HHt(:,k)) + (W(:,k) * HHt(k,k))) / HHt(k,k);
                tmp(tmp<=eps) = eps;
                W(:,k) = tmp;
            end

        elseif strcmp(options.alg, 'acc_hals')

%            if epoch > 0 % Do not recompute A and B at first pass
                % Use actual computational time instead of estimates rhoU
                eit1 = cputime;
                eit1 = cputime-eit1;
%           end
            options_halsupdt.max_epoch = change_inner_max_epoch(V', H', options);
            W = HALSupdt(W', HHt',VHt', eit1, options.alpha, options.delta, options_halsupdt);
            W = W';

        end

        % perform momentum for W
        if strcmp(options.sub_mode, 'momentum')
            [W, H, W_tmp1] = do_momentum_w(W, W_prev, H, H_prev, H_tmp1, beta, epoch, options);
        end

        % perform warm_restart
        if options.warm_restart
            [W, H, W_prev, H_prev, rel_error, beta, betamax, options] = ...
                warm_restart(V, W, H, rank, W_prev, H_prev, W_tmp1, H_tmp1, H_tmp2, rel_error, beta, betamax, epoch, options);
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(V, W, H, [], options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = W;
    x.H = H;

end


function [ mColumnVectorImage ] = ImageToColumnsSliding( mInputImage, vBlockSize )
% ----------------------------------------------------------------------------------------------- %
% [ mColumnImage ] = ImageToColumns( mInputImage, blockRadius )
%   Creates an column image from the sliding neighborhood in mInpuImage
% Input:
%   - mInputImage           -   Input image.
%                               Structure: Image Matrix (Single Channel)
%                               Type: 'Single' / 'Double'.
%                               Range: [0, 1].
%   - vBlockSize            -   Block Size.
%                               Structure: 2D Vector.
%                               Type: 'Single' / 'Double'.
%                               Range: [0, 1].
% Output:
%   - mColumnVectorImage    -   Column Vector Image.
%                               Structure: Image Matrix (Single Channel)
%                               Type: 'Single' / 'Double'.
%                               Range: [0, 1].
% Remarks:
%   1.  Prefixes:
%       -   'm' - Matrix.
%       -   'v' - Vector.
%   2.  Converts each sliding `vBlockSize(1)` by `vBlockSize(2) block of
%       `mInputImage` into a column of `mColumnVectorImage` with no zero
%       padding.
%   3.  Shouldn't be used for images larger than 400x400 and blocks of
%       51x51.
% TODO:
%   1.  I
%   Release Notes:
%   -   1.0.000     20/03/2015  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

[numRows, numCol] = size(mInputImage);
blockNumRows = vBlockSize(1);
blockNumCols = vBlockSize(2);

% Create Hankel-like indexing sub matrix.
nc = numRows - blockNumRows + 1;
nn = numCol - blockNumCols + 1;

vColumnIdx = [(0:(blockNumRows - 1))]';
vRowIdx = [1:nc];

t = vColumnIdx(:, ones(nc, 1)) + vRowIdx(ones(blockNumRows, 1), :);    % Hankel Subscripts
tt = zeros(blockNumRows * blockNumCols, nc);
rows = 1:blockNumRows;
for ii = 0:(blockNumCols - 1)
    tt(((ii * blockNumRows) + rows), :) = t + (numRows * ii);
end
mColumnVectorImageIdx = zeros((blockNumRows * blockNumCols), (nc * nn));
cols = 1:nc;
for jj = 0:(nn - 1)
    mColumnVectorImageIdx(:, ((jj * nc) + cols)) = tt + (numRows * jj);
end

mColumnVectorImage = mInputImage(mColumnVectorImageIdx);

end


%%*************************************************************************
%% HSDbicgstab
%%
%% [xx,resnrm,flag] = HSDbicgstab(A,b,M1,tol,maxit)
%%
%% iterate on  bb - (M1)*AA*x
%%
%% r = b-A*xtrue;
%%
%%*************************************************************************

function [xx,resnrm,flag] = HSDbicgstab(A,b,M1,tol,maxit,printlevel)

N = length(b);
if (nargin < 6); printlevel = 1; end
if (nargin < 5) || isempty(maxit); maxit = max(20,length(A.mat22)); end;
if (nargin < 4) || isempty(tol); tol = 1e-8; end;
tolb = min(1e-4,tol*norm(b));
flag = 1;

x = zeros(N,1);
if isstruct(A); r = b-matvec(A,x); else r = b-mexMatvec(A,x); end;
err = norm(r); resnrm(1) = err;  minresnrm = err; xx = x;
%%if (err < tolb); return; end

omega = 1.0;
r_tld = r;
%%
%%
%%
smtol = 1e-40;
for iter = 1:maxit,

    rho   = (r_tld'*r);
    if (abs(rho) < smtol)
        flag = 2;
        if (printlevel); fprintf('*'); end;
        break;
    end
    if (iter > 1)
        beta  = (rho/rho_1)* (alp/omega);
        p = r + beta*(p - omega*v);
    else
        p = r;
    end
    p_hat = precond(A,M1,p);
    if isstruct(A); v = matvec(A,p_hat); else v = mexMatvec(A,p_hat); end;
    alp = rho / (r_tld'*v);
    s = r - alp*v;
    %%
    s_hat = precond(A,M1,s);
    if isstruct(A); t = matvec(A,s_hat); else t = mexMatvec(A,s_hat); end;
    omega = (t'*s) / (t'*t);
    x = x + alp*p_hat + omega*s_hat;
    r = s - omega*t;
    rho_1 = rho;
    %%
    %% check convergence
    %%
    err = norm(r); resnrm(iter+1) = err; %#ok
    if (err < minresnrm);
        xx = x; minresnrm = err;
    end
    if (err < tolb)
        break;
    end
    if (err > 10*minresnrm)
        if (printlevel); fprintf('^'); end
        break;
    end
    if (abs(omega) < smtol)
        flag = 2;
        if (printlevel); fprintf('*'); end;
        break;
    end
end
%%
%%*************************************************************************
%%*************************************************************************
%% matvec: matrix-vector multiply.
%% matrix = [A.mat11, A.mat12; A.mat12', A.mat22]
%%*************************************************************************

function Ax = matvec(A,x)

m = length(A.mat11); m2 = length(x)-m;
if (m2 > 0)
    x1 = full(x(1:m));
else
    x1 = full(x);
end
Ax = mexMatvec(A.mat11,x1);
if (m2 > 0)
    x2 = full(x(m+1:m+m2));
    Ax = Ax + mexMatvec(A.mat12,x2);
    Ax2 = mexMatvec(A.mat12,x1,1) + mexMatvec(A.mat22,x2);
    Ax = [Ax; Ax2];
end
%%*************************************************************************
%% precond:
%%*************************************************************************

function Mx = precond(A,L,x)

m = L.matdim; m2 = length(x)-m;
if (m2 > 0)
    x1 = full(x(1:m));
else
    x1 = full(x);
end
if (m2 > 0)
    x2 = x(m+1:m+m2);
    w = linsysolvefun(L,x1);
    z = mexMatvec(A.mat12,w,1) -x2;
    z = L.Mu \ (L.Ml \ (L.Mp*z));
    x1 = x1 - mexMatvec(A.mat12,z);
end
%%
Mx = linsysolvefun(L,x1);
%%
if (m2 > 0)
    Mx = [Mx; z];
end
%%*************************************************************************


%++++++++++++++++++++++++++++++++++++++++++++
%PURPOSE: TO DETERMINE THE OPTIMUM VALUE OF
%OVER-RELAXATION (OMEGA) FOR APPLICATION
%IN SOR GS, PICKED OVER MINIMUM VALUE OF
%SOR/GS ITERATION.
%++++++++++++++++++++++++++++++++++++++++++++
function [T]=optimum(om1,om2)
N=200;
eps=1e-6;%error
L=2;
dx=L/N;
n2=25;  %n^2=hP/kA
tb=400+273.15;
ta=34+273.15;
kira=0;

global omega eps N tb ta
warning off

[gs,T]=GS;

omega=om1;
while omega<om2
[sor,T]=SOR;
kira=kira+1;
wye(kira)=sor/gs;
exx(kira)=omega;
omega=omega+0.01;
end

plot(exx,wye);
ylabel('#SOR/#GS RATIO');
xlabel('OMEGA');
grid


function [adfstat,pval,critval,resid,lags,ICs]=augdfautolag(y,p,maxlags,IC)
% Dickey-Fuller and Augmented Dickey Fuller with automatic lag selection
%
% USAGE:
%  [ADFSTAT,PVAL,CRITVAL] = augdfautolag(Y,P,LAGS,IC)
%  [ADFSTAT,PVAL,CRITVAL,RESID,LAGS] = augdfautolag(Y,P,LAGS,IC)
%
% INPUTS:
%  Y         - A T by 1 vector of data
%  P         - Order of the polynomial of include in the ADF regression:
%                0 : No deterministic terms
%                1 : Constant
%                2 : Time Trend
%                3 : Constant, DGP assumed to have a time trend
%  MAXLAGS   - The maximum number of lags to include in the ADF test
%  IC        - [OPTIONAL] String, either 'AIC' (default) or 'BIC' to choose the criteria to select
%                the model
%
% OUTPUTS:
%  ADFSTAT   - Dickey-Fuller statistic
%  PVAL      - Probability the series is a unit root
%  CRITVALS  - A 6 by 1 vector with the [.01 .05 .1 .9 .95 .99] values from the DF distribution
%  LAGS      - The selected number of lags
%  IC        - The value at all lags of the selected IC
%
% COMMENTS:
%
% See also AUGDF

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3.0.1    Date: 1/1/2007

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin<3 || nargin>4
    error('3 or 4 inputs required.')
end
T=length(y);
if T<=(maxlags+1)
    error('Length of data must be larger than LAGS')
end
if size(y,1)~=T,
    y=y';
end
if size(y,2)~=1
    error('Y must be a column vector')
end
if ~isscalar(maxlags) && maxlags>0 && floor(maxlags)==maxlags
    error('LAGS must be a positive integer')
end
if ~isscalar(p)
    error('P must be a scalar integer in {0, 1, 2, 3}')
end
if ~ismember(p,[0 1 2 3])
    error('P must be a scalar integer in {0, 1, 2, 3}')
end
if nargin==3
    IC='AIC';
else
    if ~ischar(IC)
        error('IC must be a string, either ''AIC'' or ''BIC''');
    elseif ~ismember(IC,{'AIC','BIC'})
        error('IC must be a string, either ''AIC'' or ''BIC''');
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Setup common to all problems
%y=y-y(1);
ydiff=diff(y);
[ydiffcurr, ydifflags]=newlagmatrix(ydiff,maxlags); %#ok<ASGLU>
T=length(y);
Y=y(maxlags+2:T);
tau=length(Y);
%
switch p
    case 0
        % Case 1
        i=0;
        X=y(maxlags+1:T-1);
        rho = X\Y;
        % Compute the errors
        e= Y-X*rho;
        s2(i+1)=e'*e/tau;
        K(i+1)=size(X,2);

        % Loop
        for i=1:maxlags
            X=[y(maxlags+1:T-1) ydifflags(:,1:i)];
            rho = X\Y;
            % Compute the errors
            e= Y-X*rho;
            s2(i+1)=e'*e/tau;
            K(i+1)=size(X,2);
        end

    case {1,3}
        %Case 2
        i=0;
        X=[ones(size(Y)) y(maxlags+1:T-1)];
        rho = X\Y;
        % Compute the errors
        e= Y-X*rho;
        s2(i+1)=e'*e/tau;
        K(i+1)=size(X,2);

        % Loop
        for i=1:maxlags
            X=[ones(size(Y)) y(maxlags+1:T-1) ydifflags(:,1:i)];
            rho = X\Y;
            % Compute the errors
            e= Y-X*rho;
            s2(i+1)=e'*e/tau;
            K(i+1)=size(X,2);
        end

    case 2
        %Case 4
        i=0;
        X=[ones(size(Y)) y(maxlags+1:T-1) (1:tau)'];
        rho = X\Y;
        % Compute the errors
        e= Y-X*rho;
        s2(i+1)=e'*e/tau;
        K(i+1)=size(X,2);

        % Loop
        for i=1:maxlags
            X=[ones(size(Y)) y(maxlags+1:T-1) (1:tau)' ydifflags(:,1:i)];
            rho = X\Y;
            % Compute the errors
            e= Y-X*rho;
            s2(i+1)=e'*e/tau;
            K(i+1)=size(X,2);
        end
end

if strcmp(IC,'AIC')
    ICs=log(s2) + 2*K/tau;
else
    ICs=log(s2) + K*log(tau)/tau;
end
[~,lags]=min(ICs);
lags=lags-1;
[adfstat,pval,critval,resid]=augdf(y,p,lags);


function [logCr,logr]=gencorint(x,dim,tau,logr,p,w,svd,q)
%Syntax: [logCr,logr]=gencorint(x,dim,tau,logr,p,w,svd,q)
%________________________________________________________
%
% Calculates the generalized Correlation Integral (Cr) of a time
% series x.
%
% logCr is the the value of log(Cr).
% logr is log(range).
% x is the time series.
% dim is the embedding dimension.
% tau is the time delay.
% p is defines the norm.
% w is the Theiler's correction.
% svd is the number of singular values taken into account.
% q is the generalization index.
%
%
% References:
%
% Grassberger P, Procaccia I (1983): Characterization of strange
% attractors. Physical Review Letters 50(5):346-349
%
% Theiler J (1986): Spurious dimension from correlation algorithms
% applied tolimited time-series data. Physical Review A 34(3):2427-
% 2432
%
% Albano A M, Muench J, Schwartz C, Mees A I, Rapp P E, (1988):
% Singular-value decomposition and the Grassberger-Procaccia algorithm.
% Physical Review A38:3017-3026
%
%
% Alexandros Leontitsis
% Department of Education
% University of Ioannina
% 45110 - Dourouti
% Ioannina
% Greece
%
% University e-mail: me00743@cc.uoi.gr
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% June 15, 2001.

if nargin<1 | isempty(x)==1
   error('You should provide a time series.');
else
   % x must be a vector
   if min(size(x))>1
      error('Invalid time series.');
   end
   x=x(:);
   % n is the time series length
   n=length(x);
end

if nargin<2 | isempty(dim)==1
   dim=2;
else
   % dim must be either a scalar or a vector
   if min(size(dim))>1
      error('dim must be a scalar or a vector.');
   end
   % dim must be an integer
   dim=round(dim);
   % dim values must be above 0
   dim=dim(find(dim>0));
end

if nargin<3 | isempty(tau)==1
   tau=1;
else
   % tau must be either a scalar or a vector
   if min(size(tau))>1
      error('tau must be a scalar or a vector.');
   end
   % tau must be an integer
   tau=round(tau);
   % tau values must be above 0
   tau=tau(find(tau>0));
end

if nargin<4  | isempty(logr)==1
   r=(max(x)-min(x))/10*(1:20)'/20;
   logr=log10(r);
else
    % logr must be either a scalar or a vector
    if min(size(dim))>1
        error('logr must be a scalar or a vector.');
    end
    % if it is a scalar, it determines the maximum range
    if length(logr)==1
        div=logr;
        % div must be positive
        if div<=0
            error('logr must be a positive scalar or vector');
        end
        r=(max(x)-min(x))/div*(1:20)'/20;
        logr=log10(r);
    else
        logr=logr(:);
        r=10.^logr;
    end
end

if nargin<5 | isempty(p)==1
   p=inf;
else
   % p must be either a scalar or a vector
   if min(size(dim))>1
      error('p must be a scalar.');
   end
   % p values must be positive
   p=p(find(p>0));
end

if nargin<6  | isempty(w)==1
   w=1;
else
   % w must be either a scalar or a vector
   if min(size(w))>1
      error('w must be either a scalar or a vector.');
   end
   % w must be an integer
   w=round(w);
   % w must be positive
   w=w(find(w>0));
end

if nargin<7 | isempty(svd)==1
    svd=[];
else
    % svd must be a scalar or a vector
    if min(size(svd))>1
        error('svd must be either a scalar or a vector.');
    end
    % svd must be an integer
    svd=round(svd);
    % svd must be positive
    svd=svd(find(svd>0));
end

if nargin<8 | isempty(q)==1
    q=2;
else
    % q must be either a scalar or a vector
    if min(size(q))>2
        error('q must be either a scalar or a vector.');
    end
end

% Only one of dim, tau, p, w, or q should be vector
l=[length(dim),length(tau),length(p),length(w),length(svd),length(q)];
if length(find(l>1))>1
   error('Only one of dim, tau, p, w, svd, or q should be vector.');
end

m=max(l);
dim=ones(1,m).*dim;
tau=ones(1,m).*tau;
p=ones(1,m).*p;
w=ones(1,m).*w;
if isempty(svd)==0
   svd=ones(1,m).*svd;
end
q=ones(1,m).*q;

for i=1:m

   % Reconstruct the time-delay phase-space
   [Y,T]=phasespace(x,dim(i),tau(i));
   if isempty(svd)==0
      svd(i)=min(svd(i),dim(i));
      % SVD on X
      [u,s,v]=svd(Y,0);

      % Calculate the Principal Components
      pc=Y*v;

      % Reconstruct the first svd Principal Components
      Y=pc(:,1:svd(i))*v(:,1:svd(i))';
   end

   % Initialize the logCr
   Cr=zeros(size(r));

   if q(i)==2 % ...fast

       for i1=1:T-w(i)
           for i2=i1+w(i):T
               dist=norm(Y(i1,:)-Y(i2,:),p(i));
               s=find(r>dist);
               Cr(s)=Cr(s)+1;
           end
       end
       Cr=2*Cr/T/(T-1);
       logCr(:,i)=log10(Cr);

   else % slow...

       for i1=1:T
           c1=zeros(size(r));
           for i2=1:T
               if i1<i2-(w(i)-1) | i1>i2+(w(i)-1)
                   dist=norm(Y(i1,:)-Y(i2,:),p(i));
                   s=find(r>dist);
                   c1(s)=c1(s)+1;
               end
           end
           c1=c1/(T-1);
           if q(i)~=1
               Cr=Cr+c1.^(q(i)-1);
           else
               Cr=Cr+c1;
           end
       end
       if q(i)~=1
           Cr=Cr/T;
           logCr(:,i)=log10(Cr.^(1/(q(i)-1)));
       else
           Cr=log10(Cr)/T;
           logCr(:,i)=Cr;
       end

   end
end


function determ = fourier_sine_determinant ( n )

%*****************************************************************************80
%
%% FOURIER_SINE_DETERMINANT returns the determinant of the FOURIER_SINE matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real DETERM, the determinant.
%
  if ( mod ( n, 2 ) == 1 )
    determ = + 1.0;
  else
    determ = - 1.0;
  end

  return
end


function F  = ...
    obj_actuator_param_pulse_FFT_method(x,frequency_20,frequency_100)
% Computes objective function for determination of basic parameters of
% the proportional valve actuator that match the requirted frequency
% response. The required and actual frequency responses are compared by
% the frequency at which phase shift in -90 deg takes place. The frequency
% response of a nonlinear system is obtained by processing the pulse
% transient characteristic with the FFT algorythm.
% Copyright 2010 MathWorks, Inc.

% frequency_20   - frequency (Hz) at phase shift in -pi/2 at 20% input signal
% frequency_100  - frequency (Hz) at phase shift in -pi/2 at 100% input signal

model = 'actuator_freq_testrig_pulse_FFT_method';
load_system(model);

assignin('base','act_gain', x(1));
assignin('base','time_const', x(2));
assignin('base','act_saturation', x(3));

sim(model);

y_20 = yout(:,2);               % Pulse transient characteristic at 20% input
y_100 = yout(:,1);              % Pulse transient characteristic at 100% input
fs = 1000;                      % Sampling frequency
n = length(y_20);               % Window length = Transform length
y_20_fft = fft(y_20,n);         % Discrete Fourier Transform
y_100_fft = fft(y_100,n);       % Discrete Fourier Transform
f0 = (0:n/2-1)*(fs/n);          % Shifted frequency range, positive range
y_20_0 = fftshift(y_20_fft);    % Shifted DFT at 20% input
y_100_0 = fftshift(y_100_fft);  % Shifted DFT at 100% input
% Phase characteristic at 20% input for positive frequencies after unwrap
phase_20 = unwrap(angle(y_20_0(257:end)));
% Phase characteristic at 100% input for positive frequencies after unwrap
phase_100 = unwrap(angle(y_100_0(257:end)));

% Computing frequency at 90 deg phase shift by interpolation of phase
% characteristics
frq_20 = interp1(phase_20,f0,-pi/2);
frq_100 = interp1(phase_100,f0,-pi/2);

% Objective function as a sum of squared differences between the specified
% and computed frequencies at phase shift angle in -pi/2
F = (frequency_20 - frq_20)^2 + (frequency_100 - frq_100)^2;

end

% EOF


classdef poisson_naive_bayes_CL

% poisson_naive_bayes_CL is a classifier (CL) object that implements a
%   Poisson Naive Bayes classifier.  For each class, the expected number of occurances
%   (denoted lambda) is calculated separately for each feature/neuron, by taking the mean
%   values from the training data for each class.  To evaluate whether a given test point
%   belongs to class i, the log of the likelihood function is calculated using the lambda values as
%   parameters of Poisson distributions for each feature/neuron (i.e., a separate Poisson distribution
%   is calculated for each neuron), and the probability of observing a given number of spikes for
%   a particular neuron is calculated using the lambda value for that
%   neuron.  The overall likelihood value is calculated by multiplying the
%   probabilities for each neuron together (i.e., Naive Bayes classifiers assume that each
%   neuron is independent), or equivalently, adding the log of the probabilities for
%   each neuron together.  The class with the highest likelihood value is chosen as the
%   predicted label, and the decision values are the log likelihood values.
%
%
% Like all CL objects, there are two main methods, which are:
%
%  1.  cl = train(cl, XTr, YTr)
%       This method takes the training data (XTr, YTr) and learns a mean vector
%       (i.e., the lambda values) for each class.
%
%  2. [predicted_labels decision_values] = test(cl, XTe)
%       This method takes the test data an[d calculates the log of the likelihood
%       function for each class (which are the decision values), and returns the class
%       with the highest likelihood as the predicted_label.
%
%  Notes:
%
%  1. This classifier assumes that all feature values are integers.
%
%  2. If the estimate rate parameter (lambda) for any feature is 0 (i.e., if the mean of a feature in the training data is 0),
%       then this 0 lambda estimate will be replaced by the value 1/(num_training_points_in_class_k +1); i.e., we will assume
%       that there is one more training point in which an event occurred.  This prevents errors if an event occurred on a test point
%       but the estimated lambda was 0.
%
%
%   XTr and XTe are in the form [num_features x num_examples]
%   YTr is in the form [num_examples x 1]
%

%==========================================================================

%     This code is part of the Neural Decoding Toolbox.
%     Copyright (C) 2011 by Ethan Meyers (emeyers@mit.edu)
%
%     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 3 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 should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.

%==========================================================================

  properties
        lambdas = [];   % the expected number of occurances for each neuron for each class
        labels = [];  % the unique labels for the different classes
    end

    methods

        % constructor
        function cl = poisson_naive_bayes_CL
        end

        function cl = train(cl, XTr, YTr)

            % added sanity check
            if size(YTr, 2) ~= size(XTr, 2)  &&  size(YTr, 1) ~= size(XTr, 2)
               error('Number of columns in YTr, and XTr must be the same (i.e., there must be one and exactly one label for each data point)')
            end

            % sanity check to make sure the training data only contains integers
%            if sum(sum(abs(round(XTr) - XTr))) > 10.^-2  % (not having it be exactly zero to account for round off error)
%                error(['The training data must only contain integers to use this classifier.  Make sure the data was loaded as integers (e.g., using the appropriate flag in ' ...
%                    'basic_DS/generalization_DS datasources, and that only appropriate feature preprocessors were used (e.g., do not use the zscore_normalize_FP)'])
%            end

            cl.labels = unique(YTr);

            for iLabel = 1:length(cl.labels )

                lambdas(:, iLabel) = mean(XTr(:, (YTr == cl.labels(iLabel))), 2);

                % If the rate parameter (lambda) is zero, there can be problems if the test data has a value greater than zero
                % (i.e., will get zero probability, which creates problems when taking the log).
                % We will deal with this problem by assuming that there is one more data point with a 1 (when all training data has zero occurances)
                zero_lambda_replacement_value = 1/(length(find(YTr == cl.labels(iLabel))) + 1);   % if found zeros for all trials, assume that there is one more trial where a 1 was found...
                lambdas((lambdas(:, iLabel) == 0), iLabel) = zero_lambda_replacement_value;

            end

            cl.lambdas = lambdas;

        end

        function [predicted_labels decision_values] = test(cl, XTe)

            % sanity check to make sure the test data only contains integers
%            if sum(sum(abs(round(XTe) - XTe))) > 10.^-2   % (not having it be exactly zero to account for round off error)
%                error(['The test data must only contain integers to use this classifier.  Make sure the data was loaded as integers (e.g., using the appropriate flag in ' ...
%                    'basic_DS/generalization_DS datasources, and that only appropriate feature preprocessors were used (e.g., do not use the zscore_normalize_FP)'])
%            end

            % compute simultaneously all p-values for all features and test points (easy b/c everything is independent)
            curr_lambdas = repmat(cl.lambdas, [1, 1, size(XTe, 2)]);
            XTe_repmat_for_all_classes = permute(repmat(XTe, [1, 1, size(cl.lambdas, 2)]), [1 3 2]);
            %p_vals = exp(-curr_lambdas + XTe_repmat_for_all_classes  .* log(curr_lambdas) - gammaln(XTe_repmat_for_all_classes  + 1));
            %log_likelihoods = squeeze(sum(log(p_vals)));

            log_likelihoods = squeeze(sum(-curr_lambdas + XTe_repmat_for_all_classes  .* log(curr_lambdas) - gammaln(XTe_repmat_for_all_classes  + 1), 1)); % possibly even faster...
            % % %log_likelihoods = squeeze(sum(-curr_lambdas + XTe_repmat_for_all_classes  .* log(curr_lambdas))); % since the factorial term does not depend on the class, code might be faster
                                                                                % if this term is removed.  The decision values returned
                                                                                % will no longer be exactly the log likelihood values but rather the log likelihood
                                                                                % values minus a constant.  Using this will make AUROC values worse, so don't do it!!!!

           % prior to version 1.0.4 the following line was used.  I added a the sum over the first dimention (i.e., sum(XXX, 1)) so that the code will run even when only a single site is used
           %log_likelihoods = squeeze(sum(-curr_lambdas + XTe_repmat_for_all_classes  .* log(curr_lambdas) - gammaln(XTe_repmat_for_all_classes  + 1))); % possibly even faster...

            [vals inds] = randmax(log_likelihoods);
            predicted_labels  = cl.labels(inds);
            decision_values = log_likelihoods';

        end

    end  % end public methods

end   % end classdef


function [Z, Z_L, Z_U, T, P, rho, c, g, mu, nu, k, n, n_sum] = atmo(alt,division,units)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Program:    1976 Standard Atmosphere Calculator[0-1000 km]
%   Author:     Brent Lewis(RocketLion@gmail.com)
%               University of Colorado-Boulder
%   History:    Original-1/10/2007
%               Revision-1/12/2007-Corrected for changes in Matlab versions
%               for backward compatability-Many thanks to Rich
%               Rieber(rrieber@gmail.com)
%   Input:      alt:        Final Geometric Altitude[km]
%               division:   Reporting points for output arrays[km]
%                           (.01 km & Divisible by .01 km)
%               units:      1-[Metric]
%                           2-{English}
%   Default:    Values used if no input
%               alt:        1000 km
%               division:   1 km
%               units:      Metric
%   Output:     Each value has a specific region that it is valid in with this model
%               and is only printed out in that region
%               Z:          Total Reporting Altitudes[0<=alt<=1000 km][km]{ft}
%               Z_L:        Lower Atmosphere Reporting Altitudes[0<=alt<=86 km][km]{ft}
%               Z_U:        Upper Atmosphere Reporting Altitudes[86<=alt<=1000 km][km]{ft}
%               T:          Temperature array[0<=alt<=1000 km][K]{R}
%               P:          Pressure array[0<=alt<=1000 km][Pa]{in_Hg}
%               rho:        Density array[0<=alt<=1000 km][kg/m^3]{lb/ft^3}
%               c:          Speed of sound array[0<=alt<=86 km][m/s]{ft/s}
%               g:          Gravity array[0<=alt<=1000 km][m/s^2]{ft/s^2}
%               mu:         Dynamic Viscosity array[0<=alt<=86 km][N*s/m^2]{lb/(ft*s)}
%               nu:         Kinematic Viscosity array[0<=alt<=86 km][m^2/s]{ft^2/s}
%               k:          Coefficient of Thermal Conductivity
%                           array[0<=alt<=86 km][W/(m*K)]{BTU/(ft*s*R)}
%               n:          Number Density of individual gases
%                           (N2 O O2 Ar He H)[86km<=alt<=1000km][1/m^3]{1/ft^3}
%               n_sum:      Number Density of total gases
%                           [86km<=alt<=1000km][1/m^3]{1/ft^3}
%   Acknowledgements:       1976 U.S. Standard Atmosphere
%                           Prof. Adam Norris-Numerical Analysis Class
%                           Steven S. Pietrobon USSA1976 Program
%   Notes:                  Program uses a 5-point Simpson's Rule in 10
%                           meter increments.  Results DO vary by less 1%
%                           compared to tabulated values and is probably
%                           caused by different integration techniques
%   Examples:               atmo() will compute the full atmosphere in 1 km
%                           increments and output in Metric Units
%                           atmo(10) will compute the atmosphere between 0
%                           and 10 km in 1 km increments and output in
%                           Metric Units
%                           atmo(20,.1,2) will compute the atmosphere
%                           between 0 and 20 km in 100 m increments and
%                           output in English Units
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin == 0
    alt = 1000;
    division = 1;
    units = 1;
elseif nargin == 1
    division = 1;
    units = 1;
elseif nargin == 2
    units = 1;
end

%   Error Reporting
if nargin > 3
    error('Too many inputs')
elseif mod(division,.01) ~= 0
    error('Divisions must be multiples of .01 km')
elseif units ~= 1 && units ~= 2
    error('Units Choice Invalid[1-Metric,2-English]')
elseif alt<0 || alt>1000
    error('Program only valid for 0<altitudes<1000 km')
end

%   Matrix Pre-allocation
if alt <= 86
    Z_L = (0:division:alt)';
    Z_U = [];
    n = [];
else
    Z_L = (0:division:86)';
    Z_U = (86:division:alt)';
    if mod(86,division) ~= 0
        Z_L = [Z_L; 86];
    end
    if mod(alt-86,division) ~= 0
        Z_U = [Z_U; alt];
    end
end
T_L = zeros(size(Z_L));
T_M_L = T_L;
T_U = zeros(size(Z_U));

%   Conversion Factor Used in 80<alt<86 km
Z_M = 80:.5:86;
M_M_0 = [1 .999996 .999989 .999971 .999941 .999909 ...
    .999870 .999829 .999786 .999741 .999694 .999641 .999579];

%   Constants
M_0 = 28.9644;
M_i = [28.0134; 15.9994; 31.9988; 39.948; 4.0026; 1.00797];
beta = 1.458e-6;
gamma = 1.4;
g_0 = 9.80665;
R = 8.31432e3;
r_E = 6.356766e3;
S = 110.4;
N_A = 6.022169e26;

%   Temperature
for i = 1 : length(Z_L)
    T_L(i,1) = atmo_temp(Z_L(i));
    T_M_L(i,1) = T_L(i,1);
    if Z_L(i) > 80 && Z_L(i) < 86
        T_L(i,1) = T_L(i)*interp1(Z_M,M_M_0,Z_L(i));
    end
end
for i = 1 : length(Z_U)
    T_U(i,1) = atmo_temp(Z_U(i));
end

%   Number Density
if alt > 86
    n = atmo_compo(alt,division);
    n_sum = sum(n,2);
else
    n = [];
    n_sum = [];
end

%   Pressure
P_L = atmo_p(Z_L);
P_U = atmo_p(Z_U,T_U,n_sum);

%   Density
rho_L = M_0*P_L./(R*T_M_L);
if ~isempty(P_U)
    rho_U = n*M_i/N_A;
else
    rho_U = [];
end

%   Speed of Sound
c = sqrt(gamma*R*T_M_L/M_0);
%   Dynamic Viscosity
mu = beta*T_L.^1.5./(T_L+S);
%   Kinematic Viscosity
nu = mu./rho_L;
%   Thermal Conductivity Coefficient
k = 2.64638e-3*T_L.^1.5./(T_L+245*10.^(-12./T_L));

%   Combine Models
T = [T_L(1:end-1*double(~isempty(T_U)));T_U];
P = [P_L(1:end-1*double(~isempty(T_U)));P_U];
rho = [rho_L(1:end-1*double(~isempty(T_U)));rho_U];
Z = [Z_L(1:end-1*double(~isempty(T_U)));Z_U];

%   Gravity
g = g_0*(r_E./(r_E+Z)).^2;

if units == 2
    unit_c = [3.048e-1 3.048e-1 3.048e-1 5/9 0.0001450377 1.6018463e1...
        3.048e-1 3.048e-1 1.488163944 9.290304e-2 6.226477504e-3...
        3.531466672e2 3.531466672e2];
    Z = Z/unit_c(1);
    Z_L = Z_L/unit_c(2);
    Z_U = Z_U/unit_c(3);
    T = T/unit_c(4);
    P = P/unit_c(5);
    rho = rho/unit_c(6);
    c = c/unit_c(7);
    g = g/unit_c(8);
    mu = mu/unit_c(9);
    nu = nu/unit_c(10);
    k = n/unit_c(11);
    n_sum = n_sum/unit_c(12);
end


function [ss_estimates,ss_estimates_constant,ss_estimates_exogenous,ss_estimates_contribution_exo]=ssestimates(ss_record,n,T,cband)

% function [ss_estimates]=ssestimates(ss_record,n,T,cband)
% calculates the point estimate (median), lower bound and upper bound of the steady-state from the posterior distribution
% inputs:  - cell 'ss_record': record of the gibbs sampler draws for the steady-state
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'T': number of sample time periods (defined p 7 of technical guide)
%          - scalar 'cband': confidence level for VAR coefficients
% outputs: - cell 'ss_estimates': lower bound, point estimates, and upper bound for the steady-state

% create first the cell that will contain the estimates
ss_estimates=cell(n,1);
ss_estimates_constant=cell(n,1);
ss_estimates_exogenous=cell(n,1);
ss_estimates_contribution_exo=cell(1,1);
% for each variable and each sample period, compute the median, lower and upper bound from the Gibbs sampler records
% consider variables in turn
for ii=1:n
   % consider sample periods in turn
   for jj=1:T
   % compute first the lower bound
   ss_estimates{ii,1}(1,jj)=quantile(ss_record{ii,1}(:,jj),(1-cband)/2);
   % then compute the median
   ss_estimates{ii,1}(2,jj)=quantile(ss_record{ii,1}(:,jj),0.5);
   % finally compute the upper bound
   ss_estimates{ii,1}(3,jj)=quantile(ss_record{ii,1}(:,jj),(1-(1-cband)/2));
% % %    if m>1
% % %    % same for only the constant
% % %    ss_estimates_constant{ii,1}(1,jj)=quantile(ss_record_constant{ii,1}(:,jj),(1-cband)/2);
% % %    % then compute the median
% % %    ss_estimates_constant{ii,1}(2,jj)=quantile(ss_record_constant{ii,1}(:,jj),0.5);
% % %    % finally compute the upper bound
% % %    ss_estimates_constant{ii,1}(3,jj)=quantile(ss_record_constant{ii,1}(:,jj),(1-(1-cband)/2));
% % %    % same for only the exogenous
% % %    ss_estimates_exogenous{ii,1}(1,jj)=quantile(ss_record_exogenous{ii,1}(:,jj),(1-cband)/2);
% % %    % then compute the median
% % %    ss_estimates_exogenous{ii,1}(2,jj)=quantile(ss_record_exogenous{ii,1}(:,jj),0.5);
% % %    % finally compute the upper bound
% % %    ss_estimates_exogenous{ii,1}(3,jj)=quantile(ss_record_exogenous{ii,1}(:,jj),(1-(1-cband)/2));
% % %    % compute contribution from only the exogenous part
% % %    ss_estimates_contribution_exo{ii,1}(1,jj)=(quantile(ss_record_constant{ii,1}(:,jj),0.5)-quantile(ss_record{ii,1}(:,jj),0.5));
% % %    else
% % %    ss_estimates_constant=[];
% % %    ss_estimates_exogenous=[];
% % %    end
   end
end


function [gen, ave1, ave2] = fusion_strategy(features_a, features_b, source_a, source_b, unit)

[m,n] = size(features_a);
[m1,n1] = size(source_a);
ave_temp1 = zeros(m1,n1);
ave_temp2 = zeros(m1,n1);
weight_ave_temp1 = zeros(m1,n1);
weight_ave_temp2 = zeros(m1,n1);

for i=2:m-1
    for j=2:n-1
        A1 =sum(sum(features_a(i-1:i+1,j-1:j+1)))/9;
        A2 =sum(sum(features_b(i-1:i+1,j-1:j+1)))/9;

        % weight average
        weight_ave_temp1(((i-2)*unit+1):((i-1)*unit),((j-2)*unit+1):((j-1)*unit)) = A1/(A1+A2);
        weight_ave_temp2(((i-2)*unit+1):((i-1)*unit),((j-2)*unit+1):((j-1)*unit)) = A2/(A1+A2);
        ave_temp1(((i-2)*unit+1):((i-1)*unit),((j-2)*unit+1):((j-1)*unit)) = A1;
        ave_temp2(((i-2)*unit+1):((i-1)*unit),((j-2)*unit+1):((j-1)*unit)) = A2;
%         % choose max
%         if A1>A2
%             gen_temp(((i-2)*unit+1):((i-1)*unit),((j-2)*unit+1):((j-1)*unit)) = ones(unit,unit);
%             temp_mask(i,j) = 1;
%         end
    end
end
% figure;imshow(temp_mask);
weight_ave_temp1 = weight_ave_temp1(1:m1,1:n1);
weight_ave_temp2 = weight_ave_temp2(1:m1,1:n1);
% figure;imshow(weight_ave_temp1);
% figure;imshow(weight_ave_temp2);

gen = source_a.*weight_ave_temp1 + source_b.*weight_ave_temp2;
% figure;imshow(gen);

ave1 = ave_temp1;
ave2 = ave_temp2;
end


function [xi] = logSE3(chi)
C = chi(1:3,1:3);
r = chi(1:3,4);
[vecs,eigs] = eig(C);
[~,idx] = min(abs(diag(eigs)-1));
a = vecs(:,idx);

phi = acos(1/2*(trace(C)-1));
if phi == 0
    a = zeros(3,1);
    iJ = eye(3);
else
    C1 = cos(phi)*eye(3) + (1-cos(phi))*a*transpose(a) + sin(phi)*[[0,-a(3),a(2)];[a(3),0,-a(1)];[-a(2),a(1),0]];
    C2 = cos(-phi)*eye(3) + (1-cos(-phi))*a*transpose(a) + sin(-phi)*[[0,-a(3),a(2)];[a(3),0,-a(1)];[-a(2),a(1),0]];
    if norm(C2-C)<norm(C1-C)
        phi = -phi;
    end
    iJ = phi/2*cot(phi/2)*eye(3) + (1-phi/2*cot(phi/2))*a*a' - phi/2*[[0,-a(3),a(2)];[a(3),0,-a(1)];[-a(2),a(1),0]];
end
rho = iJ*r;
xi = real([phi*a;rho]);
end


function b = GetB_Values(protocol)
% Computes the b value for each measurement in the protocol
%
% b = GetB_Values(protocol)
% returns an array of b values, one for each measurement defined
% in the list of measurements in the protocol.
%
% author: Daniel C Alexander (d.alexander@ucl.ac.uk)
%

GAMMA = 2.675987E8;
if(strcmp(protocol.pulseseq, 'PGSE') || strcmp(protocol.pulseseq, 'STEAM'))
    modQ = GAMMA*protocol.smalldel.*protocol.G;
    diffTime = protocol.delta - protocol.smalldel/3;
    b = diffTime.*modQ.^2;

elseif(strcmp(protocol.pulseseq, 'DSE'))
    b = getB_ValuesDSE(protocol.G, protocol.delta1, protocol.delta2, protocol.delta3, protocol.t1, protocol.t2, protocol.t3);

elseif(strcmp(protocol.pulseseq, 'OGSE'))
    b = GetB_ValuesOGSE(protocol.G, protocol.delta, protocol.smalldel, protocol.omega);
end


function [X,Y,Z] = idp3elli(idp,IDP,ns,NP)

%IDP3ELLI 3D ellipsoid from Gaussian IDP.
%   [X,Y,Z] = IDP3ELLI(x0,P,ns,NP) gives X, Y and Z coordinates of the
%   points corresponding to the 2 biggest semi-diametres of the ellipsoid
%   defined by the covariances matrix P and centered at x0:
%
%        (x-x0)'*(P^-1)*(x-x0) = ns^2.
%
%   where P and x0 are obtained by transforming the input inverse-depth
%   point idp and covariance IDP to an euclidean point:
%
%       x0 = idp2p(idp)
%       P = J*IDP*J'
%
%   being J the Jacobian of the conversion function. This conversion is
%   performed internally by the function PROPAGATEUNCERTAINTY.
%
%   The ellipsoid can be plotted in a 3D graphic by just creating a line
%   with line(X,Y,Z).
%
%   See also COV3ELLI, LINE, IDP2P, PROPAGATEUNCERTAINTY.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

[p,P] = propagateUncertainty(idp,IDP,@idp2p);

[X,Y,Z] = cov3elli(p,P,ns,NP);

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB 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 should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE