%DEMGMM3 Demonstrate density modelling with a Gaussian mixture model.
%
%	Description
%	 The problem consists of modelling data generated by a mixture of
%	three Gaussians in 2 dimensions with a mixture model using diagonal
%	covariance matrices.  The priors are 0.3, 0.5 and 0.2; the centres
%	are (2, 3.5), (0, 0) and (0,2); the covariances are all axis aligned
%	(0.16, 0.64), (0.25, 1) and the identity matrix. The first figure
%	contains a scatter plot of the data.
%
%	A Gaussian mixture model with three components is trained using EM.
%	The parameter vector is printed before training and after training.
%	The user should press any key to continue at these points.  The
%	parameter vector consists of priors (the column), and centres (given
%	as (x, y) pairs as the next two columns).  The diagonal entries of
%	the covariance matrices are printed separately.
%
%	The second figure is a 3 dimensional view of the density function,
%	while the third shows the axes of the 1-standard deviation circles
%	for the three components of the mixture model.
%
%	See also
%	GMM, GMMINIT, GMMEM, GMMPROB, GMMUNPAK
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Generate the data
ndata = 500;

% Fix the seeds for reproducible results
randn('state', 42);
rand('state', 42);
data = randn(ndata, 2);
prior = [0.3 0.5 0.2];
% Mixture model swaps clusters 1 and 3
datap = [0.2 0.5 0.3];
datac = [0 2; 0 0; 2 3.5];
datacov = [1 1;1 0.25; 0.4*0.4 0.8*0.8];
data1 = data(1:prior(1)*ndata,:);
data2 = data(prior(1)*ndata+1:(prior(2)+prior(1))*ndata, :);
data3 = data((prior(1)+prior(2))*ndata +1:ndata, :);

% First cluster has axis aligned variance and centre (2, 3.5)
data1(:, 1) = data1(:, 1)*0.4 + 2.0;
data1(:, 2) = data1(:, 2)*0.8 + 3.5;

% Second cluster has axis aligned variance and centre (0, 0)
data2(:,2) = data2(:, 2)*0.5;

% Third cluster is at (0,2) with identity matrix for covariance
data3 = data3 + repmat([0 2], prior(3)*ndata, 1);

% Put the dataset together again
data = [data1; data2; data3];

clc
disp('This demonstration illustrates the use of a Gaussian mixture model')
disp('with diagonal covariance matrices to approximate the unconditional')
disp('probability density of data in a two-dimensional space.')
disp('We begin by generating the data from a mixture of three Gaussians')
disp('with axis aligned covariance structure and plotting it.')
disp(' ')
disp('The first cluster has centre (0, 2).')
disp('The second cluster has centre (0, 0).')
disp('The third cluster has centre (2, 3.5).')
disp(' ')
disp('Press any key to continue')
pause

fh1 = figure;
plot(data(:, 1), data(:, 2), 'o')
set(gca, 'Box', 'on')

% Set up mixture model
ncentres = 3;
input_dim = 2;
mix = gmm(input_dim, ncentres, 'diag');

options = foptions;
options(14) = 5;	% Just use 5 iterations of k-means in initialisation
% Initialise the model parameters from the data
mix = gmminit(mix, data, options);

% Print out model
disp('The mixture model has three components and diagonal covariance')
disp('matrices.  The model parameters after initialisation using the')
disp('k-means algorithm are as follows')
disp('    Priors        Centres')
disp([mix.priors' mix.centres])
disp('Covariance diagonals are')
disp(mix.covars)
disp('Press any key to continue.')
pause

% Set up vector of options for EM trainer
options = zeros(1, 18);
options(1)  = 1;		% Prints out error values.
options(14) = 20;		% Number of iterations.

disp('We now train the model using the EM algorithm for 20 iterations.')
disp(' ')
disp('Press any key to continue.')
pause

[mix, options, errlog] = gmmem(mix, data, options);

% Print out model
disp(' ')
disp('The trained model has priors and centres:')
disp('    Priors        Centres')
disp([mix.priors' mix.centres])
disp('The data generator has priors and centres')
disp('    Priors        Centres')
disp([datap' datac])
disp('Model covariance diagonals are')
disp(mix.covars)
disp('Data generator covariance diagonals are')
disp(datacov)
disp('Note the close correspondence between these parameters and those')
disp('of the distribution used to generate the data.')
disp(' ')
disp('Press any key to continue.')
pause

clc
disp('We now plot the density given by the mixture model as a surface plot.')
disp(' ')
disp('Press any key to continue.')
pause

% Plot the result
x = -4.0:0.2:5.0;
y = -4.0:0.2:5.0;
[X, Y] = meshgrid(x,y);
X = X(:);
Y = Y(:);
grid = [X Y];
Z = gmmprob(mix, grid);
Z = reshape(Z, length(x), length(y));
c = mesh(x, y, Z);
hold on
title('Surface plot of probability density')
hold off
drawnow

clc
disp('The final plot shows the centres and widths, given by one standard')
disp('deviation, of the three components of the mixture model.  The axes')
disp('of the ellipses of constant density are shown.')
disp(' ')
disp('Press any key to continue.')
pause

% Try to calculate a sensible position for the second figure, below the first
fig1_pos = get(fh1, 'Position');
fig2_pos = fig1_pos;
fig2_pos(2) = fig2_pos(2) - fig1_pos(4);
fh2 = figure('Position', fig2_pos);

h = plot(data(:, 1), data(:, 2), 'bo');
hold on
axis('equal');
title('Plot of data and covariances')
for i = 1:ncentres
  v = [1 0];
  for j = 1:2
    start=mix.centres(i,:)-sqrt(mix.covars(i,:).*v);
    endpt=mix.centres(i,:)+sqrt(mix.covars(i,:).*v);
    linex = [start(1) endpt(1)];
    liney = [start(2) endpt(2)];
    line(linex, liney, 'Color', 'k', 'LineWidth', 3)
    v = [0 1];
  end
  % Plot ellipses of one standard deviation
  theta = 0:0.02:2*pi;
  x = sqrt(mix.covars(i,1))*cos(theta) + mix.centres(i,1);
  y = sqrt(mix.covars(i,2))*sin(theta) + mix.centres(i,2);
  plot(x, y, 'r-');
end
hold off

disp('Note how the data cluster positions and widths are captured by')
disp('the mixture model.')
disp(' ')
disp('Press any key to end.')
pause

close(fh1);
close(fh2);
clear all;


%   Distribution code Version 1.0 -- 02/31/2020 by Wei Liu Copyright 2020
%
%   The code is created based on the method described in the following paper
%   [1] "Real-time Image Smoothing via Iterative Least Squares", Wei Liu, Pingping Zhang,
%        Xiaolin Huang, Jie Yang, Chunhua Shen and Ian Reid, ACM Transactions on Graphics,
%        presented at SIGGRAPH 2020.
%
%   The code and the algorithm are for non-comercial use only.

%  ---------------------- Input------------------------
%  F:              input image, can be gray image or RGB color image
%  lambda:     \lambda in Eq.(1), control smoothing strength
%  p:              the power norm in the Charbonnier penalty in Eq. (2)
%  eps:          the small constant number in the Charbonnier penalty in Eq. (2)
%  iter:           iteration number of the ILS in Eq. (8)

%  ---------------------- Output------------------------
%  U:             smoothed image

function U =ILS_LNorm(F, lambda, p, eps, iter)

F = single(F); % 'single' precision is very important to reduce the computational cost

gamma = 0.5 * p - 1;
c =  p * eps^gamma;

[N, M, D] = size(F);
sizeI2D = [N, M];

otfFx = psf2otf_Dx(sizeI2D); % equal to otfFx = psf2otf(fx, sizeI2D) where fx = [1, -1];
otfFy = psf2otf_Dy(sizeI2D); % equal to otfFy = psf2otf(fy, sizeI2D) where fy = [1; -1];

Denormin = abs(otfFx).^2 + abs(otfFy ).^2;
Denormin = repmat(Denormin, [1, 1, D]);
Denormin = 1 + 0.5 * c * lambda * Denormin;

U = F; % smoothed image

Normin1 = fft2(U);

for k = 1: iter

    % Intermediate variables \mu update, in x-axis and y-axis direction
    u_h = [diff(U,1,2), U(:,1,:) - U(:,end,:)];
    u_v = [diff(U,1,1); U(1,:,:) - U(end,:,:)];

    mu_h = c .* u_h - p .* u_h .* (u_h .* u_h + eps) .^ gamma;
    mu_v = c .* u_v - p .* u_v .* (u_v .* u_v + eps) .^ gamma;

    % Update the smoothed image U
    Normin2_h = [mu_h(:,end,:) - mu_h(:, 1,:), - diff(mu_h,1,2)];
    Normin2_v = [mu_v(end,:,:) - mu_v(1, :,:); - diff(mu_v,1,1)];

    FU = (Normin1 + 0.5 * lambda * (fft2(Normin2_h + Normin2_v))) ./ Denormin;
    U = real(ifft2(FU));

    Normin1 = FU;  % This helps to further enlarge the smoothing strength

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Main problem is minimize \| A x \|_1 subject to A x = b
% We introduce z = A x - b
% x \in R^n A \in R^{m x n} b \in R^m
% m < n
% ADMM terms
% minimize f(x) + g(z) subject to A x + B z = c
% f(x) : {x \in R^n | Ax = b}
% g(z) : \| z \|_1
% minimize f(x) + \| z \|_1 subject to x - z = 0
% A : 1
% B : -1
% c : 0
%
% x update x  = proj (z - u) over the set {Ax = b}
% z update: z = shrinkage(x + u)
% u update : u = u + x - z
%
% Primal residual r = A x  + B z - c :  x - z
% Dual residual s = rho A^T B (z_new - z_old) = rho (z_old - z_new)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [x, history] = bp(A, b, options)
    if nargin < 2 || nargin > 3
        error('Invalid arguments');
    end
    if nargin < 3
        options = struct;
    end
    % weight for the quadratic penalty term
    rho = 1;
    if isfield(options, 'rho')
        rho = options.rho;
    end
    % weight for the relaxation
    alpha = 1;
    if isfield(options, 'alpha')
        alpha = options.alpha;
    end
    % size of the matrix
    [M, N] = size(A);
    % Main optimization variable
    if isfield(options, 'x')
        x = options.x;
    else
        x = zeros(N, 1);
    end
    % Auxiliary optimization variable z = Ax - b
    % Goal of ADMM iterations is to bring z closer to Ax - b
    if isfield(options, 'z')
        z = options.z;
    else
        z = zeros(N, 1);
    end
    % scaled Lagrangian variable
    if isfield(options, 'u')
        u = options.u;
    else
        u = zeros(N, 1);
    end
    % verbosity
    verbose = 0;
    if isfield(options, 'verbose')
        verbose = options.verbose;
    end
    % Maximum number of ADMM iterations
    max_iterations = 1000;
    if isfield(options, 'max_iterations')
        max_iterations = options.max_iterations;
    end
    % absolute tolerance
    eps_abs = 1e-4;
    if isfield(options, 'absolute_tolerance')
        eps_abs = options.absolute_tolerance;
    end
    % relative tolerance
    eps_rel = 1e-2;
    if isfield(options, 'relative_tolerance')
        eps_rel = options.relative_tolerance;
    end
    %  Compute and cache factorizations
    AAt = A*A';
    P = eye(N) - A' * (AAt\A);
    % pseudo-inverse based solution of equation Ax=b
    q = A' * (AAt \b);
    % import relevant functions
    import spx.opt.shrinkage;
    if verbose
        fprintf('%3s\t%10s\t%10s\t%10s\t%10s\t%10s\n', 'iter', ...
          'r norm', 'eps pri', 's norm', 'eps dual', 'objective');
    end
    % perform ADMM iterations
    for k=1:max_iterations
        % preserve old value of z
        z_old = z;
        % update x as a projection on to the set {Ax = b}
        % we add the projection of (z-u) to the null-space of A and add
        % it to the pseudo-inverse solution of Ax = b
        x = P*(z-u) + q;
        % relaxation
        if alpha ~= 1
            x_hat = alpha*x + (1-alpha)*z;
        else
            x_hat = x;
        end
        % update z
        z = shrinkage(x_hat + u, 1/rho);
        % update u
        u = u + (x_hat - z);
        % primal residual
        r = x - z;
        r_norm = norm(r);
        % dual residual
        s = rho * (z_old - z);
        s_norm = norm(s);
        % upper bound on primal residual
        eps_primal = sqrt(N) * eps_abs + eps_rel * max([norm(x), norm(z)]);
        eps_dual = sqrt(N) * eps_abs + eps_rel * norm(rho*u);
        if verbose
            % value of objective function
            objective_value = objective(x);
            fprintf('%3d\t%10.4f\t%10.4f\t%10.4f\t%10.4f\t%10.2f\n', k, ...
            r_norm, eps_primal, s_norm, eps_dual, objective_value);
        end
        if r_norm < eps_primal && s_norm < eps_dual
            % We have achieved convergence
            break;
        end
    end
end

% objective value
function obj = objective(x)
    obj = norm(x,1);
end


function quad_error = monomial_quadrature ( dim_num, expon, point_num, ...
  weight, x, rule )

%*****************************************************************************80
%
%% MONOMIAL_QUADRATURE applies a quadrature rule to a monomial.
%
%  Discussion:
%
%    This routine assumes that the integral being approximated is that of
%    a multidimensional monomial, integrated over the [-1,+1] hypercube,
%    with a Legendre weight (that is, w(x) = 1).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer EXPON(DIM_NUM), the exponents.
%
%    Input, integer POINT_NUM, the number of points in the rule.
%
%    Input, real WEIGHT(POINT_NUM), the quadrature weights.
%
%    Input, real X(DIM_NUM,POINT_NUM), the quadrature points.
%
%    Input, integer RULE, the index of the rule.
%    1, "CC", Clenshaw Curtis Closed Fully Nested rule.
%    2, "F1", Fejer 1 Open Fully Nested rule.
%    3, "F2", Fejer 2 Open Fully Nested rule.
%    4, "GP", Gauss Patterson Open Fully Nested rule.
%    5, "GL", Gauss Legendre Open Weakly Nested rule.
%    6, "GH", Gauss Hermite Open Weakly Nested rule.
%    7, "LG", Gauss Laguerre Open Non Nested rule.
%
%    Output, real QUAD_ERROR, the quadrature error.
%

%
%  Get the exact value of the integral of the monomial.
%
  if ( 1 <= rule & rule <= 5 )
    exact = monomial_integral_legendre ( dim_num, expon );
  elseif ( rule == 6 )
    exact = monomial_integral_hermite ( dim_num, expon );
  elseif ( rule == 7 )
    exact = monomial_integral_laguerre ( dim_num, expon );
  end
%
%  Evaluate the monomial at the quadrature points.
%
  value = monomial_value ( dim_num, point_num, x, expon );
%
%  Compute the quadrature sum.
%
  quad = weight * value';
%
%  Absolute error if EXACT = 0, relative error otherwise:
%
  if ( exact == 0.0 )
    quad_error = abs ( quad - exact );
  else
    quad_error = abs ( quad - exact ) / abs ( exact );
  end

  return
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

% DFT of a circularly shifted sequence

% circular shift in time
x=[ 1 0 3 4 7];
m=2;
xc=circshift(x',m);
Left=dft(xc);
Left.'

X=dft(x);
N=length(x);
k=0:N-1;
Right=X.*exp(-j*2*pi*m*k/N);
Right.'

% circular shift in frequency
x=[ 1 0 3 4 7];
N=length(x);
n=0:N-1;
m=2;
Left=dft(x.*exp(j*2*pi*n*m/N));
Left.'

X=dft(x);
Right=circshift(X.',m)


function Res = AccSumK(p,K)
%ACCSUMK      Res represents K-fold faithful rounding of sum(p)
%
%   Res = AccSumK(p,K)
%
%On return, Res represents a K-fold faithful rounding of sum(p), also in the
%  presence of underflow. Input vector p may be single or double precision.
%
%Implements Algorithm 6.4 from
%  S.M. Rump, T. Ogita, S. Oishi: Accurate Floating-point Summation II:
%    Sign, K-fold Faithful and Rounding to Nearest, Siam J. Sci. Comput.,
%    31(2):1269-1302, 2008.
%Requires (4m+5K+3)n flops for m executions of repeat-until loop in the
%  first and one execution of the repeat-until loops in subsequent calls
%  of TransformK.
%
%Reference implementation! Slow due to interpretation!
%

% written  03/03/07     S.M. Rump
% modified 05/09/09     S.M. Rump  rounding to nearest, complex input
%

  if ~isreal(p)
    Res = complex(AccSumK(real(p),K),AccSumK(imag(p),K));
    return
  end

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

  if isa(p,'double')
    nmax = 2^26-2;          % nmax = 67,108,864
  else
    nmax = 2^12-2;          % nmax = 4,094
  end
  if length(p)>nmax
    error(['maximum length of input vector for AccSumK ' int2str(nmax) '.'])
  end

  R = 0;
  if isa(p,'double'), prec='double'; else prec='single'; end
  Res = zeros(1,K,prec);
  [Res(1),R,p,sigma,Ms] = TransformK(p,R);
  if abs(Res(1))<=realmin(prec)
    if rndold, setround(rndold); end
    return
  end
  for k=2:K
    [Res(k),R,p,sigma,Ms] = TransformK(p,R,sigma,Ms);
    if abs(Res(k))<=realmin(prec)
      if rndold, setround(rndold); end
      return
    end
  end

  if rndold
    setround(rndold)
  end


%% Transient diffusion equation evaluated using FVTool
%
% adapted from A.A. Eftekhari by M.H.V. Werts (2020)
%
% GNU Octave 4.2.2 was used for development
%
% This script is intended to be run from the command line interface.
% It is called in this manner from within the accompanying Jupyter
% Python notebook.
%
% It should be inside the 'FVTool' directory tree as downloaded/cloned
% from Github, under
%   './Examples/External/Diffusion1DSpherical_Analytic-vs-FVTool-vs-Fipy'
%
% Script calculates diffusion in a 1D spherical geometry for an 'infinite'
% medium, with the initial condition that all mass at $t = 0$ is
% homogeneously confined inside a sphere of radius $a$. This is sometimes
% called a 'spherical initial condition'.
%
% see J. Crank (1975) "The Mathematics of Diffusion", 2nd Ed.,
%      Clarendon Press (Oxford), pages 29-30
%      Equation 3.8, Figure 3.1
%
% The transient diffusion equation reads
%
% $$\alpha\frac{\partial c}{\partial t}+\nabla.\left(-D\nabla c\right)=0,$$
%
% where $c$ is the independent variable (concentration, temperature, etc),
% $D$ is the diffusion coefficient, and $\alpha$ is a constant (1, here).
%
%
clc; clear;
more off;
run('../../../FVToolStartUp.m')

%% Define the domain and create a mesh structure
% Here we work in a 1D spherical coordinate system (r coordinate)
L = 10.0;  % domain length
Nx = 2000; % number of cells
m = createMeshSpherical1D(Nx, L);

%% Create the boundary condition structure
BC = createBC(m); % all Neumann boundary condition structure

%% define the transfer coeffs
D = createCellVariable(m, 1.0);
alfa = createCellVariable(m, 1.0);

%% define initial condition
c_init = 0;
c_old = createCellVariable(m, c_init, BC); % initial values
r = c_old.domain.cellcenters.x;
c_old.value(r<1.0) = 1.0;

%% calculate volumes of FV cellslices
%  We use this for demonstrating mass conservation
cellA = m.facecenters.x(1:end-1);
cellB = m.facecenters.x(2:end);
cellvol = 4/3 .* pi .* (cellB.^3 - cellA.^3);
cellsum = sum(cellvol)

c = c_old; % assign the old value of the cells to the current values

t = 0.0; % master time
deltat = 0.0625/20; % time step

% output total mass in the system
m_tot = sum(c.value(2:end-1) .* cellvol);
t,m_tot

%% loop for "time-stepping" the solution
% It outputs the spatial profile C(r) after
% 20, 80 and 320 time-steps
% This corresponds to t=0.0625, t=0.25 and t=1, respectively.
ti = 0
for s=[20,60,240]
  for n=1:s
      [M_trans, RHS_trans] = transientTerm(c, deltat, alfa);
      Dave = harmonicMean(D);
      Mdiff = diffusionTerm(Dave);
      [Mbc, RHSbc] = boundaryCondition(BC);
      M = M_trans-Mdiff+Mbc;
      RHS = RHS_trans+RHSbc;
      c = solvePDE(m,M, RHS);
      t += deltat;
      c_old = c;
  endfor
  m_tot = sum(c.value(2:end-1) .* cellvol);
  n,t,m_tot
  % The following writes the result to a file
  %  adapted from visualizeCells with domain.dimension = 1.8
  x = [c.domain.facecenters.x(1); c.domain.cellcenters.x; c.domain.facecenters.x(end)];
  cval = [0.5*(c.value(1)+c.value(2)); c.value(2:end-1); 0.5*(c.value(end-1)+c.value(end))];
  ti += s;
  filename = ["diffusion1Dspherical_FVTool_tstep",num2str(ti),".mat"]
  save('-6',filename,'x','cval');
endfor


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%  	System response to sinusoidal inputs

t=0:.1:30;
x1=3*cos(t+pi/3);
plot(t,x1);
legend('input signal x(t)')
ylim([-4 4]);

figure
w0=1;
Hw0=(3*(j*w0)^2+4j*w0+2)/(-w0^2+j*w0+3)
magn=abs(Hw0)
phas=angle(Hw0)
y1=3*abs(Hw0)*cos(t+pi/3+angle(Hw0));
plot(t,y1);
legend('system response')
ylim([-7 7]);

figure
num=[3 4 2];
den=[1 1 3];
yls=lsim(num,den,x1,t);
plot(t,yls);
legend('system response by lsim')

figure
plot(t,x1,t,y1,'o')
legend('x(t)','y(t)')
ylim([-7 9]);


% l1qc_logbarrier.m
%
% Solve quadratically constrained l1 minimization:
% min ||x||_1   s.t.  ||Ax - b||_2 <= \epsilon
%
% Reformulate as the second-order cone program
% min_{x,u}  sum(u)   s.t.    x - u <= 0,
%                            -x - u <= 0,
%      1/2(||Ax-b||^2 - \epsilon^2) <= 0
% and use a log barrier algorithm.
%
% Usage:  xp = l1qc_logbarrier(x0, A, At, b, epsilon, lbtol, mu, cgtol, cgmaxiter)
%
% x0 - Nx1 vector, initial point.
%
% A - Either a handle to a function that takes a N vector and returns a K
%     vector , or a KxN matrix.  If A is a function handle, the algorithm
%     operates in "largescale" mode, solving the Newton systems via the
%     Conjugate Gradients algorithm.
%
% At - Handle to a function that takes a K vector and returns an N vector.
%      If A is a KxN matrix, At is ignored.
%
% b - Kx1 vector of observations.
%
% epsilon - scalar, constraint relaxation parameter
%
% lbtol - The log barrier algorithm terminates when the duality gap <= lbtol.
%         Also, the number of log barrier iterations is completely
%         determined by lbtol.
%         Default = 1e-3.
%
% mu - Factor by which to increase the barrier constant at each iteration.
%      Default = 10.
%
% cgtol - Tolerance for Conjugate Gradients; ignored if A is a matrix.
%     Default = 1e-8.
%
% cgmaxiter - Maximum number of iterations for Conjugate Gradients; ignored
%     if A is a matrix.
%     Default = 200.
%
% Written by: Justin Romberg, Caltech
% Email: jrom@acm.caltech.edu
% Created: October 2005
%

function xp = l1qc_logbarrier(x0, A, At, b, epsilon, lbtol, mu, cgtol, cgmaxiter)

largescale = isa(A,'function_handle');

if (nargin < 6), lbtol = 1e-3; end
if (nargin < 7), mu = 10; end
if (nargin < 8), cgtol = 1e-8; end
if (nargin < 9), cgmaxiter = 200; end

newtontol = lbtol;
newtonmaxiter = 50;

N = length(x0);

% starting point --- make sure that it is feasible
if (largescale)
  if (norm(A(x0)-b) > epsilon)
    disp('Starting point infeasible; using x0 = At*inv(AAt)*y.');
    AAt = @(z) A(At(z));
    [w, cgres] = cgsolve(AAt, b, cgtol, cgmaxiter, 0);
    if (cgres > 1/2)
      disp('A*At is ill-conditioned: cannot find starting point');
      xp = x0;
      return;
    end
    x0 = At(w);
  end
else
  if (norm(A*x0-b) > epsilon)
    disp('Starting point infeasible; using x0 = At*inv(AAt)*y.');
    opts.POSDEF = true; opts.SYM = true;
    [w, hcond] = linsolve(A*A', b, opts);
    if (hcond < 1e-14)
      disp('A*At is ill-conditioned: cannot find starting point');
      xp = x0;
      return;
    end
    x0 = A'*w;
  end
end
x = x0;
u = (0.95)*abs(x0) + (0.10)*max(abs(x0));

% disp(sprintf('Original l1 norm = %.3f, original functional = %.3f', sum(abs(x0)), sum(u)));

% choose initial value of tau so that the duality gap after the first
% step will be about the origial norm
tau = max((2*N+1)/sum(abs(x0)), 1);

lbiter = ceil((log(2*N+1)-log(lbtol)-log(tau))/log(mu));
% disp(sprintf('Number of log barrier iterations = %d\n', lbiter));

totaliter = 0;
dispProgress('Log barrier', 0, lbiter);
for ii = 1:lbiter

  [xp, up, ntiter] = l1qc_newton(x, u, A, At, b, epsilon, tau, newtontol, newtonmaxiter, cgtol, cgmaxiter);
  totaliter = totaliter + ntiter;

%   disp(sprintf('\nLog barrier iter = %d, l1 = %.3f, functional = %8.3f, tau = %8.3e, total newton iter = %d\n', ...
%     ii, sum(abs(xp)), sum(up), tau, totaliter));
  dispProgress('Log barrier',ii/lbiter);

  x = xp;
  u = up;

  tau = mu*tau;

end
dispProgress('Log barrier', 'Close');

end


function [f_alias, cond_N] = sphArrayAliasLim(R, Nmic, maxN, mic_dirs_rad, mic_weights)
%SPHARRAYALIASLIM Get estimates of the aliasing limit of a spherical array
%
%   First estimate takes into account only the radius and a nominal order
%   that the array is expected to support, it is the simplest one and it
%   expresses the kR = maxN rule.
%   The second estimate is based on the number of microphones, and it can
%   be more relaxed than the first, if the nominal supported order is less
%   than maxN<floor(sqrt(Nmic)-1).
%   The third takes into account microphone numbers and directions, and it
%   is based on the orthogonality of the SH matrix for the microphone
%   positions expressed through the condition number.
%
%   Inputs:
%       R:      radius of spherical array
%       Nmic:   number of microphones
%       maxN:   expected maximum supported order of the array
%       mic_dirs_rad:   nMicx2 matrix of [azi elev] directions of the
%           microphones on the array
%       mic_weights: (optional) nMicx1 vector of weights used to improve
%           orthogonality of the SH transform
%
%   Outputs:
%       f_alias:    the alising limit estimates
%       cond_N:     the condition number of all orders up to one above
%           floor(sqrt(Nmic)-1)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SPHARRAYALIASLIM.M - 11/7/2013
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

c = 343;

% conventional kR=N assumption
f_alias(1) = c*maxN/(2*pi*R);

% based on the floor of the number of microphones, uniform arrangement
f_alias(2) = c*floor(sqrt(Nmic)-1)/(2*pi*R);

% based on condition number of the SHT matrix
aziElev2aziIncl = @(dirs) [dirs(:,1) pi/2-dirs(:,2)];

maxOrder = ceil(sqrt(Nmic)-1);
if (nargin<5), mic_weights = []; end

cond_N = checkCondNumberSHT(maxOrder, aziElev2aziIncl(mic_dirs_rad), 'real', mic_weights);
% plot(cond_N)
trueMaxOrder = find(cond_N<10^4, 1, 'last')-1;
f_alias(3) = c*trueMaxOrder/(2*pi*R);

end


function [ p, l, u ] = gfpp_plu ( n, alpha )

%*****************************************************************************80
%
%% GFPP_PLU returns the PLU factorization of the GFPP matrix.
%
%  Discussion
%
%    This factorization assumes that Gaussian elimination is performed
%    without pivoting.  If ALPHA is not between 0 and 1, then the
%    PLU factors returned here will not be the PLU factors derived
%    from Gaussian elimination with pivoting.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real ALPHA, determines subdiagonal elements.
%
%    Output, real P(N,N), L(N,N), U(N,N), the P, L, U factors
%    of the matrix.
%
  p = zeros ( n, n );
  for i = 1 : n
    p(i,i) = 1.0;
  end

  l = zeros ( n, n );
  for i = 1 : n
    l(i,1:i-1) = - abs ( alpha );
    l(i,i) = 1.0;
    l(i,i+1:n) = 0.0;
  end

  u = zeros ( n, n );
  u(1:n,1:n) = 0.0;
  for i = 1 : n
    u(i,i) = 1.0;
  end

  u(1,n) = 1.0;
  for i = 2 : n
    u(i,n) = 1.0 + abs ( alpha ) * sum ( u(1:i-1,n) );
  end

  return
end


function hg=gauss_kernel(k,nd,f,m)

% function hg=gauss_kernel(k,nd,f,m)
% ------------------------------------------------------------------------
%
%
% ------------------------------------------------------------------------

%%
switch m
    case {'sigma',1}
        %NOTE: This method is equivalent to using hg = fspecial('gaussian',[k k], S)
        S=f3;
        switch nd
            case 1
                x = linspace(-((k-1)/2),((k-1)/2),k);
                hg=exp(-(x.^2)./(2*S^2));
            case 2
                [x,y] = meshgrid(linspace(-((k-1)/2),((k-1)/2),k));
                hg=exp(-(x.^2 + y.^2)./(2*S^2));
            case 3
                [x,y,z] = meshgrid(linspace(-((k-1)/2),((k-1)/2),k));
                hg=exp(-(x.^2 + y.^2 + z.^2)./(2*S^2));
        end
    case {'width',2}
        % Here "f" defines "where the bell curve is" at the edges of the
        % kernel e.g. f=2 results in twice the standard deviation.
        switch nd
            case 1
                x = linspace(-f,f,k);
                hg=exp(-(x.^2)./2);
            case 2
                [x,y] = meshgrid(linspace(-f,f,k));
                hg=exp(-(x.^2 + y.^2)./2);
            case 3
                [x,y,z] = meshgrid(linspace(-f,f,k));
                hg=exp(-(x.^2 + y.^2 + z.^2)./2);
        end
    otherwise
        warning('False input for method! Valid options are "sigma" and "width"');
end

hg(hg<eps*max(hg(:)))=0;

%Making sure mask sums to 1
hg=hg./sum(hg(:));

end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [ v, more ] = tetrahedron_lattice_layer_point_next ( c, v, more )

%*****************************************************************************80
%
%% TETRAHEDRON_LATTICE_LAYER_POINT_NEXT: next tetrahedron lattice layer point.
%
%  Discussion:
%
%    The tetrahedron lattice layer L is bounded by the lines
%
%      0 <= X,
%      0 <= Y,
%      0 <= Z,
%      L - 1 < X / C(1) + Y / C(2) + Z/C(3) <= L.
%
%    In particular, layer L = 0 always contains the single point (0,0).
%
%    This function returns, one at a time, the points that lie within
%    a given tetrahedron lattice layer.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 July 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer C(4), coefficients defining the
%    lattice layer in entries 1 to 3, and the laver index in C(4).
%    The coefficients should be positive, and C(4) must be nonnegative.
%
%    Input/output, integer V(3).  On first call for a given layer,
%    the input value of V is not important.  On a repeated call for the same
%    layer, the input value of V should be the output value from the previous
%    call.  On output, V contains the next lattice layer point.
%
%    Input/output, logical MORE.  On input, set MORE to FALSE to indicate
%    that this is the first call for a given layer.  Thereafter, the input
%    value should be the output value from the previous call.  On output,
%    MORE is TRUE if the returned value V is a new point.
%    If the output value is FALSE, then no more points were found,
%    and V was reset to 0, and the lattice layer has been exhausted.
%
  n = 3;
%
%  Treat layer C(N+1) = 0 specially.
%
  if ( c(n+1) == 0 )
    if ( ~more )
      v(1:n) = 0;
      more = 1;
    else
      more = 0;
    end
    return
  end
%
%  Compute the first point.
%
  if ( ~more )

    v(1) = ( c(n+1) - 1 ) * c(1) + 1;
    v(2:3) = 0;
    more = 1;

  else

    c1n = i4vec_lcm ( n, c );

    rhs1 = c1n * ( c(n+1) - 1 );
    rhs2 = c1n *   c(n+1);
%
%  Can we simply increase X?
%
    v(1) = v(1) + 1;

    lhs = ( c1n / c(1) ) * v(1) ...
        + ( c1n / c(2) ) * v(2) ...
        + ( c1n / c(3) ) * v(3);

    if ( lhs <= rhs2 )
%
%  No.  Increase Y, and set X so we just exceed RHS1...if possible.
%
    else

      v(2) = v(2) + 1;

      v(1) = floor ( ( c(1) * ( rhs1 - ( c1n / c(2) ) * v(2) ...
                                     - ( c1n / c(3) ) * v(3) ) ) / c1n );
      v(1) = max ( v(1), 0 );

      lhs = ( c1n / c(1) ) * v(1) ...
          + ( c1n / c(2) ) * v(2) ...
          + ( c1n / c(3) ) * v(3);

      if ( lhs <= rhs1 )
        v(1) = v(1) + 1;
        lhs = lhs + c1n / c(1);
      end
%
%  We have increased Y by 1.  Have we stayed below the upper bound?
%
      if ( lhs <= rhs2 )

      else
%
%  No.  Increase Z, and set X so we just exceed RHS1...if possible.
%
        v(3) = v(3) + 1;
        v(2) = 0;
        v(1) = floor ( ( c(1) * ( rhs1 - ( c1n / c(2) ) * v(2) ...
                                       - ( c1n / c(3) ) * v(3) ) ) / c1n );
        v(1) = max ( v(1), 0 );

        lhs = ( c1n / c(1) ) * v(1) ...
            + ( c1n / c(2) ) * v(2) ...
            + ( c1n / c(3) ) * v(3);

        if ( lhs <= rhs1 )
          v(1) = v(1) + 1;
          lhs = lhs + c1n / c(1);
        end

        if ( lhs <= rhs2 )

        else
          more = 0;
          v(1:n) = 0;
        end

      end
    end
  end

  return
end


function [ n_data, x, fx ] = cin_values ( n_data )

%*****************************************************************************80
%
%% CIN_VALUES returns some values of the alternate cosine integral function.
%
%  Discussion:
%
%    The alternate cosine integral is defined by
%
%      CIN(X) = gamma + log(X) + integral ( 0 <= T <= X ) ( cos ( T ) - 1 ) / T  dT
%
%    In Mathematica, the function can be evaluated by:
%
%      EulerGamma + Log[x] - CosIntegral[x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 16;

  fx_vec = [ ...
     0.6185256314820045E-01, ...
     0.8866074809482194E-01, ...
     0.1200260139539026E+00, ...
     0.1557934976348559E+00, ...
     0.1957873187759337E+00, ...
     0.2398117420005647E+00, ...
     0.3390780388012470E+00, ...
     0.4516813164280685E+00, ...
     0.5754867772153906E+00, ...
     0.7081912003853150E+00, ...
     0.8473820166866132E+00, ...
     0.1207635200410304E+01, ...
     0.1556198167561642E+01, ...
     0.1862107181909382E+01, ...
     0.2104491723908354E+01, ...
     0.2274784183779546E+01 ];

  x_vec = [ ...
     0.5E+00, ...
     0.6E+00, ...
     0.7E+00, ...
     0.8E+00, ...
     0.9E+00, ...
     1.0E+00, ...
     1.2E+00, ...
     1.4E+00, ...
     1.6E+00, ...
     1.8E+00, ...
     2.0E+00, ...
     2.5E+00, ...
     3.0E+00, ...
     3.5E+00, ...
     4.0E+00, ...
     4.5E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function [w, A, C, sbc, fpe, th]=ARFIT_arfit(v, pmin, pmax, selector, no_const)
%ARFIT	Stepwise least squares estimation of multivariate AR model.
%
%  [w,A,C,SBC,FPE,th]=ARFIT(v,pmin,pmax) produces estimates of the
%  parameters of an m-variate AR model of order p,
%
%      v(k,:)' = w' + A1*v(k-1,:)' +...+ Ap*v(k-p,:)' + noise(C),
%
%  where w is the (m x 1) intercept vector, A1, ..., Ap are (m x m)
%  coefficient matrices, and C is a (m x m) noise covariance
%  matrix. The estimated order p lies between pmin and pmax and is
%  chosen as the optimizer of Schwarz's Bayesian Criterion.
%
%  The input matrix v must contain the time series data, with
%  columns v(:,l) representing m variables l=1,...,m and rows
%  v(k,:) representing n observations at different (equally
%  spaced) times k=1,..,n. Optionally, v can have a third
%  dimension, in which case the matrices v(:,:, itr) represent
%  the realizations (e.g., measurement trials) itr=1,...,ntr of the
%  time series. ARFIT returns least squares estimates of the
%  intercept vector w, of the coefficient matrices A1,...,Ap (as
%  A=[A1 ... Ap]), and of the noise covariance matrix C.
%
%  As order selection criteria, ARFIT computes approximations to
%  Schwarz's Bayesian Criterion and to the logarithm of Akaike's Final
%  Prediction Error. The order selection criteria for models of order
%  pmin:pmax are returned as the vectors SBC and FPE.
%
%  The matrix th contains information needed for the computation of
%  confidence intervals. ARMODE and ARCONF require th as input
%  arguments.
%
%  If the optional argument SELECTOR is included in the function call,
%  as in ARFIT(v,pmin,pmax,SELECTOR), SELECTOR is used as the order
%  selection criterion in determining the optimum model order. The
%  three letter string SELECTOR must have one of the two values 'sbc'
%  or 'fpe'. (By default, Schwarz's criterion SBC is used.) If the
%  bounds pmin and pmax coincide, the order of the estimated model
%  is p=pmin=pmax.
%
%  If the function call contains the optional argument 'zero' as the
%  fourth or fifth argument, a model of the form
%
%         v(k,:)' = A1*v(k-1,:)' +...+ Ap*v(k-p,:)' + noise(C)
%
%  is fitted to the time series data. That is, the intercept vector w
%  is taken to be zero, which amounts to assuming that the AR(p)
%  process has zero mean.
%
%  Modified 14-Oct-00
%           24-Oct-10 Tim Mullen (added support for multiple realizatons)
%
%  Authors: Tapio Schneider
%           tapio@gps.caltech.edu
%
%           Arnold Neumaier
%           neum@cma.univie.ac.at

  % n:   number of time steps (per realization)
  % m:   number of variables (dimension of state vectors)
  % ntr: number of realizations (trials)
  [n,m,ntr]   = size(v);

  if (pmin ~= round(pmin) | pmax ~= round(pmax))
    error('Order must be integer.');
  end
  if (pmax < pmin)
    error('PMAX must be greater than or equal to PMIN.')
  end

  % set defaults and check for optional arguments
  if (nargin == 3)              % no optional arguments => set default values
    mcor       = 1;               % fit intercept vector
    selector   = 'sbc';	          % use SBC as order selection criterion
  elseif (nargin == 4)          % one optional argument
    if strcmp(selector, 'zero')
      mcor     = 0;               % no intercept vector to be fitted
      selector = 'sbc';	          % default order selection
    else
      mcor     = 1; 		  % fit intercept vector
    end
  elseif (nargin == 5)          % two optional arguments
    if strcmp(no_const, 'zero')
      mcor     = 0;               % no intercept vector to be fitted
    else
      error(['Bad argument. Usage: ', ...
	     '[w,A,C,SBC,FPE,th]=AR(v,pmin,pmax,SELECTOR,''zero'')'])
    end
  end

  ne  	= ntr*(n-pmax);         % number of block equations of size m
  npmax	= m*pmax+mcor;          % maximum number of parameter vectors of length m

  if (ne <= npmax)
    error('Time series (N = %u) too short.',size(v,1))
  end

  % compute QR factorization for model of order pmax
  [R, scale]   = ARFIT_arqr(v, pmax, mcor);

  % compute approximate order selection criteria for models
  % of order pmin:pmax
  [sbc, fpe]   = ARFIT_arord(R, m, mcor, ne, pmin, pmax);

  % get index iopt of order that minimizes the order selection
  % criterion specified by the variable selector
  [val, iopt]  = min(eval(selector));

  % select order of model
  popt         = pmin + iopt-1; % estimated optimum order
  np           = m*popt + mcor; % number of parameter vectors of length m

  % decompose R for the optimal model order popt according to
  %
  %   | R11  R12 |
  % R=|          |
  %   | 0    R22 |
  %
  R11   = R(1:np, 1:np);
  R12   = R(1:np, npmax+1:npmax+m);
  R22   = R(np+1:npmax+m, npmax+1:npmax+m);

  % get augmented parameter matrix Aaug=[w A] if mcor=1 and Aaug=A if mcor=0
  if (np > 0)
    if (mcor == 1)
      % improve condition of R11 by re-scaling first column
      con 	= max(scale(2:npmax+m)) / scale(1);
      R11(:,1)	= R11(:,1)*con;
    end;
    Aaug = (R11\R12)';

    %  return coefficient matrix A and intercept vector w separately
    if (mcor == 1)
      % intercept vector w is first column of Aaug, rest of Aaug is
      % coefficient matrix A
      w = Aaug(:,1)*con;        % undo condition-improving scaling
      A = Aaug(:,2:np);
    else
      % return an intercept vector of zeros
      w = zeros(m,1);
      A = Aaug;
    end
  else
    % no parameters have been estimated
    % => return only covariance matrix estimate and order selection
    % criteria for ``zeroth order model''
    w   = zeros(m,1);
    A   = [];
  end

  % return covariance matrix
  dof   = ne-np;                % number of block degrees of freedom
  C     = R22'*R22./dof;        % bias-corrected estimate of covariance matrix

  % for later computation of confidence intervals return in th:
  % (i)  the inverse of U=R11'*R11, which appears in the asymptotic
  %      covariance matrix of the least squares estimator
  % (ii) the number of degrees of freedom of the residual covariance matrix
  invR11 = inv(R11);
  if (mcor == 1)
    % undo condition improving scaling
    invR11(1, :) = invR11(1, :) * con;
  end
  Uinv   = invR11*invR11';
  th     = [dof zeros(1,size(Uinv,2)-1); Uinv];


function BPI = myFilteredBackprojectionCentralSlice(sinogram,thetas)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% filtered back projection in the frequency domain applying central slice
% theorem-> schlegel & bille 9.2.3
% modified by: Mark Bangert
% m.bangert@dkfz.de 2011
%
% note: a) matlab puts the 0 frequency component of a fourier spectrum _not_
%          in the middle. we need to fumble around with fftshift.

% figure out how big our picture is going to be.
numOfParallelProjections = size(sinogram,1);
numOfAngularProjections  = length(thetas);

% convert thetas to radians
thetas = (pi/180)*thetas;

% set up the backprojected image
BPI = zeros(numOfParallelProjections,numOfParallelProjections);

% find the middle index of the projections
midindex = floor(numOfParallelProjections/2) + 1;

% set up the coords of the image
[xCoords,yCoords] = meshgrid(ceil(-numOfParallelProjections/2):ceil(numOfParallelProjections/2-1));

% set up filter
rampFilter = [floor(numOfParallelProjections/2):-1:0 1:ceil(numOfParallelProjections/2-1)]';

% loop over each projection
for i = 1:numOfAngularProjections

    % figure out which projections to add to which spots
    rotCoords = round(midindex + xCoords*sin(thetas(i)) + yCoords*cos(thetas(i)));

    % check which coords are in bounds
    indices   = find((rotCoords > 0) & (rotCoords <= numOfParallelProjections));
    newCoords = rotCoords(indices);

    % filter
    filteredProfile = real( ifft( ifftshift( rampFilter.*fftshift(fft(sinogram(:,i)) ) ) ) );

    % summation
    BPI(indices) = BPI(indices) + filteredProfile(newCoords)./numOfAngularProjections;

    % visualization on the fly
    imagesc(BPI)
    drawnow

end


function dq = dqconj(dv,WHICHCONJ)

% DQCONJ   Dual quaternion conjugate
%
%      DQ = DQCONJ(DV) returns the dual quaternion conjugate of the dual
%      quaternion DV. DV (resp. DQ) is a 8-vector representing a dual
%      quaternion or an array 8*N (column i represents dual quaternion i)
%      where N is the number of dual quaternions. The default type of
%      conjugate is 'point' (see below). DQ has the same size as DV.
%
%       DQ = DQCONJ(DV,WHICHCONJ) returns the type of dual quaternion conjugate
%       corresponding to WHICHCONJ (DV = Q0+eps*Q1):
%         - 'point': mixed conjugate: dV* = Q0*-eps*Q1* (used for point
%         transformations).
%         - 'line': quaternion conjugate: dV* = Q0*+eps*Q1* (used for line
%         transformations).
%         - 'other': dual number conjugate: dV* = Q0-eps*Q1
%
% See also QCONJ, DQMULT

if nargin < 2
    WHICHCONJ = 'point'; % default is 'mixed' conjugate' (point transformation)
end

sdv = size(dv);
if sdv == [1 8]
    dv = dv.';
    sdv = size(dv);
end

% wrong size
if sdv(1) ~= 8
    error('DualQuaternion:DQconj:wrongsize',...
        '%d rows in array dv. It should be 8.',sdv(1));
end

n = sdv(2);
dq = sym(zeros(8,n));
switch WHICHCONJ
    case 'point' % % case of point transformation
        dq(1:4,:) = qconj(dv(1:4,:));
        dq(5:8,:) = -qconj(dv(5:8,:));
    case 'line' % case of line transformation
        dq(1:4,:) = qconj(dv(1:4,:));
        dq(5:8,:) = qconj(dv(5:8,:));
    case 'other' % dual number conjugate
        dq(1:4,:) = dv(1:4,:);
        dq(5:8,:) = -dv(5:8,:);
    otherwise
        error('DualQuaternion:DQconj:wrongsize',...
            'WHICHCONJ must be ''point'', ''line'' or ''other'' ');
end


function y = fshift(x,s)
% FSHIFT Fractional circular shift
%   Syntax:
%
%       >> y = fshift(x,s)
%
%   FSHIFT circularly shifts the elements of vector x by a (possibly
%   non-integer) number of elements s. FSHIFT works by applying a linear
%   phase in the spectrum domain and is equivalent to CIRCSHIFT for integer
%   values of argument s (to machine precision).

% (c) 2005 Francois Bouffard
%          fbouffar@gel.ulaval.ca

needtr = 0; if size(x,1) == 1; x = x(:); needtr = 1; end;
N = size(x,1);
r = floor(N/2)+1; f = ((1:N)-r)/(N/2);
p = exp(-j*s*pi*f)';
y = ifft(fft(x).*ifftshift(p)); if isreal(x); y = real(y); end;
if needtr; y = y.'; end;


function polygon_solid_angle_3d_test ( )

%*****************************************************************************80
%
%% POLYGON_SOLID_ANGLE_3D_TEST tests POLYGON_SOLID_ANGLE_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 May 2015
%
%  Author:
%
%    John Burkardt
%
  dim_num = 3;
  test_num = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POLYGON_SOLID_ANGLE_3D_TEST\n' );
  fprintf ( 1, '  POLYGON_SOLID_ANGLE_3D computes the solid angle\n' );
  fprintf ( 1, '  subtended by a planar polygon in 3D as viewed from\n' );
  fprintf ( 1, '  a point P.\n' );

  for test = 1 : test_num
%
%  One eighth of sphere surface, on the unit sphere surface.
%
    if ( test == 1 )

      n = 3;

      v = [ ...
        1.0, 0.0, 0.0; ...
        0.0, 1.0, 0.0; ...
        0.0, 0.0, 1.0 ]';

      p(1:3,1) = [ 0.0; 0.0; 0.0 ];
%
%  Reverse order of vertices.
%
    elseif ( test == 2 )

      n = 3;

      v = [ ...
        1.0, 0.0, 0.0; ...
        0.0, 0.0, 1.0; ...
        0.0, 1.0, 0.0 ]';

      p(1:3,1) = [ 0.0; 0.0; 0.0 ];
%
%  One eighth of sphere surface, on the unit sphere surface,
%  translated by (1,2,3).
%
    elseif ( test == 3 )

      n = 3;

      v = [ ...
        2.0, 2.0, 3.0; ...
        1.0, 3.0, 3.0; ...
        1.0, 2.0, 4.0 ]';

      p(1:3,1) = [ 1.0; 2.0; 3.0 ];
%
%  One eighth of sphere surface, but on sphere of radius 2.
%
    elseif ( test == 4 )

      n = 3;

      v = [ ...
        2.0, 0.0, 0.0; ...
        0.0, 2.0, 0.0; ...
        0.0, 0.0, 2.0 ]';

      p(1:3,1) = [ 0.0; 0.0; 0.0 ];

    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  TEST # %d\n', test );
    fprintf ( 1, '\n' );

    r8vec_print ( dim_num, p, '  The viewing point P:' );

    r8mat_transpose_print ( dim_num, n, v, '  The polygon vertices V:' );

    solid_angle = polygon_solid_angle_3d ( n, v, p );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Solid angle subtended: %f\n', solid_angle );

  end

  return
end


function coeffs=terms2MultiDimPolyMat(termMat)
%%TERMS2MULTIDIMPOLYMAT Given a multidimensional polynomial represented as
%                    a 2D matrix of terms (monomials), convert it into a
%                    hypermatrix of coefficients suitable for use with
%                    functions such as polyValMultiDim and where convn can
%                    be used to multiply two such polynomials.
%
%INPUTS: termMat An (n+1)XnumTerms matrix such that
%                termMat(:,i)=[c,a1,a2,...,an] is a monomial term where c
%                is the value of of the monomial coefficient and the
%                monomial is
%                x1^(a1-1)*x2^(a2-1)*x3^(a3-1)...xn^(an-1). The ordering
%                of the terms in termMat does not matter.
%
%OUTPUTS: coeffs A hypermatrix of the coefficients for the multivariate
%               polynomial. These are arranged such that
%               coeffs(a1,a2,a3...an) corresponds to the coefficient of an
%               x1^(a1-1)*x2^(a2-1)*x3^(a3-1)...xn^(an-1) term.  Thus, the
%               number of indices coeffs takes is equal to the
%               dimensionality of x (not counting singleton dimensions at
%               the end of coeffs). Note that this ordering is the reverse
%               that used in the 1D polyval function that is built into
%               Matlab. The number of elements for each index in coeffs is
%               the maximum order of that dimension +1.
%
%This function is the opposite of multiDimHyperMat2Terms. See
%multiDimHyperMat2Terms for usage examples.
%
%January 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numTerms=size(termMat,2);
numIdx=size(termMat,1)-1;

numDims=zeros(1,numIdx);

if(isscalar(numDims))
    numDims=[numDims,1];
end

for idx=1:numIdx
    numDims(idx)=max(termMat(idx+1,:))+1;
end

coeffs=zeros(numDims);
for curTerm=1:numTerms
    idx=nDim2Index(numDims,termMat(2:end,curTerm)+1);
    coeffs(idx)=coeffs(idx)+termMat(1,curTerm);
end

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function FD = FermiDiract(z,nu)
%% Fermi-Diract function
% Implementation of the Fermi-Diract Statistical function
% in latex words:
%
% $$F_\nu(z)=\frac{1}{\Gamma(\nu)} \int_{0}^{\infty}\frac{x^{\nu-1}}{z^{-1}e^x+1}
%   \approx \sum_{l=1}^{\infty}(-1)^{l-1}\frac{z^l}{l^\nu}$$
%
% As we can notice this function is of the order $\nu$

l   = 1:50; % up to l = 50 to ensure a fair accuaracy
fd  = (-1).^(l-1) .* (z.^l) ./ (l.^nu);
FD  = sum(fd);
return


function [Az, El, D] = topocent(X,dx)
%TOPOCENT  Transformation of vector dx into topocentric coordinate
%          system with origin at X.
%          Both parameters are 3 by 1 vectors.
%          Output: D    vector length in units like the input
%                  Az   azimuth from north positive clockwise, degrees
%                  El   elevation angle, degrees

%Kai Borre 11-24-96
%Copyright (c) by Kai Borre
%$Revision: 1.0 $  $Date: 1997/09/26  $

dtr = pi/180;
[phi,lambda,~] = togeod(6378137,298.257223563,X(1),X(2),X(3));
cl = cos(lambda*dtr); sl = sin(lambda*dtr);
cb = cos(phi*dtr); sb = sin(phi*dtr);
F = [-sl -sb*cl cb*cl;
      cl -sb*sl cb*sl;
       0    cb   sb];
local_vector = F'*dx;
E = local_vector(1);
N = local_vector(2);
U = local_vector(3);
hor_dis = sqrt(E^2+N^2);
if hor_dis < 1.e-20
   Az = 0;
   El = 90;
else
   Az = atan2(E,N)/dtr;
   El = atan2(U,hor_dis)/dtr;
end
if Az < 0
   Az = Az+360;
end
D = sqrt(dx(1)^2+dx(2)^2+dx(3)^2);
%%%%%%%%% end topocent.m %%%%%%%%%


clear all; close all;
figure(1)
% underdetermined
n=20; m=100
A=rand(n,m); b=rand(n,1);

cvx_begin;
variable x2(m)
minimize( norm(x2,2) );
subject to
A*x2 == b;
cvx_end;

cvx_begin;
variable x1(m)
minimize( norm(x1,1) );
subject to
A*x1 == b;
cvx_end;

subplot(3,1,1), bar(x2,'FaceColor',[.6 .6 .6],'EdgeColor','k')
subplot(3,1,2), bar(x1,'FaceColor',[.6 .6 .6],'EdgeColor','k')
subplot(3,2,5), hist(x2,40)
subplot(3,2,6), hist(x1,40)

subplot(3,1,1), axis([0 100 -.2 .2]), subplot(3,1,2), axis([0 100 -.4 .4])
subplot(3,2,5), axis([-.15 .15 0 82]), set(gca,'Ytick',[0 40 80],'Xtick',[-0.1 0 0.1])
h = findobj(gca,'Type','patch'); h.FaceColor = [0.6 0.6 0.6]; h.EdgeColor = 'k';
subplot(3,2,6), axis([-.15 .15 0 82]), set(gca,'Ytick',[0 40 80],'Xtick',[-0.1 0 0.1])
h = findobj(gca,'Type','patch'); h.FaceColor = [0.6 0.6 0.6]; h.EdgeColor = 'k';

%%
clear A
clear b
clear x
figure(2)

% overdetermined
n=500; m=100;
A=rand(n,m);
b=rand(n,1);
xdag=pinv(A)*b;

lam=[0 0.1 0.5];
for j=1:3

    cvx_begin;
    variable x(m)
    minimize( norm(A*x-b,2) + lam(j)*norm(x,1) );
    cvx_end;

    subplot(4,1,j),bar(x,'FaceColor',[.6 .6 .6],'EdgeColor','k')
    subplot(4,3,9+j), hist(x,20)
end

for j=1:3

    subplot(4,1,j), axis([0 100 -.15 .15])
    subplot(4,3,9+j), axis([-.15 .15 0 80]), set(gca,'Ytick',[0 40 80])
    h = findobj(gca,'Type','patch');
    h.FaceColor = [0.6 0.6 0.6];
    h.EdgeColor = 'k';
end

%%
% matrix overdetermined system

clear A
clear b
clear x
figure(3)

% overdetermined
n=300; m=60; p=20;
A=rand(n,m); b=rand(n,p);

lam=[0 0.1];
for j=1:2
    cvx_begin;
    variable x(m,p)
    minimize(norm(A*x-b,2) + lam(j)*norm(x,1));
    cvx_end;
    subplot(2,1,j), pcolor(x.'), colormap(hot), colorbar
end

%%
subplot(2,1,1), set(gca,'Fontsize',[15],'Xtick',[1 20 40 60],'Ytick',[1 10 20])
subplot(2,1,2), set(gca,'Fontsize',[15],'Xtick',[1 20 40 60],'Ytick',[1 10 20])


function [x, y, z] = geodetic_to_ecf(lat, lon, alt)
%GEODETIC_TO_ECF Convert geodetic coordinates to ECF
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////
%
%
% Convert geodetic latitude, longitude, and altitude to ECF (Earth
% Centered Fixed) coordinates.
%
% USAGE:
%   pos_ecf = geodetic_to_ecf(pos_lla)
%   [pos_ecf_x, pos_ecf_y, pos_ecf_z] = geodetic_to_ecf(lat, lon, alt)
%
% INPUTS:
%   pos_lla - required : geodetic latitude, longitude, and altitude   [deg, deg, m]
%
% OUTPUTS:
%   pos_ecf - required : ecf x, y, z coordinates                      [m, m, m]
%
% NOTES:
%   Zhu, J. Conversion of Earth-centered, Earth-fixed coordinates to
%   geodetic coordinates. IEEE Transactions on Aerospace and Electronic
%   Systems, 30, 3 (July 1994), 957-962.
%
% VERSION:
%   1.0
%     - Sean Hatch 20070911
%     - initial version
%   1.1
%     - Wade Schwartzkopf 20130708
%     - vectorized and componentwise data handling
%
% TODO:

% define constants
e2 = 6.6943799901377997e-3; % eccentricity squared of Earth (WGS 84 value)
a = 6378137.0;              % semimajor radius of the Earth (WGS 84 value)

% Handle different forms in input arguments
if nargin==3 % Componentwise inputs, separate arguments for lat,lon,alt
    % Nothing to do.  Processing uses this form.
elseif size(lat,1)==3 % Array of 3-element vectors
    alt = lat(3,:);
    lon = lat(2,:);
    lat = lat(1,:);
elseif numel(lat)==3 % Horizontal 3-element vector
    alt = lat(3);
    lon = lat(2);
    lat = lat(1);
else
    error('WGS_84_NORM:INVALID_INPUTS', 'Invalid inputs.');
end

% calculate distance to surface of ellipsoid
R = a ./ sqrt(1.0 - e2 .* sind(lat) .* sind(lat));

% calculate coordinates
x = (R + alt) .* cosd(lat) .* cosd(lon);
y = (R + alt) .* cosd(lat) .* sind(lon);
z = (R + alt - e2 .* R) .* sind(lat);

if nargout < 2 % Matrix/vector form, rather than componentwise form, was requested
    x = [x; y; z];
end

end

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


function [L, D, P, rho] = ldlt_skew(A)
%LDLT_SKEW  Block LDL^T factorization for a skew-symmetric matrix.
%           Given a real, skew-symmetric A,
%           [L, D, P, RHO] = LDLT_SKEW(A) computes a permutation P,
%           a unit lower triangular L, and a block diagonal D
%           with 1x1 and 2x2 diagonal blocks, such that P*A*P' = L*D*L'.
%           A partial pivoting strategy of Bunch is used.
%           RHO is the growth factor.

%           Reference:
%           J. R. Bunch, A note on the stable decomposition of skew-symmetric
%              matrices. Math. Comp., 38(158):475-479, 1982.
%           N. J. Higham, Accuracy and Stability of Numerical Algorithms,
%              Second edition, Society for Industrial and Applied Mathematics,
%              Philadelphia, PA, 2002; chap. 11.

%           This routine does not exploit skew-symmetry and is not designed to
%           be efficient.

if ~isreal(A) | ~isequal(triu(A,1)',-tril(A,-1))
    error('Must supply real, skew-symmetric matrix.')
end

n = length(A);
k = 1;
D = zeros(n);
L = eye(n);
pp = 1:n;
if nargout >= 4
   maxA = norm(A(:), inf);
   rho = maxA;
end

while k < n

      if max( abs(A(k+1:n,k)) ) == 0

         s = 1;
         % Nothing to do.

      else

         s = 2;

         if k < n-1
            [colmaxima, rowindices] = max( abs(A(k+1:n, k:k+1)) );
            [biggest, colindex] = max(colmaxima);
            row = rowindices(colindex)+k; col = colindex+k-1;

            % Permute largest element into (k+1,k) position.
            % NB: k<->col permutation must be done before k+1<->row one.
            A( [k, col], : ) = A( [col, k], : );
            A( :, [k, col] ) = A( :, [col, k] );
            A( [k+1, row], : ) = A( [row, k+1], : );
            A( :, [k+1, row] ) = A( :, [row, k+1] );
            L( [k, col], : ) = L( [col, k], : );
            L( :, [k, col] ) = L( :, [col, k] );
            L( [k+1, row], : ) = L( [row, k+1], : );
            L( :, [k+1, row] ) = L( :, [row, k+1] );
            pp( [k, col] ) = pp( [col, k] );
            pp( [k+1, row] ) = pp( [row, k+1] );
         end

         E = A(k:k+1,k:k+1);
         D(k:k+1,k:k+1) = E;
         C = A(k+2:n,k:k+1);
         temp = C/E;
         L(k+2:n,k:k+1) = temp;
         A(k+2:n,k+2:n) = A(k+2:n,k+2:n) + temp*C';  % Note the plus sign.
         % Restore skew-symmetry.
         A(k+2:n,k+2:n) = 0.5 * (A(k+2:n,k+2:n) - A(k+2:n,k+2:n)');

         if nargout >= 4, rho = max(rho, max(max(abs(A(k+2:n,k+2:n)))) ); end

     end

     k = k + s;
     if k >= n-2, D(k:n,k:n) = A(k:n,k:n); break, end;

end

if nargout >= 3, P = eye(n); P = P(pp,:); end
if nargout >= 4, rho = rho/maxA; end


function mbasis = basis_matrix_overhauser_nonuni ( alpha, beta )

%*****************************************************************************80
%
%% BASIS_MATRIX_OVERHAUSER_NONUNI sets up the nonuniform Overhauser spline basis matrix.
%
%  Discussion:
%
%    This basis matrix assumes that the data points P1, P2, P3 and
%    P4 are not uniformly spaced in T, and that P2 corresponds to T = 0,
%    and P3 to T = 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, BETA.
%    ALPHA = || P2 - P1 || / ( || P3 - P2 || + || P2 - P1 || )
%    BETA  = || P3 - P2 || / ( || P4 - P3 || + || P3 - P2 || ).
%
%    Output, real MBASIS(4,4), the basis matrix.
%
  mbasis(1,1) = - ( 1.0 - alpha ) * ( 1.0 - alpha ) / alpha;
  mbasis(1,2) =   beta + ( 1.0 - alpha ) / alpha;
  mbasis(1,3) =   alpha - 1.0 / ( 1.0 - beta );
  mbasis(1,4) =   beta * beta / ( 1.0 - beta );

  mbasis(2,1) =   2.0 * ( 1.0 - alpha ) * ( 1.0 - alpha ) / alpha;
  mbasis(2,2) = ( - 2.0 * ( 1.0 - alpha ) - alpha * beta ) / alpha;
  mbasis(2,3) = ( 2.0 * ( 1.0 - alpha ) ...
    - beta * ( 1.0 - 2.0 * alpha ) ) / ( 1.0 - beta );
  mbasis(2,4) = - beta * beta / ( 1.0 - beta );

  mbasis(3,1) = - ( 1.0 - alpha ) * ( 1.0 - alpha ) / alpha;
  mbasis(3,2) =   ( 1.0 - 2.0 * alpha ) / alpha;
  mbasis(3,3) =   alpha;
  mbasis(3,4) =   0.0;

  mbasis(4,1) =   0.0;
  mbasis(4,2) =   1.0;
  mbasis(4,3) =   0.0;
  mbasis(4,4) =   0.0;

  return
end


function mean = dirichlet_mix_mean ( comp_num, elem_num, a, comp_weight )

%*****************************************************************************80
%
%% DIRICHLET_MIX_MEAN returns the means of a Dirichlet mixture PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer COMP_NUM, the number of components in the Dirichlet
%    mixture density, that is, the number of distinct Dirichlet PDF's
%    that are mixed together.
%
%    Input, integer ELEM_NUM, the number of elements of an observation.
%
%    Input, real A(ELEM_NUM,COMP_NUM), the probabilities for
%    element ELEM_NUM in component COMP_NUM.
%    Each A(I,J) should be positive.
%
%    Input, real COMP_WEIGHT(COMP_NUM), the mixture weights of the densities.
%    These do not need to be normalized.  The weight of a given component is
%    the relative probability that that component will be used to generate
%    the sample.
%
%    Output, real MEAN(ELEM_NUM), the means for each element.
%
  comp_weight_sum = sum ( comp_weight );

  mean(1:elem_num) = 0.0;

  for comp_i = 1 : comp_num
    comp_mean(1:elem_num) = dirichlet_mean ( elem_num, a(1:elem_num,comp_i) );
    mean(1:elem_num) = mean(1:elem_num) ...
      + comp_weight(comp_i) * comp_mean(1:elem_num);
  end

  mean(1:elem_num) = mean(1:elem_num) / comp_weight_sum;

  return
end


% SUMMARY:  Calculate probability of gauss mix model
% AUTHOR:   QIUQIANG KONG, Queen Mary University of London
% Created:  19-09-2015
% Modified: 15-11-2015 Modify output size
%           20-11-2015 debug the order of [p,M]
% -----------------------------------------------------------
% input
%   X               input data; size: N*p; dim 1: num of data, dim 2: feature dim
%   pi              prior of mix; dim 1: mix num
%   mu              size: p*M; dim 1: feature dim, dim 2: mix num
%   Sigma           size: p*p*M; dim 1,2: feature dim, dim 3: mix num
% output
%   probs           probability of input data, size: N*1
% ===========================================================
function probs = Gmmpdf(X, prior, mu, Sigma)
N = size(X,1);          % num of data
[p,M] = size(mu);       % mix num & feature dim
probs = zeros(N,1);     % init output array
for m = 1:M
    probs = probs + prior(m) * mvnpdf(X, mu(:,m)', Sigma(:,:,m));
end
end


function M = spm_meanm(A)
% Compute barycentre of matrix exponentials
% FORMAT M = spm_meanm(A)
% A - A 3D array, where each slice is a matrix
% M - the resulting mean
%
% Note that matrices should not be too dissimilar to each other or the
% procedure fails.
% See http://hal.archives-ouvertes.fr/hal-00699361/
%__________________________________________________________________________
% Copyright (C) 2012-2019 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_meanm.m 7563 2019-04-01 10:39:24Z guillaume $

N = size(A,3);
M = eye(size(A,1),size(A,2));

for iter = 1:1024
    S = zeros(size(M));
    for i=1:N
        L = real(logm(M\A(:,:,i)));
        S = S + L;
    end
    S = S/N;
    M = M*expm(S);
    %imagesc(M); drawnow
    %fprintf('%d\t%g\n', iter,sum(S(:).^2));
    if sum(S(:).^2)<1e-20
        break;
    end
end


function LL = dirichlet_score_family(counts, prior)
% DIRICHLET_SCORE Compute the log marginal likelihood of a single family
% LL = dirichlet_score(counts, prior)
%
% counts(a, b, ..., z) is the number of times parent 1 = a, parent 2 = b, ..., child = z
% prior is an optional multidimensional array of the same shape as counts.
% It defaults to a uniform prior.
%
% We marginalize out the parameters:
% LL = log \int \prod_m P(x(i,m) | x(Pa_i,m), theta_i) P(theta_i) d(theta_i)

% LL = log[  prod_j gamma(alpha_ij)/gamma(alpha_ij + N_ij)  *
%            prod_k gamma(alpha_ijk + N_ijk)/gamma(alpha_ijk)  ]
% Call the prod_k term U and the prod_j term  V.
% We reshape all quantities into (j,k) matrices
% This formula was first derived by Cooper and Herskovits, 1992.
% See also "Learning Bayesian Networks", Heckerman, Geiger and Chickering, MLJ 95.

ns = mysize(counts);
ns_ps = ns(1:end-1);
ns_self = ns(end);

if nargin < 2, prior = normalise(myones(ns)); end

if 1
  prior = reshape(prior(:), [prod(ns_ps) ns_self]);
  counts = reshape(counts,  [prod(ns_ps) ns_self]);
  %U = prod(gamma(prior + counts) ./ gamma(prior), 2); % mult over k
  LU = sum(gammaln(prior + counts) - gammaln(prior), 2);
  alpha_ij = sum(prior, 2); % sum over k
  N_ij = sum(counts, 2);
  %V = gamma(alpha_ij) ./ gamma(alpha_ij + N_ij);
  LV = gammaln(alpha_ij) - gammaln(alpha_ij + N_ij);
  %L = prod(U .* V);
  LL = sum(LU + LV);
else
  CPT = mk_stochastic(prior + counts);
  LL = sum(log(CPT(:) .* counts(:)));
end


function quality=meshquality(node,elem)
%
% quality=meshquality(node,elem)
%
% compute the Joe-Liu mesh quality measure of a tetrahedral mesh
%
% author: Qianqian Fang (fangq<at> nmr.mgh.harvard.edu)
% date: 2011/02/26
%
% input:
%    node:  node coordinates of the mesh (nn x 3)
%    elem:  element table of a tetrahedral mesh (ne x 4)
%
% output:
%    quality: a vector of the same length as size(elem,1), with
%           each element being the Joe-Liu mesh quality metric (0-1) of
%           the corresponding element. A value close to 1 represents
%           higher mesh quality (1 means equilateral tetrahedron);
%           a value close to 0 means nearly degenerated element.
%
% reference:
%    A. Liu, B. Joe, Relationship between tetrahedron shape measures,
%                    BIT 34 (2) (1994) 268-287.
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

if(size(elem,2)>4)
    elem=elem(:,1:4);
end
enum=size(elem,1);
vol=elemvolume(node,elem);
edges=meshedge(elem);
ed=node(edges(:,1),:)-node(edges(:,2),:);
ed=sum((ed.*ed)');
ed=sum(reshape(ed,[enum length(ed)/enum])')';

quality=12*((3*vol).^(2/3))./ed;


function [x, y] = findLinePoints(p1,p2);
%
%   [x, y] = findLinePoints(p1,p2)
%
% Author:  I think it was me.
% Purpose:
%      Find the x,y values that fall along a line between p1 and p2.
%
% 2002.07.24 RFD & FWC- fixed bug in vertical and horizontal special cases.
%            (Was returning column vectors instead of row.)

x1 = p1(1); y1 = p1(2);
x2 = p2(1); y2 = p2(2);

if y2 == y1
  if x1 == x2
    error;
    return;
  end
  x = [x1:x2];  y = y1*ones(1,length(x));
elseif x1 == x2
  if y1 == y2
    error;
    return;
  end
  y = [y1:y2];  x = x1*ones(1,length(y));
else
  slope = (y2-y1)/(x2-x1);
  b = y1 - slope*x1;
  if abs(y2 - y1) > abs(x2 - x1)
    if y1 < y2,  y = y1:y2;
    else,        y = y2:y1;
    end
    x = round( (y - b) / slope);
  else
    if x1 < x2, x = x1:x2;
    else        x = x2:x1;
    end
    y = round(slope*x + b);
  end
end

return;


% A SYMBOLIC DERIVATION OF THE JACOBIAN MANIPULATOR OF A KUKA LBR IIWA 14
%
% Copyright (C) 2016, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function jacobian_symbolic_kuka_iiwa
% link lengths

syms q1 q2 q3 q4 q5 q6 q7
robot = load_robot('KUKA', 'LBR_IIWA_R820_COP')

d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

% matrices DH
A01 = dh_sym(q1, d(1), a(1), alpha(1));
A12 = dh_sym(q2, d(2), a(2), alpha(2));
A23 = dh_sym(q3, d(3), a(3), alpha(3));
A34 = dh_sym(q4, d(4), a(4), alpha(4));
A45 = dh_sym(q5, d(5), a(5), alpha(5));
A56 = dh_sym(q6, d(6), a(6), alpha(6));
A67 = dh_sym(q7, d(7), a(7), alpha(7));

A02 = A01*A12;
A03 = A02*A23;
A04 = A03*A34;
A05 = A04*A45;
A06 = A05*A56;
A07 = A06*A67;
A07 = simplify(A07)

z0 = [0 0 1]';
z1 = A01(1:3,3);
z2 = A02(1:3,3);
z3 = A03(1:3,3);
z4 = A04(1:3,3);
z5 = A05(1:3,3);
z6 = A06(1:3,3);

% simplify expressions
z2 = simplify(z2);
z3 = simplify(z3);
z4 = simplify(z4);
z5 = simplify(z5);
z6 = simplify(z6);

p07=A07(1:3,4);
p17=A07(1:3,4)-A01(1:3,4);
p27=A07(1:3,4)-A02(1:3,4);
p37=A07(1:3,4)-A03(1:3,4);
p47=A07(1:3,4)-A04(1:3,4);
p57=A07(1:3,4)-A05(1:3,4);
p67=A07(1:3,4)-A06(1:3,4);

% Jacobian in linear speed
Jv = [cross(z0, p07) cross(z1, p17) cross(z2, p27) cross(z3, p37) cross(z4, p47) cross(z5, p57) cross(z6, p67)];
% Jacobian in angular speed
Jw = [z0 z1 z2 z3 z4 z5 z6];

Jw = simplify(Jw)
Jv = simplify(Jv)
% singularities = det(Jv)
Js = [Jv; Jw];

q1 = 0.1
q2 = 0.1
q3 = 0.1
q4 = 0.1
q5 = 0.1
q6 = 0.1
q7 = 0.1

Js = eval(Js)

q = [q1 q2 q3 q4 q5 q6 q7]
J = manipulator_jacobian(robot, q)

J-Js

Ts = eval(A07)
T = directkinematic(robot, q)

T-Ts

function A = dh_sym(theta, d, a, alpha)
syms q1 q2 q3 q4 q5 q6 q7
% avoid almost zero elements in cos(alpha) and sin(alpha)
ca = cos(alpha);
sa = sin(alpha);
if abs(ca) < 1e-6
    ca = 0;
end
if abs(sa) < 1e-6
    sa = 0;
end

A=[cos(theta)  -ca*sin(theta)   sa*sin(theta)   a*cos(theta);
   sin(theta)   ca*cos(theta)  -sa*cos(theta)   a*sin(theta);
            0         sa             ca             d;
            0         0               0             1];


function [ grid_weight, grid_point ] = sparse_grid ( dim_num, level_max, ...
  rule, point_num )

%*****************************************************************************80
%
%% SPARSE_GRID computes a sparse grid.
%
%  Discussion:
%
%    A Smolyak construction is used to create a multidimensional sparse grid.
%
%    The user specifies:
%    * the spatial dimension of the quadrature region,
%    * the level that defines the Smolyak grid.
%    * the 1D quadrature rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, controls the size of the final
%    sparse grid.
%
%    Input, integer RULE, the index of the rule.
%    1, "CC", Clenshaw Curtis Closed Fully Nested rule.
%    2, "F1", Fejer 1 Open Fully Nested rule.
%    3, "F2", Fejer 2 Open Fully Nested rule.
%    4, "GP", Gauss Patterson Open Fully Nested rule.
%    5, "GL", Gauss Legendre Open Weakly Nested rule.
%    6, "GH", Gauss Hermite Open Weakly Nested rule.
%    7, "LG", Gauss Laguerre Open Non Nested rule.
%
%    Input, integer POINT_NUM, the number of points in the grid,
%    as determined by LEVELS_INDEX_SIZE.
%
%    Output, real GRID_WEIGHT(POINT_NUM), the weights.
%
%    Output, real  GRID_POINT(DIM_NUM,POINT_NUM), the points.
%
  if ( rule == 1 )
    [ grid_weight, grid_point ] = sparse_grid_cfn ( dim_num, level_max, ...
      rule, point_num );
  elseif ( 2 <= rule & rule <= 4 )
    [ grid_weight, grid_point ] = sparse_grid_ofn ( dim_num, level_max, ...
      rule, point_num );
  elseif ( 5 <= rule & rule <= 6 )
    [ grid_weight, grid_point ] = sparse_grid_own ( dim_num, level_max, ...
      rule, point_num );
  elseif ( 7 == rule )
    [ grid_weight, grid_point ] = sparse_grid_onn ( dim_num, level_max, ...
      rule, point_num );
  else
    grid_weight = [];
    grid_point = [];
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SPARSE_GRID - Fatal error!\n' );
    fprintf ( 1, '  Illegal input rule index = %d\n', rule );
    error ( 'SPARSE_GRID - Fatal error!' );
  end

  return
end


function [p,f] = oct3bank(x);
% OCT3BANK Simple one-third-octave filter bank.
%    OCT3BANK(X) plots one-third-octave power spectra of signal vector X.
%    Implementation based on ANSI S1.11-1986 Order-3 filters.
%    Sampling frequency Fs = 44100 Hz. Restricted one-third-octave-band
%    range (from 100 Hz to 5000 Hz). RMS power is computed in each band
%    and expressed in dB with 1 as reference level.
%
%    [P,F] = OCT3BANK(X) returns two length-18 row-vectors with
%    the RMS power (in dB) in P and the corresponding preferred labeling
%    frequencies (ANSI S1.6-1984) in F.
%
%    See also OCT3DSGN, OCT3SPEC, OCTDSGN, OCTSPEC.

% Author: Christophe Couvreur, Faculte Polytechnique de Mons (Belgium)
%         couvreur@thor.fpms.ac.be
% Last modification: Aug. 23, 1997, 10:30pm.

% References:
%    [1] ANSI S1.1-1986 (ASA 65-1986): Specifications for
%        Octave-Band and Fractional-Octave-Band Analog and
%        Digital Filters, 1993.
%    [2] S. J. Orfanidis, Introduction to Signal Processing,
%        Prentice Hall, Englewood Cliffs, 1996.

pi = 3.14159265358979;
Fs = 44100; 				% Sampling Frequency
N = 3; 					% Order of analysis filters.
F = [ 100 125 160, 200 250 315, 400 500 630, 800 1000 1250, ...
	1600 2000 2500, 3150 4000 5000 ]; % Preferred labeling freq.
ff = (1000).*((2^(1/3)).^[-10:7]); 	% Exact center freq.
P = zeros(1,18);
m = length(x);

% Design filters and compute RMS powers in 1/3-oct. bands
% 5000 Hz band to 1600 Hz band, direct implementation of filters.
for i = 18:-1:13
   [B,A] = oct3dsgn(ff(i),Fs,N);
   y = filter(B,A,x);
   P(i) = sum(y.^2)/m;
end
% 1250 Hz to 100 Hz, multirate filter implementation (see [2]).
[Bu,Au] = oct3dsgn(ff(15),Fs,N); 	% Upper 1/3-oct. band in last octave.
[Bc,Ac] = oct3dsgn(ff(14),Fs,N); 	% Center 1/3-oct. band in last octave.
[Bl,Al] = oct3dsgn(ff(13),Fs,N); 	% Lower 1/3-oct. band in last octave.
for j = 3:-1:0
   x = decimate(x,2);
   m = length(x);
   y = filter(Bu,Au,x);
   P(j*3+3) = sum(y.^2)/m;
   y = filter(Bc,Ac,x);
   P(j*3+2) = sum(y.^2)/m;
   y = filter(Bl,Al,x);
   P(j*3+1) = sum(y.^2)/m;
end

% Convert to decibels.
Pref = 1; 				% Reference level for dB scale.
idx = (P>0);
P(idx) = 10*log10(P(idx)/Pref);
P(~idx) = NaN*ones(sum(~idx),1);

% Generate the plot
if (nargout == 0)
  bar(P);
  ax = axis;
  axis([0 19 ax(3) ax(4)])
  set(gca,'XTick',[2:3:18]); 		% Label frequency axis on octaves.
  set(gca,'XTickLabels',F(2:3:length(F)));  % MATLAB 4.1c
%  set(gca,'XTickLabel',F(2:3:length(F)));  % MATLAB 5.1
  xlabel('Frequency band [Hz]'); ylabel('Power [dB]');
  title('One-third-octave spectrum')
% Set up output parameters
elseif (nargout == 1)
  p = P;
elseif (nargout == 2)
  p = P;
  f = F;
end


%[2016]-"The whale optimization algorithm"

% (9/12/2020)

function WOA = jWhaleOptimizationAlgorithm(feat,label,opts)
% Parameters
lb    = 0;
ub    = 1;
thres = 0.5;
b     = 1;     % constant

if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'b'), b = opts.b; end
if isfield(opts,'thres'), thres = opts.thres; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X   = zeros(N,dim);
for i = 1:N
	for d = 1:dim
    X(i,d) = lb + (ub - lb) * rand();
	end
end
% Fitness
fit  = zeros(1,N);
fitG = inf;
for i = 1:N
  fit(i) = fun(feat,label,(X(i,:) > thres),opts);
  % Global best
  if fit(i) < fitG
    fitG = fit(i);
    Xgb  = X(i,:);
  end
end
% Pre
curve = zeros(1,max_Iter);
curve(1) = fitG;
t = 2;
while t <= max_Iter
	% Define a, linearly decreases from 2 to 0
  a = 2 - t * (2 / max_Iter);
  for i = 1:N
    % Parameter A (2.3)
    A = 2 * a * rand() - a;
    % Paramater C (2.4)
    C = 2 * rand();
    % Parameter p, random number in [0,1]
    p = rand();
    % Parameter l, random number in [-1,1]
    l = -1 + 2 * rand();
    % Whale position update (2.6)
    if p  < 0.5
      % {1} Encircling prey
      if abs(A) < 1
        for d = 1:dim
          % Compute D (2.1)
          Dx     = abs(C * Xgb(d) - X(i,d));
          % Position update (2.2)
          X(i,d) = Xgb(d) - A * Dx;
        end
      % {2} Search for prey
      elseif abs(A) >= 1
        for d = 1:dim
          % Select a random whale
          k      = randi([1,N]);
          % Compute D (2.7)
          Dx     = abs(C * X(k,d) - X(i,d));
          % Position update (2.8)
          X(i,d) = X(k,d) - A * Dx;
        end
      end
    % {3} Bubble-net attacking
    elseif p >= 0.5
      for d = 1:dim
        % Distance of whale to prey
        dist   = abs(Xgb(d) - X(i,d));
        % Position update (2.5)
        X(i,d) = dist * exp(b * l) * cos(2 * pi * l) + Xgb(d);
      end
    end
    % Boundary
    XB = X(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
    X(i,:) = XB;
  end
  % Fitness
  for i = 1:N
    % Fitness
    fit(i) = fun(feat,label,(X(i,:) > thres),opts);
    % Global best
    if fit(i) < fitG
      fitG = fit(i);
      Xgb  = X(i,:);
    end
  end
  curve(t) = fitG;
  fprintf('\nIteration %d Best (WOA)= %f',t,curve(t))
  t = t + 1;
end
% Select features
Pos   = 1:dim;
Sf    = Pos((Xgb > thres) == 1);
sFeat = feat(:,Sf);
% Store results
WOA.sf = Sf;
WOA.ff = sFeat;
WOA.nf = length(Sf);
WOA.c  = curve;
WOA.f  = feat;
WOA.l  = label;
end


function [Pro,Res] = interpolationAMGa(A,isC)
%% INTERPOLATIONAMGA construct prolongation and restriction matrices
%
% [Pro,Res] = INTERPOLATIONAMGA(A,isC) construct prolongation and
% restriction matrices use matrix-dependent interpolation. Each fine nodes
% use only one coarse node.
%
% In the input, A is a SPD matrix and isC is a logical array to indicate
% nodes in coarse matrix. In the output Pro and Res are prolongation and
% restriction matrices satisfying Res = Pro'.
%
% The submatrix A_{cf} is used to construct the interpolation of values on
% fine nodes from that of coarse nodes. The weight is normalized to
% preserve the constant.
%
% Example
%   load lakemesh
%   A = assemblematrix(node,elem);
%   [isC,As] = coarsenAMGc(A);
%   [Pro,Res] = interpolation(As,isC);
%
% See also: coarsenAMGc, amg
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

N = size(A,1);
%% Index map between coarse grid and fine grid
allNode = (1:N)';
fineNode = allNode(~isC);
% Nf = length(fineNode);
Nc = N-length(fineNode);
coarseNode = (1:Nc)';    % coarse node index
coarseNodeFineIdx = find(isC); % coarse node index in the fine grid

%% Construct prolongation and restriction operator
Afc = A(fineNode,coarseNodeFineIdx);     % matrix-dependent interpolation
[Dsum,j] = max(abs(Afc),[],2);
idx = (Dsum ~= 0);
ip = [coarseNodeFineIdx; fineNode(idx)];   % fine node index
jp = [coarseNode; j(idx)];                 % coarse node index
sp = [ones(Nc,1); ones(length(j(idx)),1)]; % weight = 1;
Pro = sparse(ip,jp,sp,N,Nc);
Res = Pro';


function value = r8_normal_01_sample ( )

%*****************************************************************************80
%
%% R8_NORMAL_01_SAMPLE returns a unit pseudonormal R8.
%
%  Discussion:
%
%    The standard normal probability distribution function (PDF) has
%    mean 0 and standard deviation 1.
%
%    The Box-Muller method is used, which is efficient, but
%    generates two values at a time.
%
%    Typically, we would use one value and save the other for the next call.
%    However, the fact that this function has saved memory makes it difficult
%    to correctly handle cases where we want to re-initialize the code,
%    or to run in parallel.  Therefore, we will instead use the first value
%    and DISCARD the second.
%
%    EFFICIENCY must defer to SIMPLICITY.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VALUE, a sample of the standard normal PDF.
%
  r1 = r8_uniform_01_sample ( );
  r2 = r8_uniform_01_sample ( );

  x = sqrt ( - 2.0 * log ( r1 ) ) * cos ( 2.0 * pi * r2 );

  value = x;

  return
end


function g=BaylissLinearTapering(sidelobedB,N,xPoints,a)
%%BAYLISSLINEARTAPERING The Bayliss tapering is a set of complex amplitude
%          weights for a continuous linear (narrowband) aperture antenna
%          that will form a difference beam (odd symmetry about an axis)
%          and hold the four closest sidelobes to a desired level. Such a
%          tapering can be discretized and applied to the elements in a
%          circular phased array (An array of antenna elements can be
%          viewed as a discrete approximation to a continuous aperture).
%          This function will provide the tapering values at a set of
%          discrete points given by xPoints (the origin is taken to be the
%          center of the aperture). The radius of the aperture can either
%          be provided or is taken as value of the farthest point provided.
%
%INPUTS: sidelobedB The number of decibels of the ratio of the close-in
%             sidelobe voltages to the main lobe voltage. This must be a
%             negative number. A typical value is -30.
%           N The Bayliss tapering is computed using a certain number of
%             terms. Using too many terms can be undesirable as edge
%             illumination increases, as noted in [1]. If this parameter is
%             omitted or an empty matrix is passed, then the default of 17
%             is used. In [1], it is suggested that N be chosen to be
%             <2*a/lambda, where a is the radius of the aperture and
%             lambda the wavelength.
%    xyPoints An NX1 or 1XN set of N points at which the tapering values
%             should be evaluated. The center of the aperture is taken to
%             be the origin.
%           a The radius of the aperture. Tapering weights for points in
%             xPoints outside of the aperture are taken to be zero. If
%             this parameter is omitted or an empty matrix is passed, then
%             the radius is taken to be the distance of the farthest point
%             from the origin in xPoints.
%
%OUTPUTS: g The NX1 set of discretized Bayliss tapering values evaluated at
%           the points given in xPoints. All Bayliss tapering values are
%           imaginary. The coefficients are not normalized.
%
%This function implements the algorithm given in the appendix of in [1]
%using the polynomial interpolation values in the table below Figure 4.
%This approximation means that low sidelobe patterns (-45 dB and below)
%will not have good fidelity sidelobes.
%
%EXAMPLE 1:
%Here, we evaluate the tapering values for 30dB down on a fine grid of
%points to plot what the amplitude and phase of the tapering weights looks
%like.
% numPoints=300;
% xPoints=linspace(-1,1,numPoints);
% %The Bayliss tapering weights, evaluated across the aperture.
% a=1;%Aperture radius=1.
% %gTest=BaylissLinear();
% gBayliss=BaylissLinearTapering(-30,17,xPoints,a);
%
% figure(1)
% clf
% plot(xPoints,abs(gBayliss),'-b','linewidth',2)
% h1=xlabel('x');
% h2=ylabel('Imaginary Weight');
% title('Bayliss Tapering Amplitude')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
% figure(2)
% clf
% plot(xPoints,angle(gBayliss),'-b','linewidth',2)
% h1=xlabel('x');
% h2=ylabel('Imaginary Weight');
% title('Bayliss Tapering Phase')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%EXAMPLE 2:
%Here, we consider the array response when using tapering values for 30dB
%sidelobes on a linear array with lambda/2 spacing between elements.
% N=17;
% sidelobedB=-30;
% Nx=41;%There are 2*Nx+1 points total.
% %Generate points symmetric about the origin.
% xPoints=(-(Nx-1)/2):1/2:((Nx-1)/2);
% g=BaylissLinearTapering(sidelobedB,N,xPoints);
%
% %Now, display the response with the tapering
% T=diag(g);
% [Rsp,U]=standardUVBeamPattern(T,xPoints,'NormRealVal');
%
% figure(2)
% clf
% plot(U,Rsp,'-b','LineWidth',2);
% h1=xlabel('u');
% h2=ylabel('Array Response');
% title('Bayliss Weighted Array Response')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] E. T. Bayliss, "Design of monopulse antenna difference patterns with
%    low sidelobes," The Bell System Technical Journal, vol. 47, no. 5, pp.
%    623-650, May-Jun. 1968.
%
%August 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(N))
   N=17;
end

%Defined below Equation 41 in [1].
mu=((0:(N-1))+1/2)';

%This holds the coefficients for the interpolating polynomials given below
%Figure 4 in [1]. The polynomials all take the desired sidelobe level in
%decibels as an input parameter. The first row is for the term A, which
%is a translation of the SNR parameter to a parameter in the paper. The
%next four rows are xi_1 to xi_4, which are the locations of the first four
%zeroes in the modified pattern. The final row is for p_0, which is related
%to the point at which the peak of the asymptotic difference pattern=1.
polyCoeffTable=[0.30387530,-0.05042922,-0.00027989,-0.00000343,-0.00000002;
                0.98583020,-0.03338850, 0.00014064, 0.00000190, 0.00000001;
                2.00337487,-0.01141548, 0.00041590, 0.00000373, 0.00000001;
                3.00636321,-0.00683394, 0.00029281, 0.00000161, 0;
                4.00518423,-0.00501795, 0.00021735, 0.00000088, 0;
                0.47972120,-0.01456692,-0.00018739,-0.00000218,-0.00000001];

A  =polyCoeffTable(1,1)+sidelobedB*(polyCoeffTable(1,2)+sidelobedB*(polyCoeffTable(1,3)+sidelobedB*(polyCoeffTable(1,4)+sidelobedB*polyCoeffTable(1,5))));
xi1=polyCoeffTable(2,1)+sidelobedB*(polyCoeffTable(2,2)+sidelobedB*(polyCoeffTable(2,3)+sidelobedB*(polyCoeffTable(2,4)+sidelobedB*polyCoeffTable(2,5))));
xi2=polyCoeffTable(3,1)+sidelobedB*(polyCoeffTable(3,2)+sidelobedB*(polyCoeffTable(3,3)+sidelobedB*(polyCoeffTable(3,4)+sidelobedB*polyCoeffTable(3,5))));
xi3=polyCoeffTable(4,1)+sidelobedB*(polyCoeffTable(4,2)+sidelobedB*(polyCoeffTable(4,3)+sidelobedB*(polyCoeffTable(4,4)+sidelobedB*polyCoeffTable(4,5))));
xi4=polyCoeffTable(5,1)+sidelobedB*(polyCoeffTable(5,2)+sidelobedB*(polyCoeffTable(5,3)+sidelobedB*(polyCoeffTable(5,4)+sidelobedB*polyCoeffTable(5,5))));
%p0 =polyCoeffTable(6,1)+sldelobedB*(polyCoeffTable(6,2)+sldelobedB*(polyCoeffTable(6,3)+sldelobedB*(polyCoeffTable(6,4)+sldelobedB*polyCoeffTable(6,5))));

Z=zeros(N+1,1);
%Equation 15
Z(1)=0;%The Z(0) term
%Now, the moved zeros
Z(2)=xi1;
Z(3)=xi2;
Z(4)=xi3;
Z(5)=xi4;
for k=5:N
    %The location of the non-moved zeros as given by Equation 13.
    Z(k+1)=sqrt(A^2+k^2);
end

sigma=(N+1/2)/Z(N+1);

B=zeros(N,1);
for m=0:(N-1)
    %This loop implements Equation 47.
    n=1:(N-1);
    num=prod(1-((m+1/2)./(sigma*Z(1+n))).^2);
    l=[0:(m-1),(m+1):(N-1)];
    denom=prod(1-((m+1/2)./(l+1/2)).^2);
    %We just use C=1.
    B(m+1)=1/(2*1j)*(-1)^m*(m-1/2)^2*num/denom;
end

numPoints=length(xPoints);

if(nargin<4||isempty(a))
    %The maximum distance from the origin to a point is taken to be the
    %radius of the aperture.
    a=sqrt(max(sum(xPoints.^2,1)));
end

%The loop below implements Equation 41 in [1].
g=zeros(numPoints,1);
for curPoint=1:numPoints
    if(abs(xPoints(curPoint))<=a)
        %The normalized radius at this point.
        p=pi*xPoints(curPoint)/a;
        g(curPoint)=g(curPoint)+sum(B.*sin(mu*p));
    end
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function circle_integrals_test01 ( )

%*****************************************************************************80
%
%% CIRCLE_INTEGRALS_TEST01 tests CIRCLE01_SAMPLE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 January 2014
%
%  Author:
%
%    John Burkardt
%
  m = 2;
  n = 4192;
  test_num = 20;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  Use CIRCLE01_SAMPLE to compare exact and\n' );
  fprintf ( 1, '  estimated integrals along the circumference \n' );
  fprintf ( 1, '  of the unit circle in 2D.\n' );
%
%  Get sample points.
%
  seed = 123456789;
  [ x, seed ] = circle01_sample ( n, seed );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of sample points used is %d\n', n );
%
%  Randomly choose X, Y exponents.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  If any exponent is odd, the integral is zero.\n' );
  fprintf ( 1, '  We restrict this test to randomly chosen even exponents.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Ex  Ey     MC-Estimate           Exact      Error\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num

    [ e, seed ] = i4vec_uniform_ab ( m, 0, 5, seed );

    e(1:m) = e(1:m) * 2;

    value = monomial_value ( m, n, e, x );

    result = circle01_length ( ) * sum ( value(1:n) ) / n;
    exact = circle01_monomial_integral ( e );
    error = abs ( result - exact );

    fprintf ( 1, '  %2d  %2d  %14.6g  %14.6g  %10.2e\n', ...
      e(1:m), result, exact, error );

  end

  return
end


function G = get_payoff_G_matrix_from_ygrid_3d( y_1, y_2, y_3, S_0s, sigmas, R, contractParams)
%UNTITLED5 Summary of this function goes here
%   Detailed explanation goes here

payoff_type = contractParams.payoff_type;

rho12 = R(1,2);
rho23 = R(2,3);
rho13 = R(1,3);

gamma = (rho12*rho13 - rho23)/(1 - rho12^2);

if payoff_type == 1 || payoff_type == 2   % G = S_1, or G=S_2, or G=S_3
    dim = contractParams.dim;
    if dim == 1
        payoff = @(y1,y2,y3)S_0s(1)*exp(sigmas(1)*y1);
    elseif dim == 2
        payoff = @(y1,y2,y3)S_0s(2)*exp(sigmas(2)*(y2 + rho12*y1));
    else
        payoff = @(y1,y2,y3)S_0s(3)*exp(sigmas(3)*(rho13*y1 -gamma*y2 + y3));
    end

elseif payoff_type == 5  % Geometric Basket Call / Put:  G = (sqrt(S_1) * sqrt(S_2) - K)^+ and
    K = contractParams.K;
    if contractParams.call == 1
        payoff = @(y1,y2,y3) max(0, (S_0s(1)*exp(sigmas(1)*y1) * S_0s(2)*exp(sigmas(2)*(y2 + rho12*y1))* S_0s(3)*exp(sigmas(3)*(rho13*y1 -gamma*y2 + y3)))^(1/3) - K);
    else
        payoff = @(y1,y2,y3) max(0, K - (S_0s(1)*exp(sigmas(1)*y1) * S_0s(2)*exp(sigmas(2)*(y2 + rho12*y1))* S_0s(3)*exp(sigmas(3)*(rho13*y1 -gamma*y2 + y3)))^(1/3));
    end

elseif payoff_type == 6  % Arithmetic Basket Call / Put:  G = (sqrt(S_1) * sqrt(S_2) - K)^+
    K = contractParams.K;
    if contractParams.call == 1
        payoff = @(y1,y2,y3) max(0, (1/3)*(S_0s(1)*exp(sigmas(1)*y1) + S_0s(2)*exp(sigmas(2)*(y2 + rho12*y1)) + S_0s(3)*exp(sigmas(3)*(rho13*y1 -gamma*y2 + y3))) - K);
    else
        payoff = @(y1,y2,y3) max(0, K - (1/3)*(S_0s(1)*exp(sigmas(1)*y1) + S_0s(2)*exp(sigmas(2)*(y2 + rho12*y1)) + S_0s(3)*exp(sigmas(3)*(rho13*y1 -gamma*y2 + y3))));
    end

end

m_0 = length(y_1);
G = zeros(m_0, m_0, m_0);

for i=1:m_0
    for j=1:m_0
        for k=1:m_0
            G(i,j,k) = payoff(y_1(i), y_2(j), y_3(k));
        end
    end
end

end


function v=dlyapsq(a,b)
% Solves the discrete Lyapunov equation AV'VA' - V'V +BB' =0
% V is upper triangular with real non-negative diagonal entries
% this is equivalent to v=chol(dlyap(a,b*b')) but better conditioned numerically

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

[q,s]=schur(a');
[q,s]=rsf2csf(q,s);
[qd,r]=qr(b'*q,0);
% save r for testing
r0=r;
[m,n]=size(r);
u=zeros(n,n);
if m==1
   for i=1:n-1
      in=i+1:n;
      si=s(i,i);
      aa=sqrt(1-si*si');
      u(i,i)=r(1)/aa;
      u(i,in)=(u(i,i)*si'*s(i,in)+aa*r(2:end))/(eye(n-i)-si'*s(in,in));
      r=aa*(u(i,i)*s(i,in)+u(i,in)*s(in,in))-si*r(2:end);
   end
   u(n,n)=r/sqrt(1-s(n,n)*s(n,n)');

else
   w=zeros(m,1); w(m)=1;
   em=eye(m);
   for i=1:n-m
      in=i+1:n;
      si=s(i,i);
      aa=sqrt(1-si*si');
      u(i,i)=r(1,1)/aa;
      u(i,in)=(u(i,i)*si'*s(i,in)+aa*r(1,2:end))/(eye(n-i)-si'*s(in,in));
      vv=aa*(u(i,i)*s(i,in)+u(i,in)*s(in,in))-si*r(1,2:end);
      rr=zeros(m,n-i);
      rr(1:m-1,:)=r(2:end,2:end);
      [qq,r]=qrupdate(em,rr,w,vv');
   end
   for i=n-m+1:n-1
      in=i+1:n;
      si=s(i,i);
      aa=sqrt(1-si*si');
      u(i,i)=r(1,1)/aa;
      u(i,in)=(u(i,i)*si'*s(i,in)+aa*r(1,2:end))/(eye(n-i)-si'*s(in,in));
      vv=aa*(u(i,i)*s(i,in)+u(i,in)*s(in,in))-si*r(1,2:end);
      rr=zeros(n-i+1,n-i);
      rr(1:n-i,:)=r(2:end,2:end);
      [qq,rr]=qrupdate(eye(n-i+1),rr,w(m-n+i:end),vv');
      r=rr(1:n-i,:);
   end

   u(n,n)=r/sqrt(1-s(n,n)*s(n,n)');

end

v=triu(qr(u*q'));
dv=diag(v);
ix=dv~=0;
v(ix,:)=diag(abs(dv(ix))./dv(ix))*v(ix,:);
if isreal(a) & isreal(b)
   v=real(v);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = T2quaternion2(T)
%   Returns the quaternion corresponding to an homogeneous transformation
%   matrix T. Only the 3x3 rotation matrix in T is used.
%
%   See also QPROD, QUATERNION2T.
% The method implemented here was extracted from:
% Accurate Computation of Quaternions from Rotation Matrices.
% Soheil Sarabandi and Federico Thomas
%  http://www.iri.upc.edu/files/scidoc/2068-Accurate-Computation-of-Quaternions-from-Rotation-Matrices.pdf
%   Author: Arturo Gil. Universidad Miguel Hern�ndez de Elche. email:
%   arturo.gil@umh.es date:   21/04/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function Q = T2quaternion2(T)
% use only the orientation from T
R = T(1:3, 1:3);

if R(1,1) + R(2,2) + R(3,3) > 0
    Q(1) = 0.5*sqrt(1+R(1,1) + R(2,2) + R(3,3));
else
    num = (R(3,2)-R(2,3))^2 + (R(1,3)-R(3,1))^2 + (R(2,1)-R(1,2))^2;
    den = 3 - R(1,1) - R(2,2) - R(3,3);
    Q(1) = 0.5*sqrt(num/den);
end

if R(1,1) - R(2,2) - R(3,3) > 0
    Q(2) = 0.5*sqrt(1 + R(1,1) - R(2,2) - R(3,3));
else
    num = (R(3,2)-R(2,3))^2 + (R(1,3)+R(3,1))^2 + (R(2,1)+R(1,2))^2;
    den = 3 - R(1,1) + R(2,2) + R(3,3);
    Q(2) = 0.5*sqrt(num/den);
end

if -R(1,1) + R(2,2) - R(3,3) > 0
    Q(3) = 0.5*sqrt(1 - R(1,1) + R(2,2) - R(3,3));
else
    num = (R(3,2)+R(2,3))^2 + (R(1,3)-R(3,1))^2 + (R(2,1)+R(1,2))^2;
    den = 3 + R(1,1) - R(2,2) + R(3,3);
    Q(3) = 0.5*sqrt(num/den);
end

if -R(1,1) - R(2,2) + R(3,3) > 0
    Q(4) = 0.5*sqrt(1 - R(1,1) - R(2,2) + R(3,3));
else
    num = (R(3,2)+R(2,3))^2 + (R(1,3)+R(3,1))^2 + (R(2,1)-R(1,2))^2;
    den = 3 + R(1,1) + R(2,2) + R(3,3);
    Q(4) = 0.5*sqrt(num/den);
end


%computes the spectral spread from the magnitude spectrum
%> called by ::ComputeFeature
%>
%> @param X: spectrogram (dimension FFTLength X Observations)
%> @param f_s: sample rate of audio data
%>
%> @retval vss spectral spread (in Hz)
% ======================================================================
function [vss] = FeatureSpectralSpread (X, f_s)

    % get spectral centroid as index
    vsc = FeatureSpectralCentroid(X, f_s) * 2 / f_s * (size(X, 1)-1);

    % allocate memory
    vss = zeros(size(vsc));

    % compute spread
    for n = 1:size(X,2)
        vss(n) = (((0:size(X, 1)-1)-vsc(n)).^2 * X(:, n)) ./ sum(X(:, n));
    end
    vss = sqrt(vss);

    % convert from index to Hz
    vss = vss / (size(X, 1)-1) * f_s / 2;

    % avoid NaN for silence frames
    vss (sum(X, 1) == 0) = 0;
end