function [x, fx, exitFlag] = bisection_s(f,lb,ub,target,options)%#codegen
% BISECTION Fast and robust root-finding method that handles n-dim arrays.
%   bisection_s => "s" = single or serial, for turning into a mex file
%   [x,fVal,ExitFlag] = BISECTION(f,LB,UB,target,options) finds x +/- TolX
%   (LB < x < UB) such that f(x) = target +/- TolFun.
%
%   x = BISECTION(f,LB,UB) finds the root(s) of function f on the interval
%   [LB, UB], i.e. finds x such that f(x) = 0 where LB <= x <= UB. f will
%   never be evaluated outside of the interval specified by LB and UB. f
%   should have only one root and f(UB) and f(LB) must bound it. Elements
%   of x are NaN for instances where a solution could not be found.
%
%   x = BISECTION(f,LB,UB,target) finds x such that f(x) = target.
%
%   x = BISECTION(f,LB,UB,target,TolX) will terminate the search when the
%   search interval is smaller than TolX (TolX must be positive).
%
%   x = BISECTION(f,LB,UB,target,options) solves with the default
%   parameters replaced by values in the structure OPTIONS, an argument
%   created with the OPTIMSET function. Used options are TolX and TolFun.
%   Note that OPTIMSET will not allow arrays for tolerances, so set the
%   fields of the options structure manually for non-scalar TolX or TolFun.
%
%   [x,fVal] = BISECTION(f,...) returns the value of f evaluated at x.
%
%   [x,fVal,ExitFlag] = BISECTION(...) returns an ExitFlag that describes
%   the exit condition of BISECTION. Possible values of elements of
%   ExitFlag and the corresponding exit conditions are
%
%       1   Search interval smaller than TolX.
%       2   Function value within TolFun of target.
%       3   Search interval smaller than TolX AND function value within
%           TolFun of target.
%      -1   No solution found.
%
%   Any or all of f(scalar), f(array), LB, UB, target, TolX, or TolFun may
%   be scalar or n-dim arrays. All non-scalar arrays must be the same size.
%   All outputs will be this size.
%
%   Default values are target = 0, TolX = 1e-6, and TolFun = 0.
%
%   There is no iteration limit. This is because BISECTION (with a TolX
%   that won't introduce numerical issues) is guaranteed to converge if f
%   is a continuous function on the interval [UB, LB] and f(x)-target
%   changes sign on the interval.
%
%   The bisection method is very robust root-finding method. The absolute
%   error is halved at each step so the method converges linearly. However,
%   Brent's method (such as implemented in FZERO) can converge
%   superlinearly and is as robust. FZERO also has more features and input
%   checking, so use BISECTION in cases where either the optimization
%   toolbox is unavailable or if FZERO would have to be implemented in a
%   loop to solve multiple cases, in which case BISECTION will be much
%   faster because of vectorization.
%
%   Define LB, UB, target, TolX, and TolFun for each specific application
%   using great care for the following reasons:
%     - There is no iteration limit, so given an unsolvable task, such as
%       TolX = TolFun = 0, BISECTION remains in an unending loop.
%     - Spacing between very large floating point numbers is likely to be
%       greater than TolX.
%     - There is no initial check to make sure that f(x) - target changes
%       sign between LB and UB.
%     - Very large or very small numbers can introduce numerical issues.
%
%   Example 1: Find cube root of array 'target' without using NTHROOT and
%   compare speed to using FZERO.
%       options = optimset('TolX', 1e-9);
%       target = [(-100:.1:100)' (-1000:1:1000)'];
%
%       tic;
%       xfz = zeros(size(target));
%       for ii = 1:numel(target)
%           xfz(ii) = fzero(@(x) x.^3-target(ii), [-20 20], options);
%       end
%       fzero_time = toc
%
%       tic;
%       xbis = bisection(@(x) x.^3, -20, 20, target, options);
%       bisection_time = toc
%
%       fprintf('FZERO took %0.0f times longer than BISECTION.\n',...
%                   fzero_time/bisection_time)
%
%   Example 2: Find roots by varying the function coefficients.
%       [A, B] = meshgrid(linspace(1,2,6), linspace(4,12,10));
%       f = @(x) A.*x.^0.2 + B.*x.^0.87 - 15;
%       xstar = bisection(f,0,5);
%
%   See also FZERO, FMINBND, OPTIMSET, FUNCTION_HANDLE.
%
%   [x,fVal,ExitFlag] = BISECTION(f,LB,UB,target,options)

%   Copyright 2010-2013 Sky Sartorius
%   Author  - Sky Sartorius
%   Contact - www.mathworks.com/matlabcentral/fileexchange/authors/101715

% --- Process inputs. ---
% Set default values
tolX    = 1e-6;
tolFun  = 0;
if nargin == 5
    if isstruct(options)
        if isfield(options,'TolX') && ~isempty(options.TolX)
            tolX = options.TolX;
        end
        if isfield(options,'TolFun') && ~isempty(options.TolFun)
            tolFun = options.TolFun;
        end
    else
        tolX = options;
    end
end
if nargin<4 || isempty(target); target=0; end

ub_in = ub; lb_in = lb;
f = @(x) f(x) - target;

% --- Flip UB and LB if necessary. ---
isFlipped = lb>ub;
if any(isFlipped(:))
    ub(isFlipped) = lb_in(isFlipped);
    lb(isFlipped) = ub_in(isFlipped);
    ub_in = ub; lb_in = lb;
end

% --- Make sure everything is the same size for a non-scalar problem. ---
if isscalar(lb) && isscalar(ub)
    % Test if f returns multiple outputs for scalar input.
    if ~isscalar(target)
        ub = ub + zeros(size(target));
    else
        jnk = f(ub);
        if ~isscalar(jnk)
            ub = ub + zeros(size(jnk));
        end
    end
end

% Check if lb and/or ub need to be made into arrays.
if isscalar(lb) && ~isscalar(ub)
    lb = lb + zeros(size(ub));
elseif ~isscalar(lb) && isscalar(ub)
    ub = ub + zeros(size(lb));
end

x = zeros(size(lb));
testconvergence
% --- Iterate ---
iterations = 0;
while any(stillNotDone(:))
    bigger  = fx.*f(ub) > 0;
    ub(bigger)= x(bigger);
    lb(~bigger)= x(~bigger);

    testconvergence;
    iterations = iterations + 1;

    if(iterations > 10000)
%         error('Too many iterations in bisection!');
        break;
    end
end

    function testconvergence
        x=(ub+lb)/2;
        fx=f(x);
        outsideTolFun =  abs(fx)  > tolFun;
        outsideTolX =   (ub - lb) > tolX;
        stillNotDone = outsideTolX & outsideTolFun;
    end

% --- Check that f(x+tolX) and f(x-tolX) have opposite sign. ---
fu = f(min(x+tolX,ub_in));
fl = f(max(x-tolX,lb_in));
unboundedRoot = (fu.*fl) > 0;

% Throw out unbounded results if not meeting TolFun convergence criteria.
x(unboundedRoot & outsideTolFun) = NaN;

% --- Catch NaN elements of UB, LB, target, or other funky stuff. ---
x(isnan(fx)) = NaN;

% --- Characterize results. ---
fx = fx + target;
if nargout > 2
    exitFlag                                    = +~outsideTolX;
    exitFlag(~outsideTolFun)                    =  2;
    exitFlag(~outsideTolFun & ~outsideTolX)     =  3;
    exitFlag(isnan(x))                          = -1;
end

end

% V2: July     2010
% V3: December 2012
% don't remember when
%   typo line 39; added fn handle to see also; made array in example 2
%   smaller; changed wording in example 1
% 2013-08-23
%  -changed scalar*ones(...) calls to scalar+zeros(...) calls based on
%   http://undocumentedmatlab.com/blog/allocation-performance-take-2/
%  -rearranged help block and formatted a tiny bit


%% OPT05_RUN
%
%  Modified:
%
%    09 January 2008
%
   %---------------------------------------------------------------------
   %  Running testcase, from D+S, pp. 157-8; x_* = [.1,10].
   %  This tests the scaling implementation.
   %---------------------------------------------------------------------
   fprintf('---------------------------------------------------------\n')
   fprintf('Running testcase_5 as scalar optimization problem:\n' );
   fprintf('Exact solution (0.1, 10)\n')
   fprintf('---------------------------------------------------------\n')
   fname = 'opt05_fgh';
   options = [];
   options.verbose            = 0;
   options.method             = 'newton';
   options.globalization      = 'trust_region';
   options.step_tolerance     = 1.e-14;
   options.gradient_tolerance = 1.e-14;
   options.max_fevals         = 151;
   options.max_iterations     = 151;

   alpha = 10;
   fprintf ( 'Program scale ALPHA = %f\n', alpha );
   x0 = [-1.2/alpha, alpha];
   fprintf('Unscaled:\n')
%
% should require ~94 iterations
%
   x = entrust(fname, x0, options, alpha);

   fprintf('Newton produced  (%10.7e, %10.7e)\n\n',x(1),x(2))
   f = opt05_fgh ( x, 'f', alpha );
   fprintf('Value of F(X) = %f\n', f );

   options.scale_x            = [ 1/alpha; alpha ];
   fprintf('Scaled:\n')

   % should require ~24 iterations
   x = entrust(fname, x0, options, alpha);

   fprintf('Newton produced  (%10.7e, %10.7e)\n\n',x(1),x(2))
   f = opt05_fgh ( x, 'f', alpha );
   fprintf('Value of F(X) = %f\n', f );

   %---------------------------------------------------------------------
   %  Test Gauss-Newton strategies.
   %---------------------------------------------------------------------
   fprintf('---------------------------------------------------------\n')
   fprintf('Running testcase_5 as least squares problem: \n')
   fprintf('Exact solution (0.1, 10)\n')
   fprintf('---------------------------------------------------------\n')
   fname = 'opt05_rj';
   options = [];
   options.verbose            = 0;
   options.method             = 'gauss_newton';
   options.step_tolerance     = 1.e-15;
   options.globalization      = 'none';
   options.gradient_tolerance = 1.e-10;
   options.max_iterations     = 40;

   alpha = 10;
   fprintf ( 'Program scale ALPHA = %f\n', alpha );
   x0 = [-1.2/alpha, alpha];
   fprintf('Unscaled:\n')

   x = entrust(fname, x0, options, alpha);

   fprintf('Gauss-Newton produced  (%10.7e, %10.7e)\n\n',x(1),x(2))
   [ res, jac ] = opt05_rj ( x, 'f', alpha );
   fprintf('Norm of RES(X) = %f\n', norm ( res ) );

   options.scale_x            = [ 1/alpha; alpha ];
   fprintf('Scaled:\n')

   x = entrust(fname, x0, options, alpha);

   fprintf('Gauss-Newton produced  (%10.7e, %10.7e)\n\n',x(1),x(2))
   [ res, jac ] = opt05_rj ( x, 'f', alpha );
   fprintf('Norm of RES(X) = %f\n', norm ( res ) );


function spiral_test03 ( )

%*****************************************************************************80
%
%% SPIRAL_TEST03 generates a field on a regular grid and plots it.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPIRAL_TEST03:\n' );
  fprintf ( 1, '  Generate a spiral velocity field on a regular grid.\n' );
  fprintf ( 1, '  Store in GNUPLOT data and command files.\n' );

  x_lo = 0.0;
  x_hi = 1.0;
  x_num = 21;

  y_lo = 0.0;
  y_hi = 1.0;
  y_num = 21;

  [ x, y ] = grid_2d ( x_num, x_lo, x_hi, y_num, y_lo, y_hi );

  n = x_num * y_num;
  c = 1.0;

  [ u, v ] = uv_spiral ( n, x, y, c );

  header = 'spiral';
  s = 0.05;
  spiral_gnuplot ( header, n, x, y, u, v, s );

  return
end


%  Figure 3.30a      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%

clf;
num=1;
a=10;

zeta =1;
den2=[1/(zeta*a) 1];
den1=[1 2*zeta 1];
den=conv(den1,den2);
t=0:.1:8;
y1=step(num,den,t);

zeta =.7;
den2=[1/(zeta*a) 1];
den1=[1 2*zeta 1];
den=conv(den1,den2);
y2=step(num,den,t);

zeta =.5;
den2=[1/(zeta*a) 1];
den1=[1 2*zeta 1];
den=conv(den1,den2);
y3=step(num,den,t);

axis([0 5 .1 .9])
plot(t,y1,'-',t,y2,'--',t,y3,'-.'),grid
title('Fig. 3.30a Step response with extra pole, \alpha= 10')
xlabel('\omega_n t')
ylabel('y(t)')


function [F_S frames_index] = bss_make_frames(S,WINDOW,NOVERLAP)

% decompose some signal(s) into frames
%
% Usage: [F_S frames_index] = bss_make_frames(S,WINDOW,NOVERLAP)
%
% Input:
%   - S: matrix of size n x T (with T>n),
%   - WINDOW: 1 x W window
%   - NOVERLAP: number of samples overlap
%
% Output:
%   - F_S:
%       * if n=1, F_S is a n_frames x W matrix containing the frames (of length W) in
%       rows,
%       * if n>1, F_S is a n_frames x W x n tensor containing the frames
%       decomposition of each row of S.
%   - frames_index: index of the beginning of each frame in the rows of S
%
% Developers:  - Cedric Fevotte (fevotte@tsi.enst.fr) - Emmanuel Vincent
% (emmanuel.vincent@irisa.fr) - Remi Gribonval (remi.gribonval@irisa.fr)

[n,T]=size(S);

if n>T
    disp('Wrong dimensions: must have T>n.')
    return;
end

%%% Default values %%%
W=length(WINDOW); % Length of window

if T < W
    disp('Please choose a window smaller than the signals.')
    return;
end

n_frames = fix((T-NOVERLAP)/(W-NOVERLAP)); % Number of frames
% If needed the very end of the signal is removed.

frames_index = 1 + (0:(n_frames-1))*(W-NOVERLAP); % Index of beginnings of frames

F_S=zeros(n_frames,W,n); % If n=1, F_S is a 2-D tensor (matrix)

for i=1:n
    for k=1:n_frames
        F_S(k,:,i)=(S(i,frames_index(k)+(0:(W-1))).*WINDOW);
    end
end


function theta = initializeParameters(hiddenSize, visibleSize)

%% Initialize parameters randomly based on layer sizes.
r  = sqrt(6) / sqrt(hiddenSize+visibleSize+1);   % we'll choose weights uniformly from the interval [-r, r]
W1 = rand(hiddenSize, visibleSize) * 2 * r - r;
W2 = rand(visibleSize, hiddenSize) * 2 * r - r;

b1 = zeros(hiddenSize, 1);
b2 = zeros(visibleSize, 1);

% Convert weights and bias gradients to the vector form.
% This step will "unroll" (flatten and concatenate together) all
% your parameters into a vector, which can then be used with minFunc.
theta = [W1(:) ; W2(:) ; b1(:) ; b2(:)];

end


function value = r8_beta ( a, b )

%*****************************************************************************80
%
%% R8_BETA evaluates the beta function of R8 arguments.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 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 A, B, the arguments.
%
%    Output, real VALUE, the beta function of A and B.
%
  persistent alnsml
  persistent xmax

  if ( isempty ( xmax ) )
    [ xmin, xmax ] = r8_gaml ( );
    alnsml = log ( r8_mach ( 1 ) );
  end

  if ( a <= 0.0 || b <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_BETA - Fatal error!\n' );
    fprintf ( 1, '  A and B must be greater than 0.\n' );
    error ( 'R8_BETA - Fatal error!' )
  end

  if ( a + b < xmax )
    value = r8_gamma ( a ) * r8_gamma ( b ) / r8_gamma ( a + b );
    return
  end

  value = r8_lbeta ( a, b );

  value = exp ( value );

  return
end


% This script will pass sample parameters to gradient descent function and get Theta.

% I have used data consisting of populations as variables and profits as target values.

% Navigate to directory where you keep these three files on octave cli and then execute.

data = load('data_.txt');

Y = data(:, 2); % Y is a column vector with all entries in second column of data matrix.

n = length(Y);

x = [ones(n, 1), data(:,1)]; % x is the matrix with first column as ones and second data.

Theta = zeros(2, 1); % Initial Theta vector with both theta values set to zero (column).

Alpha = 0.01;

noi = 1500;

Theta = gradientdescent(x, Y, Theta, Alpha, noi);

fprintf('New theta vector is below:\n');

fprintf('%f\n', Theta);

fprintf('Expected Theta vector (approx)\n');

fprintf(' -3.6303\n 1.1664\n\n');


function e=PerecptronTst(x,y,w,b);
%==========================================
% Testing phase
%==========================================
tic
[l,p]=size(x);
e=0; % number of test errors
for i=1:l
    xx=x(i,:); % take one row
    ey=xx*w+b; % apply the perceptron classification rule
    if ey>=0.5
       ey=1;
    else
       ey=0;
    end
    if y(i)~=ey;
       e=e+1;
    end;
end
toc


%% MULTIGRID OF FOR THE STOKES EQNS IN 2D
%
% This example is to show the convergence of multigrid methods for various
% finite element approximation of the Stokes equation on the unit square:
%
%       -div(mu*grad u) + grad p = f in \Omega,
%                        - div u = 0  in \Omega,
%                              u = g_D  on \Gamma.
%
% with the pure Dirichlet boundary condition.
%

%% Setting
clear all; close all;
% [node,elem] = squaremesh([0,1,0,1],0.5);
[node,elem] = circlemesh(0,0,1,0.25);
% [node,elem] = uniformrefine(node,elem);
showmesh(node,elem);
pde = Stokesdata1;
%  pde = StokesZulehnerdata;
bdFlag = setboundary(node,elem,'Dirichlet');
option.L0 = 1;
option.maxIt = 3;
option.printlevel = 1;
option.plotflag = 0;
option.rateflag = 0;

%% MG options
option.solver = 'asmg';
option.smoothingstep = 2;
option.smootherbarSp = 'SGS';

%% CR-P0 element
display('CR-P0')
option.elemType = 'CRP0';
femStokes(node,elem,pde,bdFlag,option);

%% P2-P0 element
display('P2-P0')
option.elemType = 'P2P0';
femStokes(node,elem,pde,bdFlag,option);

%% P2-P1 element
display('P2-P1')
option.elemType = 'P2P1';
option.solver = 'asmg';
femStokes(node,elem,pde,bdFlag,option);

%% isoP2-P0 element
display('isoP2-P0')
option.elemType = 'isoP2P0';
femStokes(node,elem,pde,bdFlag,option);

%% isoP2-P1 element
display('isoP2-P1')
option.elemType = 'isoP2P1';
femStokes(node,elem,pde,bdFlag,option);

%% P1b-P1 element
display('P1b-P0')
option.elemType = 'P1bP1';
femStokes(node,elem,pde,bdFlag,option);

%% Conclusion
%


function [D,dir] = dimensions(points,varargin)

% Calculates the box-dimensions and dimension estimates of the point set
% "points". Returns also the corresponding direction vectors.

if nargin == 2
    P = varargin{1};
    points = P(points,:);
elseif nargin == 3
    P = varargin{1};
    Bal = varargin{2};
    I = vertcat(Bal{points});
    points = P(I,:);
end

if size(points,2) == 3
    X = cov(points);
    [U,S,~] = svd(X);

    dp1 = points*U(:,1);
    dp2 = points*U(:,2);
    dp3 = points*U(:,3);

    D = [max(dp1)-min(dp1) max(dp2)-min(dp2) max(dp3)-min(dp3) ...
        (S(1,1)-S(2,2))/S(1,1) (S(2,2)-S(3,3))/S(1,1) S(3,3)/S(1,1)];

    dir = [U(:,1)' U(:,2)' U(:,3)'];
else
    X = cov(points);
    [U,S,~] = svd(X);

    dp1 = points*U(:,1);
    dp2 = points*U(:,2);

    D = [max(dp1)-min(dp1) max(dp2)-min(dp2) ...
        (S(1,1)-S(2,2))/S(1,1) S(2,2)/S(1,1)];

    dir = [U(:,1)' U(:,2)'];
end


function result = simplex_nd ( func, n, v )

%*****************************************************************************80
%
%% SIMPLEX_ND approximates an integral inside a simplex in ND.
%
%  Integration region:
%
%    The simplex bounded by the origin and a convex combination of N points.
%
%  Discussion:
%
%    An N+1 point second degree formula is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur H Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function which evaluates F(X) at the N-dimensional point
%    X, of the form
%      function value = func ( n, x )
%
%    Input, integer N, the dimension of the space.
%
%    Input, real V(N+1,N), each of the
%    N+1 rows of V contains the N coordinates of one of the
%    vertices of the simplex.
%
%    Output, real RESULT, the approximate integral of the function.
%
  c = 1.0E+00 / sqrt ( n + 2 );
  w = 1.0E+00 / ( n + 1 );

  for i = 1 : n
    x(i) = w * ( 1.0E+00 - c ) * sum ( v(1:n+1,i) );
  end

  quad = 0.0E+00;

  for i = 1 : n+1

    x(1:n) = x(1:n) + c * v(i,1:n);

    quad = quad + w * feval ( func, n, x );

    x(1:n) = x(1:n) - c * v(i,1:n);

  end

  volume = simplex_volume_nd ( n, v );
  result = quad * volume;

  return
end


function r = intrand(p, m, n);
%INTRAND Random matrices from uniform integer distribution.
%
%   R = INTRAND(P) returns a matrix of random numbers chosen
%   from the uniform integer distribution with parameter P. The
%   minimum and maximum output integer numbers are P(1) and P(2).
%   R = INTRAND(P,M,N) returns an M by N matrix.
%

% Copyright (c) 2000 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.

if nargin < 1 | any(size([p(:)])~=[2 1])
  error('Incorrect range assignment in INTRAND');
end
if p(1)<p(2)
  rmin=ceil(p(1));
  rrange=floor(p(2))-rmin+1;
else
  rmin=ceil(p(2));
  rrange=floor(p(1))-rmin+1;
end

if nargin < 2
  r=floor(rand.*rrange+rmin);
else
  if nargin < 3
    n=m;
  end
  r=floor(rand([m n]).*rrange+rmin);
end


function [ vTransVectorMean, vTransVectorCov ] = UnscentedTransform( hTransformFunction, vVectorMean, mVectorCov )
%UNTITLED4 Summary of this function goes here
%   Detailed explanation goes here

vectorOrder     = size(vVectorMean, 1);
numSigmaPoints  = (2 * vectorOrder) + 1;

mVectorCovSqrt = chol(mVectorCov);
if(vectorOrder <= 3) %<! Used as k Factor in literature
    unscentedTransformScalingFactor = 3 - vectorOrder;
else
    unscentedTransformScalingFactor = 3;
end

vSigmaPoints                = zeros(vectorOrder, numSigmaPoints);
vTransformedSigmaPoints     = zeros(vectorOrder, numSigmaPoints);
vUnscentedTransformWeights  = zeros(numSigmaPoints, 1);

vTransVectorMean    = zeros(vectorOrder, 1);
vTransVectorCov     = zeros(vectorOrder, vectorOrder);

mVectorCovSqrtNormalized = sqrt(vectorOrder + unscentedTransformScalingFactor) .* mVectorCovSqrt;

vSigmaPoints(1) = 0;
vUnscentedTransformWeights(1) = unscentedTransformScalingFactor / (unscentedTransformScalingFactor + vectorOrder);
vUnscentedTransformWeights(2:end) = 1 / (2 * (unscentedTransformScalingFactor + vectorOrder));

for ii = 2:(vectorOrder + 1)
    vSigmaPoints(:, ii) = mVectorCovSqrtNormalized(:, (ii - 1));
end

vSigmaPoints(:, (vectorOrder + 2):end) = -vSigmaPoints(:, 2:(vectorOrder + 1));

for ii = 1:numSigmaPoints
    vSigmaPoints(:, ii) = vVectorMean + vSigmaPoints(:, ii);
    vTransformedSigmaPoints(:, ii) = hTransformFunction(vSigmaPoints(:, ii));
end

for ii = 1:numSigmaPoints
    vTransVectorMean = vTransVectorMean + (vUnscentedTransformWeights(ii) * vTransformedSigmaPoints(:, ii));
end

for ii = 1:numSigmaPoints
    vTransVectorCov = vTransVectorCov + ...
        (vUnscentedTransformWeights(ii) * (([vTransformedSigmaPoints(:, ii) - vTransVectorMean]) * ([vTransformedSigmaPoints(:, ii) - vTransVectorMean].')));
end

end


function bernoulli_number3_test ( )

%*****************************************************************************80
%
%% BERNOULLI_NUMBER3_TEST tests BERNOULLI_NUMBER3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BERNOULLI_NUMBER3_TEST\n' );
  fprintf ( 1, '  BERNOULLI_NUMBER3 computes Bernoulli numbers.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   I     Exact           BERNOULLI_NUMBER3\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, c0 ] = bernoulli_number_values ( n_data );

    if ( n_data == 0 )
      break
    end

    c1 = bernoulli_number3 ( n );

    fprintf ( 1, '  %2d  %14e  %14e\n', n, c0, c1 );

  end

  return
end


function x = ellipse_rs ( r, s, i )

%*****************************************************************************80
%
%% ELLIPSE_RS maps the boundary of the unit square to an ellipse.
%
%  Discussion:
%
%    The ellipse is ( 3 * cos ( THETA ), 2 * sin ( THETA ) ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, S, the coordinates of a point that lies on the
%    boundary of the square.
%
%    Input, integer I, the component of the data which is to be returned.
%
%    Output, real X, the I-th component of the data vector X, evaluated
%    at the point (R,S), which is on a corner, or edge, of the unit square.
%
  if ( r == 0.0 )
    theta = 0.25 * pi * ( 5.0 * ( 1.0 - s ) + 3.0 * s  );
  elseif ( r == 1.0 ) then
    theta = 0.25 * pi * ( - 1.0 * ( 1.0 - s ) + 1.0 * s );
  elseif ( s == 0.0 ) then
    theta = 0.25 * pi * ( 5.0 * ( 1.0 - r ) + 7.0 * r );
  elseif ( s == 1.0 ) then
    theta = 0.25 * pi * ( 3.0 * ( 1.0 - r ) + 1.0 * r );
  end

  if ( i == 1 )

    x = 3.0 * cos ( theta );

  elseif ( i == 2 ) then

    x = 2.0 * sin ( theta );

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'ELLIPSE_RS - Fatal error!\n' );
    fprintf ( 1, '  Illegal component index I = %d\n', i );
    error ( 'ELLIPSE_RS - Fatal error!' );

  end

  return
end


% SP_H1_ERROR: Evaluate the error in H^1 norm.
%
%   [errh1, errl2, errh1s, errh1_elem, errl2_elem, errh1s_elem] = sp_h1_error (space, msh, u, uex, graduex);
%
% INPUT:
%
%     space:    structure representing the space of discrete functions (see sp_scalar/sp_evaluate_col)
%     msh:      structure containing the domain partition and the quadrature rule (see msh_cartesian/msh_evaluate_col)
%     u:        vector of dof weights
%     uex:      function handle to evaluate the exact solution
%     graduex:  function handle to evaluate the gradient of the exact solution
%
% OUTPUT:
%
%     errh1:  error in H^1 norm
%     errl2:  error in L^2 norm
%     errh1s: error in H^1 seminorm
%     errh1_elem:  error in H^1 norm, for each single element
%     errl2_elem:  error in L^2 norm, for each single element
%     errh1s_elem: error in H^1 seminorm, for each single element
%
% Copyright (C) 2010 Carlo de Falco
% Copyright (C) 2011 Rafael Vazquez
% Copyright (C) 2015 Eduardo M. Garau, Rafael Vazquez
%
% 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 Octave; see the file COPYING.  If not, see
% <http://www.gnu.org/licenses/>.

function [errh1, errl2, errh1s, errh1_elem, errl2_elem, errh1s_elem] = sp_h1_error (sp, msh, u, uex, graduex)

  grad_valu = sp_eval_msh (u, sp, msh, 'gradient');
  grad_valu = reshape (grad_valu, sp.ncomp, msh.rdim, msh.nqn, msh.nel);

  for idir = 1:msh.rdim
    x{idir} = reshape (msh.geo_map(idir,:,:), msh.nqn*msh.nel, 1);
  end
  grad_valex  = reshape (feval (graduex, x{:}), sp.ncomp, msh.rdim, msh.nqn, msh.nel);

  w = msh.quad_weights .* msh.jacdet;

  [errl2, errl2_elem] = sp_l2_error (sp, msh, u, uex);
  errh1s_elem = sum (reshape (sum (sum ((grad_valu - grad_valex).^2, 1), 2), [msh.nqn, msh.nel]) .* w);
  errh1s = sqrt (sum (errh1s_elem));

  errh1  = sqrt (errl2^2 + errh1s^2);

  errh1_elem  = sqrt (errl2_elem.^2 + errh1s_elem);
  errh1s_elem = sqrt (errh1s_elem);

end


function r = gamrnd(a,b,m,n);
%GAMRND Random matrices from gamma distribution.
%   R = GAMRND(A,B) returns a matrix of random numbers chosen
%   from the gamma distribution with parameters A and B.
%   The size of R is the common size of A and B if both are matrices.
%   If either parameter is a scalar, the size of R is the size of the other
%   parameter. Alternatively, R = GAMRND(A,B,M,N) returns an M by N matrix.
%
%   Some references refer to the gamma distribution
%   with a single parameter. This corresponds to GAMRND
%   with B = 1. (See Devroye, pages 401-402.)

%   GAMRND uses a rejection or an inversion method depending on the
%   value of A.

%   References:
%      [1]  L. Devroye, "Non-Uniform Random Variate Generation",
%      Springer-Verlag, 1986

%   B.A. Jones 2-1-93
%   Copyright (c) 1993-98 by The MathWorks, Inc.
%   $Revision: 1.1.1.1 $  $Date: 2005/04/26 02:29:18 $

if nargin < 2,
   error('Requires at least two input arguments.');
end

if nargin == 2
   [errorcode rows columns] = rndcheck(2,2,a,b);
end

if nargin == 3
   [errorcode rows columns] = rndcheck(3,2,a,b,m);
end

if nargin == 4
   [errorcode rows columns] = rndcheck(4,2,a,b,m,n);
end

if errorcode > 0
   error('Size information is inconsistent.');
end

% Initialize r to zero.
lth = rows*columns;
r = zeros(lth,1);
a = a(:); b = b(:);

scalara = (length(a) == 1);
if scalara
   a = a*ones(lth,1);
end

scalarb = (length(b) == 1);
if scalarb
   b = b*ones(lth,1);
end

% If a == 1, then gamma is exponential. (Devroye, page 405).
k = find(a == 1);
if any(k)
   r(k) = -b(k) .* log(rand(size(k)));
end

k = find(a < 1 & a > 0);
% (Devroye, page 418 Johnk's generator)
if any(k)
  c = zeros(lth,1);
  d = zeros(lth,1);
  c(k) = 1 ./ a(k);
  d(k) = 1 ./ (1 - a(k));
  accept = k;
  while ~isempty(accept)
    u = rand(size(accept));
    v = rand(size(accept));
    x = u .^ c(accept);
    y = v .^ d(accept);
    k1 = find((x + y) <= 1);
    if ~isempty(k1)
      e = -log(rand(size(k1)));
      r(accept(k1)) = e .* x(k1) ./ (x(k1) + y(k1));
      accept(k1) = [];
    end
  end
  r(k) = r(k) .* b(k);
end

% Use a rejection method for a > 1.
k = find(a > 1);
% (Devroye, page 410 Best's algorithm)
bb = zeros(size(a));
c  = bb;
if any(k)
  bb(k) = a(k) - 1;
  c(k) = 3 * a(k) - 3/4;
  accept = k;
  count = 1;
  while ~isempty(accept)
    m = length(accept);
    u = rand(m,1);
    v = rand(m,1);
    w = u .* (1 - u);
    y = sqrt(c(accept) ./ w) .* (u - 0.5);
    x = bb(accept) + y;
    k1 = find(x >= 0);
    if ~isempty(k1)
      z = 64 * (w .^ 3) .* (v .^ 2);
      k2 = (z(k1) <= (1 - 2 * (y(k1) .^2) ./ x(k1)));
      k3 = k1(find(k2));
      r(accept(k3)) = x(k3);
      k4 = k1(find(~k2));
      k5 = k4(find(log(z(k4)) <= (2*(bb(accept(k4)).*log(x(k4)./bb(accept(k4)))-y(k4)))));
      r(accept(k5)) = x(k5);
      omit = [k3; k5];
      accept(omit) = [];
    end
  end
  r(k) = r(k) .* b(k);
end

% Return NaN if a or b is not positive.
r(b <= 0 | a <= 0) = NaN;

r = reshape(r,rows,columns);


function linplus_test583 ( )

%*****************************************************************************80
%
%% TEST583 tests R8STO_INVERSE.
%
%  Discussion:
%
%    This routine was modified at the suggestion of Yunong Zhang
%    of the EEE department of the University of Strathclyde.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 5;
  a = [ 4.0E+00, 2.0E+00, 0.8E+00, 0.4E+00, 0.1E+00 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST583\n' );
  fprintf ( 1, '  R8STO_INVERSE computes the inverse of a positive\n' );
  fprintf ( 1, '  definite symmetric Toeplitz matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );

  r8sto_print ( n, a, '  The symmetric Toeplitz matrix A:' );

  b = r8sto_inverse ( n, a );

  r8ge_print ( n, n, b, '  The inverse matrix B:' );

  error_max = norm ( toeplitz ( a ) * b - eye ( n ), inf );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Maximum discrepancy in A * B - I is %e\n', error_max );

  return
end


function [g,info] = wilwin(g,M,L,callfun);
%WILWIN  Compute a Wilson/WMDCT window from text or cell array
%   Usage: [g,info] = wilwin(g,M,L);
%
%   `[g,info]=wilwin(g,M,L)` computes a window that fits well with the
%   specified number of channels *M* and transform length *L*. The window itself
%   is specified by a text description or a cell array containing additional
%   parameters.
%
%   The window can be specified directly as a vector of numerical
%   values. In this case, `wilwin` only checks assumptions about transform
%   sizes etc.
%
%   `[g,info]=wilwin(g,M)` does the same, but the window must be a FIR
%   window, as the transform length is unspecified.
%
%   The window can be specified as one of the following text strings:
%
%     'gauss'      Gaussian window with optimal concentration
%
%     'dualgauss'  Riesz dual of Gaussian window with optimal concentration.
%
%     'tight'      Window generating an orthonormal basis
%
%   In these cases, a long window is generated with a length of *L*.
%
%   It is also possible to specify one of the window names from |firwin|. In
%   such a case, `wilwin` generates the specified FIR window with a length
%   of *M*.
%
%   The window can also be specified as cell array. The possibilities are:
%
%     `{'gauss',...}`
%       Additional parameters are passed to PGAUSS
%
%     `{'dual',...}`
%       Dual window of whatever follows. See the examples below.
%
%     `{'tight',...}`
%       Orthonormal window of whatever follows.
%
%   It is also possible to specify one of the window names from |firwin| as
%   the first field in the cell array. In this case, the remaining
%   entries of the cell array are passed directly to |firwin|.
%
%   Some examples::
%
%     g=wilwin('gauss',M,L);
%
%   This computes a Gaussian window of length L fitted for a system with
%   *M* channels. ::
%
%     g=wilwin({'gauss',1},M,L);
%
%   This computes a Gaussian window with equal time and frequency support
%   irrespective of *M*. ::
%
%     gd=wilwin('gaussdual',M,L);
%
%   This computes the dual of a Gaussian window fitted for a system with *M*
%   channels. ::
%
%     gd=wilwin({'tight','gauss'},M,L);
%
%   This computes the orthonormal window of the Gaussian window fitted for
%   the system. ::
%
%     g=wilwin({'dual',{'hann',20}},M,L);
%
%   This computes the dual of a Hann window of length 20.
%
%   The structure info provides some information about the computed
%   window:
%
%     `info.gauss`
%        True if the window is a Gaussian.
%
%     `info.tfr`
%        Time/frequency support ratio of the window. Set whenever it makes sense.
%
%     `info.wasrow`
%        Input was a row window
%
%     `info.isfir`
%        Input is an FIR window
%
%     `info.isdual`
%        Output is the dual window of the auxiliary window.
%
%     `info.istight`
%        Output is known to be a tight window.
%
%     `info.auxinfo`
%        Info about auxiliary window.
%
%     `info.gl`
%        Length of window.
%
%   See also: pgauss, firwin, gabwin

% Assert correct input.
complainif_argnonotinrange(nargin,2,4,mfilename);

if nargin==2
  L=[];
end;

% Basic discovery: Some windows depend on L, and some windows help define
% L, so the calculation of L is window dependant.

% Default values.
info.gauss=0;
info.wasrow=0;
info.isfir=0;
info.istight=0;
info.isdual=0;

% Manually get the list of window names
definput=arg_firwin(struct);
firwinnames =  definput.flags.wintype;

% Create window if string was given as input.
if ischar(g)
  winname=lower(g);
  switch(winname)
   case {'pgauss','gauss'}
    complain_L(L,callfun);
    g=comp_pgauss(L,2*M*M/L,0,0);
    info.gauss=1;
    info.tfr=2*M*M/L;
   case {'psech','sech'}
    complain_L(L,callfun);
    g=psech(L,2*M*M/L);
    info.tfr=a*M/L;
   case {'dualgauss','gaussdual'}
    complain_L(L,callfun);
    g=comp_pgauss(L,2*M*M/L,0,0);
    g=wildual(g,M);
    info.isdual=1;
    info.tfr=a*M/L;
   case {'tight'}
    complain_L(L,callfun);
    g=wilorth(M,L);
    info.tfr=2*M*M/L;
    info.istight=1;
   case firwinnames
    [g,firinfo]=firwin(winname,2*M,'2');
    info.isfir=1;
    if firinfo.issqpu
      info.istight=1;
    end;
   otherwise
    error('%s: Unknown window type: %s',callfun,winname);
  end;
end;

if iscell(g)
  if isempty(g) || ~ischar(g{1})
    error('First element of window cell array must be a character string.');
  end;

  winname=lower(g{1});

  switch(winname)
   case {'pgauss','gauss'}
    complain_L(L,callfun);
    [g,info.tfr]=pgauss(L,g{2:end});
    info.gauss=1;
   case {'psech','sech'}
    complain_L(L,callfun);
    [g,info.tfr]=psech(L,g{2:end});
   case {'dual'}
    gorig = g{2};
    [g,info.auxinfo] = wilwin(gorig,M,L,callfun);
    g = wildual(g,M,L);

    % gorig can be string or cell array
    if info.auxinfo.isfir && test_isfir(gorig,M)
        info.isfir = 1;
    end

    info.isdual=1;
   case {'tight'}
    gorig = g{2};
    [g,info.auxinfo] = wilwin(g{2},M,L,callfun);
    g = wilorth(g,M,L);
    % gorig can be string or cell array
    if info.auxinfo.isfir && test_isfir(gorig,M)
        info.isfir = 1;
    end
    info.istight=1;
   case firwinnames
    g=firwin(winname,g{2},'energy',g{3:end});
    info.isfir=1;
   otherwise
    error('Unsupported window type.');
  end;
end;

if isnumeric(g)
  if size(g,2)>1
    if size(g,1)==1
      % g was a row vector.
      g=g(:);
      info.wasrow=1;
    end;
  end;
end;

if rem(length(g),2*M)~=0
  % Zero-extend the window to a multiple of 2*M
  g=fir2long(g,ceil(length(g)/(2*M))*2*M);
end;

% Information to be determined post creation.
info.wasreal = isreal(g);
info.gl      = length(g);

if (~isempty(L) && (info.gl<L))
  info.isfir=1;
end;

function complain_L(L,callfun)

  if isempty(L)
    error(['%s: You must specify a length L if a window is represented as a ' ...
           'text string or cell array.'],callfun);
  end;

  function isfir=test_isfir(gorig,M)
    % Original window is FIR, dual window is FIR if length of the original
    % window is <= M. This is true if the length was not explicitly
    % defined (gorig{2}).
    if iscell(gorig) && numel(gorig)>1 && isnumeric(gorig{2}) && gorig{2}<=2*M...
         || ischar(gorig)
        isfir = 1;
    else
       isfir = 0;
    end


%% Gaussian Mixture Model (GMM)
%
% Demonstrates EM clustering, and also compares againt K-means clustering.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/gaussian_mix.py>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/kmeans.py>
%

function gaussian_mix_demo()
    %% Data
    % sample 2D points from a mixture distribution of K bivariate Gaussians
    K = 5;
    sz = 512;
    [pts, labels, mus, sigmas] = make_gaussian_mixture(K, sz);
    whos pts labels mus sigmas

    %%
    % draw points (color-coded) with the ground-truth mixtures
    clr = uint8(hsv(K) * 255);
    clr(:,4) = 0;
    img = zeros([sz sz 3], 'uint8');
    img0 = cv.circle(img, pts, 1, 'Colors',clr(labels,:), 'Thickness','Filled');
    for k=1:K
        img0 = draw_gaussian(img0, mus(k,:), sigmas{k}, [0 255 255]);
    end
    imshow(img0), title(sprintf('%d Samples, K=%d', numel(labels), K))

    %% EM
    % cluster using EM by fitting a Gaussian mixture model
    em = cv.EM();
    em.ClustersNumber = K;
    em.CovarianceMatrixType = 'Generic';
    tic
    [~,L] = em.trainEM(pts);
    toc

    %%
    % draw clustered points along with the estimated mixture model
    means = em.getMeans();
    covs = em.getCovs();
    img1 = cv.circle(img, pts, 1, 'Colors',clr(L+1,:), 'Thickness','Filled');
    for k=1:K
        img1 = draw_gaussian(img1, means(k,:), covs{k}, [0 255 255]);
    end
    figure, imshow(img1), title('Gaussian Mixtures')

    %% Kmeans
    % cluster using Kmeans
    crit = struct('type','Count+EPS', 'maxCount',30, 'epsilon',0.1);
    L = cv.kmeans(pts, K, 'Criteria',crit, 'Attempts',10);

    %%
    % draw points color-coded by assigned label
    img2 = cv.circle(img, pts, 1, 'Colors',clr(L+1,:), 'Thickness','Filled');
    figure, imshow(img2), title('Kmeans')
end

%% Helper functions

function [pts, labels, mus, sigmas] = make_gaussian_mixture(K, sz)
    %MAKE_GAUSSIAN_MIXTURE  Random points from Gaussian mixture distribution
    %
    %     [pts, labels, mus, sigmas] = make_gaussian_mixture(K, sz)
    %
    % ## Input
    % * __K__ number of components
    % * __sz__ image size, determines 2D points domain
    %
    % ## Output
    % * __pts__ matrix of 2D points
    % * __labels__ vector of corresponding component indices
    % * __mus__ K-by-2 mean of each component
    % * __sigmas__ cell array of length K, covariance of each component
    %
    % See also: gmdistribution.random
    %

    mus = zeros(K,2);
    sigmas = cell(K,1);
    pts = cell(K,1);
    labels = cell(K,1);
    for k=1:K
        mus(k,:) = (rand(1,2)*0.8 + 0.1) * sz;
        a = (rand(2,2) - 0.5) * sz * 0.1;
        sigmas{k} = (a.' * a) + sz*0.05*eye(2);
        n = 100 + randi([0 900]);
        pts{k} = my_mvnrnd(mus(k,:), sigmas{k}, n);
        labels{k} = k * ones(size(pts{k},1),1);
    end
    pts = single(cat(1, pts{:}));
    labels = int32(cat(1, labels{:}));
end

%%

function X = my_mvnrnd(mu, sigma, num)
    %MY_MVNRND  Random points from Gaussian distribution
    %
    %     X = my_mvnrnd(mu, sigma, num)
    %
    % ## Input
    % * __mu__ 1x2 mean vector
    % * __sigma__ 2x2 covariance matrix
    % * __num__ number of points to generate
    %
    % ## Output
    % * __X__ matrix of 2D points, num-by-2
    %
    % See also: mvnrnd, randn
    %

    if mexopencv.require('stats')
        X = mvnrnd(mu, sigma, num);
    else
        X = bsxfun(@plus, randn(num,numel(mu)) * cholcov(sigma), mu);
    end
end

%%

function img = draw_gaussian(img, mu, sigma, clr)
    %DRAW_GAUSSIAN  Draw a bivariate Gaussian
    %
    %     img = draw_gaussian(img, mu, sigma, clr)
    %
    % ## Input
    % * __img__ input image on which to draw
    % * __mu__ 1x2 mean vector
    % * __sigma__ 2x2 covariance matrix
    % * __clr__ color
    %
    % ## Output
    % * __img__ output image
    %
    % Represent a 2D Gaussian with an ellipse where:
    %
    % * its mean is the center of the ellipse
    % * its covariance matrix eigenvector corresponding to largest eigenvalue
    %   is the direction of the ellipse
    % * its (scaled) eigenvalues are the major/minor axis lengths of the
    %   ellipse
    %

    [w,u,~] = cv.SVD.Compute(sigma);
    ang = atan2(u(2,1), u(1,1)) * (180/pi);
    s = sqrt(w)*3;
    img = cv.ellipse(img, mu, s, 'Angle',ang, 'Color',clr, 'LineType','AA');
end


function y = vl_nnnormalizelp(x,dzdy,varargin)
%VL_NNNORMALIZELP  CNN Lp normalization
%   Y = VL_NNNORMALIZELP(X) normalizes in Lp norm each column of
%   features in the array X, along its third dimension:
%
%       Y(i,j,k) = X(i,j,k) / sum_q (X(i,j,q).^p + epsilon)^(1/p)
%
%   [Y,N] = VL_NNNORMALIZELP(X) returns the array N containing the
%   computed norms.
%
%   DZDX = VL_NNNORMALIZELP(X, DZDY) computes the derivative of the
%   function with respect to X projected onto DZDY.
%
%   VL_NNNORMALIZE(___, 'opts', val, ...) takes the following options:
%
%   `exponent`:: 2
%      The exponent of the Lp norm. Warning: currently only even
%      exponents are supported.
%
%   `p`:: same as exponent
%
%   `epsilon`:: 0.01
%      The constant added to the sum of p-powers before taking the
%      1/p square root (see the formula above).
%
%   `dimensions`:: [3]
%      The list of dimensions along wich to operate. By default,
%      normalization is along the third dimension, usually
%      corresponding to feature channels.
%
%   `spatial`:: `false`
%      If `true`, sum along the two spatial dimensions instead of
%      along the feature channels. This is the same as setting
%      `dimensions` to [1,2].
%
%   See also: VL_NNNORMALIZE().

opts.epsilon = 1e-2 ;
opts.p = 2 ;
opts.spatial = false ;
opts = vl_argparse(opts, varargin, 'nonrecursive') ;

if ~opts.spatial
  massp = sum(x.^opts.p,3) + opts.epsilon ;
else
  massp = sum(sum(x.^opts.p,1),2) + opts.epsilon ;
end
mass = massp.^(1/opts.p) ;
y = bsxfun(@rdivide, x, mass) ;

if nargin < 2 || isempty(dzdy)
  return ;
else
  dzdy = bsxfun(@rdivide, dzdy, mass) ;
  if ~opts.spatial
    tmp = sum(dzdy .* x, 3) ;
  else
    tmp = sum(sum(dzdy .* x, 1),2);
  end
  y = dzdy - bsxfun(@times, tmp, bsxfun(@rdivide, x.^(opts.p-1), massp)) ;
end


O = 3;
Q = 2;

% "true" parameters
prior0 = normalise(rand(Q,1));
transmat0 = mk_stochastic(rand(Q,Q));
obsmat0 = mk_stochastic(rand(Q,O));

% training data
T = 5;
nex = 10;
data = dhmm_sample(prior0, transmat0, obsmat0, T, nex);

% initial guess of parameters
prior1 = normalise(rand(Q,1));
transmat1 = mk_stochastic(rand(Q,Q));
obsmat1 = mk_stochastic(rand(Q,O));

% improve guess of parameters using EM
[LL, prior2, transmat2, obsmat2] = dhmm_em(data, prior1, transmat1, obsmat1, 'max_iter', 5);
LL

% use model to compute log likelihood
loglik = dhmm_logprob(data, prior2, transmat2, obsmat2)
% log lik is slightly different than LL(end), since it is computed after the final M step


function [A,xy]= cut_grid_data

% Generate a cut-grid graph for the ICM 2006 talk example
%
% The graph has 64 nodes and 95 edges
% A is an n x m incidence matrix (n is number of nodes, m is number of edges)
% xy is the location data
%
% Original code by Arpita Ghosh, modified for ICM06 talk by Almir Mutapcic

n  = 8;
r1 = 2;
y  = ones(n,1) * (1:n);
x  = y';
x  = x(:);
y  = y(:);
dx = x * ones(1,n^2);
dy = y * ones(1,n^2);
xy = [ x, y ];

% Find the adjacency matrix, manually deleting edges to get down to size
Adj1 = tril( ( dx - dx' ) .^ 2 + ( dy - dy' ) .^2 < r1, -1 );
Adj1(49,41) = 0;
Adj1(50,42) = 0;
Adj1(16,8)  = 0;
Adj1(24,16) = 0;
Adj1(15,7)  = 0;
Adj1(23,15) = 0;
Adj1(10,1)  = 0;
Adj1(21,13) = 0;
Adj1(13,5)  = 0;
Adj1(22,14) = 0;
Adj1(14,6)  = 0;
Adj1(51,43) = 0;
Adj1(52,44) = 0;
Adj1(53,45) = 0;
Adj1(54,46) = 0;
Adj1(42,41) = 0;
Adj1(34,33) = 0;
Adj1(26,25) = 0;

% Build the incidence matrix
[i,j] = find(Adj1);
m = length(i);
A = sparse( [i;j], [1:m,1:m]', [ones(m,1);-ones(m,1)] );


function [results] = hyperPlmf(M, t, windowSize)
% HYPERPLMF Performs the PCA local matched filter (PLMF) target detection algorithm
%   Performs the PCA local matched filter (PLMF) target detection algorithm.
%
% Usage
%   [results] = hyperPlmf(M, target, windodwSize)
% Inputs
%   M - dd matrix of HSI data (m x n x p)
%   t - target of interest (p x 1)
% Outputs
%   results - vector of detector output (m x n)
%
% References
%   Sofa, Geva, Rotman. "Improved covariance matrices for point target detection in hyperspectral
% data."  IEEE International Conference on Microwaves, Communications, Antennas and Electronics
% Systems, 2009. COMCAS 2009.

% windowSize must be odd number
if ~mod(windowSize,2)
    error('windowSize must be an odd number.')
end

if (length(size(M)) ~= 3)
    error('M must be 3-dimensional matrix.')
end

[h,w,p] = size(M);
N = h*w;

% Remove mean from the target
M = hyperConvert2d(M);
u = mean(M.').';

[Mpca,V,lambda] = hyperPct(M,p);
t_pct = V.'*(t-u);

% Create map to get neighbors
map = 1:N;
map = reshape(map,h,w);

R_hat = hyperCov(Mpca);
G = inv(R_hat);

results = zeros(h,w);
s = floor(windowSize/2)+1;
for k=s:(h-s)
    for kk=s:(w-s)
        midIdx = map(k,kk);
        neighborhoodIdx = map((k-s+1):(k+s-1),(kk-s+1):(kk+s-1));

        Mlocal = M(:,neighborhoodIdx(:));
        [~,~,lambdaLocal] = hyperPct(Mlocal,p);

        y = Mpca(:,midIdx);
        results(k,kk) = sum((t_pct.*y)./max(lambdaLocal,lambda));
    end
end


classdef TestDft
    %TestDft

    methods (Static)
        function test_1d_real
            % 1x100 real signal
            x = sin(linspace(0, 6*pi, 100));

            % 1D DFT
            y = cv.dft(x);
            validateattributes(y, {class(x)}, {'vector', 'numel',numel(x)});

            % inverse transform
            xx = cv.dft(y, 'Inverse',true, 'Scale',true);
            validateattributes(xx, {class(x)}, {'vector', 'numel',numel(x)});

            err = norm(x - xx);
        end

        function test_1d_complex
            % 1x100 complex signal
            x = sin(linspace(0, 6*pi, 100)); % real
            x(:,:,2) = 0;                    % imag

            % 1D DFT
            y = cv.dft(x, 'ComplexInput',true);
            validateattributes(y, {class(x)}, {'size',size(x)});

            % inverse transform
            xx = cv.dft(y, 'Inverse',true, 'Scale',true);
            validateattributes(xx, {class(x)}, {'size',size(x)});

            err = norm(x(:) - xx(:));
        end

        function test_2d
            % image (256x256)
            img = cv.imread(fullfile(mexopencv.root(), 'test', 'blox.jpg'), ...
                'Grayscale',true);
            I = double(img);

            % 2D DFT
            J = cv.dft(I, 'ComplexOutput',true);
            validateattributes(J, {class(I)}, {'size',[size(I) 2]});

            % 2D DFT is simply the 1D DFT, performed along the rows and then
            % along the columns: fft2(I) = fft(fft(I).').'
            J2 = cv.dft(I, 'Rows',true, 'ComplexOutput',true);
            J2 = permute(J2, [2 1 3]);
            J2 = cv.dft(J2, 'Rows',true, 'ComplexOutput',true);
            J2 = permute(J2, [2 1 3]);
            %assert(norm(J(:) - J2(:)) < 1e-9);

            % inverse transform
            II = cv.dft(J, 'Inverse',true, 'Scale',true, 'RealOutput',true);
            validateattributes(II, {class(I)}, {'2d', 'size',size(I)});
            err = norm(I - II);
        end

        function test_ccs_1d
            % real 1D vector
            if true
                N = 100; % even case
            else
                N = 101; % odd case
            end
            x = sin(linspace(0, 6*pi, N));
            x = x + randn(size(x));

            % 1D DFT (packed CCS format)
            y = cv.dft(x);

            % un-pack CCS into full complex array (both even/odd length cases)
            if rem(N, 2) == 0
                dc = y(1);           % DC component
                pos = y(2:2:end-1);  % positive frequencies (real part)
                neg = y(3:2:end-1);  % negative frequencies (imaginary part)
                nyq = y(end);        % nyquist frequency
                yy = cat(3, ...
                    [dc, pos, nyq, fliplr(pos)], ...
                    [0, neg, 0, fliplr(-neg)]);
            else
                dc = y(1);
                pos = y(2:2:end);
                neg = y(3:2:end);
                nyq = [];  % no nyquist
                yy = cat(3, ...
                    [dc, pos, fliplr(pos)], ...
                    [0, neg, fliplr(-neg)]);
            end

            % compare
            y2 = cv.dft(x, 'ComplexOutput',true);
            assert(isequal(y2, yy));

            % compare against builtin FFT function
            yy = complex(yy(:,:,1), yy(:,:,2));
            y3 = fft(x);
            %assert(norm(y3 - yy) < 1e-9);
        end

        function test_ccs_2d
            %TODO
            error('mexopencv:testskip', 'todo');
        end

        function test_error_argnum
            try
                cv.dft();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end


function [rho,rhou,Ener] = Euler1D(rho, rhou, Ener, FinalTime)

% function [rho, rhou, Ener] = Euler1D(rho, rhou, Ener, FinalTime)
% Purpose  : Integrate 1D Euler equations until FinalTime starting with
%            initial conditions [rho, rhou, Ener]

Globals1D;

% Parameters
gamma = 1.4; CFL = 1.0; time = 0;

% Prepare for adaptive time stepping
mindx = min(x(2,:)-x(1,:));

% Limit initial solution
rho =SlopeLimitN(rho); rhou=SlopeLimitN(rhou); Ener=SlopeLimitN(Ener);

% outer time step loop
while(time<FinalTime)

  Temp = (Ener - 0.5*(rhou).^2./rho)./rho;
  cvel = sqrt(gamma*(gamma-1)*Temp);
  dt = CFL*min(min(mindx./(abs(rhou./rho)+cvel)));

  if(time+dt>FinalTime)
    dt = FinalTime-time;
  end

  % 3rd order SSP Runge-Kutta

  % SSP RK Stage 1.
  [rhsrho,rhsrhou,rhsEner]  = EulerRHS1D(rho, rhou, Ener);
  rho1  = rho  + dt*rhsrho;
  rhou1 = rhou + dt*rhsrhou;
  Ener1 = Ener + dt*rhsEner;

  % Limit fields
  rho1  = SlopeLimitN(rho1); rhou1 = SlopeLimitN(rhou1); Ener1 = SlopeLimitN(Ener1);

  % SSP RK Stage 2.
  [rhsrho,rhsrhou,rhsEner]  = EulerRHS1D(rho1, rhou1, Ener1);
  rho2   = (3*rho  + rho1  + dt*rhsrho )/4;
  rhou2  = (3*rhou + rhou1 + dt*rhsrhou)/4;
  Ener2  = (3*Ener + Ener1 + dt*rhsEner)/4;

  % Limit fields
  rho2  = SlopeLimitN(rho2); rhou2 = SlopeLimitN(rhou2); Ener2 = SlopeLimitN(Ener2);

  % SSP RK Stage 3.
  [rhsrho,rhsrhou,rhsEner]  = EulerRHS1D(rho2, rhou2, Ener2);
  rho  = (rho  + 2*rho2  + 2*dt*rhsrho )/3;
  rhou = (rhou + 2*rhou2 + 2*dt*rhsrhou)/3;
  Ener = (Ener + 2*Ener2 + 2*dt*rhsEner)/3;

  % Limit solution
  rho =SlopeLimitN(rho); rhou=SlopeLimitN(rhou); Ener=SlopeLimitN(Ener);

  % Increment time and adapt timestep
  time = time+dt;
end
return


function g = sigmoid(z)

% Computes sigmoid of z

g = 1.0 ./ (1.0 + exp(-z));


function regresdiagplot(xdist,ydist,cutoffx,cutoffy,k,multi,attrib,labsd,labresd)

%REGRESDIAGPLOT makes a regression outlier map.
% The score distances (SD)/mahalanobis distances (MD)/robust distances (RD) and
% residual distances (ResD) calculated in a PCR or PLS analysis, or in a
% univariate or multivariate regression are plotted on the x and y-axis, respectively.
% The cutoff values marked by a red line indicate which observations are
% outlying with respect to the majority of the data. The terminology used is
% as indicated in the tabular:
%                         small SD/MD/RD  | large SD/MD/RD
%      ----------------------------------------------------------
%      small ResD/StdRes | regular point      | good leverage point
%      large ResD/StdRes | vertical outlier   | bad leverage point
%
% For more details see:
%    Hubert, M., Verboven, S. (2003),
%    "A robust PCR method for high-dimensional regressors,"
%    Journal of Chemometrics, 17, 438-452.
%
% Required input arguments:
%
%  xdist   : Score distances from a PCA analysis or
%            Mahalanobis distances from LS or MLR analysis or
%            Robust distances from LTS or MCDREG analysis,
%  ydist   : Residual distances of the PCR method or
%            Standardized residuals of one of the regression methods,
%  cutoffx : Cutoff value for the SD/MD/RD,
%  cutoffy : Cutoff value for the residual-distance,
%  k       : Number of  principal components used in the PCR/PLS method, or zero
%            if not available.
%  multi   : 0 for univariate analysis, 1 for multivariate analysis
%  attrib  : String identifying the method used =  'LS', 'MLR', 'LTS', 'MCDREG', 'RPCR',
%           'CPCR', 'RSIMPLS', 'CSIMPLS'
%
% Optional inputs:
%   labsd    : number of displayed points with largest distance on x-axes (default = 3)
%   labresd  : number of displayed points with largest distance on y-axes (default = 3)
%
%
% I/O: regresdiagplot(out.sd,out.rd,out.cutoff.sd,out.cutoff.rd,k,multi,attrib,labsd,labresd)
%
% Example: regresdiagplot(out.sd,out.rd,out.cutoff.sd,out.cutoff.rd,out.k,1,'RPCR',5,5)
%          regresdiagplot(out.md,out.stdres,out.cutoffmd,0,0,0,'LS',5,4)
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Written by Sabine Verboven
% Created 25 February 2002
% Revised : 12/02/2004
%

if nargin==8
    labresd=3;
end
if nargin==7
    labsd=3;
    labresd=3;
end
if nargin<7
    error('A required input variable is missing!')
end

if ischar(xdist) | ischar(ydist)
    mess=sprintf(['Warning: A singularity was detected in the analysis. ',...
            '\n No regression outlier map available.']);
    disp(mess)
    return
end
%all LTS-analysis in RPCR are intercept included!!!
if multi==1 %multivariate analysis
    x=xdist; %distances on x-axis: SD, MD or RD
    y=ydist; %residual distances
    quanty=cutoffy;
    quantx=cutoffx;
else  %univariate analysis
    quantx=cutoffx;
    quanty=sqrt(chi2inv(0.975,1));
    x=xdist;   %distances on x-axis: SD, MD, RD
    y=ydist;   %standardized residuals!!!
end

plot(x,y,'o');
set(gcf,'Name', 'Regression outlier map', 'NumberTitle', 'off');
hold on
xmin=0;%max(0,min(x));%-0.5;
xmax=max([quantx max(x)])+0.5;

if multi==1
    ymin=0;%max(0,min(y));%-1;
    if size(y,2)==1
        y=y';
    end
    ymax=max([y,quanty])+1;
    xlim([xmin,xmax]);
    ylim([ymin,ymax]);
    ylimi=get(gca,'Ylim');
    line([quantx,quantx],[ylimi(1),ylimi(2)],'Linestyle','-','Color','r');
    xlimi=get(gca,'Xlim');
    line([xlimi(1),xlimi(2)],[quanty,quanty],'Linestyle','-','Color','r');
    if labsd
        plotnumbers(x',y',labsd,labresd,2)%sort by  x and  y
    end
    ylabel('Residual distance')
else
    ymin=min([-4 min(y)])-0.5;
    ymax=max([4 max(y)])+0.5;
    xlim([xmin,xmax]);
    ylim([ymin,ymax]);
    ylimi=get(gca,'Ylim');
    ylimi=[min(ylimi(1),min([-4 min(y)]))-0.5,max(ylimi(2),max([4 max(y)]))+0.5];
    line([quantx,quantx],[ylimi(1),ylimi(2)],'Linestyle','-','Color','r');
    xlimi=get(gca,'Xlim');
    xlimi=[min(xlimi(1),min(x))-1,max(xlimi(2),max([quantx max(x)]))+1];
    line([xlimi(1),xlimi(2)],[quanty,quanty],'Linestyle','-','Color','r');
    line([xlimi(1),xlimi(2)],[-quanty,-quanty],'Linestyle','-','Color','r');
    if labsd
        plotnumbers(x',y',labsd,labresd,3)%sort by x and abs(y)
    end
    ylabel('Standardized residual')
end

box on

switch attrib
    case 'LS'
        xlabel('Mahalanobis distance');
        ylabel('Standardized LS residual');
    case 'MLR'
        xlabel('Mahalanobis distance');
    case 'LTS'
        xlabel('Robust distance computed by MCD');
        ylabel('Standardized LTS residual');
    case 'MCDREG'
        xlabel('Robust distance computed by MCD');
    case {'CPCR'}
        xlabel(['Score distance (',num2str(k),' LV)']);
    case {'RPCR'}
        xlabel(['Score distance (',num2str(k),' LV)']);
    case {'CSIMPLS'}
        xlabel(['Score distance (',num2str(k),' LV)']);
    case {'RSIMPLS'}
        xlabel(['Score distance (',num2str(k),' LV)']);

end
title(attrib)
hold off


function [A,B,D,E,I]=bpf1(X)
% Multiple Feedback Bandpass Filter
% Note that complex frequency variable s is not used here.
% These arrays are real, not complex.
%
R1=X(1);R2=X(2);R3=X(3);C1=X(4);C2=X(5);
%
N=2;M=1;U=1;Y=1;
%
A1=[0 1 1;
   1/R3 1 0;
   1 0 0];
%
B2=[-(1/R1+1/R2) 0 1/R1;
   0 0 0;
   1 -1 0];
%
P=diag([C1 C2]);
%
% The following statements never change from circuit to circuit.
%
V=A1\B2;H=V(U+1:U+N,1:N+M);I=eye(N);
AB=P\H;A=AB(1:N,1:N);B=AB(1:N,N+1:N+M);
D=V(Y:Y,1:N);E=V(Y:Y,N+1:N+M);


% the MWF beamformer
% r1MWF:    h = PhiN^-1 * PhiX * refVec / (mu + trace(PhiN^-1 * PhiX)
% SDWMWF:   h = (PhiX + mu * PhiN)^-1 * PhiX * refVec
% input:    PhiN, (Nch, Nch, Nbin) the noise covariance matrix
%           PhiX, (Nch, Nch, Nbin) the speech covariance matrix
%           mu, the speech distortion/noise reduction trade-off parameter
%           refMic, the reference microphone, default = 1
% output:   h, (Nch, Nbin)  the beamformer coefficients
% % Ziteng Wang @ 201812

function h = MWF(PhiX, PhiN, mu, refMic, choice)
if nargin < 3
    mu = 1;                 % typical value {0, 1}
elseif nargin < 4
    refMic = 1;
elseif nargin < 5
    choice = 'r1MWF';       % collected from 'r1MWF' 'SDWMWF'
end

[Nch, ~, Nbin] = size(PhiX);
refVec = zeros(Nch, 1);
refVec(refMic) = 1;
lambda = zeros(Nbin, 1);
h = zeros(Nch, Nbin);

for bin = 1:Nbin
    if rcond(PhiN(:,:,bin)) < eps
        disp(['bin ' num2str(bin) ': Noise covariance ill-conditioned.']);
        PhiN(:,:,bin) = PhiN(:,:,bin) + 1e-10 * eye(Nch);
    end
    if strcmp(choice, 'r1MWF')
        tmp = PhiN(:,:,bin) \ PhiX(:,:,bin);
        lambda(bin) = trace(tmp);
        h(:,bin) = tmp * refVec / (mu + lambda(bin));
        %%% Gain factor to normalize the steering vector. The results are
        %%% better. Needs further checking
        %gain_factor = norm(PhiX(:,refMic,bin) / PhiX(refMic,refMic,bin));
        %h(:,bin) = gain_factor * h(:,bin);
    elseif strcmp(choice, 'SDWMWF') % SDW-MWF is more sensitive to estimation errors.
        tmp = (PhiX(:,:,bin) + mu * PhiN(:,:,bin)) \ PhiX(:,:,bin);
        h(:,bin) = tmp * refVec;
    end
end


function [ heat_long, heat_lat, mse_doa ] = create_heatmap( doa_meters12, doa_meters13, doa_meters23, rx1_lat, rx1_long, rx2_lat, rx2_long, rx3_lat, rx3_long, resolution, geo_ref_lat, geo_ref_long )
%create_heatmap_kl Creates a heatmap for based on mean squared error

%    returns:
%    heat_long: longitudes of heatmap points
%    heat_lat: latitudes of heatmap points
%    mse_doa: heatmap magnitudes

    disp('creating heatmap... ');

    num_points = resolution; % points in one dimension (creates squared area)

    % defines the area, where the heatmap is displayed (around the geodetic
    % reference point)
    lat_span = 0.05;
    start_lat = geo_ref_lat - lat_span;
    stop_lat  = geo_ref_lat + lat_span;

    long_span = 0.2;
    start_long = geo_ref_long - long_span;
    stop_long  = geo_ref_long + long_span;

    % create heatmap
    heat_lat  = linspace(start_lat,  stop_lat,  num_points);
    heat_long = linspace(start_long, stop_long, num_points);
    mse_doa = zeros(num_points, num_points);

    for lat_idx = 1:num_points
        for long_idx = 1:num_points
            % calculate mean squared error of current point in terms of tdoa

            % distance current point to receivers
            dist_to_rx1 = dist_latlong( heat_lat(lat_idx), heat_long(long_idx), rx1_lat, rx1_long, geo_ref_lat, geo_ref_long );
            dist_to_rx2 = dist_latlong( heat_lat(lat_idx), heat_long(long_idx), rx2_lat, rx2_long, geo_ref_lat, geo_ref_long );
            dist_to_rx3 = dist_latlong( heat_lat(lat_idx), heat_long(long_idx), rx3_lat, rx3_long, geo_ref_lat, geo_ref_long );

            % current doa in meters
            current_doa12 = dist_to_rx1 - dist_to_rx2;
            current_doa13 = dist_to_rx1 - dist_to_rx3;
            current_doa23 = dist_to_rx2 - dist_to_rx3;

            % error doa
            doa_error = (current_doa12 - doa_meters12)^2 + (current_doa13 - doa_meters13)^2 + (current_doa23 - doa_meters23)^2;
            mse_doa(long_idx, lat_idx) = doa_error;
        end
    end

    mse_doa = 1./mse_doa;
    mse_doa = mse_doa .* (1/max(max(mse_doa)));

    disp('creating heatmap done! ');
end


function act = softRect(x)
%  act = softRect(x)
%-----------------------------------------------------
% Soft rectification activation function
%-----------------------------------------------------

act = log(1 + exp(x));


function lagrange_basis_display_test ( )

%*****************************************************************************80
%
%% LAGRANGE_BASIS_DISPLAY_TEST tests the LAGRANGE_BASIS_DISPLAY library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 August 2012
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LAGRANGE_BASIS_DISPLAY_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the LAGRANGE_BASIS_DISPLAY_TEST library.\n' );
%
%  Plot the 11 standard "basis vectors".
%
  a = 0.0;
  b = 1.0;
  m = 10;

  standard_basis_display ( a, b, m )
%
%  Plot the 11 Lagrange "basis vectors" for evenly spaced nodes.
%
  a = 0.0;
  b = 1.0;
  m = 10;
  xd = linspace ( a, b, m + 1 )

  lagrange_basis_display ( a, b, m, xd )
%
%  Plot the 11 Lagrange "basis vectors" for Chebyshev-spaced nodes.
%
  a = -1.0;
  b = 1.0;
  m = 10;
  xd(1:m+1) = cos ( pi * ( 2 * ( m + 1 ) + 1 - 2 * (1:m+1) ) / ( 2 * ( m + 1 ) ) );

  lagrange_basis_display ( a, b, m, xd )
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LAGRANGE_BASIS_DISPLAY_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function [samples] = to_qpsk(bitvec)
    assert(iscolumn(bitvec) || isrow(bitvec), "Bit vector must be a row or column vector");
    assert(mod(length(bitvec), 2) == 0, "Length of bit vector must be an even number")

    % Keep the power of each point at 1.0
    amplitude = 1/sqrt(2);

    samples = zeros(length(bitvec) / 2, 1);

    for idx=1:2:length(bitvec)
        bits = bitvec(idx:idx+1);

        if (bits == [0, 0])
            sample = amplitude + amplitude * 1j;
        elseif (bits == [0 1])
            sample = amplitude - amplitude * 1j;
        elseif (bits == [1 0])
            sample = -amplitude + amplitude * 1j;
        else
            sample = -amplitude - amplitude * 1j;
        end

        samples((idx + 1) / 2) = sample;
    end
end


function [ deltaHD , deltaSEN , deltaPRO ] = CV2XMode4_common( lambda , Pt , distance, Psen , step_dB , noise , coding);

% CV2XMode4_common is a script that calculates the probability of packet
% loss due to half-duplex transmissions, the probability of packet loss
% due to a received signal power below the sensing power threshold and the
% probability of packet loss due to propagation effects for different
% Tx-Rx distances based on the models described in the following paper:
%
%    Manuel Gonzalez-Martín, Miguel Sepulcre, Rafael Molina-Masegosa, Javier Gozalvez,
%    "Analytical Models of the Performance of C-V2X Mode 4 Vehicular Communications",
%    IEEE Transactions on Vehicular Technology, Vol. 68, Issue 2, Feb. 2019. DOI: 10.1109/TVT.2018.2888704
%    Final version available at: https://ieeexplore.ieee.org/document/8581518
%    Post-print version available at: https://arxiv.org/abs/1807.06508
%
% CV2XMode4_common is called from the main script, CV2XMode4.
%
% Input parameters:
%   lambda: packet transmission frequency in Hz. .
%   Pt: transmission power in dBm.
%   distance: distance between tramsmitter and receiver in meters. It can be a vector with multiple distances.
%   Psen: sensing threshold in dBm.
%   step_dB: discrete steps to compute the PDF of the SNR and SINR in dB.
%   noise: noise corresponding to the DATA field of each message. Assumes a noise figure of 9dB and 10MHz channel (background noise of -95dBm). The total number of RBs in 10MHz is 50.
%   coding: ID of the coding used to identify the BLER curve
%
% Output metrics:
%    deltaHD: probability of packet loss due to half-duplex transmissions for different Tx-Rx distances
%    deltaSEN: probability of packet loss due to a received signal power below the sensing power threshold for different Tx-Rx distances
%    deltaPRO: probability of packet loss due to propagation effects for different Tx-Rx distances
%
% The equations that are identified with a number between brackets in this script are the ones
% that also appear in the paper so that they can be easily identified.

    D = length(distance);

    deltaHD(1:D) = lambda/1000;  % Equation (7)

    [PL_E_R, std_dev_E_R] = get_PL_SH(distance);   % Obtains pathloss and shadowing for different Tx-Rx distances.
    deltaSEN = 0.5 * ( 1 - erf( (Pt - PL_E_R - Psen)./(std_dev_E_R*sqrt(2)) ) );         % Equation (10)

    [SNR, PDF_SNR] = get_SINRdistribution( Pt - PL_E_R , -180 , std_dev_E_R , 3 , noise , Psen , step_dB); % Obtains the PDF of the SNR experienced by Rx (without interference).
    deltaPRO = get_BLER( SNR , PDF_SNR , coding , step_dB);   % Equation (13)

end


function value = local_sum(input, window_size)
% LOCAL_SUM Fast computation of 2D local sum using integral images.
%
% Algorithm is described in Viola and Jones, "Robust real-time face
% detection", International Journal of Computer Vision, 2004.  Computation
% time is independent of window size. Precision may be an issue for very
% large images, but a few thousand by a few thousand images seems to work
% with no issues.
%
% Suprisingly, for small window sizes, MATLAB's conv2 actually seems to be
% a faster way to do this, but this implementation gets much faster as
% window sizes get larger.  MATLAB Central's fastrunmean demonstrates
% another way of computing fast means/sums.  Fastrunmean is not quite as
% fast as this integral image implementation, but doesn't have the same
% potential precision issues for very large images.
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

padsize = floor(window_size/2);
padded = padarray(input, padsize+1, 0, 'pre'); % Zeropad to handle local sums at edges
padded = padarray(padded, padsize, 0, 'post'); % Zeropad to handle local sums at edges
padded = cumsum(cumsum(double(padded),1),2); % Create integral image
value = padded(1:size(input,1),1:size(input,2))-... % Compute sum over local window
    padded(1:size(input,1),(window_size(2)+1):end)-...
    padded((window_size(1)+1):end,1:size(input,2))+...
    padded((window_size(1)+1):end,(window_size(2)+1):end);

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function [ C ] = gsp_gwft(G,g,f, param )
%GSP_GWFT Generalized windowed Fourier transform
%   Usage:  C = gsp_gwft(G,g,f, param );
%           C = gsp_gwft(G,g,f);
%
%   Input parameters:
%         G     : Graph
%         g     : Window (graph signal or kernel)
%         f     : Graph signal (column vector)
%         param : Structure of optional parameter
%   Output parameters:
%         C     : Coefficient.
%
%   This function compute the graph windowed Fourier transform of a signal
%   *f* with the window *g*. The function returns a matrix of size N^2*N.
%
%   *param* a Matlab structure containing the following fields:
%
%     * *param.verbose* : 0 no log, 1 print main steps, 2 print all steps.
%       By default, it is 1.
%     * *param.lowmemory* : use less memory. By default, it is 1.
%
%   Reference: shuman2013windowed

% Author : Nathanael Perraudin
% Testing: test_gwft

% Optional input arguments
if nargin<4, param=struct; end
if ~isfield(param, 'verbose'), param.verbose=1 ; end
if ~isfield(param, 'lowmemory'), param.lowmemory=1 ; end

if ~isfield(G,'U')
   error(['GSP_GWFT: You need first to compute the Fourier basis. ',...
       'You can do it with the function gsp_compute_fourier_basis']);
end

if sum(G.U(:,1)<eps(2))
    error(['GSP_GWFT: The current implementation of this function is ',...
       'not working for disconnected graphs'])
end

Nf = size(f,2);

if iscell(g)
    g = gsp_igft(G,g{1}(G.e));
end

if isa(g, 'function_handle')
    g = gsp_igft(G,g(G.e));
end

if ~param.lowmemory
    % Compute the Frame into a big matrix
    Frame=gsp_gwft_frame_matrix(G,g,param);

    C=Frame'*f;

    C=reshape(C,G.N,G.N,Nf);
else
    % Compute the translate of g
    ghat=G.U'*g;
    Ftrans=sqrt(G.N)*G.U*(repmat(ghat,1,G.N).*G.U');
    C=zeros(G.N);
    for jj = 1:Nf
        for ii=1:G.N
             C(:,ii,jj)=(repmat(1./G.U(:,1),1,G.N) .* ...
                      G.U.*repmat(Ftrans(:,ii),1,G.N))'*f(:,jj);
        end
    end

end


function linpack_d_test11 ( )

%*****************************************************************************80
%
%% TEST11 tests DGEFA and DGESL.
%
%  Discussion:
%
%    In this example, we solve a relatively large linear system.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 100;
  lda = n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST11\n' );
  fprintf ( 1, '  For a general matrix,\n' );
  fprintf ( 1, '  DGEFA computes the LU factors;\n' );
  fprintf ( 1, '  DGESL solves a factored linear system;\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Assign values to the matrix A and the right hand side B.
%
%  The problem is just an enlarged version of the
%  problem for N = 5, which is:
%
%  Matrix A is ( n -1 -1 -1 -1)    Right hand side B is  (1)
%              (-1  n -1 -1 -1)                          (1)
%              (-1 -1  n -1 -1)                          (1)
%              (-1 -1 -1  n -1)                          (1)
%              (-1 -1 -1 -1  n)                          (1)
%
%  Solution is   (1)
%                (1)
%                (1)
%                (1)
%                (1)
%
  b(1:n) = 1.0;

  a(1:n,1:n) = -1.0;
  for i = 1 : n
    a(i,i) = n;
  end
%
%  Factor the matrix.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Factor the matrix\n' );

  [ a, ipvt, info ] = dgefa ( a, lda, n );

  if ( info ~= 0 )
    fprintf ( 1, '  DGEFA returned an error flag INFO = %d\n', info );
    return
  end
%
%  Solve the linear system.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the linear system.\n' );

  job = 0;
  b = dgesl ( a, lda, n, ipvt, b, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The first and last five entries of the solution:\n' );
  fprintf ( 1, '  (All of them should be 1.)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    if ( i <= 5 | n-5 < i )
      fprintf ( 1, '  %6d  %14f\n', i, b(i) );
    end
    if ( i == 5 )
      fprintf ( 1, '  ......  ..............\n' );
    end
  end

  return
end


function output=rotateAround(image, pointY, pointX, angle, varargin)
% ROTATEAROUND rotates an image.
%   ROTATED=ROTATEAROUND(IMAGE, POINTY, POINTX, ANGLE) rotates IMAGE around
%   the point [POINTY, POINTX] by ANGLE degrees. To rotate the image
%   clockwise, specify a negative value for ANGLE.
%
%   ROTATED=ROTATEAROUND(IMAGE, POINTY, POINTX, ANGLE, METHOD) rotates the
%   image with specified method:
%       'nearest'       Nearest-neighbor interpolation
%       'bilinear'      Bilinear interpolation
%       'bicubic'       Bicubic interpolation
%    The default is 'nearest'.
%
%   Example
%   -------
%       image=imread('eight.tif');
%       imshow(rotateAround(image, 1, 1, 10));
%
%   See also IMROTATE, PADARRAY.

%   Contributed by Jan Motl (jan@motl.us)
%   $Revision: 1.0 $  $Date: 2013/02/22 16:58:01 $

% Parameter checking.
numvarargs = length(varargin);
if numvarargs > 1
    error('myfuns:somefun2Alt:TooManyInputs', ...
        'requires at most 1 optional input');
end
optargs = {'nearest'};    % Set defaults for optional inputs
optargs(1:numvarargs) = varargin;
[method] = optargs{:};    % Place optional args in memorable variable names

% Initialization.
[imageHeight imageWidth] = size(image);
centerX = floor(imageWidth/2+1);
centerY = floor(imageHeight/2+1);

dy = centerY-pointY;
dx = centerX-pointX;

% How much would the watched point shift if rotated around the img center.
[theta, rho] = cart2pol(-dx,dy);
[newX, newY] = pol2cart(theta+deg2rad(angle), rho);
shiftX = round(pointX-(centerX+newX));
shiftY = round(pointY-(centerY-newY));

% Pad the image to preserve the whole image during the rotation.
padX = abs(shiftX);
padY = abs(shiftY);

padded = padarray(image, [padY padX]);

% Rotate the image around the center.
rot = imrotate(padded, angle, 'crop', method);

% Crop the image.
output = rot(padY+1-shiftY:end-padY-shiftY, padX+1-shiftX:end-padX-shiftX);


function outsig=rampdown(L,wintype)
%RAMPDOWN  Falling ramp function
%   Usage: outsig=rampdown(siglen);
%
%   `rampdown(siglen)` will return a falling ramp function of length
%   *siglen*. The ramp is a sinusoid starting from one and ending at
%   zero. The ramp is centered such that the first element is always one and
%   the last element is not quite zero, such that the ramp fits with
%   following zeros.
%
%   `rampdown(L,wintype)` will use another window for ramping. This may be
%   any of the window types from |firwin|. Please see the help on |firwin|
%   for more information. The default is to use a piece of the Hann window.
%
%   See also: rampup, rampsignal, firwin

complainif_argnonotinrange(nargin,1,2,mfilename);

if nargin==1
  wintype='hann';
end;

win=firwin(wintype,2*L,'inf');
outsig=win(1:L);


function d=center(x,op);

%  CENTER  Delay calculation for Wavelet transform alignment.
%
%          CENTER (X, OP) calculates the delay for filter in X
%          according to the alignment method indicated in OP.
%          This delay is used by Wavelet transform functions.
%          The value of OP can be:
%              0 : First Absolute Maxima Location
%              1 : Zero delay in analysis (Full for synthesis).
%              2 : Mass center (sum(m*d)/sum(m))
%              3 : Energy center (sum(m^2 *d)/sum(m^2))
%
%          If no output argument is given, then the vector X will
%          be plotted in the current figure, and a color line will be
%          marking the result.(red: OP=0; green: OP=1; cyan: OP=2;
%          blue: OP=4)
%
%          See also: WTCENTER, WTMETHOD

%--------------------------------------------------------
% Copyright (C) 1994, 1995, 1996, by Universidad de Vigo
%
%
% Uvi_Wave 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, or (at your option) any
% later version.
%
% Uvi_Wave is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
% FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
% for more details.
%
% You should have received a copy of the GNU General Public License
% along with Uvi_Wave; see the file COPYING.  If not, write to the Free
% Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
%
%       Author: Sergio J. Garcia Galan
%       e-mail: Uvi_Wave@tsc.uvigo.es
%--------------------------------------------------------

lx=length(x);
l=1:lx;

if op==1
	d=0;
else
	if op==2
		xx=abs(x(:)');
		L=l;
	end
	if op==3
		xx=x(:)'.^2;
		L=l;
	end
	if op==0
		[mx,d]=max(abs(x));
	else

		d=sum(xx.*L)/sum(xx);
	end
end

if nargout==0,
  cad='rgcbk';
  plot(x)
  l=line([d,d],[min(x),max(x)]);
  set(l,'Color',cad(op+1));
end


 function [basis, Hw] = deconv_z(psf, fun, t)
% psf: 3-tap symmetric psf: [a _b_ a]
% H(z) = 1 / (az + b + a/z)

if numel(psf) == 1
	basis = fun(t) / psf;
	Hw = @(t) psf * ones(size(t));
return
end

if length(psf) ~= 3, error 'not done', end
a = psf(1);
b = psf(2);
c = b / a / 2;
p = -c + sign(c) * sqrt(c^2 - 1); % pole

scale = 1/a * 1/(p - 1/p);
basis = fun(t);
for n=1:9
	basis = basis + p^n * (fun(t-n) + fun(t+n));
end
basis = scale * basis;

Hw = @(om) 1 ./ (b + 2 * a * cos(om));


function r = intrand(p, m, n);
%INTRAND Random matrices from uniform integer distribution.
%
%   R = INTRAND(P) returns a matrix of random numbers chosen
%   from the uniform integer distribution with parameter P. The
%   minimum and maximum output integer numbers are P(1) and P(2).
%   R = INTRAND(P,M,N) returns an M by N matrix.
%
%
% Copyright (c) 2000 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.

if nargin < 1 | any(size([p(:)])~=[2 1])
  error('Incorrect range assignment in INTRAND');
end
if p(1)<p(2)
  rmin=ceil(p(1));
  rrange=floor(p(2))-rmin+1;
else
  rmin=ceil(p(2));
  rrange=floor(p(1))-rmin+1;
end

if nargin < 2
  r=floor(rand.*rrange+rmin);
else
  if nargin < 3
    n=m;
  end
  r=floor(rand([m n]).*rrange+rmin);
end


function y=fshift(x,f,fs)

% y=fshift(x,f,fs)
%
% Shift time domain signal x, sampled at a sampling rate of fs by
% f. fs defaults to 2.

% Copyright 2012 Evrytania LLC (http://www.evrytania.com)
%
% Written by James Peroulas <james@evrytania.com>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

error(nargchk(2,3,nargin));
error(chk_param(x,'x','vector','horizontal'));
error(chk_param(f,'f','scalar','real'));
if (nargin<3)
  fs=2;
end
error(chk_param(fs,'fs','scalar','real','>',0));

y=x.*exp((j*pi*f/(fs/2))*(0:length(x)-1));


%PREX_CONFMAT PRTools example on confusion matrix, scatterplot and gridsize
%
% Prtools example code to show the use of confusion matrix,
% scatterplot and gridsize.

help prex_confmat

delfigs
randstate = randreset;
echo on
                % Load 8-class 2D problem
  randn('state',1);
  rand('state',1);
  a = gendatm;
                % Compute the Nearest Mean Classifier
  w = nmc(a);
                % Scatterplot
  figure;
  gridsize(30);
  scatterd(a,'legend');
                % Plot the classifier
  plotc(w);
  title([getname(a) ', Gridsize 30']);
                % Set higher gridsize
  gridsize(100);
  figure;
  scatterd(a,'legend');
  plotc(w);
  title([getname(a) ', Gridsize 100']);

                % Classify training set
  d = a*w;
                % Look at the confusion matrix and compare it to the scatterplot
  confmat(d);

echo off
randreset(randstate);
showfigs
c = num2str(gridsize);
disp(' ')
disp('   Classifier plots are inaccurate for small gridsizes. The standard');
disp('   value of 30 is chosen because of the speed, but it is too low to');
disp('   ensure good plots. Other gridsizes may be set by gridsize(n).')
disp('   Compare the two figures and appreciate the difference.')


function img = discreteCube(varargin)
%DISCRETECUBE discretize a 3D cube
%
%   IMG = discreteCube(LX, LY, LZ, CUBE)
%   Creates a 3D image of a cube, given coordinates of voxel centers in LX,
%   LY and LZ, and parameters of the shape in CUBE.
%
%   CUBE has the format:
%   [XC YC ZC SIDE], with (XC YC ZC) being the coordinates of the center,
%   and SIDE being the side length of the cube.
%
%   Oriented cubes are represented as follow:
%   [XC YC ZC SIDE THETA PHI PSI], with (THETA, PHI) being the angle of the
%   normal of the cube (THETA is colatitude, PHI is longitude), and PSI is
%   rotation of the cube around the normal. All angles are in degrees.
%
%   IMG = discreteCube(DIM, ...)
%   DIM is the size of image, with the format [x0 dx x1;y0 dy y1;z0 dz z1]
%
%   IMG = discreteCube(DIM, CENTER, SIDE, THETA, PHI)
%   Also specify spherical angle of the normal of a face of the cube. THETA
%   is the angle from the vertical, and PHI is the angle from the 0x axis,
%   both between 0 and 360.
%
%   IMG = discreteCube(DIM, CENTER, SIDE, THETA, PHI, PSI)
%   Also consider 'roll' of the cube around its normal axis. PSI is an
%   angle given in degrees between 0 and 360.
%
%   IMG = discreteCube(DIM, CUBE)
%   send parameters in a row vector, where CUBE contains at least the
%   center coordinate, and possibly the other parameters.
%
%   Example
%   img = discreteCube(1:100, 1:100, 1:100, [50 50 50 30 10 60 45]);
%   img = discreteCube([1 1 100;1 1 100;1 1 100], [50 50 50], 30, 10);
%   img = discreteCube([1 1 100;1 1 100;1 1 100], [50 50 50 30 10]);
%
%   See Also
%   imShapes, discreteBall, discreteEllipsoid, discreteCylinder, drawCube
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2006-02-27
% Copyright 2006 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   HISTORY
%   03/11/2006: concatenate transforms, add doc for PSI
%   04/03/2009: use meshgrid
%   30/04/2009: update transforms
%   29/05/2009: use more possibilities for specifying grid
%   22/01/2010: fix auto center with odd image size
%   2011-06-21 use degrees for angles

% compute coordinate of image voxels
[lx, ly, lz, varargin] = parseGridArgs3d(varargin{:});
[x, y, z] = meshgrid(lx, ly, lz);

% default parameters
center  = [lx(ceil(end/2)) ly(ceil(end/2)) lz(ceil(end/2))];
side    = center;
theta   = 0; phi = 0; psi=0;

% process input parameters
if length(varargin)==1
    var = varargin{1};
    center = var(:,1:3);
    if size(var, 2)>3
        side = var(:,4);
    end
    if size(var, 2)>4
        theta = var(:,5);
    end
    if size(var, 2)>5
        phi = var(:,6);
    end
    if size(var, 2)>6
        psi = var(:,7);
    end

elseif ~isempty(varargin)
    center = varargin{1};
    if length(varargin)>1
        side = varargin{2};
    end
    if length(varargin)>2
        theta = varargin{3};
    end
    if length(varargin)>3
        phi = varargin{4};
    end
    if length(varargin)>4
        psi = varargin{5};
    end
end

if length(side) == 1
    side = [side side side];
end

% compute coordinate of image voxels in cube reference system
trans = composeTransforms3d(...
    createTranslation3d(-center),...
    createRotationOz(-deg2rad(phi)),...
    createRotationOy(-deg2rad(theta)), ...
    createRotationOz(-deg2rad(psi)));
[x, y, z] = transformPoint3d(x, y, z, trans);

% create image: simple threshold over 3 dimensions
img = abs(x) <= side(1)/2 & abs(y) <= side(2)/2 & abs(z) <= side(3)/2;


function [mi entropy fd_bins] = mutualinformationx(x,y,fd_bins,permtest)
% MUTUALINFORMATIONX   Compute mutual information between two vectors
%
%   Inputs:
%       x,y     :  data matrices of equal size
%
%   Optional inputs:
%       bins    :  number of bins to use for distribution discretization
%      permtest :  perform permutation test and return mi in standard-Z values
%
%   Outputs:
%       mi      :  mutual information in bits
%       entropy :  entropy of x, y, and joint
%       nbins   :  number of bins used for discretization
%                  (based on Freedman-Diaconis rule)
%
%  Mike X Cohen (mikexcohen@gmail.com)

if nargin<2, error('Specify two inputs.'); end
if length(x)~=length(y), error('X and Y must have equal length'); end

%% determine the optimal number of bins for each variable

% vectorize in the case of matrices
x=x(:); y=y(:);

if nargin<3 || isempty(fd_bins)
    n            = length(x);
    maxmin_range = max(x)-min(x);
    fd_bins1     = ceil(maxmin_range/(2.0*iqr(x)*n^(-1/3))); % Freedman-Diaconis

    n            = length(y);
    maxmin_range = max(y)-min(y);
    fd_bins2     = ceil(maxmin_range/(2.0*iqr(y)*n^(-1/3)));

    % and use the average...
    fd_bins = ceil((fd_bins1+fd_bins2)/2);
end

%% bin data

edges = linspace(min(x),max(x),fd_bins+1);
[nPerBin1,bins1] = histc(x,edges);

edges = linspace(min(y),max(y),fd_bins+1);
[nPerBin2,bins2] = histc(y,edges);

%% compute entropies

% recompute entropy with optimal bins for comparison
hdat1 = hist(x,fd_bins);
hdat1 = hdat1./sum(hdat1);
hdat2 = hist(y,fd_bins);
hdat2 = hdat2./sum(hdat2);

% convert histograms to probability values
for i=1:2
    eval([ 'entropy(' num2str(i) ') = -sum(hdat' num2str(i) '.*log2(hdat' num2str(i) '+eps));' ]);
end

%% compute joint probabilities

jointprobs = zeros(fd_bins);
for i1=1:fd_bins
    for i2=1:fd_bins
        jointprobs(i1,i2) = sum(bins1==i1 & bins2==i2);
    end
end
jointprobs=jointprobs./sum(jointprobs(:));

entropy(3) = -sum(jointprobs(:).*log2(jointprobs(:)+eps));

%% mutual information

mi = sum(entropy(1:2)) - entropy(3);

%% optional permutation testing

if nargin==4

    npermutes = 500;
    n = length(bins2);

    perm_mi = zeros(1,npermutes);

    for permi=1:npermutes

        jointprobs = zeros(fd_bins);

        % shuffle bins
        binbreak = randsample(round(n*.8),1,1)+round(n*.1);
        switch mod(permi,4)
            case 0, bins2 = [ bins2(binbreak:end);    bins2(1:binbreak-1) ];
            case 1, bins2 = [ bins2(end:-1:binbreak); bins2(1:binbreak-1) ];
            case 2, bins2 = [ bins2(binbreak:end);    bins2(binbreak-1:-1:1) ];
            case 3, bins2 = [ bins2(end:-1:binbreak); bins2(binbreak-1:-1:1) ];
        end

        for i1=1:fd_bins
            for i2=1:fd_bins
                jointprobs(i1,i2) = sum(bins1==i1 & bins2==i2);
            end
        end
        jointprobs=jointprobs./sum(jointprobs(:));

        perm_jentropy = -sum(jointprobs(:).*log2(jointprobs(:)+eps));

        % mutual information
        perm_mi(permi) = sum(entropy(1:2)) - perm_jentropy;
    end

    mi = (mi-mean(perm_mi))/std(perm_mi);
end

%%
% simplified replacement for randsample
function y = randsample(x,n,junk)
y=randperm(x);
y=y(1:n);


function res = reconWUpyr(pyr, pind, daub_order);

% RES = reconWUpyr(PYR, INDICES, DAUB_ORDER)

% Reconstruct image from its separable undecimated orthonormal QMF/wavelet pyramid
% representation, as created by buildWUpyr.
%
% PYR is a vector containing the N pyramid subbands, ordered from fine
% to coarse.  INDICES is an Nx2 matrix containing the sizes of
% each subband.
%
% DAUB_ORDER: specifies the order of the daubechies wavelet filter used

% JPM, Univ. de Granada, 03/2003, based on Rice Wavelet Toolbox
% functions "mrdwt" and  "mirdwt", and on Matlab Pyrtools from Eero Simoncelli.

Nor = 3;
Nsc = (size(pind,1)-2)/Nor-1;
h = daubcqf(daub_order);

yh = [];

nband = 1;
last = prod(pind(1,:)); % empty "high pass residual band" for compatibility with full steerpyr 2
for nsc = 1:Nsc+1,  % The number of scales corresponds to the number of pyramid levels (also for compatibility)
    for nor = 1:Nor,
        nband = nband +1;
        first = last + 1;
        last = first + prod(pind(nband,:)) - 1;
        band = pyrBand(pyr,pind,nband);
        sh = (daub_order/2 - 1)*2^nsc;  % approximate phase compensation
        if nsc > 2,
            band = expand(band, 2^(nsc-2));
        end
        if nor == 1,        % horizontal
            band = shift(band, [-sh -2^(nsc-1)]);
        elseif nor == 2,    % vertical
            band = shift(band, [-2^(nsc-1) -sh]);
        else
            band = shift(band, [-sh -sh]);    % diagonal
        end
        yh = [yh band];
    end
end

nband = nband + 1;
band = pyrBand(pyr,pind,nband);
lpr = expand(band,2^Nsc);

res= mirdwt(lpr,yh,h,Nsc+1);


function [f_I] = ToolInstFreq(X,iHop,fs)
    % get phase
    phi         = angle(X);

    % phase offset
    omega       = pi*iHop/size(X,2)*(0:size(X,2)-1);

    % unwrapped difference
    deltaphi    = omega + princarg_I(phi(2,:)-phi(1,:)-omega);

    % instantaneous frequency
    f_I         = deltaphi/iHop/(2*pi)*fs;
end

function phase = princarg_I(phi)
    phase = mod(phi + pi,-2*pi) + pi;
end


function [Y, spectrum] = sllemap(G, d, sch)
%SLLEMAP Solves Laplacian Eigenmap Embedding
%
% $ Syntax $
%   - Y = sllemap(G, d)
%   - Y = sllemap(G, d, sch)
%   - [Y, spectrum] = sllemap(...)
%
% $ Arguments $
%   - G:        The affinity graph (in any acceptable form): n x n
%   - d:        The embedding dimension
%   - sch:      The scheme to use
%   - Y:        The solved embedded coordinates (d x n)
%
% $ Description $
%   - Y = sllemap(G, d) uses the default scheme to solve the Laplacian
%     Eigenmap embedding in a d-dimensional space.
%
%   - Y = sllemap(G, d, sch) uses the specified scheme to solve the
%     Laplacian Eigenmap embedding in a d-dimensional space.
%
%     Three schemes are implemented to resolve the problem, they are
%     under three different formulations:
%       (1) 'minLI':
%           objective: minimize sum_ij w_ij ||y_i - y_j||^2
%                      s.t. forall i, ||y_i||^2 = 1
%           in matrix form, it is expressed as:
%               minimize y^T * L * y,  s.t. y^T * y = 1
%       (2) 'minLD':
%           objective: minimize sum_ij w_ij ||y_i - y_j||^2
%                      s.t. forall i, d_ii ||y_i||^2 = 1
%           in matrix form, it is expressed as:
%               minimize y^T * L * y, s.t. y^T * D * y = 1
%           This is the original formulation in many papers in the fields
%           of spectral learning, clustering and manifold learning.
%       (3) 'maxWD': (default)
%           objective: maximize sum_ij w_ij <y_i, y_j>
%                      s.t. forall i, d_ii ||y_i||^2 = 1
%           in matrix form, it is expressed as:
%               maximize y^T * W * y, s.t. y^T * D * y = 1
%           In theory, this objective is equivalent to 'minLD'. However,
%           due to that its implementation is based on finding the
%           eigenvectors corresponding to the largest eigenvalues instead
%           of the smallest ones, thus it is much more efficient and
%           numerically stable. Hence, it is selected as the default
%           scheme.
%
%   - [Y, spectrum] = sllemap(...) additionally outputs the spectrum
%     of the embedding. The definition of the spectrum varies for different
%     schemes:
%     'minLI': the spectrum is the eigenvalues of L, in ascending order
%     'minLD': the spectrum is the eigenvalues of D^(-1/2) * L * D^(-1/2),
%              in ascending order
%     'maxWD': the spectrum is the eigenvalues of D^(-1/2) * W * D^(-1/2),
%              in descending order.
%
% $ Remarks $
%   - If the input graph does not have edge values or it is logical,
%     it just assume 1 between neighboring samples and 0 for other pairs.
%
%   - The embedding dimension d should be strictly less than the number
%     of samples n.
%
% $ History $
%   - Created by Dahua Lin, on Sep 12nd, 2006
%

%% parse and verify input arguments

if nargin < 2
    raise_lackinput('sllemap', 2);
end

gi = slgraphinfo(G, {'square'});
n = gi.n;
if strcmp(gi, 'adjmat')
    if isnumeric(G)
        W = G;
    else
        W = double(G);
    end
else
    W = sladjmat(G);
end

if d >= n
    error('sltoolbox:invalidarg', ...
        'The embeded dimension d should be strictly less than n');
end

if nargin < 3 || isempty(sch)
    sch = 'maxWD';
end

%% main delegation

% L = Di + Dj - Wij - Wji
% we let
%   W = Wij + Wji
%   D = Di + Dj = diag(diag(W))
%   L = D - W
W = W + W';

switch sch
    case 'maxWD'
        [Y, spectrum] = solve_maxWD(W, d);
    case 'minLD'
        [Y, spectrum] = solve_minLD(W, d);
    case 'minLI'
        [Y, spectrum] = solve_minLI(W, d);
    otherwise
        error('sltoolbox:invalidarg', ...
            'Invalid scheme for solving eigenmap: %s', sch);
end

%% Core functions

function [Y, spectrum] = solve_maxWD(W, d)

vD = calcDv(W);
W = calcNormalizeMat(W, vD);

[spectrum, Y] = slsymeig(W, d+1, 'descend');
spectrum = spectrum(2:d+1);
Y = Y(:, 2:d+1)';
Y = denormY(Y, vD);

function [Y, spectrum] = solve_minLD(W, d)

vD = calcDv(W);
L = calcL(vD, W);
L = calcNormalizeMat(L, vD);

[spectrum, Y] = slsymeig(L, d+1, 'ascend');
spectrum = spectrum(2:d+1);
Y = Y(:, 2:d+1)';
Y = denormY(Y, vD);

function [Y, spectrum] = solve_minLI(W, d)

vD = calcDv(W);
L = calcL(vD, W);

[spectrum, Y] = slsymeig(L, d+1, 'ascend');
spectrum = spectrum(2:d+1);
Y = Y(:, 2:d+1)';

%% Computation routines

function vD = calcDv(W)

vD = sum(W, 1)';

function L = calcL(vD, W)

n = length(vD);
if issparse(W)
    D = sparse((1:n)', (1:n)', vD, n, n, n);
    L = D - W;
else
    L = -W;
    dinds = (1:n)'*(n+1) - n;
    L(dinds) = L(dinds) + vD;
end

function Mn = calcNormalizeMat(M, vD)

vD(vD < eps) = eps;
cv = 1 ./ sqrt(vD);

if issparse(M)
    n = size(M,1);
    Mn = M;
    for i = 1 : n
        Mn(:,i) = Mn(:,i) * cv(i);
    end
    for i = 1 : n
        Mn(i,:) = Mn(i,:) * cv(i);
    end
else
    rv = cv';
    Mn = slmulrowcols(M, rv, cv);
end

function Y = denormY(Y, vD)

vD = vD';
vD(vD < eps) = eps;
Y = slmulvec(Y, 1 ./ sqrt(vD), 2);


% Chapter 4 - Electromagnetic Waves and Optical Resonators.
% Program_4a - Iteration of the Ikeda Map.
% Copyright Birkhauser 2013. Stephen Lynch.

% Chaotic attractor for the Ikeda map (Figure 4.11(b)).
clear
echo off
A=10;
B=0.15;
N=10000;
E=zeros(1,N);x=zeros(1,N);y=zeros(1,N);
E(1)=A;x(1)=A;y(1)=0;
for n=1:N
    E(n+1)=A+B*E(n)*exp(1i*abs(E(n))^2);
    x(n+1)=real(E(n+1));
    y(n+1)=imag(E(n+1));
end
axis([8 12 -2 2])
axis equal
plot(x(50:N),y(50:N),'.','MarkerSize',1);
fsize=15;
set(gca,'XTick',8:1:12,'FontSize',fsize)
set(gca,'YTick',-2:1:2,'FontSize',fsize)
xlabel('Real E','FontSize',fsize)
ylabel('Imag E','FontSize',fsize)

% End of Program_4a.


function dn = dnmult(dn0,dn1)

% DMULT dual number multiplication
%
%   DN = DNMULT(DN0,DN1) returns the dual number DN which is the dual number
%     multiplication of the dual numbers DN0 and DN1
%      - DN0 (resp. DN1) is a dual number (DN0 = a0 + eps*b0, eps^2 = 0).
%         It is a 2-vector or a 2*N array (column i represents dual number
%         i) where N is the number of dual numbers. DN0 and DN1 must have
%         the same size.
%      - DN is a dual number. It is a 2*N array (each column is the dual
%          multiplication of the corresponding columns in DN0 and DN1)

s0 = size(dn0);
s1 = size(dn1);
if s0 == [1 2]
    dn0 = dn0.';
    s0 = size(dn0);
end
if s1 == [1 2]
    dn1 = dn1.';
    s1 = size(dn1);
end

% wrong format
if s0(1) ~= 2 || s1(1) ~= 2
    error('DualQuaternion:Dmult:wrongsize',...
        '%d rows in the DN0 array and %d rows in the DN1 array. It should be 2 for both.',...
        s0(1),s1(1));
end

% sizes do not match
n1 = s0(2);
n2 = s1(2);
if n1 ~= n2
    error('DualQuaternion:Dmult:wrongsize',...
        '%d dual numbers in DN0 array and %d dual numbers in DN1 array.They should be equal.',...
        n1,n2);
end

dn = sym(zeros(2,n1));
dn(1,:) = dn0(1,:).*dn1(1,:);
dn(2,:) = dn0(1,:).*dn1(2,:)+dn0(2,:).*dn1(1,:);


function [kPa] = mmHg2kPa(mmHg)
% Convert pressure from millimeters of mercury at 0 degrees C to kilopascals
% Chad Greene 2012
kPa = mmHg*.133322;


% Author:
% - Mehrtash Harandi (mehrtash.harandi at gmail dot com)
%
% This file is provided without any warranty of
% fitness for any purpose. You can redistribute
% this file and/or modify it under the terms of
% the GNU General Public License (GPL) as published
% by the Free Software Foundation, either version 3
% of the License or (at your option) any later version.

function outStein = Compute_Stein_Metric(Set1,Set2)

simFlag = false;
if (nargin < 2)
    Set2 = Set1;
    simFlag = true;
end

l1 = size(Set1,3);
l2 = size(Set2,3);
outStein = zeros(l2,l1);

if (simFlag)
    for tmpC1 = 1:l1
        X = Set1(:,:,tmpC1);
        for tmpC2 = tmpC1+1:l2
            Y = Set2(:,:,tmpC2);
            outStein(tmpC2,tmpC1) = log(det(0.5*(X+Y))) -  0.5*(log(det(X*Y)));
            if  (outStein(tmpC2,tmpC1) < 1e-10)
                outStein(tmpC2,tmpC1) = 0.0;
            end
            outStein(tmpC1,tmpC2) = outStein(tmpC2,tmpC1);
        end
    end

else
    for tmpC1 = 1:l1
        X = Set1(:,:,tmpC1);
        for tmpC2 = 1:l2
            Y = Set2(:,:,tmpC2);
            outStein(tmpC2,tmpC1) = log(det(0.5*(X+Y))) -  0.5*(log(det(X*Y)));
            if  (outStein(tmpC2,tmpC1) < 1e-10)
                outStein(tmpC2,tmpC1) = 0.0;
            end
        end
    end
end

end