function inside = p05_inside ( m, n, point )

%*****************************************************************************80
%
%% P05_INSIDE reports if a point is inside the region in problem 05.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real POINT(M,N), the coordinates of the points.
%
%    Output, logical INSIDE(N), is TRUE if the point is in the region.
%
  center1 = [  0.0, 0.0 ];
  center2 = [ -0.4, 0.0 ];
  r1 =   1.00;
  r2 =   0.55;

  inside(1:n) =                                                   ...
       center1(2) <=   point(2,1:n)                               ...
       &                                                          ...
                     ( point(1,1:n) - center1(1) ).^2             ...
                   + ( point(2,1:n) - center1(2) ).^2  <= r1 * r1 ...
       &                                                          ...
       r2 * r2 <=    ( point(1,1:n) - center2(1) ).^2             ...
                   + ( point(2,1:n) - center2(2) ).^2;

  return
end


function lbeta_test ( )

%*****************************************************************************80
%
%% LBETA_TEST tests R4_LBETA and R8_LBETA.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LBETA_TEST:\n' );
  fprintf ( 1, '  Test BETA_LOG_VALUES, R4_LBETA, R8_LBETA.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             A               B     LBETA(A,B)\n' );
  fprintf ( 1, '                                R4_LBETA(A,B)         Diff\n' );
  fprintf ( 1, '                                R8_LBETA(A,B)         Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, a, b, fx1 ] = beta_log_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_lbeta ( single ( a ), single ( b ) );
    fx3 = r8_lbeta ( a, b );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14.4f  %14.4g  %14.6g\n', a, b, fx1 );
    fprintf ( 1, '                                  %14.6g  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                                  %14.6g  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  return
end


function [ dim_num, node_num, element_num, element_order, node_data_num, ...
  node_coord, element_node, node_data ] = tec_read ( tec_file_name )

%*****************************************************************************80
%
%% TEC_READ reads finite element data from a TEC file.
%
%  Discussion:
%
%    This program reads a TEC file containing finite element data,
%    and writes that data out to three files that constitute an FEM model,
%    that is,
%    * a file of node coordinates;
%    * a file of elements defined by the nodes that form them;
%    * a file of node data.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, character TEC_FILE_NAME(*), the name of the TEC file.
%
%    Output, integer DIM_NUM, the spatial dimension, inferred from the
%    names of the variables.
%
%    Output, integer NODE_NUM, the number of nodes, determined by the
%    "N=" argument.
%
%    Output, integer ELEMENT_NUM, the number of elements, inferred from the
%    "E=" argument.
%
%    Output, integer ELEMENT_ORDER, the order of the elements, inferred from
%    the "ZONETYPE=" argument.
%
%    Output, integer NODE_DATA_NUM, the number of data items per node,
%    inferred from the the number of node data items, minus those which are
%    inferred to be spatial coordinates.
%
%    Output, real NODE_COORD(DIM_NUM,NODE_NUM), the coordinates of nodes.
%
%    Output, integer ELEMENT_NODE(ELEMENT_ORDER,ELEMENT_NUM);
%    the global index of local node I in element J.
%
%    Output, real NODE_DATA(NODE_DATA_NUM,NODE_NUM), the data values associated
%    with each node.
%
  dim_num = -1;
  node_num = -1;
  element_num = -1;
  element_order = -1;
  node_data_num = -1;
  node_coord = [];
  element_node = [];
  node_data = [];

  tec_file_unit = fopen ( tec_file_name );

  if ( tec_file_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEC_READ - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', tec_file_name );
    error ( 'TEC_READ - Error!' );
    return;
  end
%
%  Read and parse the TITLE line.
%  But it is optional, so you may have just read the VARIABLES line instead!
%
  line = ' ';

  while ( s_len_trim ( line ) == 0 )
    line = fgetl ( tec_file_unit );
  end
%
%  Read the VARIABLES line.
%
%  Because the TITLE line is apparently optional, we may have already
%  read the VARIABLES line!
%
  if ( s_begin ( line, 'TITLE=' ) )
    line = ' ';
    while ( s_len_trim ( line ) == 0 )
      line = fgetl ( tec_file_unit );
    end
  end

  if ( ~s_begin ( line, 'VARIABLES=' ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEC_READ - Fatal error!\n' );
    fprintf ( 1, '  The VARIABLES = line is missing in the file.\n' );
    error ( 'TEC_READ - Fatal error!' );
  end
%
%  Parse the VARIABLES line.
%  VARIABLES = name1 name2 name3...
%  The names may be quoted, and are separated by quotes, commas or spaces.
%
  [ variable_num, variable_name_length, variable_name ] ...
    = tec_variable_line_parse ( line );
%
%  Based on the variable names, determine the spatial dimension and the number
%  of node data items.
%
%  For now, we SIMPLY ASSUME that the spatial coordinates are listed first.
%  Hence, when we read the node data, we assume that the first DIM_NUM values
%  represent X, Y and possibly Z.
%
  dim_num = 0;
  node_data_num = variable_num;

  begin = 0;
  for variable = 1 : variable_num
    if ( variable_name_length(variable) == 1 )
      name = variable_name(begin+1);
      if ( ch_eqi ( name, 'X' ) || ...
          ch_eqi ( name, 'Y' ) || ...
          ch_eqi ( name, 'Z' ) )
        dim_num = dim_num + 1;
        node_data_num = node_data_num - 1;
      end
    end
    begin = begin + variable_name_length(variable);
  end
%
%  Read and parse the ZONE line.
%
  line = ' ';
  while ( s_len_trim ( line ) == 0 )
    line = fgetl ( tec_file_unit );
  end

  if ( ~s_begin ( line, 'ZONE' ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEC_READ - Fatal error!\n' );
    fprintf ( 1, '  The ZONE = line is missing in the file.\n' );
    error ( 'TEC_READ - Fatal error!' );
  end

  [ node_num, element_num, element_type ] = tec_zone_line_parse ( line );
%
%  Based on ELEMENT_TYPE, determine the element order.
%
  if ( s_eqi ( element_type, 'FETRIANGLE' ) )
    element_order = 3;
  elseif ( s_eqi ( element_type, 'FEQUADRILATERAL' ) )
    element_order = 4;
  elseif ( s_eqi ( element_type, 'FETETRAHEDRON' ) )
    element_order = 4;
  elseif ( s_eqi ( element_type, 'FEBRICK' ) )
    element_order = 8;
  else
    element_order = -1;
  end
%
%  Build up the format string for reading DIM_NUM + NODE_DATA_NUM reals.
%
  format = ' ';

  for i = 1 : dim_num + node_data_num
    format = strcat ( format, ' %f' );
  end

  node_coord = zeros ( dim_num, node_num );
  node_data = zeros ( node_data_num, node_num );
%
%  Now read the node coordinates and node data.
%
  for node = 1 : node_num

    line = ' ';
    while ( s_len_trim ( line ) == 0 )
      line = fgetl ( tec_file_unit );
    end

    [ x, count ] = sscanf ( line, format );

    if ( count == dim_num + node_data_num )
      node_coord(1:dim_num,node) = x(1:dim_num);

      node_data(1:node_data_num,node) = x(dim_num+1:dim_num+node_data_num);
    end

  end
%
%  Build up the format string for reading ELEMENT_ORDER integers.
%
  format = ' ';

  for element = 1 : element_order
    format = strcat ( format, ' %d' );
  end

  element_node = zeros ( element_order, element_num );
%
%  Now read the element data.
%
  for element = 1 : element_num

    line = ' ';
    while ( s_len_trim ( line ) == 0 )
      line = fgetl ( tec_file_unit );
    end

    [ x, count ] = sscanf ( line, format );

    if ( count == element_order )
      element_node(1:element_order,element) = x(1:element_order);
    end

  end

  fclose ( tec_file_unit );

  return
end


 function [sino, Hk, hn, nn] = fbp2_sino_filter(type, sino, varargin)
%function [sino, Hk, hn, nn] = fbp2_sino_filter(type, sino, [options])
%|
%| Apply ramp-like filters to sinogram(s) for 2D FBP image reconstruction.
%| Both parallel-beam and fan-beam tomographic geometries are supported.
%| This approach of sampling the band-limited ramp avoids the aliasing that
%| would be caused by sampling the ramp directly in the frequency domain.
%|
%| in
%|	type		'arc' (3rd generation CT) or 'flat' (for parallel too)
%|	sino	[nb (L)] sinogram(s)
%| options
%|	dr | ds	(real)	sample spacing (in distance units, e.g., cm) (default 1)
%|	dsd	(real)	source-to-detector distance, for 'arc' case only.
%|	extra		# of extra sinogram radial samples to keep (default: 0)
%|	npad		# of padded samples. (default: 0, means next power of 2)
%|	decon1		deconvolve effect of linear interpolator? (default: 1)
%|	window	[npad]	samples of apodization window function
%|			for [-np/2,...,np/2-1]. (default: '' = plain ramp)
%|			or a string like 'hann' for some predefined windows
%| out
%|	sino	[nb (L)] filtered sinogram rows
%|	Hk	[npad]	apodized ramp filter frequency response
%|	hn	[npad]	samples of band-limited ramp filter
%|	nn	[npad]	[-np/2,...,np/2-1] vector for convenience
%|
%| Copyright 2005-12-19, Jeff Fessler, University of Michigan

if nargin == 1 && streq(type, 'test'), fbp2_sino_filter_test, return, end
if nargin < 2, ir_usage, end

arg.ds = 1;
arg.dsd = [];
arg.window = '';
arg.extra = 0;
arg.npad = 0;
arg.decon1 = false;
arg = vararg_pair(arg, varargin, 'subs', {'dr', 'ds'; 'Dsd', 'dsd'});

dims = size(sino);
sino = reshape(sino, dims(1), []);
[sino Hk hn nn] = fbp2_sino_filter_do(type, sino, ...
	arg.ds, arg.dsd, arg.window, arg.extra, arg.npad, arg.decon1);
sino = reshape(sino, [size(sino, 1) dims(2:end)]);

%
% fbp2_sino_filter_do()
%
function [sino, Hk, hn, nn] = fbp2_sino_filter_do(type, sino, ...
	ds, dsd, window, extra, npad, decon1);

[nb na] = size(sino);
if ~npad
	npad = 2^ceil(log2(2*nb-1)); % padded size
%	printm('nb=%d npad=%d', nb, npad)
end
sino = [sino; zeros(npad-nb,na)]; % padded sinogram

[hn nn] = fbp_ramp(type, npad, ds, dsd);

Hk = reale(fft(fftshift(hn)));

Hk = Hk .* fbp2_window(npad, window);

Hk = ds * Hk; % differential for discrete-space convolution vs integral

% linear interpolation is like blur with a triangular response,
% so we can compensate for this approximately in frequency domain
if decon1
	Hk = Hk ./ fftshift(nufft_sinc(nn / npad).^2);
end

sino = ifft_sym( fft(sino, [], 1) .* repmat(Hk, [1 na]), [], 1); % apply filter

% trick: possibly keep extra column(s) for zeros!
sino = sino([1:(nb+extra)],:);
sino([(nb+1):(nb+extra)],:) = 0;

%
% test
%
function fbp2_sino_filter_test
nb = 2^5;
sino = zeros(nb,2); sino(nb/2+1,1) = 1;
ds = 1;
dsd = 30;
[sino1 H1 h1 nn] = fbp2_sino_filter('arc', sino, 'ds', ds, 'dsd', dsd);
[sino2 H2 h2 nn] = fbp2_sino_filter('flat', sino, 'ds', ds);
[sino2 H3 h2 nn] = fbp2_sino_filter('flat', sino, 'ds', ds, 'decon1', 1);
%max_percent_diff(h1, h2) % small!
%max_percent_diff(H1, H2) % small!

if im
	clf, subplot(121)
	plot(nn, h1, 'o', nn, h2, '+')
	xlabel 'n', ylabel 'h[n]'
	axis tight
	legend('arc', 'flat')

	subplot(122)
	plot(nn, fftshift(H1), 'o', nn, fftshift(H2), '+', nn, fftshift(H3), '.')
	xlabel 'k', ylabel 'H[k]', axis tight, axisy(0, 0.5)
	legend('arc', 'flat', 'decon', 'location', 'north')
end


function indx = multigrid_index0 ( dim_num, order_1d, order_nd )

%*****************************************************************************80
%
%% MULTIGRID_INDEX0 returns an indexed multidimensional grid.
%
%  Discussion:
%
%    For dimension DIM, the second index of INDX may vary from
%    0 to ORDER_1D(DIM)-1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension of the points.
%
%    Input, integer ORDER_1D(DIM_NUM), the order of the
%    rule in each dimension.
%
%    Input, integer ORDER_ND, the product of the entries of ORDER_1D.
%
%    Output, integer INDX(DIM_NUM,ORDER_ND), the indices of the points in
%    the grid.  The second dimension of this array is equal to the
%    product of the entries of ORDER_1D.
%
  p = 0;
  indx = zeros(dim_num,order_nd);

  a = [];
  more = 0;

  while ( 1 )

    [ a, more ] = vec_colex_next2 ( dim_num, order_1d, a, more );

    if ( ~more )
      break
    end

    p = p + 1;

    indx(1:dim_num,p) = a(1:dim_num)';

  end

  return
end


function [ x, out, opts ] = tfocs_TS( smoothF, affineF, projectorF, x0, opts )
% TFOCS_TS Tseng's modification of Nesterov's 2007 method.
% [ x, out, opts ] = tfocs_TS( smoothF, affineF, projectorF, x0, opts )
%   Implements Tseng's modification of the Nesterov 2007 method.
%   A variety of calling sequences are supported; type
%      help tfocs_help
%   for a full explanation.

% Nov 17 2016, hack for Matlab R2016b not allowing nargin/nargout in
% tfocs_initialize:
narginn = nargin; nargoutt = nargout;

% Initialization
alg = 'TS';
algorithm = 'Tseng''s single-projection modification of Nesterov''s 2007 method';
alpha = 0; beta = 0; mu = 0; L = 0; % Necessary due to MATLAB quirk
tfocs_initialize
if nargin == 0, return; end

while true,

      x_old =   x;
    A_x_old = A_x;
      z_old =   z;
    A_z_old = A_z;

    % The backtracking loop
    L_old      = L;
   	L          = L * alpha;
    theta_old  = theta;
    while true,

    	% Acceleration
        theta = 2 ./ ( 1 + sqrt( 1 + 4 * L / L_old / theta_old^2 ) );

        % Next iterate
        if theta < 1,
              y = ( 1 - theta ) *   x_old + theta *   z_old;
            A_y = ( 1 - theta ) * A_x_old + theta * A_z_old;
            f_y = Inf; g_Ay = []; g_y = []; C_y = Inf;
        end

        % Compute function values
        if isempty( g_y ),
            if isempty( g_Ay ), [ f_y, g_Ay ] = apply_smooth( A_y ); end
            g_y = apply_linear( g_Ay, 2 );
        end

        % Accumulated gradient
        if theta == 1,
            x_cent = x_old;
            g_a = g_y;
        else
            g_a = ( 1 - theta ) * g_a_old + theta * g_y;
        end
        step = 1 / ( theta^2 * L );
        [ C_z, z ] = apply_projector( x_cent - step * g_a, step );
        A_z = apply_linear( z, 1 );

        % New iterate
        if theta == 1,
            x   = z;
            A_x = A_z;
            C_x = C_z;
        else
            x   = ( 1 - theta ) *   x_old + theta *   z;
            A_x = ( 1 - theta ) * A_x_old + theta * A_z;
            C_x = Inf;
        end
        f_x = Inf; g_Ax = []; g_x = [];

        % Bactracking test
        tfocs_backtrack
        if do_break, break; end % new, for R2015b compatibility

    end

    % Collect data, evaluate stopping criteria, and print status
    tfocs_iterate
    if do_break, break; end % new, for R2015b compatibility

    g_a_old = g_a;

end

% Final processing
tfocs_cleanup

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function plotData(X, y)
%PLOTDATA Plots the data points X and y into a new figure
%   PLOTDATA(x,y) plots the data points with + for the positive examples
%   and o for the negative examples. X is assumed to be a Mx2 matrix.

% Create New Figure
figure; hold on;

% ====================== YOUR CODE HERE ======================
% Instructions: Plot the positive and negative examples on a
%               2D plot, using the option 'k+' for the positive
%               examples and 'ko' for the negative examples.
%

% Find Indices of Positive and Negative Examples

pos = find(y==1);
neg = find(y == 0);
% Plot Examples

plot(X(pos, 1), X(pos, 2), 'k+','LineWidth', 2,'MarkerSize', 7);
plot(X(neg, 1), X(neg, 2), 'ko', 'MarkerFaceColor', 'y', 'MarkerSize', 7);

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

hold off;

end


%TENSOR Class for dense tensors.
%
%TENSOR Methods:
%   and         - Logical AND (&) for tensors.
%   collapse    - Collapse tensor along specified dimensions.
%   contract    - Contract tensor along two dimensions (array trace).
%   ctranspose  - is not defined for tensors.
%   disp        - Command window display of a tensor.
%   display     - Command window display of a tensor.
%   double      - Convert tensor to double array.
%   end         - Last index of indexing expression for tensor.
%   eq          - Equal (==) for tensors.
%   find        - Find subscripts of nonzero elements in a tensor.
%   full        - Convert to a (dense) tensor.
%   ge          - Greater than or equal (>=) for tensors.
%   gt          - Greater than (>) for tensors.
%   innerprod   - Efficient inner product with a tensor.
%   isequal     - for tensors.
%   issymmetric - Verify that a tensor X is symmetric in specified modes.
%   ldivide     - Left array divide for tensor.
%   le          - Less than or equal (<=) for tensor.
%   lt          - Less than (<) for tensor.
%   minus       - Binary subtraction (-) for tensors.
%   mldivide    - Slash left division for tensors.
%   mrdivide    - Slash right division for tensors.
%   mtimes      - tensor-scalar multiplication.
%   mttkrp      - Matricized tensor times Khatri-Rao product for tensor.
%   ndims       - Return the number of dimensions of a tensor.
%   ne          - Not equal (~=) for tensors.
%   nnz         - Number of nonzeros for tensors.
%   norm        - Frobenius norm of a tensor.
%   not         - Logical NOT (~) for tensors.
%   nvecs       - Compute the leading mode-n vectors for a tensor.
%   or          - Logical OR (|) for tensors.
%   permute     - Permute tensor dimensions.
%   plus        - Binary addition (+) for tensors.
%   power       - Elementwise power (.^) operator for a tensor.
%   rdivide     - Right array divide for tensors.
%   reshape     - Change tensor size.
%   scale       - Scale along specified dimensions of tensor.
%   size        - Tensor dimensions.
%   squeeze     - Remove singleton dimensions from a tensor.
%   subsasgn    - Subscripted assignment for a tensor.
%   subsref     - Subscripted reference for tensors.
%   symmetrize  - Symmetrize a tensor X in specified modes.
%   tenfun      - Apply a function to each element in a tensor.
%   tensor      - Create tensor.
%   times       - Array multiplication for tensors.
%   transpose   - is not defined on tensors.
%   ttm         - Tensor times matrix.
%   ttsv        - Tensor times same vector in multiple modes.
%   ttt         - Tensor mulitplication (tensor times tensor).
%   ttv         - Tensor times vector.
%   uminus      - Unary minus (-) for tensors.
%   uplus       - Unary plus (+) for tensors.
%   xor         - Logical EXCLUSIVE OR for tensors.
%
% See also
%   TENSOR_TOOLBOX

function t = tensor(varargin)
%TENSOR Create tensor.
%
%   X = TENSOR(A,SIZ) creates a tensor from the multidimensional
%   array A. The SIZ argument specifies the desired shape of A.
%
%   X = TENSOR(A) creates a tensor from the multidimensional array
%   Z, using SIZ = size(A).
%
%   X = TENSOR(S) copies a tensor S.
%
%   X = TENSOR(A) converts an sptensor, ktensor, ttensor, or tenmat object
%   to a tensor.
%
%   X = TENSOR creates an empty, dense tensor object.
%
%   Examples
%   X = tensor(rand(3,4,2)) %<-- Tensor of size 3 x 4 x 2
%   Y = tensor(rand(3,1),3) %<-- Tensor of size 3
%   Z = tensor(rand(12,1),[3 4 1]) %<-- Tensor of size 3 x 4 x 1
%
%   See also TENSOR, TENSOR/NDIMS.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

% EMPTY/DEFAULT CONSTRUCTOR
if nargin == 0
    t.data = [];
    t.size = [];
    t = class(t, 'tensor');
    return;
end

% CONVERSION/COPY CONSTRUCTORS
% Note that we pass through this if/switch statement if the first argument
% is not any of these cases.
if (nargin == 1)
    v = varargin{1};
    switch class(v)
        case 'tensor',
            % COPY CONSTRUCTOR
            t.data = v.data;
            t.size = v.size;
            t = class(t, 'tensor');
            return;
        case {'ktensor','ttensor','sptensor','symtensor','symktensor'},
            % CONVERSION
            t = full(v);
            return;
        case 'tenmat',
            % RESHAPE TENSOR-AS-MATRIX
            % Here we just reverse what was done in the tenmat constructor.
            % First we reshape the data to be an MDA, then we un-permute
            % it using ipermute.
            sz = tsize(v);
            order = [v.rdims,v.cdims];
            data = reshape(v.data, [sz(order) 1 1]);
            if numel(order) >= 2
                t.data = ipermute(data,order);
            else
                t.data = data;
            end
            t.size = sz;
            t = class(t, 'tensor');
            return;
    end
end

% CONVERT A MULTIDIMENSIONAL ARRAY
if (nargin <= 2)

    % Check first argument
    data = varargin{1};
    if ~isa(data,'numeric') && ~isa(data,'logical')
        error('First argument must be a multidimensional array.')
    end

    % Create or check second argument
    if nargin == 1
        siz = size(data);
    else
        siz = varargin{2};
        if ~isempty(siz) && ndims(siz) ~= 2 && size(siz,1) ~= 1
            error('Second argument must be a row vector.');
        end
    end

    % Make sure the number of elements matches what's been specified
    if isempty(siz)
        if ~isempty(data)
            error('Empty tensor cannot contain any elements');
        end
    elseif prod(siz) ~= numel(data)
        error('Size of data does not match specified size of tensor');
    end

    % Make sure the data is indeed the right shape
    if ~isempty(data) && ~isempty(siz)
        data = reshape(data,[siz 1 1]);
    end

    % Create the tensor
    t.data = data;
    t.size = siz;
    t = class(t, 'tensor');
    return;

end

error('Unsupported use of function TENSOR.');


function [hrf, fit, e, param] = Fit_Spline(tc, TR, Run, T)
% function [hrf, fit, e, param] = Fit_Spline(tc, TR, Run, T)
%
% Fits Spline model
%
% INPUTS:
%
% tc    - time course
% TR    - time resolution
% Runs  - expermental design
% T     - length of estimated HRF
%
% OUTPUTS:
%
% hrf   - estimated hemodynamic response function
% fit   - estimated time course
% e     - residual time course
% param - estimated amplitude, height and width
%
% Created by Martin Lindquist on 03/06/23

numstim = length(Run);  % Number conditions
len = length(Run{1});   % length of run
t=1:TR:T;
tlen = length(t);       % Number of time points in HRF

K = 8;                         % Number of b-spline basis (This cxan be set to specification)
norder = 4;                    % Order of b-spline basis (This cxan be set to specification)

% Create design matrix
basis = create_bspline_basis([0,tlen], K+3, norder);
B = eval_basis((1:tlen),basis);
B = B(:,3:end-1);

Wi = zeros(len, numstim*K);
for j=1:numstim
    Wji = tor_make_deconv_mtx3(Run{j},tlen,1);
    Wi(:,(j-1)*K+1:j*K) = Wji(:,1:tlen)*B;
end

X = [ones(len,1) Wi];

% Fit model
b = pinv(X)*tc;
e = tc-X*b;
fit = X*b;

b2 = reshape(b(2:end),K,numstim);

% Get parameters

hrf = B*b2;

for i=1:numstim
    param(:,i) = get_parameters2(hrf(:,i),1:length(t));
end

end

% END MAIN FUNCTION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [AtA,A] = corrMatrix3D(obj)
% calucate 3D correlation matrix
%
% (c) Thomas Kuestner
% ---------------------------------------------------------------------

nCha = size(obj.kCalib,4);

if(isreal(obj.kCalib))
    A = zeros(prod(obj.calibSize - obj.kernelSize + 1), prod(obj.kernelSize)*nCha,obj.measPara.precision);
else
    A = complex(zeros(prod(obj.calibSize - obj.kernelSize + 1), prod(obj.kernelSize)*nCha,obj.measPara.precision),zeros(prod(obj.calibSize - obj.kernelSize + 1), prod(obj.kernelSize)*nCha,obj.measPara.precision));
end
% A = [];
counter = 1;
for n=1:nCha
    if(isreal(obj.kCalib(:,:,:,n)))
        if(strcmp(obj.measPara.precision,'single'))
            A(:,counter:counter+prod(obj.kernelSize)-1) = im3colRSingle(obj.kCalib(:,:,:,n),obj.kernelSize).';
        else
            A(:,counter:counter+prod(obj.kernelSize)-1) = im3colR(obj.kCalib(:,:,:,n),obj.kernelSize).'; % before: tmp =
        end
    else
        if(strcmp(obj.measPara.precision,'single'))
            A(:,counter:counter+prod(obj.kernelSize)-1) = im3colCSingle(obj.kCalib(:,:,:,n),obj.kernelSize).';
        else
            A(:,counter:counter+prod(obj.kernelSize)-1) = im3colC(obj.kCalib(:,:,:,n),obj.kernelSize).';
        end
    end
    counter = counter + prod(obj.kernelSize);
% 	A = [A, tmp];
end

AtA = A'*A;

end


% Demo for partially observable oscillatory system.
% This demo inverts a model of a linear oscillatory system, which is
% observed through a nonlinear sigmoid function.

clear variables
close all

% Choose basic settings for simulations
n_t = 2e2;
delta_t = 2e-1;         % integration time step (Euler method)
f_fname = @f_lin2D;
g_fname = @g_sigmoid;

u       = zeros(1,n_t);

% Build options structure for temporal integration of SDE
inF.deltat      = delta_t;
inF.a           = 0.1;
inF.b           = 0.9e-2;
inG.scale = 2;
inG.y0          = -1;
inG.ind         = 1; % only x(1) is partially observable
options.inF     = inF;
options.inG     = inG;

% Parameters of the simulation
alpha   = 1e1;
sigma   = 1e1;
theta   = 1;
phi     = [];

% Build priors for model inversion
priors.muX0 = zeros(2,1);
priors.SigmaX0 = 1e0*eye(2);
priors.muTheta = 0*ones(1,1);
priors.SigmaTheta = 1e0*eye(1);
priors.a_alpha = 1e0;
priors.b_alpha = 1e0;
priors.a_sigma = 1e0;
priors.b_sigma = 1e0;

for t=1:n_t
    priors.iQx{t} = eye(2);
    priors.iQx{t}(1,1) = 1e2;
end

% Build options and dim structures for model inversion
options.priors      = priors;
options.backwardLag  = 16;
dim.n_theta         = 1;
dim.n_phi           = 0;
dim.n               = 2;

% options.checkGrads = 1;

% Build time series of hidden states and observations
[y,x,x0,eta,e] = VBA_simulate (n_t,f_fname,g_fname,theta,phi,u,alpha,sigma,options);

% display time series of hidden states and observations
displaySimulations(y,x,eta,e);
% disp('--paused--')
% pause

% Call inversion routine
% [posterior,out] = VBA_onlineWrapper(y,u,f_fname,g_fname,dim,options);
[posterior,out] = VBA_NLStateSpaceModel(y,u,f_fname,g_fname,dim,options);

% Display results
displayResults(posterior,out,y,x,x0,theta,phi,alpha,sigma)

% Make predictions
try
    options = out.options;
    [xs,ys,xhat,vx,yhat,vy] = VBA_comparePredictions(...
        n_t,theta,phi,u,alpha,sigma,options,posterior,dim);
catch
    disp('------!!Unable to form predictions!!------')
end


% ------------------------------------------------------
% SwarmOps - Heuristic optimization for Matlab
% Copyright (C) 2003-2010 Magnus Erik Hvass Pedersen.
% Please see the file license.txt for license details.
% SwarmOps on the internet: http://www.Hvass-Labs.org/
% ------------------------------------------------------

% Example optimization problem. You may use this as
% a starting point for custom problems.
% Parameters:
%     x; position in the search-space.
%     data; data-struct for optimization problem.
% Returns:
%     fitness; the measure to be minimized.
function fitness = myproblem(x, data)
    % Retrieve data from struct.
    r = data.MyExtraData;

    % Displace position.
    t = x-r;

    % Compute and return fitness.
    fitness = (2*t(1)-t(2))^2 + ...
              (3*t(2)-2*t(3))^2 + ...
              (4*t(3)-3*t(4))^2 + ...
              (t(4)-4*t(1))^2 + ...
              sum(t.^2);
end

% ------------------------------------------------------


function [y]=fconv(x, h)
% Fast Parallelised Convolution
%   [y] = FCONV(x, h) convolves x and h in the frequency domain
%         to +-1.
%
%      x = input vector
%      h = input vector
%
%      See also CONV
%

Ly=size(x,1)+size(h,1)-1;  %
Ly2=pow2(nextpow2(Ly));    % Find smallest power of 2 that is > Ly

if isa(x, 'gpuArray')
Ly  = gpuArray(Ly);
Ly2 = gpuArray(Ly2);
end

X=fft(x, Ly2);             % Fast Fourier transform
H=fft(h, Ly2);	           % Fast Fourier transform
Y=X.*H;        	           %
y=real(ifft(Y, Ly2));      % Inverse fast Fourier transform
y=y(1:1:Ly,:);             % Take just the first N elements


function [dVVectECI] = getNTW2ECIdvVect(dVVectNTW, rVect, vVect)
%getNTWdvVect Summary of this function goes here
%   Detailed explanation goes here
    rVect = reshape(rVect, 3,1);
    vVect = reshape(vVect, 3,1);
    dVVectNTW = reshape(dVVectNTW, 3,1);

    tHat = vVect/norm(vVect);

    wHatC = crossARH(rVect,vVect);
    wHat = wHatC/norm(wHatC);

    nHatC = crossARH(tHat,wHat);
    nHat = nHatC/norm(nHatC);

    ECI2TWNRotMat = [tHat,wHat,nHat];
%     TWN2ECIRotMat = inv(ECI2TWNRotMat);
    TWN2ECIRotMat = ECI2TWNRotMat'; %rotation matrix, inv = transpose

    dVVectECI = TWN2ECIRotMat \ dVVectNTW;
end


function [ alphas, betas, triX, mask, xmin, ymin, npix ] = InitialisePieceWiseAffine( triangulation, sourcePoints )
%INITIALISEPIECEWICEAFFINE Summary of this function goes here
%   Detailed explanation goes here

    triangulation = triangulation + 1;
    numPoints = size(sourcePoints, 1);

    numTris = size(triangulation, 1);

    alphas = zeros(size(triangulation, 1), 3);
    betas = zeros(size(triangulation, 1), 3);

    xs = sourcePoints(:,1);
    ys = sourcePoints(:,2);

	for i = 1:numTris

        j = triangulation(i, 1);
        k = triangulation(i, 2);
        l = triangulation(i, 3);

        c1 = ys(l) - ys(j);
        c2 = xs(l) - xs(j);
        c4 = ys(k) - ys(j);
        c3 = xs(k) - xs(j);

        c5 = c3*c1 - c2*c4;

        alphas(i, 1) = (ys(j) * c2 - xs(j) * c1) / c5;
        alphas(i, 2) = c1/c5;
        alphas(i, 3) = -c2/c5;

        betas(i, 1) = (xs(j) * c4 - ys(j) * c3)/c5;
        betas(i, 2) = -c4/c5;
        betas(i, 3) = c3/c5;
    end

    xmin = min(xs);
    ymin = min(ys);

    xmax = max(xs);
    ymax = max(ys);

	w = int32(xmax - xmin + 1);
    h = int32(ymax - ymin + 1);

    mask = zeros(h, w);
    triX = zeros(h, w);

    shape = [xs, ys];

    for i = 1:h
        for j = 1:w

            currTri = findTriangle(double([double(j)-1+xmin, double(i)-1+ymin])', triangulation, shape);
            if(currTri ~= -1)
                triX(i, j) =  currTri - 1;
                mask(i, j) = 1;
            else
                triX(i, j) = -1;
            end
        end
    end
    npix = sum(sum(mask));
end

function [tri] = findTriangle(point, tris, controlPoints)

    numTris = size(tris, 1);
    tri = -1;

    for i=1:numTris

        if(PointInTriangle(point, controlPoints(tris(i,1),:)', controlPoints(tris(i,2),:)', controlPoints(tris(i,3),:)'))
           tri = i;
           break;
        end

    end

end

function inTriangle = PointInTriangle(point, v1, v2, v3)

    inTriangle = SameSide(point, v1,v2,v3) && SameSide(point, v2,v1,v3) && SameSide(point, v3,v1,v2);

end

function sameSide = SameSide(toTest,v1,v2,v3)

    x0 = toTest(1);
    y0 = toTest(2);

    x1 = v1(1);
    x2 = v2(1);
    x3 = v3(1);

    y1 = v1(2);
    y2 = v2(2);
    y3 = v3(2);

    x = (x3-x2)*(y0-y2) - (x0-x2)*(y3-y2);
    y = (x3-x2)*(y1-y2) - (x1-x2)*(y3-y2);
    if(x*y >= 0)
        sameSide = 1;
    else
        sameSide = 0;
    end
%     cross1 = cross( v3 - v2, toTest - v2);
%     cross2 = cross( v3 - v2, v1 - v2);
%
%     sameSide = (cross1 * cross2') >= 0;
end


function [offsetsRows, offsetsCols, distances] = templateMatchingNaive(row, col,...
    patchSize, searchWindowSize, image)
% This function should for each possible offset in the search window
% centred at the current row and col, save a value for the offsets and
% patch distances, e.g. for the offset (-1,-1)
% offsetsRows(1) = -1;
% offsetsCols(1) = -1;
% distances(1) = 0.125;

% The distance is simply the SSD over patches of size patchSize between the
% 'template' patch centred at row and col and a patch shifted by the
% current offset

% NOTE : row and col are the coordinates of the pixel we want our pach to be centered.
% We need the delta to see when we're sliding the patch if we're still inside the image
% or not
delta = floor(patchSize/2);

[rows columns dimensions] = size(image);

% Let's grab the reference patch centered at (row, col)
reference_patch = double(image(row-delta:row+delta, col-delta:col+delta));

% We'll have as many patches as the window Area which is
% (searchWindowSize*searchWindowSize)
% Therefore we'll have the same number for the distances and for the offsets
% in the X and Y direction
distances = zeros(1,searchWindowSize*searchWindowSize);
offsetsRows = zeros(1,searchWindowSize*searchWindowSize);
offsetsCols = zeros(1,searchWindowSize*searchWindowSize);

distances_index = 1;

% We use the delta window to center the window to the pixel we want to
% denoise.
delta_window = floor(searchWindowSize/2);

% CLIP the search window agains the image
% Suppose we want to denoise the pixel x and center the
%
% |------------|
% |     #######|####################
% |     #x     |                   #
% |-----#------|                   #
%       #                          #
%       #                          #
%       #                          #
%       ############################
%
%

start_rows = max(row-delta_window, 1+delta);
start_columns = max(col-delta_window, 1+delta);

end_rows = min(row+delta_window, rows-delta);
end_columns = min(col+delta_window, columns-delta);

% Loop through the clipped window
for row_searchWindow = start_rows:end_rows
    for column_searchWindow = start_columns:end_columns

        %obtain the patch we want to compare with
        patch = double(image(row_searchWindow-delta:row_searchWindow+delta, column_searchWindow-delta:column_searchWindow+delta));

        %compute the difference
        sum_squared_distance = sum(sum(double(reference_patch - patch).^2));

        %store the results
        distances(distances_index) = sum_squared_distance;
        offsetsRows(distances_index) = row_searchWindow - row;
        offsetsCols(distances_index) = column_searchWindow - col;
        distances_index = distances_index + 1;

    end
end

end


% Color Similarity Non-local Pixel Affinities
% This function implements the affinity based on color differences
% first used for image matting in the paper
% Qifeng Chen, Dingzeyu Li, Chi-Keung Tang, "KNN Matting", IEEE
% TPAMI, 2013.
% All parameters other than image are optional. The output is a sparse
% matrix which has non-zero element for the non-local neighbors of
% the pixels given by binary map inMap.
% - K defines the number of neighbors from which LLE weights are
%   computed.
% - outMap is a binary map that defines where the nearest neighbor
%   search is done.
% - xyWeight determines how much importance is given to the spatial
%   coordinates in the nearest neighbor selection.
% - When useHSV is false (default), the search is done i [r g b x y] space,
%   otherwise the feature space is [cos(h) sin(h), s, v, x, y].

function Wcs = colorSimilarityAffinities(image, K, inMap, outMap, xyWeight, useHSV)

    [h, w, ~] = size(image);
    N = h * w;

    if ~exist('K', 'var') || isempty(K)
        K = 5;
    end
    if ~exist('inMap', 'var') || isempty(inMap)
        inMap = true(h, w);
    end
    if ~exist('outMap', 'var') || isempty(outMap)
        outMap = true(h, w);
    end
    if ~exist('xyWeight', 'var') || isempty(xyWeight)
        xyWeight = 0.05;
    end
    if ~exist('useHSV', 'var') || isempty(useHSV)
        useHSV = false;
    end

    if useHSV
        image = rgb2hsv(image);
        image = cat(3, cos(image(:, :, 1)) * 2 * pi, sin(image(:, :, 1)) * 2 * pi, image(:,:,2:3));
    end

    [~, neighInd, ~] = findNonlocalNeighbors(image, K, xyWeight, inMap, outMap);

    % This behaviour below, decreasing the xy-weight and finding a new set of neighbors, is taken
    % from the public implementation of KNN matting by Chen et al.
    [inInd, neighInd2, features] = findNonlocalNeighbors(image, ceil(K / 5), xyWeight / 100, inMap, outMap);
    neighInd = [neighInd, neighInd2];
    features(:, end-1 : end) = features(:, end-1 : end) / 100;

    inInd = repmat(inInd, [1, size(neighInd, 2)]);
    flows = max(1 - sum(abs(features(inInd(:), :) - features(neighInd(:), :)), 2) / size(features, 2), 0);

    Wcs = sparse(inInd(:), neighInd(:), flows, N, N);
    Wcs = (Wcs + Wcs') / 2; % If p is a neighbor of q, make q a neighbor of p
end


function element_num = grid_element_num ( code, nelemx, nelemy )

%*****************************************************************************80
%
%% GRID_ELEMENT_NUM returns the number of elements in a grid.
%
%  Discussion:
%
%    The number of elements generated will be NELEMX * NELEMY for
%    quadrilaterals, or 2 * NELEMX * NELEMY for triangles.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string CODE, identifies the element desired.
%    Legal values include 'Q4', 'Q8', 'Q9', 'Q12', 'Q16', 'QL', 'T3',
%    'T4', 'T6' and 'T10'.
%
%    Input, integer NELEMX, NELEMY, the number of quadrilaterals along the
%    X and Y directions.
%
%    Output, integer ELEMENT_NUM, the number of elements in the grid.
%
  if ( s_eqi ( code, 'Q4' ) )
    element_num = grid_q4_element_num ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'Q8' ) )
    element_num = grid_q8_element_num ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'Q9' ) )
    element_num = grid_q9_element_num ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'Q12' ) )
    element_num = grid_q12_element_num ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'Q16' ) )
    element_num = grid_q16_element_num ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'QL' ) )
    element_num = grid_ql_element_num ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'T3' ) )
    element_num = grid_t3_element_num ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'T4' ) )
    element_num = grid_t4_element_num ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'T6' ) )
    element_num = grid_t6_element_num ( nelemx, nelemy );
  elseif ( s_eqi ( code, 'T10' ) )
    element_num = grid_t10_element_num ( nelemx, nelemy );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'GRID_ELEMENT_NUM - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of CODE = "%s".', code );
    element_num = -1;
    error ( 'GRID_ELEMENT_NUM - Fatal error!' )
  end

  return
end


function [Vs]=trisurfsmoothHC(F,V,IND_V,alp,bet,tol,n,disp_on)

%Humphreys Classes Smoothening

% alpha [0..1] influence of original/previous points 0-> full previous, 1-> full original
% beta  [0..1] e.g. > 0.5

if isempty(IND_V)
    [~,IND_V]=patchIND(F,V);
end

L=IND_V>0;
Xp=NaN(size(IND_V));  Yp=NaN(size(IND_V));  Zp=NaN(size(IND_V));

p=V; cc=0; i=1;
while cc==0
    q=p;
    Xp(L)=p(IND_V(L),1); Yp(L)=p(IND_V(L),2); Zp(L)=p(IND_V(L),3);
    p=[gnanmean(Xp,2) gnanmean(Yp,2) gnanmean(Zp,2)]; %Mean of neighbourhood, Laplacian operation
    SSQD_new=gnansum((p(:)-q(:)).^2);
    if i>1
        SSQD_ratio=SSQD_new./SSQD_old;
        if disp_on
            disp(['Iteration ',num2str(i),', SSQD ratio: ',num2str(SSQD_ratio)]);
        end
        if abs(1-SSQD_ratio)<=tol
            cc=1; %Convergence tolerance achieved
            if disp_on
                disp('Convergence tolerance on SSQD ratio reached!');
            end
        end
    else
        if disp_on
            disp(['Iteration ',num2str(i),', SSQD initial: ',num2str(SSQD_new)]);
        end
    end

    if i>=n
        cc=1;
        if disp_on
            disp('Maximum number of iteration reached!');
        end
    end
    SSQD_old=SSQD_new;

    b=p-((alp.*V)+((1-alp).*q)); %Difference at centre vertex
    Xp(L)=b(IND_V(L),1); Yp(L)=b(IND_V(L),2); Zp(L)=b(IND_V(L),3);
    c=[gnanmean(Xp,2) gnanmean(Yp,2) gnanmean(Zp,2)]; %Mean of difference at centre vertex of neighbourhood
    p=p-((bet.*b)+(1-bet).*(c));
    i=i+1;
end

Vs=p;

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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 K=centKer(K)

%function K=centKer(K)
%
% Centeres the matrix K
%
%INPUTS
% K = the kernel matrix to be centered
%
%OUTPUTS
% Kc = the centered kernel matrix
%
%
%For more info, see www.kernel-methods.net

% original kernel matrix stored in variable K
% output uses the same variable K
% K is of dimension ell x ell
% D is a row vector storing the column averages of K
% E is the average of all the entries of K
ell = size(K,1);
D = sum(K) / ell;
E = sum(D) / ell;
J = ones(ell,1) * D;
K = K - J - J' + E;


function gT = simwhiteKernGradX(kern, t1, t2)

% SIMWHITEKERNGRADX Gradient of SIM-WHITE kernel with respect to a point t.
% FORMAT
% DESC computes the gradient of the SIM-White (Single Input Motif - White)
% kernel with respect to the input positions.
% ARG kern : kernel structure for which gradients are being computed.
% ARG t1 : locations against which gradients are being computed.
% RETURN gT : the returned gradients. The gradients are returned in
% a matrix which is numData x numInputs x numData. Where numData is
% the number of data points and numInputs is the number of input
% dimensions in t1 (currently always one).
%
% FORMAT
% DESC computes the gradident of the SIM-White (Single Input Motif - White)
% kernel with respect to the input positions where both the row
% positions and column positions are provided separately.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG t1 : row locations against which gradients are being computed.
% ARG t2 : column locations against which gradients are being computed.
% RETURN gT : the returned gradients. The gradients are returned in
% a matrix which is numData2 x numInputs x numData1. Where numData1 is
% the number of data points in t1, numData2 is the number of data
% points in t2 and numInputs is the number of input dimensions in t1
% (currently always one).
%
% SEEALSO simwhiteKernParamInit, kernGradX, simwhiteKernDiagGradX
%
% COPYRIGHT : David Luengo, 2009

% KERN

if nargin < 3
  t2 = t1;
end
if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end

% Initialisation of the gradient matrix
gT = zeros(size(t1, 1), 1, size(t2, 1));

% Parameters of the kernel required in the computation
variance = kern.variance;
decay = kern.decay;
sensitivity = kern.sensitivity;
isStationary = kern.isStationary;

c = 0.5 * variance * (sensitivity^2);
if (isStationary == false)
    for i = size(t1, 1)
        gT(i, 1, :) =  - sign(t1(i)-t2) .* exp(-decay*abs(t1(i)-t2)) ...
                + exp(-decay*(t1(i)+t2));
    end
else
    for i = size(t1, 1)
        gT(i, 1, :) =  - sign(t1(i)-t2) .* exp(-decay*abs(t1(i)-t2));
    end
end
gT = c * gT;


function peaks = PeakDetection(x,ff,varargin)
%
% peaks = PeakDetection(x,f,flag),
% Overview: R-peak detector based on max search
%
% inputs:
% x: vector of input data
% ff: approximate ECG beat-rate in Hertz, normalized by the sampling frequency
% flag: search for positive (flag=1) or negative (flag=0) peaks. By default
% the maximum absolute value of the signal, determines the peak sign.
%
% output:
% peaks: vector of R-peak impulse train
%
% Notes:
% - The R-peaks are found from a peak search in windows of length N; where
% N corresponds to the R-peak period calculated from the given f. R-peaks
% with periods smaller than N/2 or greater than N are not detected.
% - The signal baseline wander is recommended to be removed before the
% R-peak detection
%
%
% Open Source ECG Toolbox, version 1.0, November 2006
% Released under the GNU General Public License
% Copyright (C) 2006  Reza Sameni
% Sharif University of Technology, Tehran, Iran -- GIPSA-Lab, INPG, Grenoble, France
% reza.sameni@gmail.com

% This program is free software; you can redistribute it and/or modify it
% under the terms of the GNU General Public License as published by the
% Free Software Foundation; either version 2 of the License, or (at your
% option) any later version.
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
% Public License for more details. You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

N = length(x);
peaks = zeros(1,N);

th = .5;
rng = floor(th/ff);

if(nargin==3)
    flag = varargin{1};
else
    flag = abs(max(x))>abs(min(x));
end

if(flag)
    for j = 1 : N
        %         index = max(j-rng,1):min(j+rng,N);
        if(j>rng && j<N-rng)
            index = j-rng:j+rng;
        elseif(j>rng)
            index = N-2*rng:N;
        else
            index = 1:2*rng;
        end

        if(max(x(index))==x(j))
            peaks(j) = 1;
        end
    end
else
    for j = 1 : N
        %         index = max(j-rng,1):min(j+rng,N);
        if(j>rng && j<N-rng)
            index = j-rng:j+rng;
        elseif(j>rng)
            index = N-2*rng:N;
        else
            index = 1:2*rng;
        end

        if(min(x(index))==x(j))
            peaks(j) = 1;
        end
    end
end

% remove fake peaks
I = find(peaks);
d = diff(I);
% z = find(d<rng);
peaks(I(d<rng))=0;


function p03_demo ( iteration_max, h, fh )

%*****************************************************************************80
%
%% P03_DEMO runs the 2D demo problem #3, with mesh size H.
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Modified:
%
%    06 February 2006
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, integer ITERATION_MAX, the maximum number of iterations that DISTMESH
%    should take.  (The program might take fewer iterations if it detects convergence.)
%
%    Input, real H, the mesh spacing parameter.
%
%    Input, external FH, the mesh density function.
%
  if ( nargin < 1 )
    iteration_max = 300;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P03_DEMO - Note:\n' );
    fprintf ( 1, '  No value of ITERATION_MAX was supplied.\n' );
    fprintf ( 1, '  The default value ITERATION_MAX = %d will be used.\n', ...
      iteration_max );
  end

  if ( nargin < 2 )
    h = 0.15;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P03_DEMO - Note:\n' );
    fprintf ( 1, '  No value of H was supplied.\n' );
    fprintf ( 1, '  The default value H = %f will be used.\n', h );
  end

  if ( nargin < 3 )
    fh = @p03_fh;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P03_DEMO - Note:\n' );
    fprintf ( 1, '  No value of FH was supplied.\n' );
    fprintf ( 1, '  The default uniform density function will be used.\n', h );
  end
%
%  Put the random number generator into a fixed initial state.
%
  rand ( 'state', 111 );
%
%  Set the rendering method for the current figure to Z-buffering.
%
  set ( gcf, 'rend', 'z' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Problem 3:\n' );
  fprintf ( 1, '  The square with a circular hole, h = %f\n', h )

  fd = @p03_fd;
  box = [ -1.0,-1.0; 1.0,1.0 ];
  fixed = [ -1.0,-1.0; -1.0,1.0; 1.0,-1.0; 1.0,1.0 ];

  [ p, t ] = distmesh_2d ( fd, fh, h, box, iteration_max, fixed );

  post_2d ( p, t, fh )
%
%  Write a PostScript image of the triangulation.
%
  [ node_num, junk ] = size ( p );
  [ tri_num, junk ] = size ( t );
  p = p';
  t = t';
  node_show = 0;
  triangle_show = 1;

  triangulation_order3_plot ( 'p03_mesh.eps', node_num, p, tri_num, ...
    t, node_show, triangle_show );
%
%  Write a text file containing the nodes.
%
  r8mat_write ( 'p03_nodes.txt', 2, node_num, p );
%
%  Write a text file containing the triangles.
%
  i4mat_write ( 'p03_elements.txt', 3, tri_num, t );

  return
end


function [fitness,erfc,sd_r,md] = compute_event_exceptionality(traces,N,robust_std)
%{
    Define a metric and order components according to the probabilty if some "exceptional events" (like a spike).
    Suvh probability is defined as the likeihood of observing the actual trace value over N samples given an estimated noise distribution.
    The function first estimates the noise distribution by considering the dispersion around the mode.
    This is done only using values lower than the mode. The estimation of the noise std is made robust by using the approximation std=iqr/1.349.
    Then, the probavility of having N consecutive eventsis estimated.
    This probability is used to order the components.

    Parameters:
    -----------

    traces: array
        Fluorescence traces

    Returns:
    --------

    fitness: array
        value estimate of the quality of components (the lesser the better)

    erfc: array
        probability at each time step of observing the N consequtive actual trace values given the distribution of noise

    noise_est: array
        the components ordered according to the fitness

%}

T=size(traces,2);

md = max(mode_robust(traces, 2),0);

if ~exist('N','var'); N=5; end
if ~exist('robust_std','var')
    robust_std = false;
end

ff1 = bsxfun(@minus,traces,md');
% only consider values under the mode to determine the noise standard deviation
ff1 = -ff1 .* (ff1 < 0);
if robust_std
    % compute 25 percentile
    ff1 = sort(ff1, 2);
    ff1(ff1 == 0) = nan;
    Ns = round(sum(ff1 > 0, 2) * .5);
    iqr_h = zeros(size(traces,1));
    idx = 1;
    for idx = 1:size(ff1,1)
        el = ff1(idx,:);
        iqr_h(idx) = ff1(idx, -Ns(idx));
    end
    % approximate standard deviation as iqr/1.349
    sd_r = 2 * iqr_h / 1.349;

else
    Ns = sum(ff1 > 0, 2);
    sd_r = sqrt(sum(ff1.^2, 2)./ Ns);
end
% compute z value
z = bsxfun(@times,bsxfun(@minus,traces,md'),1./(3 * sd_r));
% probability of observing values larger or equal to z given normal
% distribution with mean md and std sd_r
mu = 0;
sigma = 1;
pd = makedist('Normal',mu,sigma);
erf = 1 - cdf(pd,z);
% use logarithm so that multiplication becomes sum
erf = log(erf);
filt = ones(1,N);
% moving sum
erfc = conv2(1,filt,erf,'same');
erfc = erfc(:,1:T);

% select the maximum value of such probability for each trace
fitness = min(erfc, [], 2);

% ordered = np.argsort(fitness)

function outp = hsm_(data)
outp = [];
if numel(data) == 1
    outp = data(1);
elseif numel(data) == 2
    outp = mean(data(:));
elseif numel(data) == 3
    i1 = data(2) - data(1);
    i2 = data(3) - data(2);
    if i1 < i2
        outp = mean(data(1:2));
    elseif i2 > i1
        outp = mean(data(2:end));
    else
        outp = data(2);
    end
else

    % wMin = data[-1] - data[0]
    wMin = inf;
    N = idivide(int32(numel(data)),2,'floor') + mod(numel(data),2)-1;

    for i = 1:N
        w = data(i + N - 1) - data(i);
        if w < wMin
            wMin = w;
            j = i;
        end
    end
    outp = hsm_(data(j:j + N));
end

function dataMode = mode_robust(inputData, axis)
    %{
    Robust estimator of the mode of a data set using the half-sample mode.

    .. versionadded: 1.0.3
    """
    %}
    if ~isempty(axis)

        if axis == 2
            dataMode = arrayfun(@(n)  mode_robust(inputData(n,:),[]), 1:size(inputData,1));
        elseif axis == 1
            dataMode = arrayfun(@(n)  mode_robust(inputData(:,n),[]), 1:size(inputData,2));
        else
            error('axis can be 1 or two')
        end
    else
        % Create the function that we can use for the half-sample mode
        data = inputData(:);
        % The data need to be sorted for this to work
        data = sort(data);
        % Find the mode
        dataMode = hsm_(data);
    end


function output=user3sym()

%**************************************************************************
 %*************************************************************************
                %//input signal form transmitter side for symbol based\\
 clear all;
close all;
clc;
% us=input('enter the number of users');
% nb1=input('number of bits for user1');
% m1=input('enter the message sequence1');
% nb2=input('number of bits for user2');
% m2=input('enter the message sequence2');
% nb3=input('number of bits for user3');
% m3=input('enter the message sequence3');
us=3;
nb1=5;
nb2=5;
nb3=5;
m1=[1 1 0 0 1];
m2=[1 0 1 0 1];
m3=[1 1 0 1 0];

                    %//message1 into polar form\\

for i=1:nb1
    if m1(i)==1
        mb1(i)=m1(i);
    else
        mb1(i)=-1;
    end
end
  % disp(mb1);

                %//message2 into polar form\\

for i=1:nb2
    if m2(i)==1
        mb2(i)=m2(i);
    else
        mb2(i)=-1;
    end
end
  %disp(mb2);

                %//message3 into polar form\\

for i=1:nb3
    if m3(i)==1
        mb3(i)=m3(i);
    else
        mb3(i)=-1;
    end
end
  %disp(mb3);

                  %//generation of maximal length sequence1\\

  f1=1;f2=1;f3=0;f4=0;f5=1;
m=(2^5)-1;
for j=1:m
    p=xor(f2,f5);
    f5=f4;
    op1(j)=f5;
    f4=f3;
    f3=f2;
    f2=f1;
    f1=p;

end
  %disp(op1);

                    %//msequence1 into polar form\\

    for i=1:31

         if op1(i)==1
             pb1(i)=1;
         else
         pb1(i)=-1;
        end
    end
    %disp(op1);
    %disp(pb1);

                    %//generation of maximal length sequence2\\

  f1=1;f2=0;f3=0;f4=1;f5=1;
m=(2^5)-1;
for j=1:m
    p=xor(f3,f5);
    f5=f4;
    op2(j)=f5;
    f4=f3;
    f3=f2;
    f2=f1;
    f1=p;

end
  %disp(op2);

                    %//msequence2 into polar form\\

    for i=1:31
        if op2(i)==1
             pb2(i)=1;
         else
         pb2(i)=-1;
        end
    end
    %disp(op2);
    %disp(pb2);

                     %//generation of maximal length sequence3\\

  f1=1;f2=1;f3=1;f4=0;f5=1;
m=(2^5)-1;
for j=1:m
    p=xor(f4,f5);
    f5=f4;
    op3(j)=f5;
    f4=f3;
    f3=f2;
    f2=f1;
    f1=p;

end
  %disp(op3);

                    %//msequence3 into polar form\\

    for i=1:31

         if op3(i)==1
             pb3(i)=1;
         else
         pb3(i)=-1;
        end
    end
    %disp(op3);
    %disp(pb3);

                    %//spreading signals from transmitter side\\

                         %//first transmit bit\\

k=1;
for i=1:nb1
    for j=1:31
        tb1(k)=mb1(i)*pb1(j);
        k=k+1;
    end
end
%disp(tb1);

                     %//second transmit bit\\

k=1;
for i=1:nb2
    for j=1:31
        tb2(k)=mb2(i)*pb2(j);
        k=k+1;
    end
end
%disp(tb2);

                    %//third transmit bit\\

k=1;
for i=1:nb3
    for j=1:31
        tb3(k)=mb3(i)*pb3(j);
        k=k+1;
    end
end
%disp(tb3);

                 %//Addition of three  signals transmitted in the channel\\

n=1;
for i=1:k-1
    tb(n)=tb1(n)+tb2(n)+tb3(n);
    n=n+1;
end
%disp(tb);
chn=awgn(tb,10);

                         %//%receiver side\\
                    %//reconstruction of user2 signal\\

s2=0;
t2=1;
for i=0:31:k-32
    for j=1:31
        ob2(t2)=chn(i+j)*pb2(j)+s2;
        s2=ob2(t2);
        if ob2(t2)>0
            ob2(t2)=1;
        else ob2(t2)=0;
        end
    end
    t2=t2+1;
    s2=0;
end
%disp('second message is')
%disp(ob2);
n1=awgn(ob2,10);
 figure(1);
grid on;
subplot(2,2,1);
plot(ob2);
title('Received signal with out noise for SB');
xlabel('Time');
ylabel('Amplitude');
%g=awgn(ob2,10);
%disp(g);
subplot(2,2,2);
plot(chn);
title('Received signal with noise for SB');
xlabel('Time');
ylabel('Amplitude');

                    %//reconstruction of received signal from noise\\

for i=1:nb2
     if chn(i)>0
            g2(i)=1;
        else g2(i)=0;
        end
    end

% disp(g2);

                 % //calculating optimum weight using minimum variance\\

 ob=randint(1,31);
q=length(10);
%q1=input('enter the sequence');
s=randsrc(1,q);
c1=complex(ob,s);
%disp(c);
d=conj(c1);
d1=d*d';
%disp(d1);
m=mean(d1);
lamda=5;
teta=45;
k=((2*pi)/lamda);
K=4;
for i=0:K-1
    v=(exp(j*k*i*d*sin(teta)))';
end
%disp(v);
v1=conj(v)';
teta=30;
k=((2*pi)/lamda);
K=5;
for i=0:K-1
    eta=exp(sqrt(-1)*k*i*d*sin(teta))';
end
%u1=[2+2j 3-1j 4+3j 5+2j]'
%m=randsrc(1,124);
%a=randsrc(1,124);
l=length(10);
%l1=input('enter the sequence');
u1=complex(ob2,l);
u=eta*u1;
u2=conj(u)';
u3=u*u2;
Ru=mean(u3);

%Ru1=imresize(Ru,[124 124]);
%a1=inv(Ru1);
%a1=Ru1';
a1=Ru';
%disp(a1);
%calculating beta value
%g=1;
dem1=v*v1*a1;
beta1=dem1.^-1;
 %optimum weight
%Wopt=beta*a1*v';
temp1=beta1*v';
Wopt1=temp1*a1;
%disp(Wopt1);
 subplot(2,2,3);
plot(Wopt1);
title('Received signal with optimum weight for SB');
xlabel('Time');
ylabel('weight');

                    %//beamformer output\\

o1=conj(Wopt1)*ob2;
%disp('Beamformer output for SB')
%disp(o1);
 subplot(2,2,4);
plot(o1);
title('Beamformer output  for SB');
xlabel('Time');
ylabel('Beam former output');

                    %//SINR CALCULATION FOR SB CONFIGURATION\\

%s=length(m2);
%q1=input('enter the sequence');
s=randsrc(1,q);
c=complex(ob2,s);

hop1=conj(pb2)';
hob1=conj(c)';
nr3=hop1*hob1';
sum2=nr3'*Wopt1;
%disp('sum2');
%disp(sum2);
cm2=sum2'*sum2;
signal1=mean(cm2);
%disp('cm2');
%disp(cm2);
hop2=conj(pb2)';
hx2=conj(n1)';
nr5=hop2*hx2';
 sum3=nr5'*Wopt1;
 %disp('sum');
 %disp(sum);
  cm3=sum3'*sum3;
  noise1=mean(cm3);
 %disp('cm3');
 %disp(cm3);
 SSINR=signal1/noise1;
 disp('SSINR');
 disp(SSINR);

                %//GRAPH FOR DOA VERSUS SINR\\

    DOA=-80:10:80;
    figure(4);
    plot(DOA,SSINR,'B-*');
    title('simulation for DOA versus SSINR');
    xlabel('DOA in degree');
    ylabel('SINR in db');

                %// BER CALCULATION FOR SB CONFIGURATION\\

       k1=biterr(m2,g2);
       output=k1;
       disp('biterror rate for Sb configuration');
       disp(k1);

                 %//GRAPH FOR NUMBER USERS VERSUS BER\\

     figure(5);
     plot(us,k1,'R-*');

     title('simulation for NUMBER USERS VERSUS BER SYMBOL BASED) ');
     xlabel('NUMBER USERS');
     ylabel('BER');


%% When is the probability distribution of the data important?
% The null-hypothesis in the significance tests for WT, XWT and WTC is
% normally distributed AR1 noise. The AR1 coefficient and process variance
% is chosen so that it best fits the observed data. It is therefore quite
% important that the data is close to normal and is reasonably well
% modeled by a Gaussian AR1 process. Otherwise we can trivially reject
% the null-hypothesis and the significance level calculated by the program
% is not appropriate. However, the Central Limit Theorem tells us that the
% distribution tends towards normality as we convolute with longer and longer
% wavelets (_in the absence of long-range persistence_). This means that the
% data distribution is only really important on the shortest scales.
% So, if we are primarily looking at longer scales we do not need to
% worry so much about the distribution. However, for the WT and XWT the
% color of the noise is very important and a very non-normal distribution
% will affect the performance of the ar1 estimators (ar1.m & ar1nv.m).
% The WTC is relatively insensitive to the colour of the noise in the
% significance test (see next question).


function thresholdOpt = learnThetaMS(params,scale)

fprintf('Learning theta_MS for scale = %d \n',scale);
bestScoreThreshold = -inf;
level = find(params.MS.scale == scale);

for idxThr = 1:length(params.MS.domain(level,:)) %loop over the possible threshold values

    threshold = params.MS.domain(level,idxThr);
    struct = load([params.trainingImages 'structGT.mat']);
    structGT= struct.structGT;%training GT

    scoreThreshold = 0;

    for idxImgGT = 1:length(structGT) %for every img in GT

        img = imread([params.trainingImages '/' structGT(idxImgGT).img]);

        saliencyMAP = saliencyMap(img,params.MS.filtersize,scale);%compute the saliency map - for the current scale
        thrmap = saliencyMAP >= threshold;
        salmap = saliencyMAP .* thrmap;
        thrmapIntegralImage = computeIntegralImage(thrmap);
        salmapIntegralImage =  computeIntegralImage(salmap);
        scoreScale = slidingWindowComputeScore(double(saliencyMAP), scale, 1, 1, threshold, salmapIntegralImage, thrmapIntegralImage);

        [xmin ymin xmax ymax score] = nms4d(double(scoreScale),scale,scale,params.MS.sizeNeighborhood);%non maximum supression

        indexPositive = find(score > 0);
        xmin = xmin(indexPositive);
        ymin = ymin(indexPositive);
        xmax = xmax(indexPositive);
        ymax = ymax(indexPositive);
        score = score(indexPositive);

        for idxObject = 1:size(structGT(idxImgGT).boxes,1) %for every annotated object

            gtBox = structGT(idxImgGT).boxes(idxObject,:);
            [height width ~] = size(img);
            gtBox(1) = gtBox(1)*scale/width;
            gtBox(3) = gtBox(3)*scale/width;
            gtBox(2) = gtBox(2)*scale/height;
            gtBox(4) = gtBox(4)*scale/height;

            maxPascalScore = 0;
            for w = 1:length(score)
                nmsBox = [xmin(w) ymin(w) xmax(w) ymax(w)];
                pascalScore = computePascalScore(gtBox,nmsBox);
                if maxPascalScore < pascalScore
                    maxPascalScore = pascalScore;
                end
            end
            scoreThreshold = scoreThreshold + maxPascalScore;
        end

    end

    if bestScoreThreshold < scoreThreshold
        bestScoreThreshold = scoreThreshold;
        thresholdOpt = threshold;
    end
    fprintf('Best current theta_MS for scale = %d is %f \n',scale,thresholdOpt)
end

end

function saliencyMAP = saliencyMap(inImg,filtersize,scale)

inImg = im2double(rgb2gray(inImg));
inImg = imresize(inImg,[scale,scale],'bilinear');

%Spectral Residual
myFFT = fft2(inImg);
myLogAmplitude = log(abs(myFFT));
myPhase = angle(myFFT);
mySmooth = imfilter(myLogAmplitude,fspecial('average',filtersize),'replicate');
mySpectralResidual = myLogAmplitude-mySmooth;
saliencyMAP = abs(ifft2(exp(mySpectralResidual+1i*myPhase))).^2;

%After Effect
saliencyMAP = imfilter(saliencyMAP,fspecial('disk',filtersize));
saliencyMAP = mat2gray(saliencyMAP);
end


function [V, policy, iter, cpu_time] = mdp_policy_iteration(P, R, discount, policy0, max_iter, eval_type)

% mdp_policy_iteration  Resolution of discounted MDP
%                       with policy iteration algorithm
% Arguments ---------------------------------------------------------------
% Let S = number of states, A = number of actions
%   P(SxSxA) = transition matrix
%              P could be an array with 3 dimensions or
%              a cell array (1xA), each cell containing a matrix (SxS) possibly sparse
%   R(SxSxA) or (SxA) = reward matrix
%              R could be an array with 3 dimensions (SxSxA) or
%              a cell array (1xA), each cell containing a sparse matrix (SxS) or
%              a 2D array(SxA) possibly sparse
%   discount = discount rate, in ]0, 1[
%   policy0(S) = starting policy, optional
%   max_iter = maximum number of iteration to be done, upper than 0,
%              optional (default 1000)
%   eval_type = type of function used to evaluate policy:
%              0 for mdp_eval_policy_matrix, else mdp_eval_policy_iterative
%              optional (default 0)
% Evaluation --------------------------------------------------------------
%   V(S)   = value function
%   policy(S) = optimal policy
%   iter     = number of done iterations
%   cpu_time = used CPU time
%--------------------------------------------------------------------------
% In verbose mode, at each iteration, displays the number
% of differents actions between policy n-1 and n

% MDPtoolbox: Markov Decision Processes Toolbox
% Copyright (C) 2009  INRA
% 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.
%    * Neither the name of the <ORGANIZATION> nor the names of its contributors
%      may be used to endorse or promote products derived from this software
%      without specific prior written permission.
% 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 HOLDER 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.

cpu_time = cputime;

global mdp_VERBOSE;

% check of arguments
if iscell(P); S = size(P{1},1); else S = size(P,1); end;
if discount <= 0 || discount >= 1
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: Discount rate must be in ]0; 1[')
    disp('--------------------------------------------------------')
elseif nargin > 3 && (size(policy0,1)~=S || any(mod(policy0,1)) || any(policy0<1) || any(policy0>S))
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: policy0 must a (1xS) vector with integer from 1 to S')
    disp('--------------------------------------------------------')
elseif nargin > 4 && max_iter <= 0
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: The maximum number of iteration must be upper than 0')
    disp('--------------------------------------------------------')
else

    PR = mdp_computePR(P,R);

    % initialization of optional arguments
    if nargin < 6; eval_type = 0; end;
    if nargin < 5; max_iter = 1000; end;
    if nargin < 4;
        % initialization of policy:
        % the one wich maximizes the expected immediate reward
        [nil, policy0] = mdp_bellman_operator(P,PR,discount,zeros(S,1));
    end;

    if mdp_VERBOSE; disp('  Iteration  Number_of_different_actions'); end;

    iter = 0;
    policy = policy0;
    is_done = false;
    while ~is_done
        iter = iter + 1;
        if  (eval_type==0)
            V = mdp_eval_policy_matrix(P,PR,discount,policy);
        else
            V = mdp_eval_policy_iterative(P,PR,discount,policy);
        end;
        [nil, policy_next] = mdp_bellman_operator(P,PR,discount,V);

	n_different = sum(policy_next ~= policy);
        if mdp_VERBOSE; disp(['       ' num2str(iter) '                 '  num2str(n_different)]); end;

        if all(policy_next==policy) || iter == max_iter
            is_done = true;
        else
            policy = policy_next;
        end;
    end;

end;

cpu_time = cputime - cpu_time;


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function call_price_fft = CallPricingFFT(model,n,S,K,T,r,d,varargin)

lnS = log(S);
lnK = log(K);

%optAlpha = optimalAlpha(model,lnS,lnK,T,r,d,varargin{:});
optAlpha = .75;

DiscountFactor = exp(-r*T);

%-------------------------
%--- FFT Option Pricing --
%-------------------------
% from: Option Valuation Using the Fast Fourier Transform,
%       Peter Carr, March 1999, pp 10-11
%-------------------------

% predefined parameters
FFT_N = 2^n;                               % must be a power of two (2^14)
FFT_eta = 0.05;                             % spacing of psi integrand

% effective upper limit for integration (18)
% uplim = FFT_N * FFT_eta;

FFT_lambda = (2 * pi) / (FFT_N * FFT_eta);  %spacing for log strike output (23)
FFT_b = (FFT_N * FFT_lambda) / 2;           % (20)

uvec = 1:FFT_N;
%log strike levels ranging from lnS-b to lnS+b
ku = - FFT_b + FFT_lambda * (uvec - 1);     %(19)

jvec = 1:FFT_N;
vj = (jvec-1) * FFT_eta;

% optimal alpha illustration (payoff independent)
% alpharange = -3:0.1:8;
% resultrangef = zeros(1,length(alpharange));
% resultrangef1 = zeros(1,length(alpharange));
% eps = 0.000001;
% for n = 1:length(alpharange)
%     resultrangef(n)= (-alpharange(n) * log(K) + log(psialpha(model,alpharange(n),lnS,T,r,d,varargin{:})));
%     resultrangef1(n)= (-(alpharange(n) + eps) * log(K) ...
%                         + log(psialpha(model,alpharange(n)+eps,lnS,T,r,d,varargin{:})));
%     resultrangef1(n)= (resultrangef1(n) - resultrangef(n)) / eps;
% end
% plot(alpharange,resultrangef); hold on; plot(alpharange,resultrangef1, 'g'); hold off;

%applying FFT
tmp = DiscountFactor * psi(model,vj,optAlpha,lnS,T,r,d,varargin{:}) .* exp(1i * vj * (FFT_b)) * FFT_eta;
tmp = (tmp / 3) .* (3 + (-1).^jvec - ((jvec - 1) == 0) );   %applying simpson's rule
cpvec = real(exp(-optAlpha .* ku) .* fft(tmp) / pi);        %call price vector resulting in equation 24

indexOfStrike = floor((lnK + FFT_b)/FFT_lambda + 1);
iset = max(indexOfStrike)+1:-1:min(indexOfStrike)-1;
xp = ku(iset);
yp = cpvec(iset);
call_price_fft = real(interp1(xp,yp,lnK));

end

%analytical formula for zhi in equation ( 6 ) of Madan's paper
function ret = psi(model,v,alpha,varargin)
  ret = exp(feval(@CharacteristicFunctionLib, model, v - (alpha + 1) * 1i,varargin{:})) ./ (alpha.^2 + alpha - v.^2 + 1i * (2 * alpha + 1) .* v);
end

% function ret = psialpha(model,alpha,varargin)
%   ret = exp(feval(@CharacteristicFunctionLib, model, - (alpha + 1) * 1i,varargin{:}))./ (alpha.^2 + alpha);
% end


function [x, funVal, ValueL]=tree_mtLeastR(A, y, z, opts)
%
%%
% Function mcLeastR:
%      Least Squares Loss for Multi-task Learning
%             with the tree structured group Lasso Regularization
%
%% Problem
%
%  min  1/2 sum_j || A_j x_j - y_j||^2 + z * sum_i sum_j w_j ||x^ij_{G_ji}||
%
%  x^i denotes the i-th row of x
%  x_j denotes the j-th column of x
%  y_j denotes the j-th column of y
%
%  G_i's are nodes with tree structure
%
%  We assume that the tasks are of a tree structure
%
%  The tree structured group information is contained in
%  opts.ind, which is a 3 x nodes matrix, where nodes denotes the number of
%  nodes of the tree.
%
%  opts.ind_t(1,:) contains the starting index
%  opts.ind_t(2,:) contains the ending index
%  opts.ind_t(3,:) contains the corresponding weight (w_j)
%
%  Note:
%  1) If each element of x^j is a leaf node of the tree and the weight for
%  this leaf node are the same, we provide an alternative "efficient" input
%  for this kind of node, by creating a "super node" with
%  opts.ind_t(1,1)=-1; opts.ind_t(2,1)=-1; and opts.ind_t(3,1)=the common weight.
%
%  2) If the features are well ordered in that, the features of the left
%  tree is always less than those of the right tree, opts.ind(1,:) and
%  opts.ind(2,:) contain the "real" starting and ending indices. That is to
%  say, x^j( opts.ind_t(1,j):opts.ind_t(2,j) ) denotes x^j_{G_j}. In this case,
%  the entries in opts.ind_t(1:2,:) are within 1 and k (the number of tasks).
%
%
%  If the features are not well ordered, please use the input opts.G for
%  specifying the index so that
%   x^j( opts.G ( opts.ind_t(1,j):opts.ind_t(2,j) ) ) denotes x^j_{G_j}.
%  In this case, the entries of opts.G are within 1 and n, and the entries of
%  opts.ind_t(1:2,:) are within 1 and length(opts.G).
%
% The following example shows how G and ind works:
%
% G={ {1, 2}, {4, 5}, {3, 6}, {7, 8},
%     {1, 2, 3, 6}, {4, 5, 7, 8},
%     {1, 2, 3, 4, 5, 6, 7, 8} }.
%
% ind_t={ [1, 2, 100]', [3, 4, 100]', [5, 6, 100]', [7, 8, 100]',
%       [9, 12, 100]', [13, 16, 100]', [17, 24, 100]' },
%
% where each node has a weight of 100.
%
%  3) we use opts.ind to denote the starting and ending indices for the
%  samples of different tasks.
%  Samples for the the first task is in
%    A ( (opts.ind(1)+1):opts.ind(2), :  )
%
%% Input parameters:
%
%  A-         Matrix of size m x n
%                A can be a dense matrix
%                         a sparse matrix
%                         or a DCT matrix
%  y -        Response vector (of size mxk)
%  z -        Regularization parameter (z >=0)
%  opts-      optional inputs (default value: opts=[])
%
%% Output parameters:
%  x-         Solution (of size n x k)
%  funVal-    Function value during iterations
%
%% Copyright (C) 2009-2010 Jun Liu, and Jieping Ye
%
% You are suggested to first read the Manual.
%
% For any problem, please contact with Jun Liu via j.liu@asu.edu
%
% Last modified on October 3, 2010.
%
%% Related papers
%
% [1] Jun Liu and Jieping Ye, Moreau-Yosida Regularization for
%     Grouped Tree Structure Learning, NIPS 2010
%
%% Related functions:
%
%  sll_opts
%
%%

%% Verify and initialize the parameters
%%
if (nargin <4)
    error('\n Inputs: A, y, z, and opts (.ind, .ind_t) should be specified!\n');
end

[m,n]=size(A);

if (size(y,1) ~=m)
    error('\n Check the length of y!\n');
end

if (z<0)
    error('\n z should be nonnegative!\n');
end

opts=sll_opts(opts); % run sll_opts to set default values (flags)

% restart the program for better efficiency
%  this is a newly added function
if (~isfield(opts,'rStartNum'))
    opts.rStartNum=opts.maxIter;
else
    if (opts.rStartNum<=0)
        opts.rStartNum=opts.maxIter;
    end
end

%% Detailed initialization
%%
%% Normalization

% Please refer to sll_opts for the definitions of mu, nu and nFlag
%
% If .nFlag =1, the input matrix A is normalized to
%                     A= ( A- repmat(mu, m,1) ) * diag(nu)^{-1}
%
% If .nFlag =2, the input matrix A is normalized to
%                     A= diag(nu)^{-1} * ( A- repmat(mu, m,1) )
%
% Such normalization is done implicitly
%     This implicit normalization is suggested for the sparse matrix
%                                    but not for the dense matrix
%

if (opts.nFlag~=0)
    if (isfield(opts,'mu'))
        mu=opts.mu;
        if(size(mu,2)~=n)
            error('\n Check the input .mu');
        end
    else
        mu=mean(A,1);
    end

    if (opts.nFlag==1)
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=n)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,1)/m).^(0.5); nu=nu';
        end
    else % .nFlag=2
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=m)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,2)/n).^(0.5);
        end
    end

    ind_zero=find(abs(nu)<= 1e-10);    nu(ind_zero)=1;
    % If some values in nu is typically small, it might be that,
    % the entries in a given row or column in A are all close to zero.
    % For numerical stability, we set the corresponding value to 1.
end

if (~issparse(A)) && (opts.nFlag~=0)
    fprintf('\n -----------------------------------------------------');
    fprintf('\n The data is not sparse or not stored in sparse format');
    fprintf('\n The code still works.');
    fprintf('\n But we suggest you to normalize the data directly,');
    fprintf('\n for achieving better efficiency.');
    fprintf('\n -----------------------------------------------------');
end

%% Group & Others

% Initialize ind
if ~isfield(opts,'ind')
    error('\n In tree_mtLeastR, .ind should be specified');
else
    ind=opts.ind;
    k=length(ind)-1;

    if ind(k+1)~=m
        error('\n Check opts.ind');
    end
end

% Initialize ind
if (~isfield(opts,'ind_t'))
    error('\n In tree_mtLeastR, the field .ind_t should be specified');
else
    ind_t=opts.ind_t;

    if (size(ind_t,1)~=3)
        error('\n Check opts.ind_t');
    end
end

GFlag=0;
% if GFlag=1, we shall apply general_altra
if (isfield(opts,'G'))
    GFlag=1;

    G=opts.G;
    if (max(G) >k || max(G) <1)
        error('\n The input G is incorrect. It should be within %d and %d',1,k);
    end
end

%% Starting point initialization

ATy=zeros(n, k);
% compute AT y
for i=1:k
    ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group

    if (opts.nFlag==0)
        tt =A(ind_i,:)'*y(ind_i,1);
    elseif (opts.nFlag==1)
        tt= A(ind_i,:)'*y(ind_i,1) - sum(y(ind_i,1)) * mu';
        tt=tt./nu(ind_i,1);
    else
        invNu=y(ind_i,1)./nu(ind_i,1);
        tt=A(ind_i,:)'*invNu - sum(invNu)*mu';
    end

    ATy(:,i)= tt;
end

% process the regularization parameter
if (opts.rFlag==0)
    lambda=z;
else % z here is the scaling factor lying in [0,1]
%     if (z<0 || z>1)
%         error('\n opts.rFlag=1, and z should be in [0,1]');
%     end

    computedFlag=0;
    if (isfield(opts,'lambdaMax'))
        if (opts.lambdaMax~=-1)
            lambda=z*opts.lambdaMax;
            computedFlag=1;
        end
    end

    if (~computedFlag)
        if (GFlag==0)
            lambda_max=findLambdaMax_mt(ATy, n, k, ind_t, size(ind_t,2));
        else
            lambda_max=general_findLambdaMax_mt(ATy, n, k, G, ind_t, size(ind_t,2));
        end

        % As .rFlag=1, we set lambda as a ratio of lambda_max
        lambda=z*lambda_max;
    end
end

% The following is for computing lambdaMax
% we use opts.lambdaMax=-1 to show that we need the computation.
%
% One can use this for setting up opts.lambdaMax
if (isfield(opts,'lambdaMax'))

    if (opts.lambdaMax==-1)
        if (GFlag==0)
            lambda_max=findLambdaMax_mt(ATy, n, k, ind_t, size(ind_t,2));
        else
            lambda_max=general_findLambdaMax_mt(ATy, n, k, G, ind_t, size(ind_t,2));
        end

        x=lambda_max;
        funVal=lambda_max;
        ValueL=lambda_max;

        return;
    end
end

% initialize a starting point
if opts.init==2
    x=zeros(n,k);
else
    if isfield(opts,'x0')
        x=opts.x0;
        if ( size(x,1)~=n || size(x,2)~=k )
            error('\n Check the input .x0');
        end
    else
        x=ATy;  % if .x0 is not specified, we use ratio*ATy,
        % where ratio is a positive value
    end
end

Ax=zeros(m,1);
% compute Ax: Ax_i= A_i * x_i
for i=1:k
    ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group
    m_i=ind(i+1)-ind(i);          % number of samples in the i-th group

    if (opts.nFlag==0)
        Ax(ind_i,1)=A(ind_i,:)* x(:,i);
    elseif (opts.nFlag==1)
        invNu=x(:,i)./nu; mu_invNu=mu * invNu;
        Ax(ind_i,1)=A(ind_i,:)*invNu -repmat(mu_invNu, m_i, 1);
    else
        Ax(ind_i,1)=A(ind_i,:)*x(:,i)-repmat(mu*x(:,i), m, 1);
        Ax(ind_i,1)=Ax./nu(ind_i,1);
    end
end

if (opts.init==0)
    % ------  This function is not available
    %
    % If .init=0, we set x=ratio*x by "initFactor"
    % Please refer to the function initFactor for detail
    %

    % Here, we only support starting from zero, due to the complex tree
    % structure

    x=zeros(n,k);
end

%% The main program

%% The Armijo Goldstein line search schemes + accelearted gradient descent

if (opts.mFlag==0 && opts.lFlag==0)

    bFlag=0; % this flag tests whether the gradient step only changes a little

    L=1;
    % We assume that the maximum eigenvalue of A'A is over 1

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,k);

    alphap=0; alpha=1;

    for iterStep=1:opts.maxIter
        % --------------------------- step 1 ---------------------------
        % compute search point s based on xp and x (with beta)
        beta=(alphap-1)/alpha;    s=x + beta* xxp;

        % --------------------------- step 2 ---------------------------
        % line search for L and compute the new approximate solution x

        % compute the gradient (g) at s
        As=Ax + beta* (Ax-Axp);

        % compute ATAs : n x k
        for i=1:k
            ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group

            if (opts.nFlag==0)
                tt =A(ind_i,:)'*As(ind_i,1);
            elseif (opts.nFlag==1)
                tt= A(ind_i,:)'*As(ind_i,1) - sum(As(ind_i,1)) * mu';
                tt=tt./nu(ind_i,1);
            else
                invNu=As(ind_i,1)./nu(ind_i,1);
                tt=A(ind_i,:)'*invNu - sum(invNu)*mu';
            end

            ATAs(:,i)= tt;
        end

        % obtain the gradient g
        g=ATAs-ATy;

        % copy x and Ax to xp and Axp
        xp=x;    Axp=Ax;

        while (1)
            % let s walk in a step in the antigradient of s to get v
            % and then do the L1/Lq-norm regularized projection
            v=s-g/L;

            % tree overlapping group Lasso projection
            ind_work(1:2,:)=ind_t(1:2,:);
            ind_work(3,:)=ind_t(3,:) * (lambda / L);

            if (GFlag==0)
                x=altra_mt(v, n, k, ind_work, size(ind_work,2));
            else
                x=general_altra_mt(v, n, k, G, ind_work, size(ind_work,2));
            end

            v=x-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute Ax: Ax_i= A_i * x_i
            for i=1:k
                ind_i=(ind(i)+1):ind(i+1);     % indices for the i-th group
                m_i=ind(i+1)-ind(i);          % number of samples in the i-th group

                if (opts.nFlag==0)
                    Ax(ind_i,1)=A(ind_i,:)* x(:,i);
                elseif (opts.nFlag==1)
                    invNu=x(:,i)./nu; mu_invNu=mu * invNu;
                    Ax(ind_i,1)=A(ind_i,:)*invNu -repmat(mu_invNu, m_i, 1);
                else
                    Ax(ind_i,1)=A(ind_i,:)*x(:,i)-repmat(mu*x(:,i), m, 1);
                    Ax(ind_i,1)=Ax./nu(ind_i,1);
                end
            end

            Av=Ax -As;
            r_sum=norm(v,'fro')^2; l_sum=Av'*Av;

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is ||Av||_2^2 <= L * ||v||_2^2
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                % fprintf('\n L=%5.6f',L);
            end
        end

        % --------------------------- step 3 ---------------------------
        % update alpha and alphap, and check whether converge
        alphap=alpha; alpha= (1+ sqrt(4*alpha*alpha +1))/2;

        ValueL(iterStep)=L;

        xxp=x-xp;   Axy=Ax-y;
        funVal(iterStep)=Axy'*Axy/2;

        for i=1:n
            xRow=x(i,:);

            if (GFlag==0)
                tree_norm=treeNorm(xRow, k, ind_t, size(ind_t,2));
            else
                tree_norm=general_treeNorm(xRow, k, G, ind_t, size(ind_t,2));
            end

            funVal(iterStep)=funVal(iterStep)+ lambda*tree_norm;
        end

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=norm(xp,'fro');    norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end

        % restart the program every opts.rStartNum
        if (~mod(iterStep, opts.rStartNum))
            alphap=0; alpha=1;
            xp=x; Axp=Ax; xxp=zeros(n,k); L =L/2;
        end
    end
else
    error('\n The function does not support opts.mFlag neq 0 & opts.lFlag neq 0!');
end


function [PowLoss] = PowerLoss(modres, modfit, moddf, tc, TR, Run, alpha)
% function [PowLoss] = PowerLoss(modres, modfit, moddf, tc, TR, Run, alpha)
%
% Estimates Power-loss due to mis-modeling.
%
% INPUT:
%
% modres - residuals
% modfit - model fit
% moddf  - model degrees of freedom
% tc     - time course
% TR     - time resolution
% Runs   - expermental design
% alpha  - alpha value
%
% OUTPUT:
%
% PowLoss - Estimated power loss
%
%

len = length(tc);               % length of time course
%T = round(30./TR);              % length of estimated HRF
T = 30;
tstar = tinv(1-alpha,moddf);    % t-threshold

% Fit FIR model to find 'baseline' power.
[h, fit, e] = Fit_sFIR(tc,TR,Run,T,1);
s = (1/(len-T))*e'*e;
t = 1/sqrt(s*inv(fit'*fit));
basePow = 1- nctcdf(tstar,(len-T),t);

% Compute model power.
sig = (1/moddf)*modres'*modres;
ts = 1/sqrt(sig*inv(modfit'*modfit));
modPow = 1- nctcdf(tstar,moddf,ts);

% Compute 'power loss'
PowLoss = basePow - modPow;


%
%  fft wrt the center of the array, instead of the first sample
%

%  written by John Pauly, 1992
%  (c) Board of Trustees, Leland Stanford Junior University

function y=fftc(x)

y = fftshift(fft(fftshift(x)));


function [P_music, est_dirs] = sphMUSIC(sphCOV, grid_dirs, nSrc)
%SPHMUSIC DoA estimation using MUSIC in the SHD
%
%   This routine computes the MUSIC pseudo-spectrum directly in the SHD.
%   Subspace approaches such as MUSIC can offer higher spatial resolution
%   than beamforming approaches such as the steered-response power, as long
%   as the source signals are not correlated between them and with the
%   reverberant/diffuse sound.
%
%   Inputs:
%       sphCOV: (order+1)^2x(order+1)^2 covariance/correlation matrix of
%           SH signals
%       grid_dirs:  Kx2 directions of [azi elev] in rads that define a grid
%           for the power map. For easy plotting of the directional maps,
%           use grid2dirs.m to generate the directions
%       nSrc:   (optional) number of peaks to try to find, as potential DoA
%           estimates (Von-Mises peak-finding contributed by Dr. Sakari Tervo)
%
%   Outputs:
%       P_pwd:      Kx1 vector of output powers, evaluated at grid directions
%       est_dirs:   nSrcx2 [azi elev] of estimated directions from
%           peak-finding
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SPHMUSIC.M - 5/10/2016
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nSH = size(sphCOV,1);
order = sqrt(nSH)-1;
% sorted eigenvalue decomposition
V = sorted_eig(sphCOV, 'descend');
% noise subspace
Vn = V(:,nSrc+1:end);

% get steering vectors for grid points
nGrid = size(grid_dirs,1);
grid_xyz = unitSph2cart(grid_dirs);
grid_dirs2 = [grid_dirs(:,1) pi/2-grid_dirs(:,2)]; % go from azi-elevation to azi-inclination
Y_grid = getSH(order, grid_dirs2, 'real');

% get MUSIC spectrum
P_music = zeros(nGrid,1);
for ng = 1:nGrid
    stVec = Y_grid(ng,:).';
    P_music(ng) = 1 / (stVec' * Vn * Vn' * stVec);
end

% peak finding, if asked
if nargout==2 && nargin==3

    kappa  = 50; % Von-Mises concentration factor
    P_minus_peak = P_music;
    est_dirs = zeros(nSrc, 2);
    for k = 1:nSrc
        [~, peak_idx] = max(P_minus_peak);
        est_dirs(k,:) = grid_dirs(peak_idx,:);
        VM_mean = grid_xyz(peak_idx,:); % orientation of VM distribution
        VM_mask = kappa/(2*pi*exp(kappa)-exp(-kappa)) * exp(kappa*grid_xyz*VM_mean'); % VM distribution
        VM_mask = 1./(0.00001+VM_mask); % inverse VM distribution
        P_minus_peak = P_minus_peak.*VM_mask;
    end
end

end