function E = bitreslice(A,b_old, b_new, echo_on)
% function E = bitreslice(A,b_old, b_new, echo_on)
% Author: Damon Bradley
% Purpose: A is an [M by N] matrix of integers. Implicit is each element's
% representaiton in binary, which is b_old bits wide. Matrix E is output
% which is an [M by L] matrix of integers, with the bits of A concatenated
% column-wise and re-split into b_new bit-wide elements. The number of bits
% per row, b_old*N must be evenly divisible by b_new to work.
%
% Inputs:
% A         - An M by N matrix of unsigned integers
% b_old     - Number of bits to represent A, should be based on maximum value
% b_new     - New number of bits to slice each bit-concatenated row of A
% echo_on   - Choose 1 to display underlying bit representation. Don't specify otherwise

if nargin > 3,
    lookunderhood = true;
else
    lookunderhood = false;
end
[M N] = size(A);
TotalBitsPerRow = N*b_old;
L = TotalBitsPerRow/b_new;

%A = [3 4 7; 3 2 0; 10 5 5]
[MaxA pos] = max(A(:));
required_old_bits = ceil(log2(MaxA));
if b_old < required_old_bits,
    [i j] = ind2sub(size(A),pos);
    str = ['A(' num2str(i) ',' num2str(j) ')=' num2str(MaxA)];
    error(['D''oh! ' num2str(b_old) ...
        ' bits are not enough to represent largest element, (' str '), in the input matrix.' ])
end

if lookunderhood,
    B = dec2binMx(A,b_old)
    C = cell2mat(B);
    D = mat2cell(C,ones(1,M), b_new*ones(1,L))
    E = bin2decMx(D);
else
    B = dec2binMx(A,b_old);
    C = cell2mat(B);
    D = mat2cell(C,ones(1,M), b_new*ones(1,L));
    E = bin2decMx(D);
end

end

function B = dec2binMx(A, bits)
% function y = dec2binMx(x, bits)
% Author: Damon Bradley
% Purpose: perform dec2bin but on matrices. If size(A) = [M N], then B is a
% cell array of string representations of the b-bit representation of the entries of A, sized [M N].

M = size(A,1);
Z = cellstr(dec2bin(A(:),bits));
B = reshape(Z,M,[]);

end

function B = bin2decMx(A)
% function B = bin2decMx(A, bits)
% Author: Damon Bradley
% Purpose: perform bin2dec on cell array obinary entries.

B = cellfun(@bin2dec2,A);

end

function dec = bin2dec2(binstring)
    bitvec = double(binstring) == 49;
    dec = polyval(bitvec, 2);
end


function divdif_test18 ( )

%*****************************************************************************80
%
%% DIVDIF_TEST18 tests ROOTS_TO_DIF and DIF_TO_R8POLY.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 October 2006
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'DIVDIF_TEST18\n' );
  fprintf ( 1, '  ROOTS_TO_DIF computes the divided difference\n' );
  fprintf ( 1, '  polynomial with given roots;\n' );
  fprintf ( 1, '  DIF_TO_R8POLY converts it to a standard form\n' );
  fprintf ( 1, '  polynomial.\n' );
  fprintf ( 1, '\n' );

  nroots = 1;
  roots(1) = 3.0;
  r8vec_print ( nroots, roots, '  The roots:' );

  [ ntab, xtab, diftab ] = roots_to_dif ( nroots, roots );

  c = dif_to_r8poly ( ntab, xtab, diftab );

  r8poly_print ( ntab, c, '  The polynomial:' );

  nroots = 2;
  roots(1:2) = [ 3.0, 1.0 ];
  r8vec_print ( nroots, roots, '  The roots:' );

  [ ntab, xtab, diftab ] = roots_to_dif ( nroots, roots );

  c = dif_to_r8poly ( ntab, xtab, diftab );

  r8poly_print ( ntab, c, '  The polynomial:' );

  nroots = 3;
  roots(1:3) = [ 3.0, 1.0, 2.0 ];
  r8vec_print ( nroots, roots, '  The roots:' );

  [ ntab, xtab, diftab ] = roots_to_dif ( nroots, roots );

  c = dif_to_r8poly ( ntab, xtab, diftab );

  r8poly_print ( ntab, c, '  The polynomial:' );

  nroots = 4;
  roots(1:4) = [ 3.0, 1.0, 2.0, 4.0 ];
  r8vec_print ( nroots, roots, '  The roots:' );

  [ ntab, xtab, diftab ] = roots_to_dif ( nroots, roots );

  c = dif_to_r8poly ( ntab, xtab, diftab );

  r8poly_print ( ntab, c, '  The polynomial:' );

  return
end


function determ = vand2_determinant ( n, x )

%*****************************************************************************80
%
%% VAND2_DETERMINANT returns the determinant of the VAND2 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix desired.
%
%    Input, real X(N), the values that define A.
%
%    Output, real DETERM, the determinant.
%
  determ = 1.0;

  for i = 1 : n
    for j = 1 : i - 1
      determ = determ * ( x(i) - x(j) );
    end
  end

  return
end


function value = r4_cosh ( x )

%*****************************************************************************80
%
%% R4_COSH evaluates the hyperbolic cosine of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the hyperbolic cosine of X.
%
  persistent ymax

  if ( isempty ( ymax ) )
    ymax = 1.0 / sqrt ( r4_mach ( 3 ) );
  end

  y = exp ( abs ( x ) );

  value = 0.5 * y;

  if ( y < ymax )
    value = 0.5 * ( y + 1.0 / y );
  end

  return
end


function [ o, x, w ] = epn_lag_02_xiu ( n )

%*****************************************************************************80
%
%% EPN_LAG_02_XIU implements the Xiu rule for region EPN_LAG.
%
%  Discussion:
%
%    The rule has order
%
%      O = N + 1.
%
%    The rule has precision P = 2.
%
%    EPN is the N-dimensional positive space [0,+oo)^N with exponential
%    or Laguerre weight function:
%
%      w(x(1:n)) = exp ( - sum ( x(1:n) ) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Dongbin Xiu,
%    Numerical integration formulas of degree two,
%    Applied Numerical Mathematics,
%    Volume 58, 2008, pages 1515-1520.
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%
%    Output, integer O, the order.
%
%    Output, real X(N,O), the abscissas.
%
%    Output, real W(O), the weights.
%
  o = n + 1;

  x = zeros ( n, o );
  w = zeros ( o, 1 );

  for j = 1 : o

    i = 0;
    for r = 1 : floor ( n / 2 )
      arg = 2 * r * ( j - 1 ) * pi / ( n + 1 );
      i = i + 1;
      x(i,j) = sqrt ( 2.0 ) * cos ( arg );
      i = i + 1;
      x(i,j) = sqrt ( 2.0 ) * sin ( arg );
    end

    if ( i < n )
      i = i + 1;
      x(i,j) = r8_mop ( j - 1 );
    end

  end

  gamma0 = - 1.0;
  delta0 = 1.0;
  c1 = - 1.0;

  x(1:n,1:o) = ( sqrt ( gamma0 * c1 ) * x(1:n,1:o) - delta0 ) / gamma0;

  expon = 0;
  volume_1d = ep1_lag_monomial_integral ( expon );
  volume = volume_1d ^ n;

  w(1:o) = volume / o;

  return
end


function nodeSet = coloring(G,nc,qp)
%% COLORING color vertices of a graph
%
% nodeSet = coloring(G,nc) color the vertices of the graph G by nc colors.
% Vertices in the same color are disjointed except the last one. The input
% graph G is represented by the sparse pattern of a symmetric sparse
% matrix. The values of G is not used. The output nodeSet is a cell
% array and nodeSet{k} is the indices of vertices in the k-th color. By
% default, nc = 6 if not specified by the input.
%
% nodeSet = coloring(G,nc,qp) chose the vertices with a larger value qp
% first. For example, qp can be the degree of vertices. Then nodeSet{1}
% will pick up nodes with a local max degree. If no qp, random values are
% used.
%
% Example
%
%     load lakemesh
%     N = size(node,1); NT = size(elem,1);
%     t2p = sparse([1:NT,1:NT,1:NT], elem(1:NT,:), 1, NT, N);
%     G = t2p'*t2p;
%     nodeSet = coloring(G);
%     showcoloring(G,node,nodeSet);
%
% See also: showcoloring, showgraphmatrix
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Set up
N = size(G,1);
N0 = min(floor(N/100),20);  % number of the coarest nodes
if ~exist('nc','var'), nc = 8; end
nodeSet = cell(nc,1);
if ~exist('qp','var')
    qp = rand(N,1);
else
    idx = (qp ~= 0);
    qp(idx) = (1 + 0.1*rand(sum(idx),1)).*qp(idx);
end

%% Coloring
for k = 1:nc-1
    isC = false(N,1);       % C: coarse nodes
    isF = false(N,1);       % F: fine nodes
    isU = (qp>0);           % U: undecided nodes
    if ~any(isU)            % no more vertices left
        break
    end
    for m = 1:round(log10(N))
        % Mark all available nodes
        isS = isU;          % selected all undecided vertices
        S = find(isS);

        % Find marked nodes with local maximum quality
        [i,j] = find(triu(G(S,S),1));   % i,j and i<j: edges of subgraph S
        idx = (qp(S(i)) >= qp(S(j)));   % compare quality of connected vertices
        isS(S(j(idx))) = false;         % remove vertices with smaller qp
        isS(S(i(~idx))) = false;        % if qualities are equal, keep the nodes with smaller index
        isC(isS) = true;                % set selected nodes as coarse nodes

        % Remove coarse nodes and neighboring nodes from undecided set
        [i,j] = find(G(:,isC)); %#ok<*NASGU>
        isF(i) = true;                  % neighbor of C nodes are F nodes
        isU(isF | isC) = false;         % remove current C and F from U
        if sum(isU) <= N0
            break;    % only break the inner loop
        end
    end
    nodeSet{k} = find(isC);
    qp(nodeSet{k}) = 0;
end
nodeSet{k+1} = find(qp>0);  % all left vertices are added to the last color
% nodeSet = nodeSet{1:k+1};


%
% Calculation of the fractal dimension which is the slope on the
% log-log graph.
%
deriv = diff(log10(corint))./diff(log10(r));			% deriv= Vector of the appr. derivatives
r2 = r(1:(end-1));											% Forward difference approximation: deriv has one element less, and r must have the # of elements so r2

switch(dof)

    case 'newf'

        %
        % Calculation of the fractal dimension, by first calculating the
        % distances of depopulation "rd" and of saturation "rs".
        %
        rad = 0.8;				% 2rmax= linear size of the hypercube encompassing a given dataset
        ras = 7;
        v = find(r2 >= rad & r2 <= ras);					% v= Vector of the all the interevent distances that fall in the interval [rn,rs]
        lr = log10(r2(v));
        lc = log10(corint(v));

        [fd, Err] = polyfit(lr,lc,1);
        [ev, delta] = polyval(fd, log10(r), Err);

        %[fdlo,Err] = polyfit(log10(r2(v)), log10(corint(v)), 1);
        %[fd,Err] = polyfit(log10(r2(v)), log10(corint(v)), 1);
        %[deriv3, delta] = polyval(fd, log10(r2(v)), Err);

        Ha_Cax = gca;
        Hf_Cfig;
        hold on;
        Hl_gr2a = loglog(r2(v), corint(v), 'k.','Markersize',7);
        Hl_gr2b = plot(r,10.^ev,'k','Linewidth',1);
        xlabel('Interevent Distance R [km]');
        ylabel('Correlation Integral C(R)');
        set(Ha_Cax,'pos',[0.21 0.1 0.75 0.75]);

        if d == 2
            title('Correlation Integral versus the 2D Interevent Distance R');
        else
            title('Correlation Integral versus the 3D Interevent Distance R');
        end

        g = [];
        uicontrol('Units','normal','Position',[.0 .92 .15 .07],...
            'String','Scaling range', 'Callback','g = ginput(2);dof = ''newc''; dofdim')

        str1 = ['Range = ' num2str(rd,3) ' - ' num2str(rs,3) ' [km]'];
        str2 = ['D =  ' num2str(fd(1,1),3)];
        axes('pos',[0 0 1 1]); axis off; hold on;
        te1 = text(0.25, 0.8, str1 ,'Fontweight','bold');
        te2 = text(0.25, 0.75, str2, 'Fontweight', 'bold');

        %str3 = [' The scaling range is calculated as proposed by Nerenberg & Essex (1990): ' num2str(rd,3) ' - ' num2str(rs,4) ' [km] . If you wish to change the scaling range please click on the "scaling range" button'];
        %msg1 = msgbox( str3,'Fractal Dimension');

    case 'newc'

        rd = min(g(:,1)); rs = max(g(:,1));
        v = find(r2 >= rd & r2 <= rs);

        lr = log10(r2(v));
        lc = log10(corint(v));

        [fd, Err] = polyfit(lr,lc,1);
        [ev, delta] = polyval(fd, log10(r), Err);

        delete(Hl_gr2a);
        delete(Hl_gr2b);
        Hl_gr2a = loglog(r2(v), corint(v), 'k.','Markersize',7);
        Hl_gr2b = plot(r,10.^ev,'k','Linewidth',1);

        delete(te1);
        delete(te2);
        str1 = ['Range = ' num2str(rd,3) ' - ' num2str(rs,3) ' [km]'];
        str2 = ['D =  ' num2str(fd(1,1),3)];
        axes('pos',[0 0 1 1]); axis off; hold on;
        te1 = text(0.25, 0.8, str1 ,'Fontweight','bold');
        te2 = text(0.25, 0.75, str2, 'Fontweight', 'bold');

end


function [H,S,D]=sobi(X,n,p),
% program  by  A. Belouchrani and A. Cichocki
%
% Second
% Order
% Blind
% Identification
% SOBI
%**************************************************
% blind identification by joint diagonalization   *
% of correlation  matrices.                       *
%                       			  *
% ------------------------------------------------*
% THIS CODE ASSUMES TEMPORALLY CORRELATED SIGNALS *
% in estimating the cumulants                     *
% ------------------------------------------------*
%
% [H,S]=SOBI(X,m,p) produces a matrix H of dimension [m by n] and a matrix S
% of dimension [n by N] wich are respectively an estimate of the mixture matrix and
% an estimate of the source signals of the linear model from where the
% observation matrix X of dimension [m by N] derive.
% Note: > m: sensor number.
%       > n: source number by default m=n.
%       > N: sample number.
%	> p: number of correlation matrices to be diagonalized by default p=4.
%
% REFERENCES:
% A. Belouchrani, K. Abed-Meraim, J.-F. Cardoso, and E. Moulines, ``Second-order
%  blind separation of temporally correlated sources,'' in Proc. Int. Conf. on
%  Digital Sig. Proc., (Cyprus), pp. 346--351, 1993.
%
% A. Belouchrani and K. Abed-Meraim, ``Separation aveugle au second ordre de
%  sources correlees,'' in  Proc. Gretsi, (Juan-les-pins),
%  pp. 309--312, 1993.
%
%  A. Belouchrani, and A. Cichocki,
%  Robust whitening procedure in blind source separation context,
%  Electronics Letters, Vol. 36, No. 24, 2000, pp. 2050-2053.
%
%  A. Cichocki and S. Amari,
%  Adaptive Blind Signal and Image Processing, Wiley,  2002.

[m,N]=size(X);
if nargin==1,
 n=m; % source detection (hum...)

 p=4; % number of correlation matrices to be diagonalized
 end;
if nargin==2,
 p=4 ; % number of correlation matrices to be diagonalized
end;
pm=p*m; % for convenience
X=X-kron(mean(X')',ones(1,N)); % zero mean
%%%% whitening
% Rx=(X*X')/T;
% if m<n, %assumes white noise
%   [U,D]=eig(Rx); [puiss,k]=sort(diag(D));
%   ibl= sqrt(puiss(n-m+1:n)-mean(puiss(1:n-m)));
%    bl = ones(m,1) ./ ibl ;
%   BL=diag(bl)*U(1:n,k(n-m+1:n))';
%   IBL=U(1:n,k(n-m+1:n))*diag(ibl);
% else    %assumes no noise
%    IBL=sqrtm(Rx);
%    Q=inv(IBL);
% end;
% X=Q*X;
%prewitening based directly on SVD
[UU,S,VV]=svd(X',0);
Q= pinv(S)*VV';
X=Q*X;
%
%%%correlation matrices estimation
k=1;for u=1:m:pm, k=k+1; Rxp=X(:,k:N)*X(:,1:N-k+1)'/(N-k+1);
    M(:,u:u+m-1)=norm(Rxp,'fro')*Rxp; end;
%%%joint diagonalization
epsil=1/sqrt(N)/100; encore=1; V=eye(m);
while encore, encore=0;
 for p=1:m-1,
  for q=p+1:m,
   %%% Givens rotations
   g=[   M(p,p:m:pm)-M(q,q:m:pm)  ;
         M(p,q:m:pm)+M(q,p:m:pm)  ;
      i*(M(q,p:m:pm)-M(p,q:m:pm)) ];
	  [vcp,D] = eig(real(g*g')); [la,K]=sort(diag(D));
   angles=vcp(:,K(3));angles=sign(angles(1))*angles;
   c=sqrt(0.5+angles(1)/2);
   sr=0.5*(angles(2)-j*angles(3))/c; sc=conj(sr);
   oui = abs(sr)>epsil ;
   encore=encore | oui ;
   if oui , %%%update of the M and V matrices
    colp=M(:,p:m:pm);colq=M(:,q:m:pm);
    M(:,p:m:pm)=c*colp+sr*colq;M(:,q:m:pm)=c*colq-sc*colp;
    rowp=M(p,:);rowq=M(q,:);
    M(p,:)=c*rowp+sc*rowq;M(q,:)=c*rowq-sr*rowp;
    temp=V(:,p);
    V(:,p)=c*V(:,p)+sr*V(:,q);V(:,q)=c*V(:,q)-sc*temp;
   end%% if
  end%% q loop
 end%% p loop
end%% while
%%%estimation of the mixing matrix and source signals
Hf=pinv(Q)*V; %estimated mixing matrix
Sf=V'*X; %estimated sources

H = Hf(:,1:n);
S = Sf(1:n,:);


% checks the resolution limit of root-MUSIC
clear(); close all;

wavelength = 1; % normalized
d = wavelength / 2;
design = design_array_1d('ula', 10, d); % 10-element ULA
power_source = 1;
power_noise = 1;
snapshot_count = 100;

delta_theta = deg2rad(linspace(0.05, 5, 20)); % 0.1 deg - 10 deg
n_delta_theta = length(delta_theta);
n_repeat = 500;
res_prob = zeros(n_delta_theta, 1);
progressbar('reset', n_delta_theta);
for ii = 1:n_delta_theta
    n_resolved = 0;
    for rr = 1:n_repeat
        doas = [-delta_theta(ii)/2 delta_theta(ii)/2];
        [~, R] = snapshot_gen_sto(design, doas, wavelength, snapshot_count, power_noise, power_source);
        sp = rmusic_1d(R, 2, 2*pi*design.element_spacing/wavelength);
        % resolution condition:
        %   theta_1 \in (-delta_theta, 0)
        %   theta_2 \in (0, delta_theta)
        % This condition is pretty strict. It requires the estimated DOAs
        % are reasonably close to the correct ones.
        % You may change it to a milder condition.
        if sp.x_est(1) < 0 && sp.x_est(1) > -delta_theta(ii) ...
                && sp.x_est(2) > 0 && sp.x_est(2) < delta_theta(ii)
            n_resolved = n_resolved + 1;
        end
    end
    res_prob(ii) = n_resolved / n_repeat;
    progressbar('advance');
end
fprintf('\n');

plot(rad2deg(delta_theta), res_prob); grid on;
xlabel('Source separation (deg)'); ylabel('Resolution Probability');


%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL

%%This function plan a trajectory passing in N
%% points using N-1 3th order polynomial:
%% the user must choose: intial,final and intermediate points
%% timePoints is a matrix [3,N] where first row is the time,second row
%% is the position points, third row is the velocities
%% intermediate velocities (initial and final ones are imposed to be 0)
%% Output: a matrix [N-1,4] whose columns are the polynomail coefficients

function [TJ]=trajectory3Velconstraint(timePointsVels)
N=length(timePointsVels);

if(N<2)
fprintf('Error: you defined only 1 point');
return;
end
TJ=[];
%%set the initial and final velocities to 0
timePointsVels(3,1)=0;
timePointsVels(3,end)=0;
for k=1:N-1
    tk=timePointsVels(1,k);
    tk1=timePointsVels(1,k+1);
    qk=timePointsVels(2,k);
    qk1=timePointsVels(2,k+1);
    velk=timePointsVels(3,k);
    velk1=timePointsVels(3,k+1);
    coeffs=pol3interpol(tk,tk1,qk,qk1,velk,velk1);
    TJ=[TJ;coeffs'];
end

end


function b = hp(fs)

% Equiripple Highpass filter designed using the FIRPM function.
% All frequency values are in Hz.

Fstop = 0.125;           % Stopband Frequency
Fpass = 0.25;            % Passband Frequency
Dstop = 0.001;           % Stopband Attenuation
Dpass = 0.057501127785;  % Passband Ripple
dens  = 20;              % Density Factor

% Calculate the order from the parameters using FIRPMORD.
[N, Fo, Ao, W] = firpmord([Fstop, Fpass]/(fs/2), [0 1], [Dstop, Dpass]);

% Calculate the coefficients using the FIRPM function.
b  = firpm(N, Fo, Ao, W, {dens});

end


function [dy] = dmodel2D(t,y,a)
dy = zeros(2,1);
dy(1) = y(2);
dy(2) = (1 - a(2)*y(2)-a(1)*y(1))/a(3);
%


function [X,nuclearnorm] = prox_nuclear(B,lambda)

% The proximal operator of the nuclear norm of a matrix
%
% min_X lambda*||X||_*+0.5*||X-B||_F^2
%
% version 1.0 - 18/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

[U,S,V] = svd(B,'econ');
S = diag(S);
svp = length(find(S>lambda));
if svp>=1
    S = S(1:svp)-lambda;
    X = U(:,1:svp)*diag(S)*V(:,1:svp)';
    nuclearnorm = sum(S);
else
    X = zeros(size(B));
    nuclearnorm = 0;
end


clear all; close all; clear classes; clc;

%% Ag in Table I on p.4374 of Johnson and Christy, 1972 Phys. Rev. B
eV = [0.64 0.77 0.89 1.02 1.14 1.26 1.39 1.51 1.64 1.76 1.88 2.01 2.13 2.26 2.38 2.50 2.63 2.75 2.88 3.00 3.12 3.25 3.37 3.50 3.62 3.74 3.87 3.99 4.12 4.24 4.36 4.49 4.61 4.74 4.86 4.98 5.11 5.23 5.36 5.48 5.60 5.73 5.85 5.98 6.10 6.22 6.35 6.47 6.60];
n = [0.24 0.15 0.13 0.09 0.04 0.04 0.04 0.04 0.03 0.04 0.05 0.06 0.05 0.06 0.05 0.05 0.05 0.04 0.04 0.05 0.05 0.05 0.07 0.10 0.14 0.17 0.81 1.13 1.34 1.39 1.41 1.41 1.38 1.35 1.33 1.31 1.30 1.28 1.28 1.26 1.25 1.22 1.20 1.18 1.15 1.14 1.12 1.10 1.07];
k = [14.08 11.85 10.10 8.828 7.795 6.992 6.312 5.727 5.242 4.838 4.483 4.152 3.858 3.586 3.324 3.093 2.869 2.657 2.462 2.275 2.070 1.864 1.657 1.419 1.142 0.829 0.392 0.616 0.964 1.161 1.264 1.331 1.372 1.387 1.393 1.389 1.378 1.367 1.357 1.344 1.342 1.336 1.325 1.312 1.296 1.277 1.255 1.232 1.212];

%% Convert the photon energies to the wavelengths.
wvlen = PhysC.h * PhysC.c0 * 1e9 ./ eV;

%% Make the row vectors into column vectors to be used in interp1q().
eV = eV.';
n = n.';
k = k.';
wvlen = wvlen.';

%% Calculate the permittivity from n and k following the exp(+i w t) time dependence.
eps = (n - 1i*k).^2;

%% Plot real(eps) and imag(eps).  Compare with Fig.3 on p.4375 of Johnson.

nk_wvlen = 1;
eps_eV = 2;
eps_wvlen = 3;
eps_omega = 4;
q_omega = 5;
plotstyle = eps_eV;
switch plotstyle
    case nk_wvlen  % plot n and k
        loglog(wvlen, n, 'o-', wvlen, k, 'o-')
        %plot(wvlen, n, wvlen, k)
        legend('n', 'k', 'Location', 'SouthEast');
        xlabel 'wavelength (nm)'
        %axis([1e2 1e4 1e-2 1e2])
    case eps_eV  % plot real(eps) and -imag(eps)
        plot(eV, real(eps), 'o-', eV, -imag(eps), 'o-')
        legend('\epsilon_1', '\epsilon_2', 'Location', 'SouthEast');
        xlabel 'Photon Energy (eV)'
%         axis([0.5 6.5 -7 7]);
    case eps_wvlen
        plot(wvlen, real(eps), 'o-', wvlen, -imag(eps), 'o-')
        legend('\epsilon_1', '\epsilon_2', 'Location', 'SouthEast');
        xlabel 'wavelength (nm)'
        %axis([1e2 1e4 1e-2 1e2])
    case eps_omega
        plot(2*pi./wvlen, real(eps), 'o-', 2*pi./wvlen, -imag(eps), 'o-')
        legend('\epsilon_1', '\epsilon_2', 'Location', 'SouthEast');
        xlabel '\omega (c/nm)'
    case q_omega
        omega = 2*pi./wvlen;
        deps1 = real(eps(2:end)) - real(eps(1:end-1));
        domega = omega(2:end) - omega(1:end-1);
        eps1_inter = (real(eps(2:end)) + real(eps(1:end-1)))/2;
        omega_inter = (omega(2:end) + omega(1:end-1))/2;
        numer = eps1_inter + omega_inter .* (deps1./domega);
        denom = -(imag(eps(2:end)) + imag(eps(1:end-1)));  % extra factor 2
        plot(2*pi./omega_inter, numer./denom, 'o-');
        xlabel 'wavelength (nm)'
        ylabel 'electric Q';
end
title(mfilename)

%% Save data.
save(mfilename, 'eV', 'n', 'k');


function p = predictOneVsAll(all_theta, X)
%PREDICT Predict the label for a trained one-vs-all classifier. The labels
%are in the range 1..K, where K = size(all_theta, 1).
%  p = PREDICTONEVSALL(all_theta, X) will return a vector of predictions
%  for each example in the matrix X. Note that X contains the examples in
%  rows. all_theta is a matrix where the i-th row is a trained logistic
%  regression theta vector for the i-th class. You should set p to a vector
%  of values from 1..K (e.g., p = [1; 3; 1; 2] predicts classes 1, 3, 1, 2
%  for 4 examples)

m = size(X, 1);
num_labels = size(all_theta, 1);

% You need to return the following variables correctly
p = zeros(size(X, 1), 1);

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters (one-vs-all).
%               You should set p to a vector of predictions (from 1 to
%               num_labels).
%
% Hint: This code can be done all vectorized using the max function.
%       In particular, the max function can also return the index of the
%       max element, for more information see 'help max'. If your examples
%       are in rows, then, you can use max(A, [], 2) to obtain the max
%       for each row.
%

all_predictions = all_theta * X';
[M, I] = max(all_predictions);
p = I(:);

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

end


function order = gl_level_to_order ( dim_num, level )

%*****************************************************************************80
%
%% GL_LEVEL_TO_ORDER converts a GL level to a GL order.
%
%  Discussion:
%
%    Gauss-Legendre rules cannot be nested, but in order to facilitate
%    comparison with Clenshaw Curtis grids, we need to be able to
%    convert a "level" into an order.  Except for the
%    first case of LEVEL = 0, the relationship is
%
%      ORDER = 2**LEVEL + 1
%
%    Level      Order
%
%    0          1
%    1          3 =  2 + 1
%    2          5 =  4 + 1
%    3          9 =  8 + 1
%    4         17 = 16 + 1
%    5         33 = 32 + 1
%
%    In this routine, we assume that a vector of levels is given,!
%    and the corresponding orders are desired.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 November 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL(DIM_NUM), the level.
%
%    Output, integer ORDER(DIM_NUM), the order (number of points) of the
%    corresponding rule.
%
  for dim = 1 : dim_num

    if ( level(dim) < 0 )
      order(dim) = -1;
    elseif ( level(dim) == 0 )
      order(dim) = 1;
    else
      order(dim) = ( 2^level(dim) ) + 1;
    end

  end

  return
end


function r = taylorinit(v,k)
%TAYLORINIT   Initialization of taylor variable
%
%  x = taylorinit(v,k)
%
%The dependent variable x is identified and initialized with v, and Taylor
%coefficients up to k>0 are calculated. For example,
%
%  u = taylorinit(-3,5)
%
%initializes "u" to be a Taylor variable with value -3 and Taylor coefficients
%up to order k=5, i.e. [-3,1,0,0,0,0]. The default for k is 4. After initialization,
%the order of Taylor coefficients can be obtained by
%
%  k = taylororder
%
%In our example, the result will be k=5. The evaluation
%
%  f = inline('x*sin(x)-exp(x^2)')
%  y = f(u)
%
%computes the Taylor coefficients of f up to order 5. Note the first component of y
%is f(u), followed by f'(u), f''(u)/2!, f'''(u)/3!, etc. For example,
%
%  t0 = y{0}
%
%is the 0-th Taylor coefficients, namely f(u), and
%
%  sum( y{0:k} .* e.^(0:k) )
%
%is approximately equal to f(u+e) up to O(e^(k+1)). The subsequent statement
%
%  v = taylor( intval('3.14159_') )
%
%initializes v to be the constant with interval value infsup(3.14158,3.14160).
%
%Taylor expansions are implemented for univariate functions. However, it is
%sometimes useful to evaluate a function at an array of points. The statement
%
%  w = taylorinit((-3:1)',5)
%
%initializes w to be a vector of dependent variables with values -3,-2,-1,0,1. All
%operations with u are executed pointwise, so y=f(w) is executed as w.*sin(w)-exp(w.^2).
%In this case
%
%  y(3)
%
%is the Taylor expansion of f at -1 up to order 5.
%
%For other examples, see demotaylor.
%

% written  05/21/09     S.M. Rump
% modified 09/15/10     S.M. Rump
% modified 08/26/12     S.M. Rump  global variables removed
%

  setappdata(0,'INTLAB_TAYLOR_END',0);

  if nargin==0
    setappdata(0,'INTLAB_TAYLOR_ORDER',0);
    return
  end

  if ~isa(v,'double') & ~isa(v,'intval')
    error('invalid initialization of dependent Taylor variables')
  end
  if nargin==1
    setappdata(0,'INTLAB_TAYLOR_ORDER',4);
  else
    setappdata(0,'INTLAB_TAYLOR_ORDER',k);
  end
  dummy.init = v;
  r = taylor( dummy , 'taylorinit' );


function flag = validateTheta_homography(X, Theta, s)

% flag = validateMSS_homography(X, Theta, s)
%
% DESC:
% Validates the parameter vector
%
% INPUT:
% X                 = samples on the manifold
% Theta             = parameter vector
% s                 = indices of the MSS
%
% OUTPUT:
% flag              = true if the MSS is valid

% condition number threshold
T_K = 1e12;

% perform here the check on the parameter vector Theta
H = reshape(Theta, 3, 3);
K = cond(H);

if (K > T_K)
    flag = false;
else
    flag = true;
end;

return


function [C, dC] = gplvm_grad(x, X, sigma)
%GPLVM_GRAD Gradient of the Gaussian Process Latent Variable model
%
%   [C, dC] = gplvm_grad(x, no_dims, sigma)
%
% Computes the gradient of the Gaussian Process Latent Variable model.

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

    % Decode solution
    [n, d] = size(X);
    no_dims = numel(x) ./ n;
    Y = reshape(x, [n no_dims]);

    % Compute kernel matrix (in latent space)
    sum_Y = sum(Y .^ 2, 2);
    K = exp(bsxfun(@minus, bsxfun(@minus, 2 * (Y * Y'), sum_Y'), sum_Y) / (2 * sigma ^ 2));

    % Compute gradient with respect to kernel
%     invK = inv(K);
%     dLdK = invK * X * X' * invK - d * invK;
    tmp = (K \ X) * X';
    dLdK = (tmp - d * eye(n)) / K;

    % Compute gradient with respect to coordinates
    dC = zeros(n, no_dims);
    dLdK = K .* dLdK;
    for i=1:n
        dC(i,:) = sum(bsxfun(@times, dLdK(:,i), bsxfun(@minus, Y(i,:), Y)), 1);
    end
    dC = (-1 / sigma ^ 2) * dC(:);

    % Compute log-likelihood
    C = -((d * n) / 2) * log(2 * pi) - (d / 2) * log(det(K) + realmin) - .5 * trace(tmp);


function [OP,MM] = CurvedPoissonIPDG2D()

% function [OP,MM] = CurvedPoissonIPDG2D()
% Purpose: Set up a discrete Poisson matrix and mass matrix
%          using Interior Penalty Discontinuous Galerkin (IPDG).

Globals2D;

NGauss = gauss.NGauss;

% build DG derivative matrices
MM  = zeros(K*Np*Np, 3);  OP = zeros(K*Np*Np*(1+Nfaces), 3);

% global node numbering
entries = (1:Np*Np)'; entriesMM = (1:Np*Np)';
for k1=1:K
  if(~mod(k1,200)) k1, end;

  % Location of k1'th diagonal block entries in OP matrix
  rows1 = ((k1-1)*Np+1:k1*Np)'*ones(1,Np); cols1 = rows1';

  % Extract local mass matrix and cubature weights
  locmm = cub.mm(:,:,k1);
  cw  = spdiags(cub.W (:,k1), 0, cub.Ncub, cub.Ncub);

  % Evaluate derivatives of Lagrange basis functions at cubature nodes
  [cDx, cDy]  = PhysDmatrices2D(x(:,k1), y(:,k1), cub.V);

  % Evaluate local stiffness matrix
  OP11 = cDx'*cw*cDx + cDy'*cw*cDy;

  % Build element-to-element parts of stiffness matrix for element k1
  for f1=1:Nfaces

    % Find neighbor
    k2 = EToE(k1,f1); f2 = EToF(k1,f1);

    idsM = (f1-1)*NGauss+1:f1*NGauss;

    % Extract Lagrange basis function -> Gauss node interpolation matrix
    gVM = gauss.finterp(:,:,f1);
    gVP = gauss.finterp(:,:,f2);    gVP = gVP(NGauss:-1:1,:);

    % Evaluate spatial derivatives of  Lagrange basis function at Gauss nodes
    [gDxM, gDyM] = PhysDmatrices2D(x(:,k1), y(:,k1),gVM);
    [gDxP, gDyP] = PhysDmatrices2D(x(:,k2), y(:,k2),gVP);

    % Evaluate normals at Gauss nodes on face
    gnx = spdiags(gauss.nx(idsM, k1), 0, NGauss, NGauss);
    gny = spdiags(gauss.ny(idsM, k1), 0, NGauss, NGauss);
    gw  = spdiags(gauss.W(idsM, k1),  0, NGauss, NGauss);

    % Compute normal derivatives of Lagrange basis functions at Gauss nodes
    gDnM = gnx*gDxM + gny*gDyM;
    gDnP = gnx*gDxP + gny*gDyP;

    % Locate global numbers of Lagrange nodes in neighboring element
    cols2 = ones(Np,1)*((k2-1)*Np+1:k2*Np);

    % Find minimum height of two elements sharing this face
    hinv = max(Fscale( 1 + (f1-1)*Nfp, k1), Fscale( 1 + (f2-1)*Nfp, k2));

    % Set penalty scaling
    gtau = 20*(N+1)*(N+1)*hinv;

    % Determine type of face
    switch(BCType(k1,f1))
      case {Dirichlet}
        % Dirichlet boundary face variational terms
	OP11 = OP11 + ( gVM'*gw*gtau*gVM - gVM'*gw*gDnM - gDnM'*gw*gVM);

      case {Neuman}
        % Do nothing
      otherwise
	% Interior face variational terms for stiffness matrix
	OP11 = OP11 + 0.5*( gVM'*gw*gtau*gVM - gVM'*gw*gDnM - gDnM'*gw*gVM );

	OP12 =      - 0.5*( gVM'*gw*gtau*gVP + gVM'*gw*gDnP - gDnM'*gw*gVP );

        % Store self-neighbor interaction term in global stiffness matrix
        OP(entries(:), :) = [rows1(:), cols2(:), OP12(:)];
	entries = entries + Np*Np;
    end
  end

  % Store k1'th self-self interaction term in global stiffness matrix
  OP(entries(:), :)   = [rows1(:), cols1(:), OP11(:)];
  MM(entriesMM(:), :) = [rows1(:), cols1(:), locmm(:)];
  entries = entries + Np*Np; entriesMM = entriesMM + Np*Np;
end

% Convert OP and MM from coordinate storage format to Matlab's intrinsic sparse matrix format
OP = OP(1:max(entries)-Np*Np,:);
OP = myspconvert(OP, Np*K, Np*K, 1e-15); MM = myspconvert(MM, Np*K, Np*K, 1e-15);
return


function b_n = beamWeightsHypercardioid2Spherical(N)
%BEAMWEIGHTSHYPERCARDIOID2SPHERICAL Generate spherical beamweights for hypercardioids
%
%   The hypercardioid is the pattern that maximizes the directivity-factor
%   for a certain SH order N. The hypercardioid is also the plane-wave
%   decomposition beamformer in the SHD, also called 'regular' because the
%   beamweights are just the SH values on the beam-direction.
%   Since the pattern is axisymmetric only the N+1 coefficients of m=0 are
%   returned.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% BEAMWEIGHTSHYPERCARDIOID2SPHERICAL.M - 11/7/2013
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

c_n = 4*pi/(N+1)^2 * getSH(N,[0 0],'real');
b_n = zeros(N+1,1);
for n=0:N
    b_n(n+1) = c_n((n+1)^2 -n);
end


function lambda = bab_eigenvalues ( n, alpha, beta )

%*****************************************************************************80
%
%% BAB_EIGENVALUES returns the eigenvalues of the BAB matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real ALPHA, BETA, the parameters.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  for i = 1 : n
    angle = i * pi / ( n + 1 );
    lambda(i,1) = alpha + 2.0 * beta * cos ( angle );
  end

  return
end


function X = create2dbf(g1,g2,N)

n1 = length(g1);
n2 = length(g2);

X1 = zeros(n1,N,2);
for i = 1:N
    X1(:,i,1) = cos((i-1).*pi.*[0:(n1-1)]./(n1-1));
    X1(:,i,2) = sin((i-1).*pi.*[0:(n1-1)]./(n1-1));
end
X2 = zeros(n2,N,2);
for i = 1:N
    X2(:,i,1) = cos((i-1).*pi.*[0:(n2-1)]./(n2-1));
    X2(:,i,2) = sin((i-1).*pi.*[0:(n2-1)]./(n2-1));
end

X = zeros(n1,n2,N^2);
ij = 1;
for i=1:N
    for j=1:N
        for i1=1:2
            for i2=1:2
                X(:,:,ij) = repmat(X1(:,i,i1),1,n2).*repmat(X2(:,j,i2)',n1,1);
                ij = ij +1;
            end
        end
    end
end


function [x, infos] = rank2nmf(V, in_options)
% Rank-two NMF computes a rank-two NMF of X
%
%       If X >= 0 and rank(X) = 1, take W as any non-zero column of X and compute
%       H accordingly
%
%       If X >= 0 and rank(X) = 2, this is Algorithm 4.1 in Chapter 4,
%       with alpha1 = alpha2 = 0. This is an exact algorithm.
%
%       Otherwise, the input matrix has is replaced by max(0,X2) where X2 is its
%       best rank-two approximation; cf. Chapter 6.
%
% Inputs:
%       matrix      V
%       options     options
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       Nicolas Gillis,
%       "Nonnegative Matrix Factorization,"
%       SIAM, 2020.
%
%
% This file is part of NMFLibrary.
%
% This file has been ported from
%       Rank2NMF.m at https://gitlab.com/ngillis/nmfbook/-/tree/master/algorithms
%       by Nicolas Gillis (nicolas.gillis@umons.ac.be)
%
% Change log:
%
%       June 13, 2022 (Mitsuhiko Horie and Hiroyuki Kasai): Ported initial version
%

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

    % set local options
    local_options = [];
    local_options.delta = 1e-6;

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

    if options.verbose > 0
        fprintf('# Rank2-NMF: started ...\n');
    end

    if min(V(:)) < 0
        if options.verbose > 0
            fprintf('  * Rank2-NMF: The input matrix is not nonnegative: V <-- max(V,0).\n');
        end
        V = max(0,V);
    end
    [u, s, v] = svds(V, 3);

    % Case 0. rank(V) = 0
    if s(1, 1) <= eps
        if options.verbose > 0
            fprintf('  * Rank2-NMF: The input matrix is (close to) zero.');
        end
        [m,n] = size(V);
        W = zeros(m,1);
        H = zeros(1,n);
        return;
    end

    % Case 1. rank(V) = 1
    if s(2, 2) < 1e-9 * s(1, 1)
        if options.verbose > 0
            fprintf('  * Rank2-NMF: The input matrix has rank one, an optimal solution is returned.\n');
        end
        sumV = sum(V);
        [~,j] = max(sumV);
        W = V(:,j);
        H = sumV/sum(W);
        return;
    end

    % Case 2. rank(V) >= 2
    if s(3, 3) > 1e-9 * s(2, 2)
        if options.verbose > 0
	        fprintf('  * Rank2-NMF: The input matrix does not have rank two.\n');
            fprintf('  * Rank2-NMF: V <-- rank(V2,0) where V2 is the rank-2 truncated SVD of V.\n');
            fprintf('  * Rank2-NMF: The solution computed might not be optimal.\n');
        end
        [u, s, v] = svds(V, 2);
        Vj = max(u*s*v', 0);
    else
        if options.verbose > 0
            fprintf('  * Rank2-NMF: The input matrix has rank two, an optimal solution is returned.\n');
        end
        Vj = V;
    end

    % remove zero columns
    sumVj = sum(Vj);
    J = find(sumVj > 0);
    Vj = Vj(:, J);

    % normalize columns
    [m, n] = size(Vj);
    for i = 1 : n
        Vj(:, i) = Vj(:, i) / sum(Vj(:, i));
    end

    % compute extreme points of conv(Vj) hence a feasible solution W
    % given that rank(V) = 2.
    W = zeros(m, 2);
    [~, j1] = max(sum(Vj.^2));
    W(:, 1) = Vj(:, j1);
    [~, j2] = max(sum( (Vj-repmat(Vj(:, j1), 1, n)).^2));
    W(:, 2) = Vj(:, j2);

    W = full(W);
    WtW = W' * W;
    WtV = W' * V;

    % If no initial matrices are provided, H is initialized as follows:
    if ~isfield(options,'init') || isempty(options.init)
        H = nnls_init_nmflibrary(V, W, WtW, WtV);
    else
        H = options.init;
    end

    %% perform main routine
    options.alg_name = 'Rank2-NMF';
    options.V = V;
    options.W = W;
    options.main_routine_mode = true;
    options.eps0 = 0;
    options.eps = 1;

    [H, WtW, WtV, infos] = nnls_solver(V, W, options);

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

end


% Resizes subs by resizVals.
%
% Resizes subs in subs/vals image representation by resizVals.
%
% This essentially replaces each sub by sub.*resizVals.  The only subtlety
% is that in images the leftmost sub value is .5, so for example when
% resizing by a factor of 2, the first pixel is replaced by 2 pixels and so
% location 1 in the original image goes to location 1.5 in the second
% image, NOT 2.  It may be necessary to round the values afterward.
%
% USAGE
%  subs = imSubsResize( subs, resizVals, [zeroPnt] )
%
% INPUTS
%  subs        - subscripts of point locations (n x d)
%  resizVals   - k element vector of shrinking factors
%  zeroPnt     - [.5] See comment above.
%
% OUTPUTS
%  subs        - transformed subscripts of point locations (n x d)
%
% EXAMPLE
%  subs = imSubsResize( [1 1; 2 2], [2 2] )
%
%
% See also IMSUBSTOARRAY

% Piotr's Image&Video Toolbox      Version NEW
% Written and maintained by Piotr Dollar    pdollar-at-cs.ucsd.edu
% Please email me if you find bugs, or have suggestions or questions!

function subs = imSubsResize( subs, resizVals, zeroPnt )

if( nargin<3 || isempty(zeroPnt) ); zeroPnt=.5;  end

[n d] = size(subs);
[resizVals,er] = checkNumArgs( resizVals, [1 d], -1, 2 ); error(er);

% transform subs
resizVals = repmat( resizVals, [n, 1] );
subs = (subs - zeroPnt) .* resizVals + zeroPnt;


function sqi = ksqi(signal)
%kSQI Kurtosis SQI
%
% Returns the kurtosis of a signal
%
% Reference:
% Li, Q., Mark, R. G., Clifford, G. D., & Li. (2008). Robust heart rate
% estimation from multiple asynchronous noisy sources using signal quality
% indices and a Kalman filter. Physiol. Meas., 29(1), 15–32.
% http://doi.org/10.1088/0967-3334/29/1/002
%
% Input:
%   signal:         single channel (F)ECG [1xN double]
%
% Output:
%   sqi:            resulting kSQI for segment
%
% Fetal Extraction Toolbox, version 1.0, February 2014
% Released under the GNU General Public License
%
% Copyright (C) 2014 Fernando Andreotti
% Dresden University of Technology, Institute of Biomedical Engineering
% fernando.andreotti@mailbox.tu-dresden.de
%
% Last updated : 09-03-2014
%
%
% 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.

sqi=kurtosis(signal);

end


function res = qdist(a,b)
%QDIST        Implements  q(a,b)  metrical distance
%  name  qdist  is used to avoid ambiguities with variable  q
%
%     res = qdist(a,b)          max( abs(inf(a)-inf(b)) , abs(sup(a)-sup(b)) )
%
% for real intervals      max( abs(inf(a)-inf(b)) , abs(sup(a)-sup(b)) )
% for complex intervals   qdist(real(a),real(b)) + qdist(imag(a),imag(b))
%

% written  10/16/98     S.M. Rump
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

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

  if ~isa(a,'intval')
    a = intval(a);
  end
  if ~isa(b,'intval')
    b = intval(b);
  end

  if ~a.complex & ~b.complex
    res = max( abs(a.inf-b.inf) , abs(a.sup-b.sup) ) ;
  else
    res = qdist(real(a),real(b)) + qdist(imag(a),imag(b)) ;
  end

  if rndold
    setround(rndold)
  end


function [hess,pattern,symhess] = symHess(fun,nvar,isTril,var)
%SYMHESS  Returns a Symbolically Differentiated Hessian
%
%   hess = symHess(fun) uses the symbolic toolbox to automatically generate
%   the Hessian of the function handle fun. You should supply the function
%   handle in the form @(x) x(1)^2 + x(2), noting x must be the only
%   variable used, and is indexed in the equation (no vector operations).
%
%   hess = symHess(fun,nvar) specifies the number of variables in the
%   equation, assuming consecutive ordering. Useful if a variable is not
%   specified in the original equation to pad the Hessian with zeros.
%
%   hess = symHess(fun,nvar,isTril) specifies if the returned Hessian
%   should be Symmetric Lower Triangular.
%
%   [hess,pattern] = symHess(...) also returns the sparsity pattern as a
%   function handle.

%   Copyright (C) 2013 Jonathan Currie (I2C2)

if(nargin < 4), var = 'x'; end
if(nargin < 3), isTril = false; end
if(nargin < 2 || isempty(nvar)), nvar = 0; end

if(~optiCheckSymTBX())
    hess = []; symhess = [];
    return
end

if(~isa(fun,'function_handle') && ~isa(fun,'barvec'))
    error('Fun should be a function handle!');
end
if(isa(fun,'function_handle') && nargin(fun) ~= 1)
    error('fun should only have one input argument (x)');
end

%Convert function to a symbolic expression
if(isa(fun,'function_handle'))
    [symfun,ind] = func2sym(fun,var);
    ind = ind{1};
else
    symfun = sym(getEq(fun)); %convert from barvec to symbolic expression
    v = char(symvar(symfun));
    ind = 1:length(strfind(v,var));
end

%Check Symbolic Expression is Scalar
if(~isscalar(symfun))
    error('symHess only works on scalar functions');
end

%Create each symbolic partial derivative
symhess = symPartialDer(symfun,var,nvar,ind,true);

%Convert if required to lower triangular
if(isTril), symhess = tril(symhess); end

%Extract Sparsity Pattern
sp = zeros(size(symhess)); sp(symhess ~= 0) = 1; sp = sparse(sp);
pattern = @() sp;

%Return to function handle
hess = sym2func(symhess,var);


function [nll,g,H,T] = LogisticLoss(w,X,y)
% w(feature,1)
% X(instance,feature)
% y(instance,1)

[n,p] = size(X);

Xw = X*w;
yXw = y.*Xw;

nll = sum(mylogsumexp([zeros(n,1) -yXw]));

if nargout > 1
    if nargout > 2
        sig = 1./(1+exp(-yXw));
        g = -X.'*(y.*(1-sig));
    else
        g = -X.'*(y./(1+exp(yXw)));
    end
end

if nargout > 2
    H = X.'*diag(sparse(sig.*(1-sig)))*X;
end

if nargout > 3
    T = zeros(p,p,p);
    for j1 = 1:p
        for j2 = 1:p
            for j3 = 1:p
                T(j1,j2,j3) = sum(y(:).^3.*X(:,j1).*X(:,j2).*X(:,j3).*sig.*(1-sig).*(1-2*sig));
            end
        end
    end
end


% Copyright (C) 2008 Eric Chassande-Mottin, CNRS (France) <ecm@apc.univ-paris7.fr>
%
% 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; see the file COPYING. If not, see
% <https://www.gnu.org/licenses/>.

% -*- texinfo -*-
% @deftypefn  {Function File} {[@var{y}, @var{h}] =} resample (@var{x}, @var{p}, @var{q})
% @deftypefnx {Function File} {@var{y} =} resample (@var{x}, @var{p}, @var{q}, @var{h})
% Change the sample rate of @var{x} by a factor of @var{p}/@var{q}.  This is
% performed using a polyphase algorithm.  The impulse response @var{h} of
% the antialiasing filter is either specified or either designed with a
% Kaiser-windowed sinecard.
%
% Ref [1] J. G. Proakis and D. G. Manolakis,
% Digital Signal Processing: Principles, Algorithms, and Applications,
% 4th ed., Prentice Hall, 2007. Chap. 6
%
% Ref [2] A. V. Oppenheim, R. W. Schafer and J. R. Buck,
% Discrete-time signal processing, Signal processing series,
% Prentice-Hall, 1999
% @end deftypefn

function  [y, h] = resample( x, p, q, h )

if nargin<3
  error('at least 3 inputs are needed');
end

if any([p q]<=0) || any([p q]~=floor([p q]))
  error('resample.m: p and q must be positive integers');
end

% simplify decimation and interpolation factors

great_common_divisor=gcd(p,q);
if (great_common_divisor>1)
  p = double (p) / double (great_common_divisor);
  q = double (q) / double (great_common_divisor);
else
  p = double (p);
  q = double (q);
end

% filter design if required

if (nargin < 4)

  % properties of the antialiasing filter

  log10_rejection = -3.0;
  stopband_cutoff_f = 1 / (2 * max (p, q));
  roll_off_width = stopband_cutoff_f / 10.0;

  % determine filter length
  % use empirical formula from [2] Chap 7, Eq. (7.63) p 476

  rejection_dB = -20.0*log10_rejection;
  L = ceil((rejection_dB-8.0) / (28.714 * roll_off_width));

  % ideal sinc filter

  t=(-L:L)';

  % JMS: The matlab implementation uses a firls filter
  %ideal_filter=2*p*stopband_cutoff_f*sinc(2*stopband_cutoff_f*t);
  Lx = 2*10*max(p,q) + 1; % this is according to the matlab default
  ideal_filter=firls( Lx-1, [0 2*stopband_cutoff_f 2*stopband_cutoff_f 1], [1 1 0 0]);

  % determine parameter of Kaiser window
  % use empirical formula from [2] Chap 7, Eq. (7.62) p 474

  if ((rejection_dB>=21) && (rejection_dB<=50))
    beta = 0.5842 * (rejection_dB-21.0)^0.4 + 0.07886 * (rejection_dB-21.0);
  elseif (rejection_dB>50)
    beta = 0.1102 * (rejection_dB-8.7);
  else
    beta = 0.0;
  end

  % JMS: beta = 5 by default in the matlab version, this overrules the
  % above
  beta = 5;

  % apodize ideal filter response

  %h=kaiser(2*L+1,beta).*ideal_filter;
  h=kaiser(Lx,beta).*ideal_filter(:);

end

% check if input is a row vector
isrowvector=false;
if size(x,1)==1 && size(x,2)>1
  x=x(:);
  isrowvector=true;
end

% check if filter is a vector
if ~isvector(h)
  error('resample.m: the filter h should be a vector');
end

Lx = size(x,1);
Lh = length(h);
L = ( Lh - 1 )/2.0;
Ly = ceil(Lx*p/q);

% pre and postpad filter response

nz_pre = floor(q-mod(L,q));
hpad = [zeros(nz_pre,1);h];

offset = floor((L+nz_pre)/q);
nz_post = 0;
while ceil( ( (Lx-1)*p + nz_pre + Lh + nz_post )/q ) - offset < Ly
  nz_post = nz_post+1;
end
hpad = [hpad; zeros(nz_post,1)];

% filtering
if p==1 && q==1
  % no resampling needed.
  y = x;
else
  xfilt = upfirdn(x,hpad,p,q);
  y = xfilt(offset+1:offset+Ly,:);
end

if isrowvector
  y=y.';
end

%!test
%! N=512;
%! p=3; q=5;
%! r=p/q;
%! NN=ceil(r*N);
%! t=0:N-1;
%! tt=0:NN-1;
%! err=zeros(N/2,1);
%! for n = 0:N/2-1,
%!   phi0=2*pi*rand;
%!   f0=n/N;
%!   x=sin(2*pi*f0*t' + phi0);
%!   [y,h]=resample(x,p,q);
%!   xx=sin(2*pi*f0/r*tt' + phi0);
%!   t0=ceil((length(h)-1)/2/q);
%!   idx=t0+1:NN-t0;
%!   err(n+1)=max(abs(y(idx)-xx(idx)));
%! endfor;
%! rolloff=.1;
%! rejection=10^-3;
%! idx_inband=1:ceil((1-rolloff/2)*r*N/2)-1;
%! assert(max(err(idx_inband))<rejection);

%!test
%! N=512;
%! p=3; q=5;
%! r=p/q;
%! NN=ceil(r*N);
%! t=0:N-1;
%! tt=0:NN-1;
%! reject=zeros(N/2,1);
%! for n = 0:N/2-1,
%!   phi0=2*pi*rand;
%!   f0=n/N;
%!   x=sin(2*pi*f0*t' + phi0);
%!   [y,h]=resample(x,p,q);
%!   xx=sin(2*pi*f0/r*tt' + phi0);
%!   t0=ceil((length(h)-1)/2/q);
%!   idx=t0+1:NN-t0;
%!   reject(n+1)=max(abs(y(idx)));
%! endfor;
%! rolloff=.1;
%! rejection=10^-3;
%! idx_stopband=ceil((1+rolloff/2)*r*N/2)+1:N/2;
%! assert(max(reject(idx_stopband))<=rejection);

%!test
%! N=1024;
%! p=2; q=7;
%! r=p/q;
%! NN=ceil(r*N);
%! t=0:N-1;
%! tt=0:NN-1;
%! err=zeros(N/2,1);
%! for n = 0:N/2-1,
%!   phi0=2*pi*rand;
%!   f0=n/N;
%!   x=sin(2*pi*f0*t' + phi0);
%!   [y,h]=resample(x,p,q);
%!   xx=sin(2*pi*f0/r*tt' + phi0);
%!   t0=ceil((length(h)-1)/2/q);
%!   idx=t0+1:NN-t0;
%!   err(n+1)=max(abs(y(idx)-xx(idx)));
%! endfor;
%! rolloff=.1;
%! rejection=10^-3;
%! idx_inband=1:ceil((1-rolloff/2)*r*N/2)-1;
%! assert(max(err(idx_inband))<rejection);

%% Test integer-type arguments
%!test
%! N = 512;
%! f = 0.1;
%! x = sin (2*pi*f*[0:N-1]);
%! y1 = resample (x, 3, 2);
%! y2 = resample (x, uint8 (3), 2);
%! assert (y1, y2);


function [SO3G, W, M2] = quadratureSO3Grid(bandwidth, varargin)
% Calculate the nodes and weights of specific quadrature grids on SO(3)
%   'ClenshawCurtis'  - combinate the Gauss quadrature in 1st and 3rd Euler angle
%                       (alpha, gamma) with Clenshaw Curtis quadrature in 2nd
%                       Euler angle (beta)
%
% Syntax
%   [SO3G, W, M2] = quadratureSO3rid(M, 'ClenshawCurtis') quadrature grid of type Clenshaw Curtis
%

% persistent SO3G_p;
% persistent W_p;
% persistent M2_p;

if check_option(varargin, 'gauss') || check_option(varargin, 'chebyshev')
  warning('Quadrature of types gauss and chebychev are not implemented yet.')
  return
end

% if check_option(varargin, 'ClenshawCurtis')
%   M2 = bandwidth;                           % bandwidth
%   % grid has to be full sized
%   N =  (2*bandwidth+2)^2*(2*bandwidth+1);   % number of nodes
% end
%
% if ~isempty(M2_p) && M2_p == M2 && length(SO3G_p) == N
%   SO3G = SO3G_p;
%   if check_option(varargin,'Euler') && isa(SO3G,'rotation')
%     SO3G = Euler(SO3G);
%   elseif ~check_option(varargin,'Euler') && isnumeric(SO3G)
%     [CS,SS] = extractSym(varargin);
%     SO3G = orientation.byEuler(SO3G(:,:,:,1),SO3G(:,:,:,2),SO3G(:,:,:,3),'nfft',CS,SS);
%   end
%   W = W_p;
%   M2 = M2_p;
%
% else
  if check_option(varargin,'ClenshawCurtis')
    SO3G = regularSO3Grid('ClenshawCurtis','bandwidth',bandwidth,varargin{:});

    w = fclencurt2(bandwidth+1);
    if size(SO3G,2) == 1+bandwidth/2
      w = w(1:size(SO3G,2));
    end
    % W = normalized volume * 2xGauß-quadrature-weights * Clenshaw-Curtis-quadrature-weights
    % W =     1/sqrt(8*pi^2)    *      (2*pi/(2*N+2))^2     *              w_b^N
    W = 1/(2*size(SO3G,1)*size(SO3G,3))*repmat(w',size(SO3G,1),1,size(SO3G,3));
  end
%   % store the data
%   SO3G_p = SO3G;
%   W_p = W;
%   M2_p = M2;
% end

end

function w = fclencurt2(n)

  c = zeros(n,2);
  c(1:2:n,1) = (2./[1 1-(2:2:n-1).^2 ])';
  c(2,2)=1;
  f = real(ifft([c(1:n,:);c(n-1:-1:2,:)]));
  w = 2*([f(1,1); 2*f(2:n-1,1); f(n,1)])/2;
  %x = 0.5*((b+a)+n*bma*f(1:n1,2));

end


function[C] = localCorrections(x,y,a,G,rq,tol,gradOpt)
% C = localCorrections(x,y,a,G,rq,rMax,tol)
% Local correction matrix for the EBD.
% inputs :
% - x and y : arrays of size N1x2 and N2x2 (clouds of points) inside the
% unit disk.
% - a : the cutoff parameter of the EBD method
% - G : the kernel
% - rq : the radial quadrature that has been computed for G
% - rMax : such that initially, the data was X = rMax*x, Y = rMax*y (before
% rescaling).
% - tol : required tolerance.
% output : the sparse correction matrix C.

N1 = size(x,1);
N2 = size(y,1);
[I,rxyTemp] = rangesearch(y,x,a*1.05);
jdx = cell2mat(I')';
rxy = cell2mat(rxyTemp')';
idx = zeros(size(jdx));
sp_ind = 1;
for x_ind=1:length(I)
    idx(sp_ind:(sp_ind+length(I{x_ind})-1)) = x_ind;
    sp_ind = sp_ind + length(I{x_ind});
end
% Save memory
clear I;
NCI = length(rxy);

if NCI ~= 0

    if gradOpt
        exact_Interactions = G.evalDer(rxy); % Exact local interactions
        C1 = sum(abs(rq.alpha0.*Cp(rq.rho)).*rq.rho.^3); % Bound for the second derivative.
        Ninterp = fix(sqrt(C1)*a/sqrt(8*tol))+10; % Guarantees interpolation error < tol.
        xinterp = linspace(0,a*1.1,Ninterp);
        yinterp = rq.evalDer(xinterp);
        radial_quadratureNear0 = interp1(xinterp,yinterp,rxy); % we remove the radial
    else
        exact_Interactions = G.eval(rxy); % Exact local interactions
        C1 = sum(abs(rq.alpha0.*Cp(rq.rho)).*rq.rho.^2); % Bound for the second derivative.
        Ninterp = fix(sqrt(C1)*a/sqrt(8*tol))+10; % Guarantees interpolation error < tol.
        xinterp = linspace(0,a*1.1,Ninterp);
        yinterp = rq.eval(xinterp);
        radial_quadratureNear0 = interp1(xinterp,yinterp,rxy); % we remove the radial
    end

    % quadrature contribution (using interpolation to avoid evaluating at
    % all points).

    C_val = exact_Interactions - radial_quadratureNear0;
    C = sparse(idx,jdx,C_val,N1,N2);

    if gradOpt
        Tx = sparse(idx,jdx,y(jdx,1) - x(idx,1),N1,N2);
        Ty = sparse(idx,jdx,y(jdx,2) - x(idx,2),N1,N2);
        idx = idx(rxy> 1e-12);
        jdx = jdx(rxy> 1e-12);
        rxy = rxy(rxy> 1e-12);
        R =  sparse(idx,jdx,1./rxy,N1,N2);
        Cx = C.*R.*Tx;
        Cy = C.*R.*Ty;
        C = {Cx,Cy};
    end
else
    % No close interactions
    if gradOpt
        C = {sparse(N1,N2),sparse(N1,N2)};
    else
        C = sparse(N1,N2); % all zeros
    end
end


% $Id: realign_synth_pca.m 2 2009-06-16 19:24:10Z gramfort $
% $LastChangedBy: gramfort $
% $LastChangedDate: 2009-06-16 15:24:10 -0400 (Mar, 16 jui 2009) $
% $Revision: 2 $

%% build synthetic data

% synthetic data
randn('seed',0);
rand('seed',0);

clear EEG

EEG.pnts = 500;
EEG.srate = 1000; % 0.5 sec. recording
EEG.trials = 100;
EEG.nbchan = 1;

EEG.times = linspace(0,EEG.pnts / EEG.srate,EEG.pnts); % tf*100

noiselevel = 5;
[EEG,ref,latencies,snr] = generate_synth_eeg_data(EEG,noiselevel);

X = squeeze(EEG.data)';
X = X - ones(size(X,1),1)*mean(X,1);
norms = sqrt(sum(X.^2,2));

X = diag(1 ./ norms) * X;
[PC, SCORE, LATENT]=princomp(X);

[tmp,order] = sort(SCORE(:,1));

data = squeeze(EEG.data)';

%% view results
figure(400)
plot(SCORE(:,1),SCORE(:,2),'+');
xlabel('X');
ylabel('Y');
% title('Points in 2D with PCA');
% savefig('pca_embedding')

figure(401)
imagesc(data,[min(data(:)),max(data(:))])
colorbar
xlabel('Time (ms)')
ylabel('Trial')
title('Data')
set(gca,'XTick',[])

figure(402)
imagesc(data(order,:),[min(data(:)),max(data(:))])
colorbar
xlabel('Time (ms)')
ylabel('Trial')
% title('Data Ordered with PCA')
% set(gcf,'WindowButtonDownFcn','callbackRaster(EEG)'); % set the callback
set(gca,'XTick',[])
% savefig('pca_realigned')


function [ p2, dp2, p1 ] = jacobi_ss_recur ( x, n, alpha, beta, b, c )

%*****************************************************************************80
%
%% JACOBI_SS_RECUR finds the value and derivative of a Jacobi polynomial.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 October 2005
%
%  Author:
%
%    Original FORTRAN77 version by Arthur Stroud, Don Secrest.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%  Parameters:
%
%    Input, real X, the point at which polynomials are evaluated.
%
%    Input, integer N, the order of the polynomial to be computed.
%
%    Input, real ALPHA, BETA, the exponents of (1-X) and
%    (1+X) in the quadrature rule.
%
%    Input, real B(N), C(N), the recursion coefficients.
%
%    Output, real P2, the value of J(N)(X).
%
%    Output, real DP2, the value of J'(N)(X).
%
%    Output, real P1, the value of J(N-1)(X).
%
  p1 = 1.0;
  dp1 = 0.0;

  p2 = x + ( alpha - beta ) / ( alpha + beta + 2.0 );
  dp2 = 1.0;

  for i = 2 : n

    p0 = p1;
    dp0 = dp1;

    p1 = p2;
    dp1 = dp2;

    p2 = ( x - b(i) ) * p1 - c(i) * p0;
    dp2 = ( x - b(i) ) * dp1 + p1 - c(i) * dp0;

  end

  return
end


function [L, Nc] = discretize(S, N)
%DISCRETIZE   Discretize a SPINOP.
%   [L, NC] = DISCRETIZE(S, N) uses a Fourier spectral method in coefficient
%   space to discretize the SPINOP S with N grid points. L is the linear part, a
%   NxN diagonal matrix stored as a Nx1 vector, and NC is the differentiation
%   term of the nonlinear part (and hence is linear).
%
% Remark: DISCRETIZE will fail to discretize SPINOP objects which are not of the
%         right form. See HELP/SPINOP.
%
% See also SPINOP.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

%% Set-up:

% Get the domain DOM, the linear part FUNCL, the nonlinear part FUNCN, and the
% number of variables NVARS:
dom = S.domain;
funcL = S.lin;
funcNc = S.nonlinearPartCoeffs;
nVars = nargin(funcL);

% Get the variables of the workspace:
func = functions(funcL);
wrk = func.workspace{1};
names = fieldnames(wrk);
if ( isempty(names) == 0 )
    lengthNames = size(names, 1);
    for k = 1:lengthNames
        eval(sprintf('%s = wrk.(names{k});', names{k}));
    end
end

% Create a CHEBOPPREF object with TRIGSPEC discretization:
pref = cheboppref();
pref.discretization = @trigspec;

%% Discretize the linear part:

% Add the dependent variable 'x':
strL = func2str(funcL);
strL = strrep(strL, '@(', '@(x,');
funcL = eval(strL);

% USE CHEBOP, LINOP and MATRIX with TRIGSPEC discretization:
matL = matrix(linop(chebop(funcL, dom)), N, pref);
L = [];
for l = 1:nVars
    idx = (l-1)*N + 1;
    temp = full(diag(matL(idx:idx+N-1,idx:idx+N-1)));
    if ( mod(N, 2) == 0 )
        temp = fftshift(temp);
    else
        temp = ifftshift(temp);
    end
    L = [L; temp]; %#ok<*AGROW>
end

%% Disretize the differentiation term of the nonlinear part:

% If NC if a FUNCTION HANDLE, discretize it with TRIGSPEC:
if ( isa(funcNc, 'function_handle') == 1 )

    % Add the dependent variable 'x':
    strNc = func2str(funcNc);
    strNc = strrep(strNc, '@(', '@(x,');
    funcNc = str2func(strNc);

    % USE CHEBOP, LINOP and MATRIX with TRIGSPEC discretization:
    matNc = matrix(linop(chebop(funcNc, dom)), N, pref);
    Nc = [];
    for l = 1:nVars
        idx = (l-1)*N + 1;
        temp = full(diag(matNc(idx:idx+N-1,idx:idx+N-1)));
        if ( mod(N, 2) == 0 )
            temp = fftshift(temp);
        else
            temp = ifftshift(temp);
        end
        Nc = [Nc; temp]; %#ok<*AGROW>
    end

% NC might be 1 and in that case, nothing to do:
elseif ( isa(funcNc, 'double') == 1 )
    Nc = funcNc;
end

end


% SIMPDEMO
% This program solves the simple two dimensional problem in Chapter 2,
% and makes figure 2.1.
%
tol=[1.d-6,1.d-6];
%
% Create the mesh for the contour plot of \| f \|.
%
vl=.1:.5:2; vr=2:4:40; v=[vl,vr];
v=.5:4:40;
v=[.25,.5:2:40];
xr=-5:.2:5; n=length(xr); z=zeros(n,n);
for i=1:n
for j=1:n
    w=[xr(i),xr(j)]';
    z(i,j)=norm(simple(w));
end
end
%
% Newton's method
%
params=[40, 1, 0,0];
%
% x0 is a good initial iterate.
%
x0=[2,.5]';
[sn, errsn, ierrn, x_hist]=nsold(x0, 'simple', tol, params);
%
% x1 is a poor initial iterate. The iteration from x1 will stagnate
%    at a point where $F'$ is singular.
%
x1=[3,5]';
[sn2, errsn2, ierrn2, x_hist2]=nsold(x1, 'simple', tol, params);
%
% Draw a contour plot of $\| f \|$.
%
figure(1)
contour(xr,xr,z,v)
hold
%
% Use the x_hist array to plot the iterations on the coutour plot.
%
plot(x_hist(1,:),x_hist(2,:),'-*', x_hist2(1,:),x_hist2(2,:),'-o');
legend('Convergence','Stagnation');
xlabel('x_1');
ylabel('x_2');
axis([0 5 -5 5])
hold
%
% Default
%
params=[40, 1000, .5, 0];
[sc, errsd, ierrd]=nsold(x0, 'simple', tol, params);
inewt=length(errsn(:,1)); cnewt=0:inewt-1;
idefault=length(errsd(:,1)); cdefault=0:idefault-1;
figure(2)
semilogy(cnewt,errsn(:,1),'-',cdefault,errsd(:,1),'--')
legend('Newton','default')


function [] = LogScale( whichaxis,logbase,varargin)
%LogScale(whichaxis,logbase) renames the tick labels on your axes for a
%logarithmically scaled variable.
%
%INPUT
%   whichaxis   which axis did you log scale (can be 'x' 'y' or 'xy')
%   logbase     base of the logarithm you used for your points (ex 10)
%
%EXAMPLE
%   plot(log10(Xvals),Yvals)
%   LogScale('x',10)
%
%DLevenstein 2015
%%
p = inputParser;
addParameter(p,'exp',false)
addParameter(p,'nohalf',false)
parse(p,varargin{:})
expo = p.Results.exp;
nohalf = p.Results.nohalf;

%%
if strcmp(whichaxis,'y') || strcmp(whichaxis,'xy')
    range = get(gca,'YLim');
    switch nohalf
        case true
            range(1) = ceil(range(1)); range(2) = floor(range(2));
        case false
            range(1) = ceil(range(1).*2)./2; range(2) = floor(range(2).*2)./2;
    end
    ticks = [range(1):range(2)];
    if length(ticks)<=3 & ~nohalf
        ticks = [range(1):0.5:range(2)];
    end

    if length(ticks)>5
        ticks = ticks(1:2:end);
    end

    set(gca,'YTick',ticks)
    set(gca,'YTickLabels',round(logbase.^ticks,2,'significant'))

    if expo && logbase==10
        tickstrings = cellfun(@num2str,(num2cell(ticks)),'uniformoutput',false);
        tickstrings = cellfun(@(X) replace(X,'','^'),tickstrings,'uniformoutput',false);
        tickstrings = cellfun(@(X) [num2str(logbase),X(1:end-1)],tickstrings,'uniformoutput',false);
        set(gca,'YTickLabels',tickstrings)
    end

end

if strcmp(whichaxis,'x') || strcmp(whichaxis,'xy')
    range = get(gca,'XLim');
    switch nohalf
        case true
            range(1) = ceil(range(1)); range(2) = floor(range(2));
        case false
            range(1) = ceil(range(1).*2)./2; range(2) = floor(range(2).*2)./2;
    end
    ticks = [range(1):range(2)];
    if length(ticks)<=3 & ~nohalf
        ticks = [range(1):0.5:range(2)];
    end

    if length(ticks)>5
        ticks = ticks(1:2:end);
    end

    set(gca,'XTick',ticks)
    set(gca,'XTickLabels',round(logbase.^ticks,2,'significant'))

    if expo && logbase==10
        tickstrings = cellfun(@num2str,(num2cell(ticks)),'uniformoutput',false);
        tickstrings = cellfun(@(X) replace(X,'','^'),tickstrings,'uniformoutput',false);
        tickstrings = cellfun(@(X) [num2str(logbase),X(1:end-1)],tickstrings,'uniformoutput',false);
        set(gca,'XTickLabels',tickstrings)
    end
end

if strcmp(whichaxis,'z')
    range = get(gca,'ZLim');
    switch nohalf
        case true
            range(1) = ceil(range(1)); range(2) = floor(range(2));
        case false
            range(1) = ceil(range(1).*2)./2; range(2) = floor(range(2).*2)./2;
    end
    ticks = [range(1):range(2)];
    if length(ticks)<=3 & ~nohalf
        ticks = [range(1):0.5:range(2)];
    end

    if length(ticks)>5
        ticks = ticks(1:2:end);
    end

    set(gca,'ZTick',ticks)
    set(gca,'ZTickLabels',round(logbase.^ticks,2,'significant'))

    if expo
        tickstrings = cellfun(@num2str,(num2cell(ticks)),'uniformoutput',false);
        tickstrings = cellfun(@(X) replace(X,'','^'),tickstrings,'uniformoutput',false);
        tickstrings = cellfun(@(X) [num2str(logbase),X(1:end-1)],tickstrings,'uniformoutput',false);
        set(gca,'ZTickLabels',tickstrings)
    end
end

if strcmp(whichaxis,'c')
    range = get(gca,'CLim');
    switch nohalf
        case true
            range(1) = ceil(range(1)); range(2) = floor(range(2));
        case false
            range(1) = ceil(range(1).*2)./2; range(2) = floor(range(2).*2)./2;
    end
    ticks = [range(1):range(2)];
    if length(ticks)<=3 & ~nohalf
        ticks = [range(1):0.5:range(2)];
    end

    if length(ticks)>=5
        ticks = ticks(1:2:end);
    end

    cb = get(gca,'colorbar');

    set(cb,'Ticks',ticks)
    set(cb,'TickLabels',round(logbase.^ticks,1,'significant'))

    if expo
        tickstrings = cellfun(@num2str,(num2cell(ticks)),'uniformoutput',false);
        tickstrings = cellfun(@(X) replace(X,'','^'),tickstrings,'uniformoutput',false);
        tickstrings = cellfun(@(X) [num2str(logbase),X(1:end-1)],tickstrings,'uniformoutput',false);
        set(cb,'TickLabels',tickstrings)
    end

end

end


function pass = test_ivp_chebmatrix_syntax
% Test CHEBMATRIX syntax for IVPs, using full Brusselator IVP:

%% Setup
dom = [0 5];
tol = 1e-14;

%% With CHEBOP, multiple variable syntax for reference
N = chebop(@(t,u,v,w) [
    diff(u)-1-u^2*v + (w+1)*u;
    diff(v) - u*w+u^2*v;
    diff(w) - 1.5 + w*u], dom);
%% Test with left and right BCs:
N.lbc = [1;3;4];
uL = N\0;
N.lbc = [];
pass(1) = norm(feval(uL,dom(1)) - [1;3;4]) < tol;
%%
N.lbc = [];
N.rbc = [.7; 0.9; 1.2];
uR = N\0;
pass(2) = norm(feval(uR,dom(end)) - [.7; 0.9; 1.2]) < tol;
%% CHEBMATRIX syntax

% Define a new CHEBOP
M = chebop(@(t,u) [
    diff(u{1})-1-u{1}^2*u{2} + (u{3}+1)*u{1};
    diff(u{2}) - u{1}*u{3}+u{1}^2*u{2};
    diff(u{3}) - 1.5 + u{3}*u{1}], dom);

%% CHEBMATRIX left boundary conditions
M.lbc = @(u)[u{1}-1; u{2}-3; u{3}-4];
vL1 = M\0;
% Results should be identical to above
pass(3) = norm(vL1 - uL) == 0;

%% Vector LBC
M.lbc = [1; 3; 4];
vL2 = M\0;
pass(4) = norm(vL2 - uL) == 0;

%% CHEBMATRIX right boundary conditions
M.lbc = [];
M.rbc = @(u)[u{1}-.7; u{2}-0.9; u{3}-1.2];
vR1 = M\0;
% Results should be identical to above
pass(5) = norm(vR1 - uR) == 0;

%% Vector RBC
M.rbc = [.7; .9; 1.2];
vR2 = M\0;
pass(6) = norm(vR2 - uR) == 0;

end


% using MDL and AIC to find source number, and MUSIC to find DOAs
clear(); close all;

wavelength = 1; % normalized
d = wavelength / 2;
design_ula = design_array_1d('ula', 12, d);
doas = linspace(-pi/8, pi/3, 8);
power_source = 1;
power_noise = 1;
snapshot_count = 1000;
source_count = length(doas);

% stochastic (unconditional) model
[~, R] = snapshot_gen_sto(design_ula, doas, wavelength, snapshot_count, power_noise, power_source);

% source number detection
[~, l] = eig(0.5*(R+R'), 'vector');
l = flipud(l);
n_mdl = sn_mdl(l, design_ula.element_count, snapshot_count);
n_aic = sn_aic(l, design_ula.element_count, snapshot_count);
fprintf('There are %d sources.\n', source_count);
fprintf('# of sources estimated by MDL = %d\n', n_mdl);
fprintf('# of sources estimated by AIC = %d\n', n_aic);

% normal MUSIC
tic;
sp_normal = music_1d(R, source_count, design_ula, wavelength, 180, 'RefineEstimates', true);
toc;
sp_normal.true_positions = doas;
fprintf('[MUSIC] Estimated DOAs:\n');
disp(sp_normal.x_est);
plot_sp(sp_normal, 'title', 'Normal MUSIC', 'PlotType', 'Polar');

% root MUSIC
tic;
sp_root = rmusic_1d(R, source_count, 2*pi*design_ula.element_spacing/wavelength);
toc;
sp_root.true_positions = doas;
fprintf('[Root-MUSIC] Estimated DOAs:\n');
disp(sp_root.x_est);
plot_sp(sp_root, 'Title', 'Root-MUSIC');


function area = voronoi_areas_direct ( d_num, d_xyz, face_num, face_d, v_xyz )

%*****************************************************************************80
%
%% VORONOI_AREAS_DIRECT computes the area of each polygon in a Voronoi diagram.
%
%  Discussion:
%
%    This algorithm works directly from the Delaunay triangulation and the
%    Voronoi vertex locations, without having to determine the explicit
%    description of each Voronoi polygon as a list of Voronoi vertices.
%    This means this routine should be significantly faster than
%    VORONOI_AREAS, at least for larger problems.
%
%
%    The Delaunay triangle has vertices D1, D2, D3, and a Voronoi vertex
%    V which we imagine is inside the triangle, although in fact it need
%    not be.
%
%    Include now the Delaunay vertex averages, D12, D23, and D31, and
%    the Delaunay triangle can be divided into 3 pieces, contributing to
%    the Voronoi polygons associated with D1, D2 and D3 as follows:
%
%      D1 gets D1:V:D31 + D1:D12:V
%      D2 gets D2:V:D12 + D2:D23:V
%      D3 gets D3:V:D23 + D3:D31:V
%
%    The advantage of computing things this way is that it can be done
%    directly from the Delaunay triangle information.  If we must first
%    determine the explicit list of Voronoi vertices that form a particular
%    Voronoi polygon, then compute the area, it is cumbersome and expensive
%    to determine this connectivity information.
%
%    Presumably, if things have been done correctly, the cases where
%    the Voronoi vertex is outside of the Delaunay triangle will correspond
%    to situations where some of the areas come out negative, and
%    sum to the correct final result.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 May 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer D_NUM, the number of data points and Voronoi polygons.
%
%    Input, real D_XYZ(3,D_NUM), the coodinates of the data points.
%
%    Input, integer FACE_NUM, the number of faces, and Voronoi vertices.
%
%    Input, integer FACE_D(3,FACE_NUM), the data points that form
%    each triangle.
%
%    Input, real V_XYZ(3,FACE_NUM), the coordinates of the Voronoi vertices.
%
%    Output, real AREA(D_NUM), the area of each Voronoi polygon.
%
  area = zeros ( d_num, 1 );

  r = 1.0;
%
%  Consider each triangle.
%
  for face = 1 : face_num
%
%  Define some things.
%
    p1 = face_d(1,face);
    p2 = face_d(2,face);
    p3 = face_d(3,face);

    d1 = d_xyz ( 1:3, p1 );
    d2 = d_xyz ( 1:3, p2 );
    d3 = d_xyz ( 1:3, p3 );

    d12 = 0.5 * ( d1 + d2 );
    d23 = 0.5 * ( d2 + d3 );
    d31 = 0.5 * ( d3 + d1 );
    v = v_xyz(1:3,face);
%
%  Force all points to lie on the sphere.
%
    d12 = d12 / norm ( d12 );
    d23 = d23 / norm ( d23 );
    d31 = d31 / norm ( d31 );
    v = v / norm ( v );
%
%  Compute orientation and area of the 6 subtriangles.
%
    o1a = stri_vertices_to_orientation ( d1, v, d31 );
    a1a = stri_vertices_to_area ( r, d1, v, d31 );
    o1b = stri_vertices_to_orientation ( d1, d12, v );
    a1b = stri_vertices_to_area ( r, d1, d12, v );

    o2a = stri_vertices_to_orientation ( d2, v, d12 );
    a2a = stri_vertices_to_area ( r, d2, v, d12 );
    o2b = stri_vertices_to_orientation ( d2, d23, v );
    a2b = stri_vertices_to_area ( r, d2, d23, v );

    o3a = stri_vertices_to_orientation ( d3, v, d23 );
    a3a = stri_vertices_to_area ( r, d3, v, d23 );
    o3b = stri_vertices_to_orientation ( d3, d31, v );
    a3b = stri_vertices_to_area ( r, d3, d31, v );
%
%  Contribute to the Voronoi areas.
%
    area(p1) = area(p1) + o1a * a1a + o1b * a1b;
    area(p2) = area(p2) + o2a * a2a + o2b * a2b;
    area(p3) = area(p3) + o3a * a3a + o3b * a3b;

  end

  return
end


function out = ggrnd(n,p,g)
% GG - random numbers from generalized Gaussian distribution (requires
% Statistics toolbox)
%   R = GGRND(N,P,G) returns a vector of N random numbers having a
%   generalized gaussian distribution with shape parameter P, and scale
%   parameter G, i.e. p(x) \propto exp(-G*|x|^P).
%
%   If P=2 it GGRND returns normally distributed random numbers
%
%   Reference: Johnson, M. E., Computer generation of the exponential power
%   distributions", Journal of Statistical Computation and Simulation, 9,
%   pp. 239--240, 1979
%
%   See also: RANDN, GAMRND

%   Author: G. Gomez-Herrero, german.gomezherrero@ieee.org
%   $Revision: 1.0 $Date: 21/06/2007

if nargin < 3, g=1; end
if abs(p-2)<1e-3,
    x = randn(1,n);
else
    y = gamrnd(1/p,1,1,n);
    x = abs(g.*y).^(1/p);
    pos = find(rand(1,n)<0.5);
    x(pos) = -x(pos);
end
% normalize variance and return
out = x'./sqrt(var(x));
return;


function geometry_test0201 ( )

%*****************************************************************************80
%
%% TEST0201 tests CYLINDER_POINT_DIST_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  dim_num = 3;
  test_num = 6;

  dist_test = [ 3.0, 0.5, 5.0, 8.0, 1.0, 0.25 ];
  p_test = [ ...
      4.0,   0.5,  0.0; ...
     -0.5,  -1.0,  0.0; ...
      4.0,   6.0,  0.0; ...
      0.75, -10.0, 0.0; ...
      0.0,   0.0,  0.0; ...
      0.25,  1.75, 0.0 ]';
  p1 = [ 0.0, -2.0, 0.0 ];
  p2 = [ 0.0,  2.0, 0.0 ];
  r = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0201\n' );
  fprintf ( 1, '  CYLINDER_POINT_DIST_3D computes the distance\n' );
  fprintf ( 1, '  to a cylinder.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Radius R = %f\n', r );
  fprintf ( 1, '  Center of bottom disk = %10f  %10f  %10f\n', p1(1:dim_num) );
  fprintf ( 1, '  Center of top disk =    %10f  %10f  %10f\n', p2(1:dim_num) );

  for test = 1 : test_num

    p(1:dim_num) = p_test(1:dim_num,test);

    fprintf ( 1, '\n' );
    fprintf ( 1, '  P =    %10f  %10f  %10f\n', p(1:dim_num) );

    dist = cylinder_point_dist_3d ( p1, p2, r, p );

    fprintf ( 1, '  DIST (computed) = %f\n', dist );
    fprintf ( 1, '  DIST (exact) =    %f\n', dist_test(test) );

  end

  return
end


function res = normc(A)
	%% ================== File info ==========================
	% Author		: Tiep Vu (http://www.personal.psu.edu/thv102/)
	% Time created	: Tue Jan 26 22:15:40 2016
	% Last modified	: Tue Jan 26 22:15:42 2016
	% Description	: normalize columns of a matrix (each column has Euclidean norm = 1)
	%		This is a built-in function in some new version of MATLAB
	%% ================== end File info ==========================
	B = A.^2;
	C = sqrt(sum(B));
	res = A./repmat(C, size(A,1), 1);
end


function [ errors ] = test_gsp_solve_l1(  )

% Testin of gsp_solve_l1
gsp_reset_seed(0)
errors = 0;

N = 100;
Nf = 5;
G = gsp_sensor(N);
%G = gsp_2dgrid(sqrt(N));
G = gsp_compute_fourier_basis(G);

g = gsp_design_itersine(G,Nf);

s = gsp_filter_analysis(G,g{1},randn(N,1));
[alpha1, info1]  = gsp_solve_l1(G, g, s, 0 );

sp = gsp_filter_synthesis(G,g,alpha1);

if norm(sp-s)/norm(s)<1e-8
    fprintf('Test SOLVE_L1 OK\n')
else
    errors = errors +1;
    fprintf('Test SOLVE_L1 PAS OK\n')
    norm(sp-s)/norm(s)
end

[alpha2, info2]  = gsp_solve_l1(G, g, s, 0.01 );
sp = gsp_filter_synthesis(G,g,alpha2);
if norm(alpha2,1)<norm(alpha1,1)
    fprintf('Test SOLVE_L1 2 OK\n')
else
    errors = errors +1;
    fprintf('Test SOLVE_L1 2 PAS OK\n')
    norm(alpha2,1)
    norm(alpha1,1)
end

sp2 = gsp_filter_synthesis(G,g,alpha2);
if norm(sp2-s)/norm(s)<0.05
    fprintf('Test SOLVE_L1 3 OK\n')
else
    errors = errors +1;
    fprintf('Test SOLVE_L1 3 PAS OK\n')
    norm(sp2-s)/norm(s)
end

% old code... probably stupid
% % Testin of gsp_solve_l1
% gsp_reset_seed(0)
% errors = 0;
%
% N = 100;
% Nf = 5;
% param = struct;
% %param.distribute = 1;
% G = gsp_sensor(N,param);
% %G = gsp_2dgrid(sqrt(N));
% G = gsp_compute_fourier_basis(G);
% G = gsp_spectrum_cdf_approx(G);
% paramfilter.filter = gsp_design_meyer(G,Nf);
% g = gsp_design_warped_translates(G, Nf, paramfilter);
% %g = gsp_design_itersine(G,Nf);
%
% % figure
% % gsp_plot_filter(G,g)
%
% alphain = zeros(N,Nf);
%
% for ii = 1:4
%     alphain(ii,ii) = 1;
% end
%
% alphain = gsp_mat2vec(alphain);
% signal = gsp_filter_synthesis(G,g,alphain);
%
% % figure
% % gsp_plot_signal(G, signal);
% param.verbose = 0;
% param.maxit = 1000;
% % fig=figure(100);
% % param.do_sol=@(x) plot_objective(x,fig);
%
% param.tol = 1e-6;
% param.guess = alphain;
% lambda = 0.05;
% alphaout = gsp_solve_l1(G,g,signal,lambda,param);
%
% % param.guess = alphaout;
% % lambda2 = 0.1;
% % param.tol = 1e-10;
% % param.maxit = 200;
% % alphaout = gsp_solve_l0(G,g,signal,lambda2,param);
% % norm(alphaout-alphain)
%
% if norm(alphaout-alphain)<2
%     fprintf('Test SOLVE_L1 OK\n')
% else
%     errors = errors +1;
%     fprintf('Test SOLVE_L1 PAS OK\n')
%     norm(alphaout-alphain)
% end

end


function [swav]=s_wav(x,a_swav,d_swav,t_swav,li)
l=li;
x=x-t_swav;
a=a_swav;
b=(2*l)/d_swav;
n=100;
s1=(a/(2*b))*(2-b);
s2=0;
for i = 1:n
    harm3=(((2*b*a)/(i*i*pi*pi))*(1-cos((i*pi)/b)))*cos((i*pi*x)/l);
    s2=s2+harm3;
end
swav=-1*(s1+s2);


function r = toep_cholesky_upper ( n, g )

%*****************************************************************************80
%
%% TOEP_CHOLESKY_UPPER: upper Cholesky factor of a compressed Toeplitz matrix.
%
%  Discussion:
%
%    The Toeplitz matrix A is supplied in a compressed form G.
%
%    The Toeplitz matrix must be positive semi-definite.
%
%    After factorization, A = R' * R.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2012
%
%  Author:
%
%    Michael Stewart
%
%  Reference:
%
%    Michael Stewart,
%    Cholesky factorization of semi-definite Toeplitz matrices.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real G(2,N), the compressed Toeplitz matrix.
%    G(1,1:N) contains the first row.
%    G(2,2:N) contains the first column.
%
%    Output, real R(N,N), the upper Cholesky factor.
%
  r = zeros ( n, n );
  r(1,1:n) = g(1,1:n);
  g(1,2:n) = g(1,1:n-1);
  g(1,1) = 0.0;
  for i = 2 : n
    rho = - g(2,i) / g(1,i);
    g(:,i:n) = ( [ 1 rho; rho 1 ] * g(:,i:n) ) / sqrt ( ( 1 - rho ) * ( 1 + rho ) );
    r(i,i:n) = g(1,i:n);
    g(1,i+1:n) = g(1,i:n-1);
    g(1,i) = 0.0;
  end

  return
end


% ASTROTIK by Francesco Santilli
% Compute spherical coordinates of a given position.
%
% Usage: [r,alpha,delta] = spherical(X)
%
% where: X = position [L]
%        r = radius [L]
%        alpha = right ascension [rad]
%        delta = declination [rad]

function [r,alpha,delta] = spherical(X)

    if ~(nargin == 1)
        error('Wrong number of input arguments.')
    end

    D = check(X,1);

    if ~(D==3)
        error('Wrong size of an input argument.')
    end

    xx1 = X(1)^2;
    xx2 = X(2)^2;
    xx3 = X(3)^2;
    r = sqrt(xx1 + xx2 + xx3);
    xy = sqrt(xx1 + xx2);
    alpha = atan2(X(2),X(1));
    delta = atan(X(3)/xy);

end


function out_m = updownsample( in_m,out_x_sz,out_y_sz,is_fourier_flag,is_real_flag )
%
% updownsample - up-sample or down-sample an input series using fourier domain
%                input series needs to be continuous of a high degree
%
% format:   out_m = updownsample( in_m,out_x_sz,out_y_sz,is_fourier_flag,is_real_flag )
%
% input:    in_m                - input matrix for up/down sampling. can be in
%                                 space domain OR in fourier domain, in such
%                                 case, needs to be in matlab format !!!
%                                 (matlab-format = the save as given from fft/fft2)
%           out_x_sz,out_y_sz   - desired number of pixels in the output image
%           is_fourier_flag     - 1: the input is given in the fourier domain
%                                 0: the input is given in the space domain
%                                    (we need to use fft2 to convert to fourier domain)
%           is_real_flag        - 0: the input is a complex matrix -> don't use
%                                    abs() at the output, perform complex
%                                    up/down sampling
%                                 1: the input is real BUT has negative values ->
%                                    use real() at the output
%                                 2: the input is real and positive -> using
%                                    abs() at the output
%
% output:   out_m               - up/down sampled image
%
% NOTE: it is important to specify if the image is REAL or COMPLEX, since
%       if the image is REAL -> we have to use ABS() on the inverse fourier
%       transform (because of roundoff errors of the transform).
%
% NOTE: since a desired amount of pixels is needed at the output, there is
%       no attempt to use matrices which are in size of power of 2. this
%       optimization can not be used in this case
%
% NOTE: input series needs to be CONTINUOUS OF A HIGH DEGREE, since the
%       upsampling is done in the frequency domain, which samples the output
%       grid with SINE-like (harmonic) functions
%
%
% Example:  type "updownsample" for an example. it will load the matlab's
%           child image to a temporary variable, as upsample it
%
%               out = updownsample( A,300,300,0,1 );
%               figure;
%               colormap gray;
%               subplot( 1,2,1 );
%               imagesc( A );
%               subplot( 1,2,2 );
%               imagesc( out );
%

%
% Theory:   the upsampling is done by zero-padding in the input domain BETWEEN the samples,
%           then at the fourier domain, taking the single spectrum (out of the repetition of spectrums)
%           i.e. low pass with Fcutoff=PI/upsample_factor, zeroing the rest of the spectrum
%           and then doing ifft to the distribution.
%           since we have a zero padding operation in time, we need to multiply by the fourier gain.
%
%              +-----------+     +-------+     +---------+     +--------+     +--------+
%   y[n,m] --> | up-sample | --> |  FFT  | --> |   LPF   | --> | * Gain | --> |  IFFT  | --> interpolated
%              | factor M  |     +-------+     | Fc=PI/M |     +--------+     +--------+
%              +-----------+                   +---------+
%
%           this operation is the same as the following one (which has less operations):
%
%              +-------+     +--------+     +--------------+     +--------+
%   y[n,m] --> |  FFT  | --> | * Gain | --> | Zero Padding | --> |  IFFT  | --> interpolated
%              +-------+     +--------+     +--------------+     +--------+
%
%           NOTE THAT, the zero-padding must be such that the D.C. ferquency remains the D.C. frequency
%           and that the zero padding is applied to both positive and negative frequencies.
%           The zero padding actually condences the frequency -> which yields a longer series in the
%           image domain, but without any additional information, thus the operation must be an interpolation.

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

% ==============================================
%   check input parameters
% ==============================================
if (nargin==0)
    hFig    = figure( 'visible','off' );
    hImage  = image;
    Child   = get( hImage,'cdata' );
    close( hFig );
	out     = updownsample( Child,300,300,0,1 );
	figure;
	colormap gray;
	subplot( 1,2,1 );
	imagesc( Child );
	subplot( 1,2,2 );
	imagesc( out );
    return
elseif (nargin<5)
    error( 'UpDownSample - insufficient input parameters' );
end

% ==============================================
% get input image size, and calculate the gain
% ==============================================
[in_y_sz,in_x_sz] = size( in_m );
gain_x = out_x_sz/in_x_sz;
gain_y = out_y_sz/in_y_sz;

% ==============================================
% check if up/down sampling is needed at all
% ==============================================
if (gain_x == 1) & (gain_y==1)

    % same gain -> do not change sampling rate
    % ==========================================
    if is_fourier_flag
        switch is_real_flag
            case 0, out_m = ifft2( in_m );
            case 1, out_m = real( ifft2( in_m ) );
            case 2, out_m = abs( ifft2( in_m ) );
        end
    else
        out_m = in_m;
    end

else

    % upsample or downsample as needed
    % ==================================

    % convert to fourier domain, if input is given in the space domain
    if ~is_fourier_flag
        in_m = fft2(in_m);
    end

    % build grid vectors for the up/down sampling
    % ============================================
    % if the input is even & output is odd-> use floor for all
    % if the output is even & input is odd -> use ceil for all
    % other cases - don't care
    % for downsampling -> the opposite
    if (~mod( in_x_sz,2 ) & (out_x_sz>in_x_sz)) | (mod( in_x_sz,2 ) & (out_x_sz<in_x_sz))
        x_output_space  = max(floor((out_x_sz-in_x_sz)/2),0) + [1:min(in_x_sz,out_x_sz)];
        x_input_space   = max(floor((in_x_sz-out_x_sz)/2),0) + [1:min(in_x_sz,out_x_sz)];
    else
        x_output_space  = max(ceil((out_x_sz-in_x_sz)/2),0) + [1:min(in_x_sz,out_x_sz)];
        x_input_space   = max(ceil((in_x_sz-out_x_sz)/2),0) + [1:min(in_x_sz,out_x_sz)];
    end
    if (~mod( in_y_sz,2 ) & (out_y_sz>in_y_sz)) | (mod( in_y_sz,2 ) & (out_y_sz<in_y_sz))
       y_output_space  = max(floor((out_y_sz-in_y_sz)/2),0) + [1:min(in_y_sz,out_y_sz)];
       y_input_space   = max(floor((in_y_sz-out_y_sz)/2),0) + [1:min(in_y_sz,out_y_sz)];
   else
       y_output_space  = max(ceil((out_y_sz-in_y_sz)/2),0) + [1:min(in_y_sz,out_y_sz)];
       y_input_space   = max(ceil((in_y_sz-out_y_sz)/2),0) + [1:min(in_y_sz,out_y_sz)];
   end

    % perform the up/down sampling
    padded_out_m    = zeros( out_y_sz,out_x_sz );
    in_m            = fftshift(in_m);
    padded_out_m( y_output_space,x_output_space ) = in_m(y_input_space,x_input_space);
    out_m           = (gain_x*gain_y)*ifft2(ifftshift(padded_out_m));

    % check the output format real or complex
    switch is_real_flag
        case 0, % do nothing
        case 1, out_m   = real( out_m );
        case 2, out_m   = abs( out_m );
    end

end


function value = f2 ( x )

%*****************************************************************************80
%
%% F2 evaluates a function of a scalar used in defining P2(Q).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 November 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ian Sloan, Stephen Joe,
%    Lattice Methods for Multiple Integration,
%    Oxford, 1994,
%    ISBN: 0198534728,
%    LC: QA311.S56
%
%  Parameters:
%
%    Input, real X, the value of the argument.
%
%    Output, real VALUE, the value of F2(X).
%
  value = 1.0 + 2.0 * pi * pi * ( x * x - x + 1.0 / 6.0 );

  return
end


function c = imcmass(im)
% IMCMASS  Centre of mass of an image.
%
% IMCMASS computes the centre of mass of a grayscale image weighting the
% pixel coordinates by the pixel intensities.
%
% C = IMCMASS(IM)
%
%   IM is a grayscale 2D image.
%
%   C is the center of mass of the image in [row, col] format.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2015 University of Oxford
% Version: 0.1.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(1, 1);
nargoutchk(0, 1);

if (size(im, 3) > 1)
    error('IM must be a grayscale image')
end

% grid for image coordinates
[grow, gcol] = ndgrid(1:size(im, 1), 1:size(im, 2));

% centre of mass
c = single(im(:));
c = sum([c .* grow(:), c .* gcol(:)]) / sum(c);


function [] = bihist(x1,x2,n)
% BIHIST(X1,X2,N) open a new figure an plot a back to back histogram
% X1: variable 1 (right side, colour blue)
% X2: variable 2 (right side, colour green)
% n : number of classes for each histogramm

% make two dummy-histogram
[temp, d1] = hist(x1,n);  %#ok<ASGLU>
[temp, d2] = hist(x2,n);  %#ok<ASGLU>
clear temp
% calculate classes
d = min([d1 d2]):((max([d1 d2])-min([d1 d2]))/(n-1)) : max([d1 d2]);
% create the two histograms
templ = hist(x1,d);
tempr = hist(x2,d);
% scaling the histograms
templ=templ*-1;
% create bar plot
left=barh(templ,'style','hist');
hold on
right = barh(tempr,'style','hist');
plot([0 0], [1 n],'k')
% change color of left histogram
set(left,'FaceColor',[0 0 1]);
% change color of right histogram
set(right,'FaceColor',[0 1 0]);
% distance factor to frame
dt=1.2;
xlim([-max(abs([templ tempr]))*dt max(abs([templ tempr]))*dt])
% mark the y-axis
yl = get(gca,'YTick');
b = (max(d)-min(d))/(n-1);
a = max(d)-b*n;
yl2 = yl*b+a;
ds = cell(size(yl2));
for k=1:length(yl2)
    ds{k} = num2str(yl2(k),3);
end
set(gca,'YTickLabel',ds)

% rewrite the x-axis with positiv values
xl = get(gca,'XTickLabel');
xl = regexprep(cellstr(xl),'-','');
set(gca,'XTickLabel',regexprep(cellstr(xl),'-',''))


function b = r8blt_to_r8ge ( n, ml, a )

%*****************************************************************************80
%
%% R8BLT_TO_R8GE copies a R8BLT matrix to a R8GE matrix.
%
%  Discussion:
%
%    The R8BLT storage format is used for a banded lower triangular matrix.
%    The matrix is assumed to be zero below the ML-th subdiagonal.
%    The matrix is stored in an ML+1 by N array, in which the diagonal
%    appears in the first row, followed by successive subdiagonals.
%    Columns are preserved.
%
%  Example:
%
%    N = 5, ML = 2
%
%    A11   0   0   0   0
%    A21 A22   0   0   0
%    A31 A32 A33   0   0
%      0 A42 A43 A44   0
%      0   0 A53 A54 A55
%                --- ---
%                    ---
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer ML, the lower bandwidth.
%
%    Input, real A(ML+1,N), the R8BLT matrix.
%
%    Output, real B(N,N), the R8GE matrix.
%
  for i = 1 : n
    for j = 1 : n
       if ( j <= i & i <= j + ml )
          b(i,j) = a(i-j+1,j);
       else
          b(i,j) = 0.0;
       end
    end
  end

  return
end


function sphere_stereograph_test02 ( )

%*****************************************************************************80
%
%% SPHERE_STEREOGRAPH_TEST02 checks the generalized mapping.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 November 2010
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERE_STEREOGRAPH_TEST02\n' );
  fprintf ( 1, '  SPHERE_STEREOGRAPH standard mapping from sphere to plane.\n' );
  fprintf ( 1, '  SPHERE_STEREOGRAPH2 generalized mapping:\n' );
  fprintf ( 1, '  (focus and center are arbitrary)\n' );
  fprintf ( 1, '  Check that these two functions can agree.\n' );

  m = 3;
  n = 100;

  focus = [ 0, 0, -1 ]';
  center = [ 0, 0, 0 ]';
%
%  Check #1.
%
  seed = 123456789;
  p1 = uniform_on_sphere01_map ( m, n, seed );

  q1 = sphere_stereograph ( p1 );

  q2 = sphere_stereograph2 ( p1, focus, center );

  dif = sqrt ( sum ( ( q1 - q2 ).^2, 1 ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Map points from sphere to plane.\n' );
  fprintf ( 1, '  Maximum difference for %d points was %f\n', n, max ( dif ) );
%
%  Check #2.
%
  q1 = zeros(m,n);
  q1(1:m-1,1:n) = rand ( m - 1, n );
  q1(m,1:n) = 1.0;

  p1 = sphere_stereograph_inverse ( q1 );

  p2 = sphere_stereograph2_inverse ( q1, focus, center );

  dif = sqrt ( sum ( ( p1 - p2 ).^2, 1 ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Map points from plane to sphere.\n' );
  fprintf ( 1, '  Maximum difference for %d points was %f\n', n, max ( dif ) );

  return
end


function y = wmean(x, w)
%WMEAN  Weighted average or mean value.
%
%  Description
%    WMEAN(X,W) is the weighted mean value of the elements in X
%    (along first dimension) given weights W.
%
%  See also wprctile
%
% Copyright (c) 2000-2013 Aki Vehtari

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

y=sum(bsxfun(@times,x,w),1);


function t = transmission_exponential(d, beta, varargin)
%TRANSMISSION_EXPONENTIAL  Compute transmission map using given depth map based
%on the Lambert-Beer law
%   Inputs:
%       -|d|: H-by-W matrix with values of depth for processed image in meters.
%       -|beta|: positive constant that parameterizes the density of haze.
%       Larger values indicate thicker haze.
%
%   Outputs:
%       -|t|: H-by-W matrix with medium transmission values ranging in [0, 1].

t = exp(-beta * d);

end


function [elem,idx,volume] = fixorder3(node,elem)
%% FIXORDER3 fix orientation of tetrahedron
%
%   elem = FIXORDER3(node,elem) computes signed volume of all tetrahedron
%   in the triangulation and switch the vertices such that all signed
%   volume is positive.
%
%   [elem,idx,volume] = FIXORDER3(node,elem) also outputs the index set of
%   elements whose area is negative and the absolute value of area.
%
% See also fixorientation, fixorder3
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

% compute signed volume of each tetrahedron
volume = simplexvolume(node,elem);
% find tetrahedron with negative volume and switch the vertices
idx = find(volume<0);
elem(idx,[2 3]) = elem(idx,[3 2]);
volume(idx) = -volume(idx);


function bessel_j1_values_test ( )

%*****************************************************************************80
%
%% BESSEL_J1_VALUES_TEST demonstrates the use of BESSEL_J1_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESSEL_J1_VALUES_TEST:\n' );
  fprintf ( 1, '  BESSEL_J1_VALUES stores values of \n' );
  fprintf ( 1, '  the Bessel J1 function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X            J1(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = bessel_j1_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


% KERNEL = binomialFilter(size)
%
% Returns a vector of binomial coefficients of order (size-1) .

% Eero Simoncelli, 2/97.

function [kernel] = binomialFilter(sz)

if (sz < 2)
  error('size argument must be larger than 1');
end

kernel = [0.5 0.5]';

for n=1:sz-2
  kernel = conv([0.5 0.5]', kernel);
end


function [rho, stationary]=inverse_ar_roots(phi)
% Computes the inverted roots of the characteristic equation of an AR(P)
%
% USAGE:
%  [RHO, STATIONARY] = inverse_ar_roots(PARAMETERS)
%
% INPUTS:
%   PARAMETERS - A column vector containing the AR parameters in the order
%                  y(t) = mu + phi(1) y(t-1) + phi(2)y(t-2) + ... + phi(P) y(t-P) + e(t)
%
% OUTPUTS:
%   RHO        - A max(P) by 1 vector containing the inverted roots of the characteristic equation
%                  corresponding to the AR model input
%   STATIONARY - Logical indicating whether the AR process is stationary
%
% COMMENTS:
% Process is stationary if min(abs(rho)) > 1
%
%  See also ARMAROOTS, ACF

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

%Make sure phi is a row vector;

p=length(phi);
%Make sure phi is a row vector
if size(phi,1)>=size(phi,2) && min(size(phi))==1
    phi=phi';
else
    error('Phi should be a column vector.')
end
rho=roots([-fliplr(phi) 1]);
stationary=min(abs(rho))>1;


function tet_mesh_test005 ( )

%*****************************************************************************80
%
%% TET_MESH_TEST005 tests TETRAHEDRON_BARYCENTRIC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 August 2009
%
%  Author:
%
%    John Burkardt
%
  test1_num = 3;
  test2_num = 5;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST005\n' );
  fprintf ( 1, '  TETRAHEDRON_BARYCENTRIC computes the barycentric\n' );
  fprintf ( 1, '  coordinates of a point.\n' );
%
%  Choose a random tetrahedron.
%
  for test1 = 1 : test1_num

    [ tet_xyz, seed ] = r8mat_uniform_01 ( 3, 4, seed );

    r8mat_transpose_print ( 3, 4, tet_xyz, '  Random tetrahedron:' );
%
%  Choose barycentric coordinates C1 at random.
%
%  Define a point P.
%
%  Have TETRAHEDRON_BARYCENTRIC compute C2, the barycentric coordinates of P.
%
    for test2 = 1 : test2_num

      [ c1, seed ] = r8vec_uniform_01 ( 4, seed );
      c1_sum = sum ( c1(1:4) );
      c1(1:4) = c1(1:4) / c1_sum;

      p(1:3) = tet_xyz(1:3,1:4) * c1(1:4)';

      c2 = tetrahedron_barycentric ( tet_xyz, p );

      fprintf ( 1, '\n' );
      fprintf ( 1, '  C1 =  %14.6f  %14.6f  %14.6f  %14.6f\n', c1 );
      fprintf ( 1, '  C2 =  %14.6f  %14.6f  %14.6f  %14.6f\n', c2 );

    end
  end

  return
end