function filtered_image = gaussianbpf(I,d0,d1)
	% Butterworth Bandpass Filter
	% This simple  function was written for my Digital Image Processing course
	% at Eastern Mediterranean University taught by
	% Assoc. Prof. Dr. Hasan Demirel
	% for the 2010-2011 Spring Semester
	% for the complete report:
	% http://www.scribd.com/doc/51981950/HW4-Frequency-Domain-Bandpass-Filtering
	%
	% Written By:
	% Leonardo O. Iheme (leonardo.iheme@cc.emu.edu.tr)
	% 24th of March 2011
	%
	%   I = The input grey scale image
	%   d0 = Lower cut off frequency
	%   d1 = Higher cut off frequency
	%
	% The function makes use of the simple principle that a bandpass filter
	% can be obtained by multiplying a lowpass filter with a highpass filter
	% where the lowpass filter has a higher cut off frquency than the high pass filter.
	%
	% Usage GAUSSIANBPF(I,DO,D1)
	% Example
	% ima = imread('grass.jpg');
	% ima = rgb2gray(ima);
	% filtered_image = gaussianbpf(ima,30,120);
	% Gaussian Bandpass Filter
	%
	% biafra ahanonu
	% updated: 2013.11.09 [13:20:43]
	% changelog
		% 2013.11.09 [13:21:06] updated function so it no longer converts to uint8 as this causes problems. Also, changed filtered_image = fftI + filter3.*fftI; to filtered_image = filter3.*fftI;
		% 2014.06.03 - updated to do binary fft

	f = double(I);
	[nx ny] = size(f);
	% f = uint8(f);
	fftI = fft2(f,2*nx-1,2*ny-1);
	fftI = fftshift(fftI);

	% subplot(2,2,1)
	% imshow(f,[]);
	% title('Original Image')

	% subplot(2,2,2)
	% fftshow(fftI,'log')
	% title('Fourier Spectrum of Image')
	% Initialize filter.
	% filter1 = ones(2*nx-1,2*ny-1);
	% filter2 = ones(2*nx-1,2*ny-1);
	% filter3 = ones(2*nx-1,2*ny-1);
	% % ======
	% % Gaussian
	% for i = 1:2*nx-1
	%     for j =1:2*ny-1
	%         dist = ((i-(nx+1))^2 + (j-(ny+1))^2)^.5;
	%         % Use Gaussian filter.
	%         filter1(i,j) = exp(-dist^2/(2*d1^2));
	%         filter2(i,j) = exp(-dist^2/(2*d0^2));
	%         filter3(i,j) = 1.0 - filter2(i,j);
	%         filter3(i,j) = filter1(i,j).*filter3(i,j);
	%     end
	% end
	% ======
	% binary
	imageSize = size(fftI);
	ci = [round(imageSize(1)/2), round(imageSize(2)/2), 10];     % center and radius of circle ([c_row, c_col, r])
	[xx,yy] = ndgrid((1:imageSize(1))-ci(1),(1:imageSize(2))-ci(2));
	filter3 = logical((xx.^2 + yy.^2)<(ci(3)^2));
	% imagesc(mask)
	% ======
	% Update image with passed frequencies
	fftI_filtered = filter3.*fftI;
	% subplot(2,2,3)
	% fftshow(filter3,'log')
	% title('Frequency Domain Filter Function Image')
	filtered_image = ifftshift(fftI_filtered);
	filtered_image = ifft2(filtered_image,2*nx-1,2*ny-1);
	filtered_image = real(filtered_image(1:nx,1:ny));
	filtered_image = double(filtered_image);
	% filtered_image = uint8(filtered_image);
	% ======
	% imagesc(filter3)
	subplot(2,2,1)
	fftshow(fftI_filtered,'log')
	subplot(2,2,2)
	fftshow(fftI,'log')
	subplot(2,2,3)
	imagesc(I)
	subplot(2,2,4)
	imshow(filtered_image,[])
	title('Bandpass Filtered Image')


function [u,v]=pow2cep(m,c,mode)
%CEP2POW convert cepstral means and variances to the power domain
% Inputs:
%    m: vector giving means in the power domain
%    c: covariance matrix in the power domain
% mode: 'c'  pow=exp(irdct(cep))   [default]
%       'f'  pow=exp(rsfft(cep)/n)  [fft length even]
%       'fo' pow=exp(rsfft(cep)/n)  [fft length odd]
%       'i'  pow=exp(cep)           [ no transformation ]
%
% Outputs:
%    u: row vector giving the cepstral means with u(1) the 0'th cepstral coefficient
%    v: cepstral covariance matrix

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

if nargin<3 mode='c'; end
if min(size(c))==1
   v=diag(c);
end
m=m(:)';        % force to be a row vector
q=log(1+c./(m'*m));
p=log(m)-0.5*diag(q)';
if any(mode=='f')
   n=2*length(m)-2;
   if any(mode=='o')
      n=n+1;
   end
   u=rsfft(p,n);
   v=rsfft(rsfft(q,n)',n);
elseif any(mode=='i')
    u=p;
    v=q;
else
   u=rdct(p);
   v=rdct(rdct(q)');
end


function [xyz_out, indx, min_distance] = select_coordinates_near_regions(region_obj, xyz, cutoff_in_mm)
% xyz_out = select_coordinates_near_regions(region_obj, xyz, cutoff_in_mm)
%
% xyz = r x 2 matrix of XYZ mm coordinates, e.g., DB.xyz from meta-analysis
% cutoff_in_mm: xyz coordinates within this distance from any coordinate in the region object will be saved

xyz1 = cat(2, region_obj.XYZmm)'; % region object

% xyz1 : q x 3 matrix of XYZ mm coordinates
% xyz : 3 x r matrix of XYZ mm coordinates

D = dist(xyz1, xyz');

min_distance = min(D, [], 1)';

indx = min_distance < cutoff_in_mm;

xyz_out = xyz(indx, :);

end % function


% Serhat Selcuk Bucak, bucakser@msu.edu

% Lets assume that all samples are stored in data matrix V (each column is a sample)

%Execute NMF for the first n samples
%% These number are selected just for demonstration
V=rand(40,500);
n=100;
rdim=10;
maxiter=150;
[W, H, objhistory] = nmf(V(:,1:n), rdim, 0, maxiter);
% Now we can execute inmf on each new samples
maxiter=50;
A=V(:,1:n)*H';
B=H*H';
h=H(:,end); % Warm start for h
for i=n+1:size(V,2)
    i
    V_new=V(:,i);
    [W_new, h, A, B] = inmf( V_new, W, h, A, B, rdim, 0.9, 0.1, maxiter);
    H_store(:,i-n)=h; %Just for demonstration
end


% ttest2_cell() - compute unpaired t-test. Allow fast computation of
%                 multiple t-test using matrix manipulation.
%
% Usage:
%    >> [F df] = ttest2_cell( { a b } );
%    >> [F df] = ttest2_cell(a, b);
%    >> [F df] = ttest2_cell(a, b, 'inhomogenous');
%
% Inputs:
%   a,b       = data consisting of UNPAIRED arrays to be compared. The last
%               dimension of the data array is used to compute the t-test.
%   'inhomogenous' = use computation for the degree of freedom using
%                    inhomogenous variance. By default the computation of
%                    the degree of freedom is done with homogenous
%                    variances.
%
% Outputs:
%   T   - T-value
%   df  - degree of freedom (array)
%
% Example:
%   a = { rand(1,10) rand(1,10)+0.5 }
%   [T df] = ttest2_cell(a)
%   signif = 2*tcdf(-abs(T), df(1))
%
%   % for comparison, the same using the Matlab t-test function
%   [h p ci stats] = ttest2(a{1}', a{2}');
%   [ stats.tstat' p]
%
%   % fast computation (fMRI scanner volume 100x100x100 and 10 control
%   % subjects and 12 test subjects). The computation itself takes 0.5
%   % seconds instead of  half an hour using the standard approach (1000000
%   % loops and Matlab  t-test function)
%   a = rand(100,100,100,10); b = rand(100,100,100,10);
%   [F df] = ttest_cell({ a b });
%
% Author: Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 2005
%         (thank you to G. Rousselet for providing the formula for
%         inhomogenous variances).
%
% Reference:
%   Schaum's outlines in statistics (3rd edition). 1999. Mc Graw-Hill.
%   Howel, Statistical Methods for Psychology. 2009. Wadsworth Publishing.

% Copyright (C) Arnaud Delorme
%
% 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function [tval, df] = ttest2_cell(a,b,c) % assumes equal variances

    if nargin < 1
        help ttest2_cell;
        return;
    end;

    homogenous = 'homogenous';
    if nargin > 1 && isstr(b)
        homogenous = b;
    end;
    if nargin > 2 && isstr(c)
        homogenous = c;
    end;
    if iscell(a),
        b = a{2};
        a = a{1};
    end;
    if ~strcmpi(homogenous, 'inhomogenous') && ~strcmpi(homogenous, 'homogenous')
        error('Value for homogenous parameter can only be ''homogenous'' or ''inhomogenous''');
    end;

    nd    = myndims(a);
    na    = size(a, nd);
    nb    = size(b, nd);
    meana = mymean(a, nd);
    meanb = mymean(b, nd);

    if strcmpi(homogenous, 'inhomogenous')
        % inhomogenous variance from Howel, 2009, "Statistical Methods for Psychology"
        % thank you to G. Rousselet for providing these formulas
        m  = meana - meanb;
        s1 = var(a,0,nd) ./ na;
        s2 = var(b,0,nd) ./ nb;
        se = sqrt(s1 + s2);
        sd = sqrt([s1.*na, s2.*nb]);
        tval = m ./ se;

        df = ((s1 + s2).^2) ./ ((s1.^2 ./ (na-1) + s2.^2 ./ (nb-1)));
    else
        sda   = mystd(a, [], nd);
        sdb   = mystd(b, [], nd);
        sp    = sqrt(((na-1)*sda.^2+(nb-1)*sdb.^2)/(na+nb-2));
        tval  = (meana-meanb)./sp/sqrt(1/na+1/nb);
        df    = na+nb-2;
    end;

    % check values againg Matlab statistics toolbox
    % [h p ci stats] = ttest2(a', b');
    % [ tval stats.tstat' ]

function val = myndims(a)
    if ndims(a) > 2
        val = ndims(a);
    else
        if size(a,1) == 1,
            val = 2;
        elseif size(a,2) == 1,
            val = 1;
        else
            val = 2;
        end;
    end;

function res = mymean( data, varargin) % deal with complex numbers
    res = mean( data, varargin{:});
    if ~isreal(data)
        res = abs( res );
    end;

function res = mystd( data, varargin) % deal with complex numbers
    if ~isreal(data)
        res = std( abs(data), varargin{:});
    else
        res = sqrt(sum( bsxfun(@minus, data, mean( data, varargin{2})).^2, varargin{2})/(size(data,varargin{2})-1)); % 8 percent speedup
        %res = std( data, varargin{:});
    end;


A=rand(1000,1000);
A=A'*A;

tic
B=mexInvSym(A);
t=toc;
fprintf('mex-file time: %fs\n',t);

tic
B2=inv(A);
t=toc;
fprintf('matlab-file time: %fs\n',t);

sum((B(:)-B2(:)).^2)


function days = month_length_greek ( y, m )

%*****************************************************************************80
%
%% MONTH_LENGTH_GREEK returns the number of days in a Greek month.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year in which the month occurred.
%
%    Input, integer M, the number of the month.
%
%    Output, integer DAYS, the number of days
%    in the month.
%
  mdays = [ 30, 29, 30, 29, 30, 29, 29, 30, 29, 30, 29, 30, 29 ];
%
%  Copy the input.
%
  m2 = m;
  y2 = y;

  if ( m2 < 1 )
    days = 0;
    return
  end
%
%  A 13-month year.
%
  if ( year_is_embolismic_greek ( y2 ) )

    if ( 13 < m2 )
      days = 0;
      return
    end

    days = mdays(m2);

    if ( m2 == 7 && year_is_leap_greek ( y2 ) )
      days = days + 1;
    end
%
%  A 12 month year.
%
  else

    if ( m2 <= 6 )
      days = mdays(m2);
    elseif ( m2 <= 12 )
      days = mdays(m2+1);
    else
      days = 0;
    end

  end

  return
end


function linpack_d_test07 ( )

%*****************************************************************************80
%
%% TEST07 tests DGBFA and DGBSL.
%
%  Discussion:
%
%    DGBFA and DGBSL are for general banded matrices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 100;
  ml = 25;
  mu = 25;
  lda = 2*ml+mu+1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST07\n' );
  fprintf ( 1, '  For a general banded matrix,\n' );
  fprintf ( 1, '  DGBFA factors the matrix,\n' );
  fprintf ( 1, '  DGBSL solves a factored linear system.\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Assign values to matrix A and right hand side B.
%
%  We want to try a problem with a significant bandwidth.
%
  m = ml + mu + 1;
  fprintf ( 1, '  The bandwidth of the matrix is %d\n', m );

  for j = 1 : n

    ilo = max ( 1, j - mu );
    ihi = min ( n, j + ml );

    temp = 0.0;
    for i = ilo : ihi
      a(i-j+m,j) = -1.0;
      temp = temp - 1.0;
    end

    temp = temp + 1.0;
    a(m,j) = 4.0 - temp;
    b(j) = 4.0;

  end
%
%  Force B to be a column vector.
%
  b = b';
%
%  Factor the matrix A.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Factor the matrix.\n' );

  [ a, ipivot, info ] = dgbfa ( a, lda, n, ml, mu );

  if ( info ~= 0 )
   fprintf ( 1, '  Error!  DGBFA returns INFO = %d\n', info );
    return
  end
%
%  Call DGBSL to solve the linear system.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the linear system.\n' );

  job = 0;
  b = dgbsl ( a, lda, n, ml, mu, ipivot, b, job );
%
%  Print the results.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The first and last 5 entries of the solution:\n' );
  fprintf ( 1, '  (All should be 1):\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    if ( i <= 5 | n-5 < i )
      fprintf ( 1, '  %6d  %14f\n', i, b(i) );
    end
    if ( i == 5 )
      fprintf ( 1, '  ......  ..............\n' );
    end
  end

  return
end


function [ i_lo, i_hi ] = r8vec_sorted_range ( n, r, r_lo, r_hi )

%*****************************************************************************80
%
%% R8VEC_SORTED_RANGE searches a sorted vector for elements in a range.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of items in the vector.
%
%    Input, real R(N), the sorted vector.
%
%    Input, real R_LO, R_HI, the limits of the range.
%
%    Output, integer I_LO, I_HI, the range of indices
%    so that I_LO <= I <= I_HI => R_LO <= R(I) <= R_HI.  If no
%    values in R lie in the range, then I_HI < I_LO will be returned.
%

%
%  Cases we can handle immediately.
%
  if ( r(n) < r_lo )
    i_lo = 0;
    i_hi = - 1;
    return
  end

  if ( r_hi < r(1) )
    i_lo = 0;
    i_hi = - 1;
    return
  end
%
%  Are there are least two intervals?
%
  if ( n == 1 )
    if ( r_lo <= r(1) && r(1) <= r_hi )
      i_lo = 1;
      i_hi = 1;
    else
      i_lo = 0;
      i_hi = -1;
    end
    return
  end
%
%  Bracket R_LO.
%
  if ( r_lo <= r(1) )

    i_lo = 1;

  else
%
%  R_LO is in one of the intervals spanned by R(J1) to R(J2).
%  Examine the intermediate interval [R(I1), R(I1+1)].
%  Does R_LO lie here, or below or above?
%
    j1 = 1;
    j2 = n;
    i1 = floor ( ( j1 + j2 - 1 ) / 2 );
    i2 = i1 + 1;

    while ( 1 )

      if ( r_lo < r(i1) )
        j2 = i1;
        i1 = floor ( ( j1 + j2 - 1 ) / 2 );
        i2 = i1 + 1;
      elseif ( r(i2) < r_lo )
        j1 = i2;
        i1 = floor ( ( j1 + j2 - 1 ) / 2 );
        i2 = i1 + 1;
      else
        i_lo = i1;
        break;
      end

    end

  end
%
%  Bracket R_HI
%
  if ( r(n) <= r_hi )

    i_hi = n;

  else

    j1 = i_lo;
    j2 = n;
    i1 = floor ( ( j1 + j2 - 1 ) / 2 );
    i2 = i1 + 1;

    while ( 1 )

      if ( r_hi < r(i1) )
        j2 = i1;
        i1 = floor ( ( j1 + j2 - 1 ) / 2 );
        i2 = i1 + 1;
      elseif ( r(i2) < r_hi )
        j1 = i2;
        i1 = floor ( ( j1 + j2 - 1 ) / 2 );
        i2 = i1 + 1;
      else
        i_hi = i2;
        break
      end

    end

  end
%
%  We expect to have computed the largest I_LO and smallest I_HI such that
%    R(I_LO) <= R_LO <= R_HI <= R(I_HI)
%  but what we want is actually
%    R_LO <= R(I_LO) <= R(I_HI) <= R_HI
%  which we can usually get simply by incrementing I_LO and decrementing I_HI.
%
  if ( r(i_lo) < r_lo )
    i_lo = i_lo + 1;
    if ( n < i_lo )
      i_hi = i_lo - 1;
    end
  end

  if ( r_hi < r(i_hi) )
    i_hi = i_hi - 1;
    if ( i_hi < 1 )
      i_lo = i_hi + 1;
    end
  end

  return
end


% Assume a target positioned at x = 1, travelling with speed v = 0.1
state = [1;0.1;2;0;3;0.2;4;0.3];

% Create an instance of a 2D Ornstein-Uhlenbeck model
ou = OrnsteinUhlenbeckModelX('NumDims',4,...
                             'VelocityErrVariance',0.1,...
                             'DampingCoefficient',0.1,...
                             'TimestepDuration',duration(1,0,1));

% View the transition matrix and process covariance matrices
F = ou.feval();
Q = ou.covar();

% Predict the target's position and velocity after the interval has passed
newState  = ou.feval(state);

% Do the same as above, but this time add process noise to the prediction
newState2 = ou.feval(state,true);

% Generate 50 random noise samples from the dynamic model
noise = ou.random(50);

% Check how likely the predictions we made are
lik = ou.pdf(newState,state);
lik2 = ou.pdf(newState2,state); % HINT: newState2 should be less likely


close all
clear

pos_rob = [-0.5,-0.5;0.5,0.5];
pos_ref =  [1.1,1;-1.1,-1];
pos_circ = [-0.5,-0.5;0.5,0.5]/2;

% circular constraint sets
zbf_circ = @(i,pos_rob) 1-(pos_rob(i,:)-pos_circ(i,:))*(pos_rob(i,:)-pos_circ(i,:))';
% nominal control
knom = 1;
unom_rob = @(i,pos_rob) -knom*(pos_rob(i,:)-pos_ref(i,:));
% bounded distance costraints
ld = 0.5; ud = 1;
zbf_lb = @(i,j,pos_rob) (pos_rob(i,:)-pos_rob(j,:))*(pos_rob(i,:)-pos_rob(j,:))'-ld^2;
zbf_ub = @(i,j,pos_rob) ud^2-(pos_rob(i,:)-pos_rob(j,:))*(pos_rob(i,:)-pos_rob(j,:))';
% extended K-class function
akcf = 1;
ekcf = @(h) akcf*h;
% control barrier function (less or equal)
cbfa_circ = @(i,pos_rob) pos_rob(i,:)-pos_circ(i,:);
cbfb_circ = @(i,pos_rob) ekcf(zbf_circ(i,pos_rob));
cbfa_lb = @(i,j,pos_rob) -(pos_rob(i,:)-pos_rob(j,:));
cbfb_lb = @(i,j,pos_rob) ekcf(zbf_lb(i,j,pos_rob));
cbfa_ub = @(i,j,pos_rob) -cbfa_lb(i,j,pos_rob);
cbfb_ub = @(i,j,pos_rob) ekcf(zbf_ub(i,j,pos_rob));

% simulation
dt = 0.01;
T = 10;
loop = 0;
color_list = ['b','g','m','r','c'];
for t=0:dt:T
    loop = loop+1;
    % control barrier function - quadratic program
    for i=1:2
        fqp = -unom_rob(i,pos_rob)';
        aqp = cbfa_circ(i,pos_rob);
        bqp = cbfb_circ(i,pos_rob);
        for j=1:2
            if i~=j
                aqp = [aqp;cbfa_lb(i,j,pos_rob)];
                bqp = [bqp;cbfb_lb(i,j,pos_rob)];
                aqp = [aqp;cbfa_ub(i,j,pos_rob)];
                bqp = [bqp;cbfb_ub(i,j,pos_rob)];
            end
        end
        [uqp,~,is_solved] = quadprog(eye(2),fqp,aqp,bqp);
        uqp_rob(i,:) = uqp';
    end
    % update
    pos_rob = pos_rob+dt*uqp_rob;
    % plot
    for i=1:2
        plot(pos_rob(i,1),pos_rob(i,2),[color_list(i) 'o']); hold on
        circle_(pos_circ(i,:),1,'color',color_list(i));
        plot(pos_ref(i,1),pos_ref(i,2),[color_list(i) 'd']);
        quiver(pos_rob(i,1),pos_rob(i,2),uqp_rob(i,1),uqp_rob(i,2))
    end
    hold off
    drawnow
end


function y = nanmax(x,dim)
% nanmax - Max ignoring NaNs.
%
% Synopsis:
%   y = nanmax(x)
%   y = nanmax(x,dim)
%
% Arguments:
%   x: Matrix or vector
%   dim: Dimension along which sum operates. Default: First non-singleton
%       dimension.
%
% Returns:
%   y: max along the chosen dimension, treating NaNs as missing values.
%
% Description:
%   For vectors, nanmax(X) is the max of the non-NaN elements in
%   X. For matrices, nanmax(X) is a row vector containing the max
%   of the non-NaN elements in each column of X. For N-D arrays,
%   nanmax(X) operates along the first non-singleton dimension.
%
%   nanmax(X,dim) determaxes the max along the dimension dim.
%
% Examples:
%   nanmax([1 2 NaN]) returns 2.
%   nanmax([1 2 NaN], 1) returns [1 2 NaN].
%   nanmax([1 2 NaN], 2) returns 2.
%
% See also: nansum,nanmean,nanstd
%

% Author(s): Benjamax Blankertz, Anton Schwaighofer, Aug 2005

if nargin<2,
  % Operate along the first non-singleton dimension
  dim = max(find(size(x)~=1));
  if isempty(dim),
    dim = 1;
  end
end
% Replace NaNs with zeros.
nans = isnan(x);
x(nans) = -inf;

% Protect against an entire column of NaNs
y = max(x, [], dim);
allNaNs = all(nans, dim);
y(allNaNs) = NaN;


%%*************************************************************************
%% iterrefine: Iterative refinement.
%% This step is crucial to ensure that computed solution
%% is sufficiently accuraete.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*************************************************************************

   function [x,resnrm,solve_ok] = iterrefine(A,b,L,x0);

     tol = 1e-6;
     maxit = 10;
     bnorm = max(1,norm(b));
     tolb = tol*bnorm;
     x = x0;
     solve_ok = 1;
     resnrm(1) = bnorm;
%%
     for iter = 1:maxit
        x0 = x;
        if isstruct(A); r = b-matvec(A,x); else; r=b-A*x; end;
        err = norm(r);
        resnrm(iter+1) = err;

        if (err < tolb); break; end;
        if (iter > 1) & (resnrm(iter+1)/resnrm(iter) > 0.9)
           x = x0; solve_ok = 0; break;
        end
	d = linsysolvefun(L,r);
        x = x + d;
     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;
   x1 = x(1:m);
   Ax = A.mat11*x1;
   if (m2 > 0)
      x2 = x(m+[1:m2]);
      Ax = Ax + A.mat12*x2;
      Ax2 = (x1'*A.mat12)' + A.mat22*x2;
      Ax = [Ax; Ax2];
   end
   return;
%%*************************************************************************


function [ a, ipvt, info ] = dchdc ( a, lda, p, ipvt, job )

%*****************************************************************************80
%
%% DCHDC computes the Cholesky decomposition of a positive definite matrix.
%
%  Discussion:
%
%    A pivoting option allows the user to estimate the condition of a
%    positive definite matrix or determine the rank of a positive
%    semidefinite matrix.
%
%    For positive definite matrices, INFO = P is the normal return.
%
%    For pivoting with positive semidefinite matrices, INFO will
%    in general be less than P.  However, INFO may be greater than
%    the rank of A, since rounding error can cause an otherwise zero
%    element to be positive.  Indefinite systems will always cause
%    INFO to be less than P.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,P), the matrix whose decomposition is to
%    be computed.  Only the upper half of A need be stored.
%    The lower part of the array a is not referenced.
%
%    Input, integer LDA, the leading dimension of the array A.
%
%    Input, integer P, the order of the matrix.
%
%    Input, integer IPVT(P), integers that control the selection
%    of the pivot elements, if pivoting has been requested.
%    Each diagonal element A(K,K) is placed in one of three classes
%    according to the value of IPVT(K).
%
%      > 0, then X(K) is an initial element.
%      = 0, then X(K) is a free element.
%      < 0, then X(K) is a final element.
%
%    Before the decomposition is computed, initial elements are moved by
%    symmetric row and column interchanges to the beginning of the array A
%    and final elements to the end.  Both initial and final elements are
%    frozen in place during the computation and only free elements are moved.
%    At the K-th stage of the reduction, if A(K,K) is occupied by a free
%    element, it is interchanged with the largest free element A(L,L) with
%    K <= L.  IPVT is not referenced if JOB is 0.
%
%    Input, integer JOB, initiates column pivoting.
%    0, no pivoting is done.
%    nonzero, pivoting is done.
%
%    Output, real A(LDA,P), contains in its upper half the Cholesky factor
%    of the input matrix, as it has been permuted by pivoting.
%
%    Output, integer IPVT(N); IPVT(J) contains the index of the diagonal element
%    of A that was moved into the J-th position, if pivoting was requested.
%
%    Output, integer INFO, contains the index of the last positive diagonal
%    element of the Cholesky factor.
%
  pl = 1;
  pu = 0;
  info = p;

  if ( job ~= 0 )
%
%  Pivoting has been requested.
%  Rearrange the the elements according to IPVT.
%
    for k = 1 : p

      swapk = 0 < ipvt(k);

      negk = ipvt(k) < 0;

      if ( negk )
        ipvt(k) = -k;
      else
        ipvt(k) = k;
      end

      if ( swapk )

        if ( k ~= pl )

          temp(1:pl-1) = a(1:pl-1,k);
          a(1:pl-1,k) = a(1:pl-1,pl);
          a(1:pl-1,pl) = temp(1:pl-1);

          temp = a(k,k);
          a(k,k) = a(pl,pl);
          a(pl,pl) = temp;

          for j = pl+1 : p

            if ( j < k )
              temp = a(pl,j);
              a(pl,j) = a(j,k);
              a(j,k) = temp;
            elseif ( k < j )
              temp = a(k,j);
              a(k,j) = a(pl,j);
              a(pl,j) = temp;
            end

          end

          ipvt(k) = ipvt(pl);
          ipvt(pl) = k;

        end

        pl = pl + 1;

      end

    end

    pu = p;

    for k = p : -1 : pl

      if ( ipvt(k) < 0 )

        ipvt(k) = -ipvt(k);

        if ( pu ~= k )

          temp(1:k-1) = a(1:k-1,k);
          a(1:k-1,k) = a(1:k-1,pu);
          a(1:k-1,pu) = temp(1:k-1);

          temp = a(k,k);
          a(k,k) = a(pu,pu);
          a(pu,pu) = temp;

          for j = k+1 : p

            if ( j < pu )
              temp = a(k,j);
              a(k,j) = a(j,pu);
              a(j,pu) = temp;
            elseif ( pu < j )
              temp = a(k,j);
              a(k,j) = a(pu,j);
              a(pu,j) = temp;
            end

          end

          jt = ipvt(k);
          ipvt(k) = ipvt(pu);
          ipvt(pu) = jt;

        end

        pu = pu - 1;

      end

    end

  end

  for k = 1 : p
%
%  Reduction loop.
%
    maxdia = a(k,k);
    maxl = k;
%
%  Determine the pivot element.
%
    if ( pl <= k & k < pu )

      for l = k+1 : pu
        if ( maxdia < a(l,l) )
          maxdia = a(l,l);
          maxl = l;
        end
      end

    end
%
%  Quit if the pivot element is not positive.
%
    if ( maxdia <= 0.0 )
      info = k - 1;
      return
    end
%
%  Start the pivoting and update IPVT.
%
    if ( k ~= maxl )

      temp(1:k-1) = a(1:k-1,k);
      a(1:k-1,k) = a(1:k-1,maxl);
      a(1:k-1,maxl) = temp(1:k-1);

      a(maxl,maxl) = a(k,k);
      a(k,k) = maxdia;
      jp = ipvt(maxl);
      ipvt(maxl) = ipvt(k);
      ipvt(k) = jp;

    end
%
%  Reduction step.
%  Pivoting is contained across the rows.
%
    work(k) = sqrt ( a(k,k) );
    a(k,k) = work(k);

    for j = k+1 : p

      if ( k ~= maxl )

        if ( j < maxl )
          temp = a(k,j);
          a(k,j) = a(j,maxl);
          a(j,maxl) = temp;
        elseif ( maxl < j )
          temp = a(k,j);
          a(k,j) = a(maxl,j);
          a(maxl,j) = temp;
        end

      end

      a(k,j) = a(k,j) / work(k);
      work(j) = a(k,j);
      temp = -a(k,j);
      a(k+1:k+j-k,j) = a(k+1:k+j-k,j) + temp * work(k+1:k+j-k)';

    end

  end

  return
end


%% Header
% Written by Abraham Asfaw
% asfaw@princeton.edu
% Inspired by regular plot version here:
% http://www.mathworks.com/matlabcentral/fileexchange/10656-data-space-to-figure-units-conversion

%% Function
% x_point = x value that you want to be normalized
% y_point = y value that corresponds to the x_point
% axes = axes of the current plot, usually found using get(gca, 'Position')
% xlims_ = x limits of the current plot, usually found using get(gca, 'xlim')
% ylims_ = y limits of the current plot, usually found using get(gca, 'ylim')
% xlog_bool = 1 (any nonzero, really) if the plot is semilogx or loglog, zero otherwise
% ylog_bool = 1 (any nonzero, really) if the plot is semilogy or loglog, zero otherwise

function [nx, ny] = normalize_coordinate(x_point_, y_point_, axes, xlims_, ylims_, xlog_bool, ylog_bool)
    if xlog_bool && ylog_bool, % loglog plots
        x_point = log(x_point_);
        y_point = log(y_point_);
        xlims = log(xlims_);
        ylims = log(ylims_);
    elseif xlog_bool, % semilogx plots
        x_point = log(x_point_);
        y_point = y_point_;
        xlims = log(xlims_);
        ylims = ylims_;
    elseif ylog_bool, % semilogy plots
        x_point = x_point_;
        y_point = log(y_point_);
        xlims = xlims_;
        ylims = log(ylims_);
    else % plot plots
        x_point = x_point_;
        y_point = y_point_;
        xlims = xlims_;
        ylims = ylims_;
    end

    nx = ((x_point-xlims(1))/(xlims(2) - xlims(1)))*axes(3);
    ny = ((y_point-ylims(1))/(ylims(2) - ylims(1)))*axes(4);
    nx = axes(1) + nx;
    ny = axes(2) + ny;
end


function[varargout]=windtrans(varargin)
%WINDTRANS  Ekman-like transfer-functions for the wind-driven response.
%
%   G=WINDTRANS(OMEGA,Z,FC,DELTA,MU,H) returns the no-slip transfer
%   function for the wind-driven currents evaluated at frequencies OMEGA
%   and at depths Z.  G will have LENGTH(OMEGA) rows and LENGTH(Z) columns.
%
%   Here FC is the local Coriolis frequency, DELTA is the Ekman depth.
%   MU is the Madsen depth, and H is the boundary layer depth.
%
%   The units of these quantitites are important.  For consistency with
%   other routines, OMEGA and FC are in radians per day, while Z, DELTA,
%   MU, and H are all in meters.  The units of G are then m^2 s / kg.
%
%   WINDTRANS(...,'free') instead returns the free slip transfer function.
%
%   For details on the expressions implemented by this function, see
%
%        Lilly, J. M. and S. Elipot (2021). A unifying perspective on
%            transfer function solutions to the unsteady Ekman problem.
%            Fluids, 6 (2): 85, 1--36.
%   __________________________________________________________________
%
%   Special forms
%
%   By default WINDTRANS uses the general transfer function.  WINDTRANS
%   will also employ limiting expressions for special cases, as follows.
%
%       H = Inf                --  Mixed Ekman / Madsen solution
%       H = Inf, MU = 0        --  Ekman solution
%       H = Inf, DELTA = 0     --  Madsen solution
%       MU = 0                 --  Finite-layer Ekman solution
%       DELTA = 0              --  Finite-layer Madsen solution
%
%   These have to be coded separately because the full solution is singular
%   in these cases.
%   __________________________________________________________________
%
%   Computational options
%
%   When computed over a wide range of parameter space, the transfer
%   function tends to be encounter numerical overflow when the arguments to
%   Bessel functions become large, causing its computation to fail.
%
%   To avoid this problems, by default WINDTRANS switches to using a highly
%   accurate thirty-term expansion about the large-argument exponential
%   behavior of the Bessel functions when their arguments exceed 10^2.9.
%
%   Two other options are available, primarily for testing purposes. Both
%   of these other algorithms lead to artifacts and are not recommended.
%
%   WINDTRANS(...'far',...) switches instead to use the (inferior) one-term
%   expansion, also known as the far-inertial limit.
%
%   WINDTRANS(...,'general',...) uses the general formula with no switch.
%
%   Note that these options only apply to no-slip solution.  The free-slip
%   solution, which is not deemed to be physically relevant, is only
%   computed with the general formula.
%
%   For details on these algorithms, see Lilly and Elipot (2021).
%   __________________________________________________________________
%
%   'windtrans --t' runs a some tests.
%
%   Usage: G=windtrans(omega,z,fc,delta,mu,h);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2019--2021 J.M. Lilly --- type 'help jlab_license' for details

%   WINDTRANS(OMEGA,Z,FC,DELTA,MU,A,'AMP') alternately parameterizes the
%   transfer function in terms of the magntide of the near-inertial peak A
%   in 1/m, together with the Ekman and Madsen depths DELTA and MU.

%   Gradient output
%
%   [G,DDELTA,DH]=WINDTRANS(OMEGA,Z,FC,DELTA,0,H) for the Ekman-like model
%   also returns the partial derivatives with respect to DELTA and H.

if strcmp(varargin{1}, '--t')
    windtrans_test,return
end

omega=varargin{1}(:)/24/3600;%convert to rad/second
z=varargin{2};
fc=varargin{3}/24/3600;%convert to rad/second
delta=varargin{4};
mu=varargin{5};
h=varargin{6};

tol=100;
rho=1027;
str='expansion';%use the 30-term tilde expansion by default
%str='two';%use the two-term tilde expansion by default
slipstr='noslip';

bool=false(2,1);
for i=7:nargin
    istr=varargin{i}(1:3);
    if strcmpi(istr,'gen')||strcmpi(istr,'eli')||strcmpi(istr,'far')||strcmpi(istr,'ada')||strcmpi(istr,'exp')||strcmpi(istr,'two')
        str=varargin{i};
        if strcmpi(str(1:3),'exp')
            bool(1)=true;
        end
    elseif strcmpi(istr,'fre')||strcmpi(istr,'nos')
        slipstr=varargin{i};
        if strcmpi(slipstr(1:3),'fre')
            bool(2)=true;
        end
    end
end

%this is just to keep track of an unsuitable combination of input arguments
if all(bool)
     disp('Sorry, the tilde expansion algorithm is not implemented for the free-slip transfer function.')
end
%these may have been either input, or reflecting the default behavior
if strcmpi(str(1:3),'exp')&&strcmpi(slipstr(1:3),'fre')
     str='gen';
end

omega1=omega;
if length(z)~=1
    [z,omega]=meshgrid(z,omega);
end

%if isinf(h)||delta==0||mu==0
%    str='lilly';%use the lilly version of expressions for special cases
%end

%str,slipstr
G=zeros(size(omega));
if strcmpi(slipstr(1:3),'nos')
    %----------------------------------------------------------------------
    %The noslip forms
        %[G,xiz,xih,xi0]=windtrans_general_noslip(delta,fc,rho,z,mu,omega,h,str);
   %     G=windtrans_general_noslip(delta,fc,rho,z,mu,omega,h,str);
    if strcmpi(str(1:3),'gen')|| strcmpi(str(1:3),'far')||strcmpi(str(1:3),'exp')||strcmpi(str(1:3),'two')
        [G,varargout{2},varargout{3}]=windtrans_lilly_noslip(delta,fc,rho,z,mu,omega,h,str);
    elseif strcmpi(str(1:3),'eli')%asymptotic forms, elipot edition
        G=windtrans_elipot_noslip(delta,fc,rho,z,mu,omega,h);
    end
elseif strcmpi(slipstr(1:3),'fre')
    %----------------------------------------------------------------------
    %The freeslip forms

    K0=frac(1,2)*delta.^2.*abs(fc);
    K1=frac(1,2)*mu*abs(fc);

    s=sign(fc).*sign(1+omega./fc);
    zo=delta.^2./mu;
    [xiz,xih,xi0]=xis(s,zo,delta,z,omega,fc,h);

    coeff=frac(sqrt(2).*rot(-s.*pi/4),delta.*rho.*abs(fc).*sqrt(abs(1+omega./fc)));
    [k0z,i0z,k1h,i1h,k10,i10]=bessels_freeslip(xiz,xih,xi0);

    if strcmpi(str(1:3),'gen')||(strcmpi(str(1:3),'lil')&&((K0~=0)&&(K1~=0)))
        numer=i0z.*k1h+i1h.*k0z;
        denom=i1h.*k10-i10.*k1h;
        G=coeff.*frac(numer,denom);
    elseif strcmpi(str(1:3),'lil')
        %    [h,K0,K1]
        if mu==0
            coeff=frac(sqrt(2).*rot(-s*pi/4),delta.*abs(fc).*rho.*sqrt(abs((1+omega./fc))));
            cosharg=sqrt(2).*rot(s*pi/4).*frac(h-z,delta).*sqrt(abs((1+omega./fc)));
            sinharg=sqrt(2).*rot(s*pi/4).*frac(h,delta).*sqrt(abs((1+omega./fc)));
            G=coeff.*frac(cosh(cosharg),sinh(sinharg));
        elseif delta==0
            coeff=frac(2,rho*K1);
            k0z=besselk(0,2*rot(s*pi/4).*sqrt(frac(z,K1./abs(fc)).*abs(1+omega./fc)));
            k1h=besselk(1,2*rot(s*pi/4).*sqrt(frac(h,K1./abs(fc)).*abs(1+omega./fc)));
            i0z=besseli(0,2*rot(s*pi/4).*sqrt(frac(z,K1./abs(fc)).*abs(1+omega./fc)));
            i1h=besseli(1,2*rot(s*pi/4).*sqrt(frac(h,K1./abs(fc)).*abs(1+omega./fc)));
            G=coeff.*(k0z+frac(k1h.*i0z,i1h));
        end
    elseif strcmpi(str(1:3),'eli')

        delta1=sqrt(2.*K0./(omega+fc));
        delta2=K1./(omega+fc);
        xize=2*sqrt(1i*(zo+z)./delta2);
        xi0e=2*sqrt(1i*zo./delta2);
        xihe=2*sqrt(1i*(zo+h)./delta2);

        if mu==0
            coeff=frac(1,rho.*sqrt(1i*(omega+fc).*K0));
            numer=cosh((1+1i).*(h-z)./delta1);
            denom=sinh((1+1i).*h./delta1);
            G=coeff.*numer./denom;
        elseif delta==0
            coeff=(2./rho./K1);
            k0z=besselk(0,2.*sqrt(1i.*z./delta2));
            k1h=besselk(1,2*sqrt(1i.*h./delta2));
            i0z=besseli(0,2*sqrt(1i.*z./delta2));
            i1h=besseli(1,2*sqrt(1i.*h./delta2));
            G=coeff.*(k0z+frac(k1h.*i0z,i1h));
        else
            coeff=frac(1,rho.*sqrt(1i.*(omega+fc).*K0));
            k0z=besselk(0,xize);
            k10=besselk(1,xi0e);
            i10=besseli(1,xi0e);
            i0z=besseli(0,xize);
            k1h=besselk(1,xihe);
            i1h=besseli(1,xihe);
            numer=i0z.*k1h+i1h.*k0z;
            denom=i1h.*k10-i10.*k1h;
            G=coeff.*frac(numer,denom);
        end
    end
end
G(z>h)=nan;

varargout{1}=G;
%varargout{2}=xih;
%varargout{3}=xiz;
%varargout{4}=xi0;

function[xiz,xih,xi0]=xis(s,zo,delta,z,omega,fc,h)

xiz=2*sqrt(2).*rot(s.*pi/4).*(zo./delta).*sqrt((1+z./zo).*abs((1+omega./fc)));
xih=2*sqrt(2).*rot(s.*pi/4).*(zo./delta).*sqrt((1+h./zo).*abs((1+omega./fc)));
xi0=2*sqrt(2).*rot(s.*pi/4).*(zo./delta).*sqrt(abs((1+omega./fc)));

function[k0z,i0z,k0h,i0h,k10,i10]=bessels_noslip(varargin)

argz=varargin{1};
argh=varargin{2};
if length(varargin)==3
    arg0=varargin{3};
end

k0z=besselk(0,argz);
i0z=besseli(0,argz);
k0h=besselk(0,argh);
i0h=besseli(0,argh);

if nargout>4
    k10=besselk(1,arg0);
    i10=besseli(1,arg0);
end

function[k0z,i0z,k0h,i0h,k10,i10]=besseltildes_noslip(argz,argh,arg0,nterms)

k0z=besselktilde(0,argz,nterms);
i0z=besselitilde(0,argz,nterms);
k0h=besselktilde(0,argh,nterms);
i0h=besselitilde(0,argh,nterms);

k10=besselktilde(1,arg0,nterms);
i10=besselitilde(1,arg0,nterms);

function[k0z,i0z,k1h,i1h,k10,i10]=bessels_freeslip(argz,argh,arg0)

k0z=besselk(0,argz);
i0z=besseli(0,argz);
k1h=besselk(1,argh);
i1h=besseli(1,argh);

if nargout>4
    k10=besselk(1,arg0);
    i10=besseli(1,arg0);
end

function[G]=windtrans_expansion_noslip(delta,fc,rho,z,mu,omega,h,str)

zo=delta.^2./mu;
s=sign(fc).*sign(1+omega./fc);
[xiz,xih,xi0]=xis(s,zo,delta,z,omega,fc,h);
coeff=frac(sqrt(2).*rot(-s.*pi/4),delta.*rho.*abs(fc).*sqrt(abs(1+omega./fc)));

if strcmpi(str(1:3),'two')
    [k0z,i0z,k0h,i0h,k10,i10]=besseltildes_noslip(xiz,xih,xi0,2);
elseif strcmpi(str(1:3),'exp')
    [k0z,i0z,k0h,i0h,k10,i10]=besseltildes_noslip(xiz,xih,xi0,30);
end

numer=exp(xi0-xiz).*i0h.*k0z-exp(xi0+xiz-2*xih).*k0h.*i0z;
denom=i0h.*k10+exp(2*xi0-2*xih).*k0h.*i10;
G=coeff.*frac(numer,denom);

bool=(omega==-fc);
if ~isinf(h)&&~isinf(zo)
    if length(z)==1
        G(bool)=frac(4*zo,rho*abs(fc)*delta.^2).*frac(sqrt(1+h./zo)-sqrt(1+z./zo),(1+z./zo).^(1/4));
    else
        G(bool)=frac(4*zo,rho*abs(fc)*delta.^2).*frac(sqrt(1+h./zo)-sqrt(1+z(bool)./zo),(1+z(bool)./zo).^(1/4));
    end
elseif isinf(h)
    G(bool)=inf;
end

function[G,xiz,xih,xi0]=windtrans_general_noslip(delta,fc,rho,z,mu,omega,h,str)

zo=delta.^2./mu;
s=sign(fc).*sign(1+omega./fc);
[xiz,xih,xi0]=xis(s,zo,delta,z,omega,fc,h);
coeff=frac(sqrt(2).*rot(-s.*pi/4),delta.*rho.*abs(fc).*sqrt(abs(1+omega./fc)));

[k0z,i0z,k0h,i0h,k10,i10]=bessels_noslip(xiz,xih,xi0);

%numerically, better to do it like this to avoid having huge numbers dominate
%tests shows this removes the large-argument overflow when only h (not z) is large

numer=k0z./k10-(k0h./k10).*(i0z./i0h);
denom=1+(k0h./k10).*(i10./i0h);
G=coeff.*frac(numer,denom);

if strcmpi(str(1:3),'exp')||strcmpi(str(1:3),'two')
    bool=log10(abs(xiz))>2.9;
    if ~isempty(find(bool,1))
        if length(z)==1
            G(bool)=windtrans_expansion_noslip(delta,fc,rho,z,mu,omega(bool),h,str);
        else
            G(bool)=windtrans_expansion_noslip(delta,fc,rho,z(bool),mu,omega(bool),h,str);
        end
    end
elseif strcmpi(str(1:3),'far')
    bool=log10(abs(xiz))>2.9;
    if ~isempty(find(bool,1))
        if length(z)==1
            G(bool)=windtrans_farinertial_noslip(delta,fc,rho,z,mu,omega(bool),h);
        else
            G(bool)=windtrans_farinertial_noslip(delta,fc,rho,z(bool),mu,omega(bool),h);
        end
    end
end
G=windtrans_inertiallimit(G,delta,fc,rho,z,mu,omega,h);

function[G]=windtrans_farinertial_noslip(delta,fc,rho,z,mu,omega,h)

zo=delta.^2./mu;
s=sign(fc).*sign(1+omega./fc);
[xiz,xih,xi0]=xis(s,zo,delta,z,omega,fc,h);
coeff=frac(sqrt(2).*rot(-s.*pi/4),delta.*abs(fc).*rho);

numer=exp(-xiz+xi0)-exp(xiz+xi0-2*xih);
denom=1+exp(2*xi0-2*xih);
G=coeff.*frac(1,sqrt(abs(1+omega./fc)).*(1+z./zo).^(1/4)).*frac(numer,denom);
%Don't apply the inertial limit!

function[G]=windtrans_inertiallimit(G,delta,fc,rho,z,mu,omega,h)

zo=delta.^2./mu;
bool=(omega==-fc);

if ~isempty(find(bool,1))
    if ~isinf(h)&&~isinf(zo)
        if length(z)==1
            %G(bool)=frac(2*zo,rho*abs(fc)*delta.^2).*log(frac(1+h./zo,1+z./zo));
            G(bool)=frac(2,rho*abs(fc)*mu).*log(frac(1+h./zo,1+z./zo));
        else
            %G(bool)=frac(2*zo,rho*abs(fc)*delta.^2).*log(frac(1+h./zo,1+z./zo));
            G(bool)=frac(2,rho*abs(fc)*mu).*log(frac(1+h./zo,1+z(bool)./zo));
        end
    elseif ~isinf(h)&&isinf(zo)
        if length(z)==1
            G(bool)=frac(2,rho*abs(fc)*delta.^2).*(h-z);
        else
            G(bool)=frac(2,rho*abs(fc)*delta.^2).*(h-z(bool));
        end
    else
        G(bool)=inf;
    end
end

function[G,ddelta,dh]=windtrans_lilly_noslip(delta,fc,rho,z,mu,omega,h,str)

zo=delta.^2./mu;
s=sign(fc).*sign(1+omega./fc);
[xiz,xih,xi0]=xis(s,zo,delta,z,omega,fc,h);
%[delta,zo,K0,K1]

if h==inf
    if mu==0
        %Ekman solution
        coeff=frac(sqrt(2).*rot(-s.*pi/4),delta.*abs(fc).*rho);
        G=coeff.*frac(exp(-(1+s.*1i).*(z./delta).*sqrt(abs((1+omega./fc)))),sqrt(abs((1+omega./fc))));
    elseif delta==0
        %Madsen solution
        coeff=frac(4,rho*abs(fc)*mu);
        G=coeff.*besselk(0,2*sqrt(2)*rot(s.*pi/4).*sqrt(frac(z,mu).*abs(1+omega./fc)));
    else
        %Mixed solution
        k0z=besselk(0,xiz);
        k10=besselk(1,xi0);
        coeff=frac(sqrt(2).*rot(-s.*pi/4),delta.*abs(fc).*rho.*sqrt(abs((1+omega./fc))));
        G=coeff.*frac(k0z,k10);
    end
else
    if mu==0
        %finite-layer Ekman
        coeff=frac(sqrt(2).*rot(-s*pi/4),delta.*abs(fc).*rho.*sqrt(abs((1+omega./fc))));
        %sinharg=sqrt(2).*rot(s*pi/4).*frac(h-z,delta).*sqrt(abs((1+omega./fc)));
        %cosharg=sqrt(2).*rot(s*pi/4).*frac(h,delta).*sqrt(abs((1+omega./fc)));
        %G=coeff.*frac(sinh(sinharg),cosh(cosharg));
        %can't do it this way because you divide two huge numbers

        argh=sqrt(2).*rot(s*pi/4).*frac(h,delta).*sqrt(abs((1+omega./fc)));
        argz=sqrt(2).*rot(s*pi/4).*frac(z,delta).*sqrt(abs((1+omega./fc)));

        numer=exp(-argz)-exp(argz).*exp(-2*argh);
        denom=1+exp(-2*argh);
        G=coeff.*frac(numer,denom);

        bool=(omega==-fc);
        if length(z)==1
            G(bool)=frac(2,rho*abs(fc)*delta.^2).*(h-z);
        else
            G(bool)=frac(2,rho*abs(fc)*delta.^2).*(h-z(bool));
        end
    elseif delta==0
        %         if z==0
        %             coeff=frac(4,rho*abs(fc)*mu);
        %             argz=2*sqrt(2)*rot(s*pi/4).*sqrt(frac(z,mu).*abs(1+omega./fc));
        %             argh=2*sqrt(2)*rot(s*pi/4).*sqrt(frac(h,mu).*abs(1+omega./fc));
        %
        %             [k0z,i0z,k0h,i0h]=bessels_noslip(argz,argh);
        %             length(find(isnan(k0z)))
        %             length(find(isnan(i0z)))
        %             length(find(isnan(k0h)))
        %             length(find(isnan(i0h)))
        %             length(find(isnan(argh)))
        %
        %             %figure,plot(abs(argh))
        %
        %             G=-coeff.*(log(xi0)+frac(1,2)*(z./zo)+frac(k0h,i0h));
        %             G=-coeff.*(log(xi0./xih)+frac(1,2)*(z./zo));
        %             length(find(isnan(xi0./xih)))
        %
        %         else
        %finite-layer Madsen
        coeff=frac(4,rho*abs(fc)*mu);
        argz=2*sqrt(2)*rot(s*pi/4).*sqrt(frac(z,mu).*abs(1+omega./fc));
        argh=2*sqrt(2)*rot(s*pi/4).*sqrt(frac(h,mu).*abs(1+omega./fc));
        [k0z,i0z,k0h,i0h]=bessels_noslip(argz,argh);
        G=coeff.*(k0z-i0z.*frac(k0h,i0h));

        bool=(omega==-fc);
        if length(z)==1
            G(bool)=frac(1,2)*coeff.*log(h./z);
        else
            G(bool)=frac(1,2)*coeff.*log(h./z(bool));
        end
        %end
    else
        %General solution
        G=windtrans_general_noslip(delta,fc,rho,z,mu,omega,h,str);
    end
end

if mu==0
    s=sign(fc).*sign(1+omega./fc);
    Gamma=sqrt(2).*rot(s*pi/4).*sqrt(abs((1+omega./fc)));
    ddelta1=(Gamma.*frac(h,delta).*tanh(Gamma.*frac(h,delta))-1).*G./delta;
    numer=exp(Gamma.*frac(-z,delta))+exp(-Gamma.*frac(2*h-z,delta));
    denom=1+exp(-Gamma.*frac(2*h,delta));
    ddelta2=-frac(2,delta.^2.*abs(fc).*rho).*frac(h-z,delta).*frac(numer,denom);
    ddelta=ddelta1+ddelta2;
    dh1=-Gamma.*frac(1,delta).*tanh(Gamma.*frac(h,delta)).*G;
    dh2=frac(2,delta.^2.*abs(fc).*rho).*frac(numer,denom);
    dh=dh1+dh2;
else
    ddelta=[];
    dh=[];
end

function[G]=windtrans_elipot_noslip(delta,fc,rho,z,mu,omega,h)

zo=delta.^2./mu;
K0=frac(1,2)*delta.^2.*abs(fc);
K1=frac(1,2)*mu*abs(fc);

delta1=sqrt(2.*K0./(omega+fc));
delta2=K1./(omega+fc);
xiz=2*sqrt(1i*(zo+z)./delta2);
xi0=2*sqrt(1i*zo./delta2);
xih=2*sqrt(1i*(zo+h)./delta2);

if h==inf
    if K1==0
       %Ekman solution
        coeff=frac(1,rho.*sqrt(1i*(omega+fc).*K0));
        G=coeff.*exp(-z.*(1+1i)./delta1);
    elseif K0==0
        %Madsen solution
        coeff=(2./rho./K1);
        k0z=besselk(0,2.*sqrt(1i.*z./delta2));
        G=coeff.*k0z;
    else
        %Mixed solution
        coeff=frac(1,rho.*sqrt(1i.*(omega+fc).*K0));
        G=coeff.*besselk(0,xiz)./besselk(1,xi0);
    end
else
    if K1==0
        %finite-layer Ekman
        coeff=frac(1,rho.*sqrt(1i*(omega+fc).*K0));
        numer=sinh((1+1i).*(h-z)./delta1);
        denom=cosh((1+1i).*h./delta1);
        G=coeff.*numer./denom;
    elseif K0==0
        %finite-layer Madsen
        coeff=(2./rho./K1);
        k0z=besselk(0,2.*sqrt(1i.*z./delta2));
        k0h=besselk(0,2*sqrt(1i.*h./delta2));
        i0z=besseli(0,2*sqrt(1i.*z./delta2));
        i0h=besseli(0,2*sqrt(1i.*h./delta2));
        G=coeff.*(k0z-frac(k0h.*i0z,i0h));
    else
        %General solution
        coeff=frac(1,rho.*sqrt(1i.*(omega+fc).*K0));
        [k0z,i0z,k0h,i0h,k10,i10]=bessels_noslip(xiz,xih,xi0);
        numer=i0h.*k0z-k0h.*i0z;
        denom=i10.*k0h+k10.*i0h;
        G=coeff.*frac(numer,denom);
    end
end

function[]=windtrans_test
windtrans_test_gradient;
windtrans_test_limits;

function[]=windtrans_test_gradient

delta=10.^(-1:0.05:3);
h=10.^(log10(15.15):.05:5);
[dg,hg]=meshgrid(delta,h);

[G,G1,G2,G3,G4,dg1,dg2]=vzeros(size(dg));
ddelta=1e-6;dh=1e-6;
omega=-1e-4;
%omega=-1.5e-4;
%omega=-5e-4;
for j=1:length(delta)
    for i=1:length(h)
        %G=windtrans(omega,z,fc,delta,mu,h);
       [G(i,j),dg1(i,j),dg2(i,j)]=windtrans(omega,15,1e-4,dg(i,j),0,hg(i,j));
        G1(i,j)=windtrans(omega,15,1e-4,dg(i,j)-ddelta/2,0,hg(i,j));
        G2(i,j)=windtrans(omega,15,1e-4,dg(i,j)+ddelta/2,0,hg(i,j));
        G3(i,j)=windtrans(omega,15,1e-4,dg(i,j),0,hg(i,j)-dh/2);
        G4(i,j)=windtrans(omega,15,1e-4,dg(i,j),0,hg(i,j)+dh/2);
    end
end

dg1hat=frac(G2-G1,ddelta);
dg2hat=frac(G4-G3,dh);

bool=dg1hat~=0;
eps1=maxmax(log10(frac(abs(dg1hat(bool)-dg1(bool)),sqrt(squared(dg1hat(bool))+squared(dg1(bool))))));
bool=dg2hat~=0;
eps2=maxmax(log10(frac(abs(dg2hat(bool)-dg2(bool)),sqrt(squared(dg2hat(bool))+squared(dg2(bool))))));

%jpcolor(log10(delta),log10(h),real(dg1))
%figure,jpcolor(log10(delta),log10(h),log10(frac(abs(dg1hat-dg1),sqrt(squared(dg1hat)+squared(dg1)))))
%jpcolor(log10(delta),log10(h),log10(frac(abs(dg2hat-dg2),sqrt(squared(dg2hat)+squared(dg2)))))

reporttest(['WINDTRANS analytic and numerical gradients match for Ekman case'],eps1<-4&&eps2<-4)

function[]=windtrans_test_limits

N=1000;
z=[0.1:1:100];
h=[inf 200];
K0=[0 1/10 1/10];
K1=[1 0 1];
fc=1e-4;
delta=sqrt(frac(2*K0,fc));
mu=frac(2*K1,fc);

[bool1,bool2,bool3,bool4]=vzeros(length(K0),2);
[Ge,Go,Gl,Ga]=vzeros(N,length(z),length(K0),2);
slipstr='noslip';
omega=fourier(1,N,'two');

for s=[1 -1]
    for i=1:length(K0)
        for j=1:2
            Ge(:,:,i,j)=windtrans(omega,z,s/2,delta(i),mu(i),h(j),'elipot',slipstr);
            Gl(:,:,i,j)=windtrans(omega,z,s/2,delta(i),mu(i),h(j),slipstr);
            Go(:,:,i,j)=windtrans(omega,z,s/2,delta(i)+1e-14,mu(i)+0.5,h(j),slipstr,'general');
            Ga(:,:,i,j)=windtrans(omega,z,s/2,delta(i)+1e-14,mu(i)+0.5,h(j),slipstr,'two');
            Ge(:,:,i,j)=Ge(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));
            Go(:,:,i,j)=Go(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));
            Ga(:,:,i,j)=Ga(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));
            Gl(:,:,i,j)=Gl(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));

            bool1(i,j)=aresame(Ge(:,:,i,j),Gl(:,:,i,j),1e-8);
            bool2(i,j)=aresame(Ge(:,:,i,j),Go(:,:,i,j),1e-1);
            bool3(i,j)=aresame(Gl(:,:,i,j),Go(:,:,i,j),1e-1);
            bool4(i,j)=aresame(Ga(:,:,i,j),Go(:,:,i,j),1e-20);
        end
    end

    if s==1
        reporttest(['WINDTRANS Elipot and Lilly forms match for f>0 and ' slipstr ' condition'],allall(bool1))
        reporttest(['WINDTRANS general and special forms match for f>0 and ' slipstr ' condition'],allall(bool2).*allall(bool3))
        reporttest(['WINDTRANS general and expansion forms match for f>0 and ' slipstr ' condition'],allall(bool4).*allall(bool3))
    else
        reporttest(['WINDTRANS Elipot and Lilly forms match for f<0 and ' slipstr ' condition'],allall(bool1))
        reporttest(['WINDTRANS general and special forms match for f<0 and ' slipstr ' condition'],allall(bool2).*allall(bool3))
        reporttest(['WINDTRANS general and expansion forms match for f>0 and ' slipstr ' condition'],allall(bool4).*allall(bool3))
    end
end

% older test for freeslip forms; not currently  particularly relevant
% h=20;
% z=[0.1:1:20];
% [bool1,bool2,bool3]=vzeros(length(K0),1);
% [Ge,Go,Gl]=vzeros(N,length(z),length(K0));
% slipstr='freeslip';
% for s=[1 -1]
%     for i=1:length(K0)
%         for j=1:length(h)
%             i,j,s
%             Ge(:,:,i,j)=windtrans(omega,z,s/2,delta(i),mu(i),h(j),'elipot',slipstr);
%             Gl(:,:,i,j)=windtrans(omega,z,s/2,delta(i),mu(i),h(j),'lilly',slipstr);
%             Go(:,:,i,j)=windtrans(omega,z,s/2,delta(i)+1e-10,mu(i)+0.5,h(j),slipstr,'general');
%             Ge(:,:,i,j)=Ge(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));
%             Go(:,:,i,j)=Go(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));
%             Gl(:,:,i,j)=Gl(:,:,i,j)./maxmax(abs(Gl(:,:,i,j)));
%             bool1(i,j)=aresame(Ge(:,:,i,j),Gl(:,:,i,j),1e-8);
%             bool2(i,j)=aresame(Ge(:,:,i,j),Go(:,:,i,j),1e-1);
%             bool3(i,j)=aresame(Gl(:,:,i,j),Go(:,:,i,j),1e-1);
%         end
%     end
%
%     if s==1
%         reporttest(['WINDTRANS Elipot and Lilly forms match for f>0 and ' slipstr ' condition'],allall(bool1))
%         reporttest(['WINDTRANS general and special forms match for f>0 and ' slipstr ' condition'],allall(bool2).*allall(bool3))
%     else
%         reporttest(['WINDTRANS Elipot and Lilly forms match for f<0 and ' slipstr ' condition'],allall(bool1))
%         reporttest(['WINDTRANS general and special forms match for f<0 and ' slipstr ' condition'],allall(bool2).*allall(bool3))
%     end
% end


function lA = longAxis(grains,varargin)
% long axis of a grain
%
% the long axis is the direction of the largest
% <grain2d.principalComponents.html,principal component> of a grain
%
% Syntax
%   lA = grains.longAxis
%
% Input
%  grains - @grain2d
%
% Output
%  lA - @vector3d direction of the longest elongation
%
% See also
% grain2d/principalComponents

omega = principalComponents(grains);

lA = vector3d.byPolar(pi/2,omega,'antipodal');


 function [deriv, curv] = trl_dercurv(data, li, curvtype, iblock, nblock)
%function [deriv, curv] = trl_dercurv(data, li, curvtype, iblock, nblock)
%
% evaluate derivatives and curvatures for monoenergetic Poisson transmission
% negative log-likelihoods: f(x) = \sum_i h_i(l), h_i(l) = mi(l) - yi log mi(l)
% mi(l) = bi exp(-l) + ri
%
% in
%	data	{yi, bi, ri}
%				yi is [nb,na].  bi, ri are same or scalar
%				passed to trl_curvature()
%	li	[nb,#view_in_block]
%	curvtype ''		curvature type
%	iblock, nblock		for OS type methods
%
% out
%	deriv	[nb,#]		\dot hi(l)
%	curv	[nb,#]		surrogate curvature for hi at li
%
% Copyright 2004-2-1, Jeff Fessler, The University of Michigan

if nargin < 2, ir_usage, end
if nargin < 3, curvtype = 'pc'; end

yi = data{1};
bi = data{2};
ri = data{3};

if nargin == 5
	ia = iblock:nblock:size(yi,2);
	yi = yi(:,ia);
	if length(bi) > 1
		bi = bi(:,ia);
	end
	if length(ri) > 1
		ri = ri(:,ia);
	end
end

% transmission Poisson likelihood function
ei = exp(-li);
mi = bi .* ei + ri;
deriv = (1 - yi ./ mi) .* (-bi .* ei);

curv = trl_curvature(yi, bi, ri, li, curvtype);


function [out] = area_1(p1,p2,S,Smin,Smax,varargin)
%area_1

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

% Flux function
% ------------------
% Description:  Auxiliary function that calculates a variable contributing area.
% Constraints:  A <= 1
% @(Inputs):    p1   - linear scaling parameter [-]
%               p2   - exponential scaling parameter [-]
%               S    - current storage [mm]
%               Smin - minimum contributing storage [mm]
%               Smax - maximum contributing storage [mm]
%               varargin(1) - smoothing variable r (default 0.01)
%               varargin(2) - smoothing variable e (default 5.00)

if size(varargin,2) == 0
    out = min(1,p1.*(max(0,S-Smin)./(Smax-Smin)).^p2).*...
            (1-smoothThreshold_storage_logistic(S,Smin));                       % default smoothing
elseif size(varargin,2) == 1
    out = min(1,p1.*(max(0,S-Smin)./(Smax-Smin)).^p2).*...
            (1-smoothThreshold_storage_logistic(S,Smin,varargin(1)));           % user-specified smoothing
elseif size(varargin,2) == 2
     out = min(1,p1.*(max(0,S-Smin)./(Smax-Smin)).^p2).*...
            (1-smoothThreshold_storage_logistic(S,Smin,varargin(1),varargin(2))); % user-specified smoothing
end

end


function [ Tt ] = updateTransform2(T,w,v,dt)
% This function takes the input of the
normw=norm(w);
if normw>0
    R1=T(1:3,1:3);
    wCrossR1=zeros(3,3);
    for i=1:3
        wCrossR1(:,i)=cross(w,R1(:,i));
    end
    Rt=R1+wCrossR1*dt;
else
    Rt=T(1:3,1:3);
end

dx=v*dt;
xt=T(1:3,4)+dx;

Tt=[Rt,xt];
Tt=[Tt;
    0 0 0 1];
Tt=normalizeTransformMatrix(Tt);
end

function R=quat2rot(q)
R=[(1-2*q(3)*q(3)-2*q(4)*q(4)), 2*(q(1)*q(3)-q(1)*q(4)), 2*(q(2)*q(4)+q(1)*q(3));
2*(q(2)*q(3)+q(1)*q(4)), (1-2*q(2)*q(2)-2*q(4)*q(4)), 2*(q(3)*q(4)-q(1)*q(2));
2*(q(2)*q(4)-q(1)*q(3)), 2*(q(3)*q(4)+q(1)*q(2)), (1-2*q(2)*q(2)-2*q(3)*q(3))];
end

function T=normalizeTransformMatrix(t)
T=zeros(4,4);
T(:,4)=t(:,4);
for i=1:3
    T(1:3,i)=t(1:3,i)/norm(t(1:3,i));
end
end


function [A, B] = reflexKronApprox( K, m, n, Rtol )
%
%       [A, B] = reflexKronApprox( K, m, n, Rtol );
%
% computes a kronecker sum approximation to the blurring matrix K
% that arises from the input PSF under reflexive boundary conditions.
% This approximation is done a-la Nagy-Ng-Perrone (see paper for details).
%
%  Input:
%         K - psfMatrix object
%         m - size of matrix A (assumed square)
%         n - size of matrix B (assumed square)
%         Rtol - relative tolerance ( <=1 ); determines number of kron products
%                in the approximation.  If si/s1 > Rtol,
%                where si is the ith largest
%                singular value of the weighted PSF,
%                then Ai(x)Bi will be included in the approximation.
%                IF ONLY ONE TERM IN THE SUM IS DESIRED, USE Rtol = 1.
%
%  Output:
%      Cells A and B such that K \approx \sum_i[ A{i} \otimes B{i} ].
%

%  L. Perrone, 4/28/02

%  Modifications:
%  5/25/02, J. Nagy
%           Cosmetic changes to incorporate into RestoreTools
%
%  9/??/02  L. Perrone
%           This code now conforms to the new kronMatrix class,
%           where fields K.a and K.b are cell arrays containing
%           (possibly) more than one matrix, depending on Rtol.
%  11/22/02 L. Perrone
%           The kronMatrix class should work for image processing
%           problems, where it is common to use lexicographical (row)
%           ordering.  However, the kronMatrix class was designed
%           using vec(column) ordering (unlike all other classes
%           in the RestoreTools package).  Until now, the inconsistency
%           remained hidden because the PSFs in use
%           had been symmetric or close to symmetric.
%           This discrepancy has now been fixed.

P1 = K.psf;
P2 = P1.image;
PSF = P2{1};
c1 = P1.center;
center = c1{1};

[mp, np] = size(PSF);

if ( mp ~= np )
  error('For now, we expect PSF to be square')
end

%
% Compute weighted PSF.
%
c = zeros(mp,1);
c(1) = mp;
c(2:2:end) = 1;
R = chol( toeplitz(c) );

Phat = R*PSF*R';

%
% Compute SVD of weighted PSF, which is then used to construct
% the separable approximation.
%
[U,S,V] = svd( Phat );

%
% check to make sure first column looks like
% a Gaussian, and is not inverted.
%
minU = abs(min(min(U(:,1))));
maxU = max(max(abs(U(:,1))));
if minU == maxU
  U = -U;
  V = -V;
end

%
% Construct approximation.
%
A = cell(1);
B = cell(1);
i=1;
PP = zeros(size(PSF));
while S(i,i)/S(1,1) >= Rtol
  a = R \ ( U(:,i) * sqrt(S(i,i)) );
  b = R \ ( V(:,i) * sqrt(S(i,i)) );
  PP = PP + a*b';
  % Comment out the next two statements, replace with the two that
  % follow, in order to fix the 11/22/02 problem.
  %A{i} = build_toep(a, center(1), m) + buildHank(a, center(1), m);
  %B{i} = build_toep(b, center(2), n) + buildHank(b, center(2), n);
  B{i} = build_toep(a, center(1), m) + buildHank(a, center(1), m);
  A{i} = build_toep(b, center(2), n) + buildHank(b, center(2), n);
  i=i+1;
end

%
%  The following scales the approximation so that the
%  corresponding PSF approximation has the property
%  that its sum of values is the same as the sum of the
%  original PSF values.
%
cs = sqrt(sum(PSF(:))/sum(PP(:)));
for i = 1:length(A)
  A{i} = cs*A{i};
  B{i} = cs*B{i};
  AA = A{i};
  BB = B{i};
end


function CPD = root_gaussian_CPD(bnet, self, mu, Sigma, mu0, n0, alpha0, beta0)
% ROOT_GAUSSIAN_CPD Make an unconditional Gaussian distrib.
%
% CPD = root_gaussian_CPD(bnet, self, mu, Sigma)
% This defines the distribution Y ~ N(mu, Sigma),
% Pass in [] to generate a default random value for a parameter.
%
% CPD = root_gaussian_CPD(bnet, self, [], [], mu0, n0, alpha0, beta0)
% defines a Normal-Wishart prior over the parameters:
%   P(mu | lambda) = N(mu | mu0, n0*lambda)
%   P(lambda) = Wishart(lambda | alpha0, beta0)
% where lambda = inv(Sigma) is the precision matrix of mu.
% n0 is a scale factor, beta0 is a precision matrix.
% Pass in [] to generate a default value for a hyperparameter.
% mu and Sigma will be set to their prior expected values.
% See "Bayesian Theory", Bernardo and Smith (2000), p441.

if nargin==0
  % This occurs if we are trying to load an object from a file.
  CPD = init_fields;
  CPD = class(CPD, 'root_gaussian_CPD', generic_CPD(0));
  return;
elseif isa(bnet, 'root_gaussian_CPD')
  % This might occur if we are copying an object.
  CPD = bnet;
  return;
end
CPD = init_fields;

ns = bnet.node_sizes;
d = ns(self);

if nargin < 5,
  prior = [];
  if isempty(mu), mu = randn(d, 1); end
  if isempty(Sigma), Sigma = eye(d); end
else
  if isempty(mu0), mu0 = zeros(d, 1); end
  if isempty(n0), n0 = 0.1; end
  if isempty(alpha0), alpha0 = (d-1)/2 + 1; end % Wishart requires 2 alpha > d-1
  if isempty(beta0), beta0 = eye(d); end

  prior.mu = mu0;
  prior.n = n0;
  prior.alpha = alpha0;
  prior.beta = beta0;

  % set params to their mean
  mu = prior.mu;
  Sigma = prior.beta/prior.alpha; % mean of Wishart is E[lambda] = alpha*inv(beta)
end

CPD.self = self;
CPD.mu = mu;
CPD.Sigma = Sigma;
CPD.prior = prior;

clamped = 0;
CPD = class(CPD, 'root_gaussian_CPD', generic_CPD(clamped));

%%%%%%%%%%%

function CPD = init_fields()
% This ensures we define the fields in the same order
% no matter whether we load an object from a file,
% or create it from scratch. (Matlab requires this.)

CPD.self = [];
CPD.mu = [];
CPD.Sigma = [];
CPD.prior = [];


function varargout = projPointOnPolygon(point, poly, varargin)
%PROJPOINTONPOLYGON  Compute position of a point projected on a polygon
%
%   POS = projPointOnPolygon(POINT, POLYGON)
%   Compute the position of the orthogonal projection of a point on a
%   polygon.
%   POINT is a 1-by-2 row vector containing point coordinates
%   POLYGON is a N-by-2 array containing coordinates of polygon vertices
%
%   When POINT is an array of points, returns a column vector with as many
%   rows as the number of points.
%
%   [POS, DIST] = projPointOnPolygon(...)
%   Also returns the distance between POINT and POLYGON. The distance is
%   negative if the point is located inside of the polygon.
%
%   Example
%     poly = [10 10; 20 10;20 20;10 20];
%     projPointOnPolygon([15 0], poly)
%     ans =
%         0.5000
%     projPointOnPolygon([0 16], poly)
%     ans =
%         3.4000
%
%   See also
%   points2d, polygons2d, polygonPoint, projPointOnPolyline
%   distancePointpolygon
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2009-04-30,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

% eventually copy first point at the end to ensure closed polygon
if sum(poly(end, :) == poly(1,:)) ~= 2
    poly = [poly; poly(1,:)];
end

% compute position wrt outline
[pos, minDist] = projPointOnPolyline(point, poly);

% process output arguments
if nargout <= 1
    varargout{1} = pos;
elseif nargout == 2
    varargout{1} = pos;
    if inpolygon(point(:,1), point(:,2), poly(:,1), poly(:,2))
        minDist = -minDist;
    end
    varargout{2} = minDist;
end


function subset_sum_test ( )

%*****************************************************************************80
%
%% SUBSET_SUM_TEST tests the SUBSET_SUM library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 May 2012
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SUBSET_SUM_TEST:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the SUBSET_SUM library.\n' );
%
%  Problem #1.
%
  w = [ 15, 22, 14, 26, 32, 9, 16, 8 ];
  t = 53;
  [ c, index ] = subset_sum_test01 ( w, t );
%
%  There's a second solution for problem #1.
%
  subset_sum_test01 ( w, t, [ index+1, 2^8-1] );
%
%  Problem #2.
%
  w = [   267,  493,  869,  961, 1000, 1153, 1246, 1598, 1766, 1922 ];
  t = 5842;
  [ c, index ] = subset_sum_test01 ( w, t );
%
%  Problem #3.
%
  w = [  518533, 1037066, 2074132, 1648264, 796528, ...
        1593056,  686112, 1372224,  244448, 488896, ...
         977792, 1955584, 1411168,  322336, 644672, ...
        1289344,   78688,  157376,  314752, 629504, ...
        1259008 ];
  t = 2463098;
  [ c, index ] = subset_sum_test01 ( w, t );
%
%  Problem #4.
%
  w = [ 41, 34, 21, 20,  8,  7,  7,  4,  3,  3 ];
  t = 50;
  [ c, index ] = subset_sum_test01 ( w, t );
%
%  Problem #5.
%
  w = [ 81, 80, 43, 40, 30, 26, 12, 11, 9 ];
  t = 100;
  [ c, index ] = subset_sum_test01 ( w, t );
%
%  Problem #6.
%
  w = [ 1, 2, 4, 8, 16, 32 ];
  t = 22;
  r = [ 0, 2^6 - 1 ];
  [ c, index ] = subset_sum_test01 ( w, t, r );
%
%  Problem #7.
%
  w = [ 25, 27, 3, 12, 6, 15, 9, 30, 21, 19 ];
  t = 50;
  r = [ 0, 2^10 - 1 ];
  [ c, index ] = subset_sum_test01 ( w, t, r );
%
%  Problem #1.
%
  w = [ 15, 22, 14, 26, 32, 9, 16, 8 ];
  t = 53;
  count = subset_sum_test02 ( w, t );
%
%  Problem #2.
%
  w = [   267,  493,  869,  961, 1000, 1153, 1246, 1598, 1766, 1922 ];
  t = 5842;
  count = subset_sum_test02 ( w, t );
%
%  Problem #3.
%
  w = [  518533, 1037066, 2074132, 1648264, 796528, ...
        1593056,  686112, 1372224,  244448, 488896, ...
         977792, 1955584, 1411168,  322336, 644672, ...
        1289344,   78688,  157376,  314752, 629504, ...
        1259008 ];
  t = 2463098;
  count = subset_sum_test02 ( w, t );
%
%  Problem #4.
%
  w = [ 41, 34, 21, 20,  8,  7,  7,  4,  3,  3 ];
  t = 50;
  count = subset_sum_test02 ( w, t );
%
%  Problem #5.
%
  w = [ 81, 80, 43, 40, 30, 26, 12, 11, 9 ];
  t = 100;
  count = subset_sum_test02 ( w, t );
%
%  Problem #6.
%
  w = [ 1, 2, 4, 8, 16, 32 ];
  t = 22;
  r = [ 0, 2^6 - 1 ];
  count = subset_sum_test02 ( w, t );
%
%  Problem #7.
%
  w = [ 25, 27, 3, 12, 6, 15, 9, 30, 21, 19 ];
  t = 50;
  r = [ 0, 2^10 - 1 ];
  count = subset_sum_test02 ( w, t );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SUBSET_SUM_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


close all;
clear all;
clc;
rng('default');

png_export = true;
pdf_export = false;

mf = spx.graphics.Figures();

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

mf.new_figure('Recovery probability with S for different MMV recovery algorithms');

load ('bin/figure_2_success_with_s_comparison.mat');

hold all;
legends = cell(1, 4);

plot(Ss, success_with_s.ra_ormp, '-+');
legends{1} = 'RA-ORMP';
plot(Ss, success_with_s.ra_omp, '-o');
legends{2} = 'RA-OMP';
plot(Ss, success_with_s.somp, '-s');
legends{3} = 'SOMP';
plot(Ss, success_with_s.ra_thresholding, '-d');
legends{4} = 'RA-Thresholding';

grid on;
xlabel('Number of signals');
ylabel('Recovery Probability');
legend(legends, 'Location', 'southeast');
title('Comparison of recovery performance for MMV algorithms');


% cmp = MS_complexitybs(x,n);
%
% calculate the Lempel-Ziv complexity of the n-symbol stream x.
%
% x\in{0,1,2,...,(n-1)}
%
% cmp is the normalised complexity, that is the number of distinct
% symbol sequences in x, divided by the expected number of distinct
% symbols for a noise sequence.
%
% Algorithm is implemented in MS_complexitybs.c
%
% Michael Small
% michael.small@uwa.edu.au, http://school.maths.uwa.edu.au/~small/
% 24/9/03
% For further details, please see M. Small. Applied Nonlinear Time Series
% Analysis: Applications in Physics, Physiology and Finance. Nonlinear Science
% Series A, vol. 52. World Scientific, 2005. (ISBN 981-256-117-X) and the
% references therein.


function [cameraCBparameters] = RecalculateCBcalibrationParameters(cameraCBparameters,optStruct)
%% function for re-calculating the distortion parameters in STEP0.
% This function is called only in case the user selected a repeated
% analysis.
%
% INPUTS:
% * cameraCBparameters: structure previously created in STEP0
% * optStruct: a structure containing the new parameters of the distortion
%   model selected in the repeated analysis.
%
% OUTPUTS:
% * a figure of all the checkerboard images

%%

%  extract the points already detected in previous analysis
imagePoints=cameraCBparameters.imagePoints;
worldPoints=cameraCBparameters.cameraParameters.WorldPoints;

% Re-calculate camera parameters with the new distortion model
[params,~,estimationErrors] = estimateCameraParameters(imagePoints,worldPoints,...
    'NumRadialDistortionCoefficients',optStruct.NumRadialDistortionCoefficients,'EstimateTangentialDistortion',optStruct.EstimateTangentialDistortion,'EstimateSkew',optStruct.EstimateSkew);

% feed new results into cameraCBparameters
cameraCBparameters.cameraParameters=params;
cameraCBparameters.estimationErrors=estimationErrors;

%parameters after undistortion
imagePointsUndistorted=zeros(size(cameraCBparameters.imagePoints));
for ii=1:size(cameraCBparameters.imagePoints,3)
    imagePointNow=cameraCBparameters.imagePoints(:,:,ii);
    [imagePointsUndistorted(:,:,ii)] = undistortPoints(imagePointNow,cameraCBparameters.cameraParameters);
end
[paramsJ,~,estimationErrorsJ] = estimateCameraParameters(imagePointsUndistorted,worldPoints,...
    'NumRadialDistortionCoefficients',optStruct.NumRadialDistortionCoefficients,'EstimateTangentialDistortion',optStruct.EstimateTangentialDistortion,'EstimateSkew',optStruct.EstimateSkew);

% feed results after undistortion into cameraCBparameters and return
cameraCBparameters.cameraParametersAUD=paramsJ;
cameraCBparameters.estimationErrorsAUD=estimationErrorsJ;
cameraCBparameters.imagePointsAUD=imagePointsUndistorted;

end

%%
% MultiDIC: a MATLAB Toolbox for Multi-View 3D Digital Image Correlation
%
% License: <https://github.com/MultiDIC/MultiDIC/blob/master/LICENSE.txt>
%
% Copyright (C) 2018  Dana Solav
%
% If you use the toolbox/function for your research, please cite our paper:
% <https://engrxiv.org/fv47e>


function point_num = sphere_llq_grid_point_count ( lat_num, long_num )

%*****************************************************************************80
%
%% SPHERE_LLQ_GRID_POINT_COUNT counts points for a SPHERE LLQ grid.
%
%  Discussion:
%
%    A SPHERE LLQ grid imposes a grid of quadrilaterals on a sphere,
%    using latitude and longitude lines.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer LAT_NUM, LONG_NUM, the number of latitude
%    and longitude lines to draw.  The latitudes do not include the North and
%    South poles, which will be included automatically, so LAT_NUM = 5, for
%    instance, will result in points along 7 lines of latitude.
%
%    Output, integer POINT_NUM, the number of grid points.
%
  point_num = 2 + lat_num * long_num;

  return
end


% interp_table_test.m

hr = @(t, J) (1-abs(t/(J/2))) .* (abs(t) <= J/2);
hi = @(t, J) (1-abs(t/(J/2))).^2 .* (abs(t) <= J/2);

% 1D
if 1, printm '1d'
	L = 10;
	J = 6;
	s = [-J/2*L:J/2*L]'/L;
	hs = hr(s, J) + 1i * hi(s, J);
	if length(hs) ~= J*L+1, error 'size', end

	if im
		clf, subplot(211)
		plot(s, real(hs), 'c.-', s, imag(hs), 'y.-')
	end

	K = 20;
	ck = zeros(K,1);
	ck(2+1) = 1;
	ck = complexify(ck);
	tm = linspace(-2*K, 2*K, 2001)';
	fm = interp1_table_mex(ck, hs, int32(J), int32(L), tm);

	if im
		subplot(212)
		plot(tm, real(fm), 'c.-', tm, imag(fm), 'y.-')
	end
prompt
end

% 2D
if 1, printm '2d'
	L = [2^5 2^4];
	J = [6 4];
	s1 = [-J(1)/2*L(1):J(1)/2*L(1)]'/L(1);
	s2 = [-J(2)/2*L(2):J(2)/2*L(2)]'/L(2);
	h1 = 0*hr(s1, J(1)) + 1i * hi(s1, J(1));
	h2 = 0*hr(s2, J(2)) + 1i * hi(s2, J(2));
%	h1 = complexify(h1);
%	h2 = complexify(h2);

	if im
		clf, subplot(211)
		plot(	s1, real(h1), 'c.-', s1, imag(h1), 'y.-', ...
			s2, real(h2), 'g.-', s2, imag(h2), 'm.-')
	end

	K = [8 12];
	ck = zeros(K);
	ck(0+1, 0+1) = -1i;
%	ck = complexify(ck);
	t1 = linspace(-2*K(1), 2*K(1), 201)';
	t2 = linspace(-2*K(2), 2*K(2), 199)';
	t1 = linspace(0, K(1), 201)';
	t2 = linspace(0, K(2), 199)';
	[tt1 tt2] = ndgrid(t1, t2);
	tm = [tt1(:) tt2(:)];
%	tic
	fm = interp2_table_mex(ck, h1, h2, int32(J), int32(L), tm);
%	toc
	fm = reshape(fm, size(tt1));

	im(121, t1, t2, real(fm), 'real'), cbar
	im(122, t1, t2, imag(fm), 'imag'), cbar
prompt
end

% 3D
if 1, printm '3d'
	L = [2^5 2^4 2^6];
	J = [5 3 4];
	s1 = [-J(1)/2*L(1):J(1)/2*L(1)]'/L(1);
	s2 = [-J(2)/2*L(2):J(2)/2*L(2)]'/L(2);
	s3 = [-J(3)/2*L(3):J(3)/2*L(3)]'/L(3);
	h1 = 1 * hr(s1, J(1)) + 0i * hi(s1, J(1));
	h2 = 1 * hr(s2, J(2)) + 0i * hi(s2, J(2));
	h3 = 1 * hr(s3, J(3)) + 0i * hi(s3, J(3));
	h1 = complexify(h1);
	h2 = complexify(h2);
	h3 = complexify(h3);

	if im
		clf, subplot(211)
		plot(	s1, real(h1), 'c.-', s1, imag(h1), 'y.-', ...
			s2, real(h2), 'g.-', s2, imag(h2), 'm.-', ...
			s3, real(h3), 'y.-', s3, imag(h3), 'w.-')
	end

	K = [8 12 4];
	ck = zeros(K);
	ck(0+1, 0+1) = 1i;
	ck = complexify(ck);
	t1 = linspace(-2*K(1), 2*K(1), 69)';
	t2 = linspace(-2*K(2), 2*K(2), 89)';
	t3 = linspace(-2*K(3), 2*K(3), 9)';
%	t1 = linspace(0, K(1), 201)';
%	t2 = linspace(0, K(2), 199)';
	t3 = [-1 0 1];
	[tt1 tt2 tt3] = ndgrid(t1, t2, t3);
	tm = [tt1(:) tt2(:) tt3(:)];
	tic
	fm = interp3_table_mex(ck, h1, h2, h3, int32(J), int32(L), tm);
	toc
	fm = reshape(fm, size(tt1));

	im(121, t1, t2, real(fm), 'real'), cbar
	im(122, t1, t2, imag(fm), 'imag'), cbar
	if length(t3) == 1 % compare to 2D
		f2 = interp2_table_mex(ck, h1, h2, ...
			int32(J(1:2)), int32(L(1:2)), tm(:,1:2));
		f2 = reshape(f2, size(tt1));
		max_percent_diff(f2, fm)
	end
end