function [ lambda, v, it_num ] = power_method2 ( n, a, x_init, it_max, tol )

%*****************************************************************************80
%
%% POWER_METHOD2 applies the power method for possibly complex eigenvalues.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 July 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Eric VanDeVelde,
%    Concurrent Scientific Programming,
%    Springer, 1994,
%    ISBN: 0-387-94195-9,
%    LC: QA76.58.V35.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A(N,N), the matrix.
%
%    Input, real X(N), the initial estimate for the eigenvector.
%
%    Input, integer IT_MAX, the maximum number of iterations to take.
%    1 <= IT_MAX.
%
%    Input, real TOL, an error tolerance.
%
%    Output, complex LAMBDA, the estimate for the eigenvalue.
%
%    Output, complex V(N), the estimate for the eigenvector.
%
%    Output, integer IT_NUM, the number of iterations taken.
%
  it_num = 0;
%
%  Compute data necessary to start the iteration.
%
  x(1:n,1) = x_init(1:n);
  pi_xx = x(1:n)' * x(1:n);
  x(1:n) = x(1:n) / pi_xx;
  y(1:n,1) = a(1:n,1:n) * x(1:n);
  pi_xy = x(1:n)' * y(1:n);
  pi_yy = y(1:n)' * y(1:n);

  for it = 1 : it_max

    if ( pi_yy - pi_xy * pi_xy < tol * tol * pi_yy )
      lambda = pi_xy;
      v(1:n) = y(1:n) / sqrt ( pi_yy );
      return
    end

    z(1:n,1) = a(1:n,1:n) * y(1:n);

    pi_xz = x(1:n)' * z(1:n);
    pi_yz = y(1:n)' * z(1:n);
    pi_zz = z(1:n)' * z(1:n);

    alpha = - ( pi_yz - pi_xy * pi_xz ) / ( pi_yy - pi_xy * pi_xy );
    beta = ( pi_xy * pi_yz - pi_yy * pi_xz ) / ( pi_yy - pi_xy * pi_xy );
    gamma = pi_zz + alpha * alpha * pi_yy + beta * beta ...
      + 2.0 * ( alpha * pi_yz + beta * pi_xz + alpha * beta * pi_xy );

    if ( gamma < tol * tol * pi_zz & alpha * alpha < 4.0 * beta )

      lambda_real = - alpha / 2.0;
      lambda_imag = sqrt ( 4.0D+00 * beta - alpha * alpha ) / 2.0;
      lambda = lambda_real + lambda_imag * sqrt ( - 1.0 );

      v(1:n) = ( lambda * y(1:n) - z(1:n) ) ...
       / sqrt ( beta * pi_yy + alpha * pi_yz + pi_zz );

      return
    end

    x(1:n) = y(1:n) / sqrt ( pi_yy );
    y(1:n) = z(1:n) / sqrt ( pi_yy );

    pi_xy = pi_yz / pi_yy;
    pi_yy = pi_zz / pi_yy;

    it_num = it;

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POWER_METHOD2 - Fatal error!\n' );
  fprintf ( 1, '  Convergence was not reached.\n' );

  error ( 'POWER_METHOD2 - Fatal error!' );
end


% 埃特金方法加速迭代方法求解方程
clear;
format long;
tol = 1e-10;
N = 100;
x0 = 0.5;
phi = @(x) exp(-x);
%phi = @(x) (x+1)^(1/3);

for k = 1 : N
    x1 = phi(x0);
    x2 = phi(x1);
    x2 = x2 - (x2 - x1)^2 / (x2 - 2 * x1 + x0);
    if abs(x2 - x0) < tol
        fprintf('方程的正根: %10.8f\n', x1);
        break;
    end
    x0 = x2;
end
if k == N
    fprintf('迭代方法失败\n');
end
fprintf('迭代次数: %d\n', k);


function h = huber_pot(t, d)
%| function h = huber_pot(t, d)
%| huber potential function

if nargin < 1, ir_usage, end
if nargin < 2, huber_pot_test, return, end

h = t.^2 / 2;
ii = abs(t) > d;
h(ii) = d * abs(t(ii)) - d.^2/2;

function huber_pot_test
t = linspace(-9,9,101);
delta = 4;
plot(t, huber_pot(t, delta), '-', delta, huber_pot(delta, delta), 'o')


function mean = extreme_values_mean ( a, b )

%*****************************************************************************80
%
%% EXTREME_VALUES_MEAN returns the mean of the Extreme Values PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < B.
%
%    Output, real MEAN, the mean of the PDF.
%
  mean = a + b * euler_constant ( );

  return
end


function tinv = t_inv (x, v, mu, sigma)
% T_INV   Inverse of Student's T cumulative distribution function (cdf).
%    X=TINV(P,V,MU,SIGMA) returns the inverse of Student's T cdf with V degrees
%    of freedom, at the values in P, with mean MU and standard deviation
%    SIGMA.
%
%    The size of X is the common size of P and V. A scalar input
%    functions as a constant matrix of the same size as the other input.

% Copyright (c) 1995-1997, 2005-2007 Kurt Hornik

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.
if nargin < 4
  sigma = 1;
end
if nargin < 3
  mu = 0;
end
if (~isscalar(mu) || ~isscalar(sigma))
  if ~((size(x,1) == size(mu,1)) && (size(mu,1) == size(sigma,1)))
    error ('norm_inv: x, mu and sigma must be of common size or scalars');
  end
end

x = (x-mu)./sigma;

if (nargin < 2)
  error('must give atleast 2 input arguments')
end

if (~isscalar(v))

  if (size(x,1) ~= size(v,1))
    error ('tinv: x and v must be of common size or scalar');
  end
end

tinv = zeros(size(x));

k = find( (x<0) || (x > 1) || isnan(x) || (v<0));
if (any(k))
  tinv(k) = NaN;
end

k = find ((x == 0) & (v > 0));
if (any (k))
  tinv(k) = -Inf;
end

k = find ((x == 1) & (v > 0));
if (any (k))
  tinv(k) = Inf;
end

k = find ((x > 0) & (x < 1) & (v > 0) & (v < 10000));
if (any (k))
  if (isscalar (v))
    tinv(k) = (sign (x(k) - 1/2) ...
    .* sqrt (v .* (1 ./ beta_inv (2*min(x(k),1-x(k)), ...
    v/2,1/2)-1)));
  else
    tinv(k) = (sign(x(k)-1/2) ...
    .* sqrt (v(k).*(1./beta_inv(2*min(x(k), 1-x(k)), ...
    v(k)/2,1/2)-1)));
  end
end

% For large v, use the quantiles of the standard normal
k = find ((x > 0) & (x < 1) & (v >= 10000));
if (any (k))
  tinv(k) = sqrt (2)*erfinv(2*x(k)-1);
end

end

% inv = sqrt (2) * erfinv (2 * x - 1);


function [px, py, gx, gy] = position_3_link(z,P)
%[px,py,gx,gy] = POSITION_3_LINK(Z,P)
%
%FUNCTION:  This function computes the cartesian positions
%    of the CoM and tip of each link
%INPUTS:
%
%
%OUTPUTS:
%    px = [nLink X nTime] position of the end of each link
%    py = [nLink X nTime] position of the end of each link
%    gx = [nLink X nTime] position of the CoM of each link
%    gy = [nLink X nTime] position of the CoM of each link
%
%NOTES:
%    This file was automatically generated by writePosition.m

g  = P.g ; %gravity
m1 = P.m(1); % Link 1 mass
m2 = P.m(2); % Link 2 mass
m3 = P.m(3); % Link 3 mass
l1 = P.l(1); % Link 1 length
l2 = P.l(2); % Link 2 length
l3 = P.l(3); % Link 3 length
I1 = P.I(1); % Link 1 moment of inertia about its center of mass
I2 = P.I(2); % Link 2 moment of inertia about its center of mass
I3 = P.I(3); % Link 3 moment of inertia about its center of mass
d1 = P.d(1); % Link 1 distance between center of mass and parent joint
d2 = P.d(2); % Link 2 distance between center of mass and parent joint
d3 = P.d(3); % Link 3 distance between center of mass and parent joint

th1 = z(1,:);
th2 = z(2,:);
th3 = z(3,:);

nTime = length(th1);
px = zeros(3,nTime);
py = zeros(3,nTime);
gx = zeros(3,nTime);
gy = zeros(3,nTime);

px(1,:) = l1.*cos(th1);
py(1,:) = l1.*sin(th1);
gx(1,:) = d1.*cos(th1);
gy(1,:) = d1.*sin(th1);

px(2,:) = l1.*cos(th1) + l2.*cos(th2);
py(2,:) = l1.*sin(th1) + l2.*sin(th2);
gx(2,:) = d2.*cos(th2) + l1.*cos(th1);
gy(2,:) = d2.*sin(th2) + l1.*sin(th1);

px(3,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3);
py(3,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3);
gx(3,:) = d3.*cos(th3) + l1.*cos(th1) + l2.*cos(th2);
gy(3,:) = d3.*sin(th3) + l1.*sin(th1) + l2.*sin(th2);

end


function exact = p44_exact ( )

%*****************************************************************************80
%
%% P44_EXACT returns the exact integral for problem 44.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real EXACT, the value of the integral.
%
  alpha = p44_param_get ( );

  exact = ( 20.0 * sin ( 2.0^alpha ) ...
    - 2.0^alpha * cos ( 2.0^alpha ) ...
    + 2.0^alpha * exp ( -20.0 ) ) / ( 400.0 + 4.0^alpha );

  return
end


function [g,A] = minusOne_v2(f)
%minusOne Multiplies an input array by (-1)^x+y.
%   [G,A] = minusOne(F) multiplies F by (-1)^x+y to produce G. F can be
%   a 1-D or 2-D array. On the output, G = (-1)^(x+y).*F, and A is the
%   array of values (-1)^(x+y). Output G is floating point with values
%   in the same numerical range as F.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Preliminaries.

% Convert input to floating point for multiplication later.
f = im2double(f);

% Image size.
[M,N] = size(f);

x = (0:M - 1)';
y = 0:N - 1;

A = (-1).^(x + y);

% Multiply input by matrix.
g = A.*f;


function [J grad] = nnCostFunction(nn_params, ...
                                   input_layer_size, ...
                                   hidden_layer_size, ...
                                   num_labels, ...
                                   X, y, lambda)
%NNCOSTFUNCTION Implements the neural network cost function for a two layer
%neural network which performs classification
%   [J grad] = NNCOSTFUNCTON(nn_params, hidden_layer_size, numn,/_labels, ...
%   X, y, lambda) computes the cost and gradient of the neural network. The
%   parameters for the neural network are "unrolled" into the vector
%   nn_params and need to be converted back into the weight matrices.
%
%   The returned parameter grad should be a "unrolled" vector of the
%   partial derivatives of the neural network.
%

% Reshape nn_params back into the parameters Theta1 and Theta2, the weight matrices
% for our 2 layer neural network
Theta1 = reshape(nn_params(1:hidden_layer_size * (input_layer_size + 1)), ...
                 hidden_layer_size, (input_layer_size + 1));

Theta2 = reshape(nn_params((1 + (hidden_layer_size * (input_layer_size + 1))):end), ...
                 num_labels, (hidden_layer_size + 1));

% Setup some useful variables
m = size(X, 1);

% You need to return the following variables correctly
J = 0;
Theta1_grad = zeros(size(Theta1));
Theta2_grad = zeros(size(Theta2));

% ====================== YOUR CODE HERE ======================
% Instructions: You should complete the code by working through the
%               following parts.
%
% Part 1: Feedforward the neural network and return the cost in the
%         variable J. After implementing Part 1, you can verify that your
%         cost function computation is correct by verifying the cost
%         computed in ex4.m
%
% Part 2: Implement the backpropagation algorithm to compute the gradients
%         Theta1_grad and Theta2_grad. You should return the partial derivatives of
%         the cost function with respect to Theta1 and Theta2 in Theta1_grad and
%         Theta2_grad, respectively. After implementing Part 2, you can check
%         that your implementation is correct by running checkNNGradients
%
%         Note: The vector y passed into the function is a vector of labels
%               containing values from 1..K. You need to map this vector into a
%               binary vector of 1's and 0's to be used with the neural network
%               cost function.
%
%         Hint: We recommend implementing backpropagation using a for-loop
%               over the training examples if you are implementing it for the
%               first time.
%
% Part 3: Implement regularization with the cost function and gradients.
%
%         Hint: You can implement this around the code for
%               backpropagation. That is, you can compute the gradients for
%               the regularization separately and then add them to Theta1_grad
%               and Theta2_grad from Part 2.
%
% FORWARD PROPOGATION
a1 = [ones(m,1) X];
z2 = (a1*Theta1');
a2 = [ones(size(z2,1),1) sigmoid(z2)];
h_theta = sigmoid(a2*Theta2');
a3 = h_theta;
y_matrix = eye(num_labels)(y,:);
J = (-sum(sum(y_matrix.*log(h_theta))) - sum(sum((1-y_matrix).*(log(1-h_theta)))))/m;

% REGULARIZATION
regularization_term = (lambda/(2*m))*((sum(sum(Theta1(:,2:end).^2))) + sum(sum(Theta2(:,2:end).^2)));
J = J + regularization_term;

% BACK PROPOGATION
d3 = a3 - y_matrix;                                             % has same dimensions as a3
d2 = (d3*Theta2).*[ones(size(z2,1),1) sigmoidGradient(z2)];     % has same dimensions as a2

D1 = d2(:,2:end)' * a1;    % has same dimensions as Theta1
D2 = d3' * a2;    % has same dimensions as Theta2

Theta1_grad = Theta1_grad + (1/m) * D1;
Theta2_grad = Theta2_grad + (1/m) * D2;

% REGULARIZATION OF THE GRADIENT

Theta1_grad(:,2:end) = Theta1_grad(:,2:end) + (lambda/m)*(Theta1(:,2:end));
Theta2_grad(:,2:end) = Theta2_grad(:,2:end) + (lambda/m)*(Theta2(:,2:end));
% -------------------------------------------------------------

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

% Unroll gradients
grad = [Theta1_grad(:) ; Theta2_grad(:)];

end


function X = positive_definite_karcher_mean(A)
% Computes a Karcher mean of a collection of positive definite matrices.
%
% function X = positive_definite_karcher_mean(A)
%
% Input:  A 3D matrix A of size nxnxm such that each slice A(:,:,k) is a
%         positive definite matrix of size nxn.
%
% Output: A positive definite matrix X of size nxn which is a Karcher mean
%         of the m matrices in A, that is, X minimizes the sum of squared
%         Riemannian distances to the matrices in A:
%            f(X) = sum_k=1^m .5*dist^2(X, A(:, :, k))
%         The distance is defined by the natural metric on the set of
%         positive definite matrices: dist(X,Y) = norm(logm(X\Y), 'fro').
%
% This simple example is not the best way to compute Karcher means. Its
% purpose it to serve as base code to explore other algorithms. In
% particular, in the presence of large noise, this algorithm seems to not
% be able to reach points with a very small gradient norm. This may be
% caused by insufficient accuracy in the gradient computation.

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Nicolas Boumal, Sept. 3, 2013
% Contributors:
%
% Change log:
%

    % Generate some random data to test the function if none is given.
    if ~exist('A', 'var') || isempty(A)
        n = 5;
        m = 10;
        A = zeros(n, n, m);
        ref = diag(max(.1, 1+.1*randn(n, 1)));
        for i = 1 : m
            noise = 0.01*randn(n);
            noise = (noise + noise')/2;
            [V D] = eig(ref + noise);
            A(:, :, i) = V*diag(max(.01, diag(D)))*V';
        end
    end

    % Retrieve the size of the problem:
    % There are m matrices of size nxn to average.
    n = size(A, 1);
    m = size(A, 3);
    assert(n == size(A, 2), ...
           ['The slices of A must be square, i.e., the ' ...
	        'first and second dimensions of A must be equal.']);

    % Our search space is the set of positive definite matrices of size n.
    % Notice that this is the only place we specify on which manifold we
    % wish to compute Karcher means. Replacing this factory for another
    % geometry will yield code to compute Karcher means on that other
    % manifold, provided that manifold is equipped with a dist function and
    % a logarithmic map log.
    M = sympositivedefinitefactory(n);

    % Define a problem structure, specifying the manifold M, the cost
    % function and its gradient.
    problem.M = M;
    problem.cost = @cost;
    problem.grad = @grad;

    % The functions below make many redundant computations. This
    % performance hit can be alleviated by using the caching system. We go
    % for a simple implementation here, as a tutorial example.

    % Cost function
    function f = cost(X)
        f = 0;
        for k = 1 : m
            f = f + M.dist(X, A(:, :, k))^2;
        end
        f = f/(2*m);
    end

    % Riemannian gradient of the cost function
    function g = grad(X)
        g = M.zerovec(X);
        for k = 1 : m
            % Update g in a linear combination of the form
            % g = g - [something]/m.
            g = M.lincomb(X, 1, g, -1/m, M.log(X, A(:, :, k)));
        end
    end

    % Execute some checks on the derivatives for early debugging.
    % These things can be commented out of course.
    % The slopes should agree on part of the plot at least. In this case,
    % it is sometimes necessary to inspect the plot visually to make the
    % call, but it is indeed correct.
    % checkgradient(problem);
    % pause;

    % Execute this if you want to force using a proper parallel vector
    % transport. This is not necessary. If you omit this, the default
    % vector transport is the identity map, which is (of course) cheaper
    % and seems to perform well in practice.
    % M.transp = M.paralleltransp;

    % Issue a call to a solver. Default options are selected.
    % Our initial guess is the first data point.
    X = trustregions(problem, A(:, :, 1));

end


function [gd,fnorm]=sgrpdlay(x,fnorm);
%SGRPDLAY Group delay estimation of a signal.
%	[GD,FNORM]=SGRPDLAY(X,FNORM) estimates the group delay of
%	signal X at the normalized frequency(ies) FNORM.
%
%	X     : signal in the time-domain.
%	FNORM : normalized frequency. By default, FNORM is a
%               linearly spaced vector between -0.5 and 0.5 with
%               length(X) elements.
%	GD    : computed group delay. When GD equals zero, it means that
%	 	the estimation of the group delay for this frequency was
%		outside the interval [1 xrow], and therefore meaningless.
%
%	Example :
%	 N=128; x=amgauss(N,64,30).*fmlin(N,0.1,0.4);
%	 fnorm=0.1:0.04:0.38; gd=sgrpdlay(x,fnorm);
%	 t=2:N-1; instf=instfreq(x,t);
%	 plot(t,instf,gd,fnorm); axis([1 N 0 0.5]);

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

if (nargin == 0),
 error('At least one parameter required');
end;

[xrow,xcol] = size(x);
if (xcol~=1),
 error('x must have only one column');
end;
Ex=mean(abs(x).^2);
Threshold=1.0e-6;

if (nargin==1),
 Num=fft(x .* (1:xrow)');
 Den=fft(x);
 ratio=(real(Num./Den) .* (real(Num./Den)>=1) .* (real(Num./Den)<=xrow+3));
 gd=fftshift(ratio);
 if (nargout==2),
  if (rem(xrow,2)==0),
   L=xrow/2;
   fnorm=(-L:L-1)/xrow;
  else
   L=(xrow-1)/2;
   fnorm=(-L:L)/xrow;
  end;
 end;
elseif (nargin==2),
 [fnormrow,fnormcol] = size(fnorm);
 if (fnormrow~=1),
  error('FNORM must have only one row');
 end;
 expo=exp(-j*2.0*pi*fnorm'*(0:xrow-1));
 Num=expo*(x .* (1:xrow)');
 Den=expo*x;
 gd=(real(Num./Den) .* (real(Num./Den)>=1) .* (real(Num./Den)<=xrow+3));
end;


function pendulum()
% Matlab code for simulating a simple pendulum

P.g = 9.81;     %(m/s^2) gravity acceleration
P.l = 1.0;      %(m) length of pendulum
P.c = 1.0;     %(1/s) viscous damping constant
P.m = 1.0;     %(kg) pendulum mass

th0 = 0.5;  %(rad) initial angle of pendulum, from -j axis
w0 = 0.0;   %(rad/s) initial angular rate of pendulum
z0 = [th0;w0];   %Initial state vector

tSpan = [0,4]; %(s) [start, end] times for simulation
dtMax = 0.01;  %(s) maximum allowable time step

nStep = ceil(diff(tSpan)/dtMax);  %Number of simulation steps
t = linspace(tSpan(1),tSpan(2),nStep);  %(s) time vector
z = zeros(2,nStep); %  Initialize the state matrix
z(:,1) = z0;

%Run the simulation, using Euler integration
for i=2:nStep
    dt = t(i)-t(i-1);
    zNow = z(:,i-1);
    z(:,i) = zNow + dt*dynamics(zNow,P);
end

%Generate a plot of the result:
figure(1); clf;
subplot(2,1,1); plot(t,z(1,:));
xlabel('Time (s)'); ylabel('Angle (rad)');
title('Simple Damped Pendulum');
subplot(2,1,2); plot(t,z(2,:));
xlabel('Time (s)'); ylabel('Rate (rad/s)');

end

function dz = dynamics(z,P)
% Compute the dynamics of a simple damped pendulum:

th = z(1,:); w = z(2,:); %Unpack the state vector

%Dynamics:
dth = w;
dw = -(P.g/P.l)*sin(th) - (P.c/(P.m*P.l*P.l))*w;

dz = [dth; dw];  %Pack up state derivative
end


% By Sherif A. Tawfik, Faculty of Engineering, Cairo University
% [x,val,status]=IP1(f,A,b,Aeq,beq,lb,ub,M,e)
% this function solves the following mixed-integer linear programming problem
%   min f*x
%  subject to
%        A*x <=b
%        Aeq * x = beq
%        lb <= x <= ub
%        M is a vector of indeces for the variables that are constrained to be integers
%        e is the integarilty tolerance
% the return variables are :
% x : the solution
% val: value of the objective function at the optimal solution
% status =1 if successful
%        =0 if maximum number of iterations reached in he linprog function
%        =-1 if there is no solution
% Example:
%        maximize 17 x1 + 12 x2
%        subject to
% 	             10 x1 + 7 x2 <=40
%                   x1 +   x2 <= 5
%                   x1, x2 >=0 and are integers
% f=[-17, -12]; %take the negative for maximization problems
% A=[ 10  7; 1 1];
% B=[40; 5];
% lb=[0 0];
% ub=[inf inf];
% M=[1,2];
% e=2^-24;
% [x v s]= IP(f,A,B,[],[],lb,ub,M,e)

function [x,val,status]=IP1(f,A,b,Aeq,beq,lb,ub,M,e)
options = optimset('display','off');
bound=inf; % the initial bound is set to +ve infinity
[x0,val0]=linprog(f,A,b,Aeq,beq,lb,ub,[],options);
[x,val,status,b]=rec(f,A,b,Aeq,beq,lb,ub,x0,val0,M,e,bound); % a recursive function that processes the BB tree

function [xx,val,status,bb]=rec(f,A,b,Aeq,beq,lb,ub,x,v,M,e,bound)
options = optimset('display','off');
% x is an initial solution and v is the corressponding objective function value

% solve the corresponding LP model with the integarily constraints removed
[x0,val0,status0]=linprog(f,A,b,Aeq,beq,lb,ub,[],options);

% if the solution is not feasible or the value of the objective function is
% higher than the current bound return with the input intial solution
if status0<=0 | val0 > bound
    xx=x; val=v; status=status0; bb=bound;
    return;
end

% if the integer-constraint variables turned to be integers within the
% input tolerance return
ind=find( abs(x0(M)-round(x0(M)))>e );
if isempty(ind)
    status=1;
    if val0 < bound    % this solution is better than the current solution hence replace
        x0(M)=round(x0(M));
        xx=x0;
        val=val0;
        bb=val0;
    else
        xx=x;  % return the input solution
        val=v;
        bb=bound;
    end
    return
end

% if we come here this means that the solution of the LP relaxation is
% feasible and gives a less value than the current bound but some of the
% integer-constraint variables are not integers.
% Therefore we pick the first one that is not integer and form two LP problems
% and solve them recursively by calling the same function (branching)

% first LP problem with the added constraint that Xi <= floor(Xi) , i=ind(1)
br_var=M(ind(1));
br_value=x(br_var);
if isempty(A)
    [r c]=size(Aeq);
else
    [r c]=size(A);
end
A1=[A ; zeros(1,c)];
A1(end,br_var)=1;
b1=[b;floor(br_value)];

% second LP problem with the added constraint that Xi >= ceil(Xi) , i=ind(1)
A2=[A ;zeros(1,c)];
A2(end,br_var)=-1;
b2=[b; -ceil(br_value)];

% solve the first LP problem
[x1,val1,status1,bound1]=rec(f,A1,b1,Aeq,beq,lb,ub,x0,val0,M,e,bound);
status=status1;
if status1 >0 & bound1<bound % if the solution was successfull and gives a better bound
   xx=x1;
   val=val1;
   bound=bound1;
   bb=bound1;
else
    xx=x0;
    val=val0;
    bb=bound;
end

% solve the second LP problem
[x2,val2,status2,bound2]=rec(f,A2,b2,Aeq,beq,lb,ub,x0,val0,M,e,bound);

if status2 >0 & bound2<bound % if the solution was successfull and gives a better bound
    status=status2;
    xx=x2;
    val=val2;
    bb=bound2;
end


%DELTA2TR Convert differential motion  to a homogeneous transform
%
% T = DELTA2TR(D) is a homogeneous transform (4x4) representing differential
% translation and rotation. The vector D=(dx, dy, dz, dRx, dRy, dRz)
% represents an infinitessimal motion, and is an approximation to the spatial
% velocity multiplied by time.
%
% See also TR2DELTA.

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB 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.
%
% RTB 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 RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function delta = delta2tr(d)
    d = d(:);
    delta = eye(4,4) + [skew(d(4:6)) d(1:3); 0 0 0 0];


function nuclides
% Solution for a chain of radionuclides
%    using MATLAB expm
%   $Ekkehard Holzbecher  $Date: 2006/28/12 $
%--------------------------------------------------------------------------

T = 10;                % maximum time
lambda = [1; 0.1; 0.5];% decay rates
c0 = [1; 0; 0];        % initial concentrations
q = [0.1; 0; 0];       % source rates
N = 60;                % discretization of time

t = linspace (0,T,N);
B = -diag(lambda);
for i = 2:size(lambda,1)
    B(i,i-1) = lambda(i-1);
end
c = c0;

for i = 2:N
    E = expm(B*t(i));
    c = [c E*c0-(eye(size(B,1))-E)*inv(B)*q];
end
plot (t,c');
legend ('mother','daughter 1','daughter 2');
text (T/2,0.8,'Steady state:'); text (T/2,0.7,num2str(-(inv(B)*q)'));
xlabel ('time');


classdef house

methods(Static)

function [v, beta] = gen(x)
    % Generates the householder reflection vector for a given vector x
    % P = I - beta * v * v' is the householder projection matrix
    % P is an orthogonal projector
    % P *x changes x in such a way that only 1st entry is non-zero
    % GVL4: algorithm 5.1.1
    m = length(x);
    if m == 1
        v = 0; beta = 0; return;
    end
    sigma = x(2:m)' * x(2:m);
    v = [1; x(2:m)];
    if sigma == 0  && x(1) >= 0
        beta = 0;
    elseif sigma == 0 && x(1) < 0
        beta = -2;
    else
        mu = sqrt(x(1)^2 + sigma);
        if x(1) <  0
            v(1) = x(1) - mu;
        else
            v(1) = -sigma /(x(1) + mu);
        end % if
        beta = 2 * v(1)^2 / (sigma + v(1)^2);
        v = v / v(1);
    end % if
end % func

function B = premul(A, v, beta)
    % Efficiently computes B = PA  = (I - beta v v') A
    B = A - (beta * v) *(v' * A);
end % func

function B = postmul(A, v, beta)
    % Efficiently computes B = AP  = A (I - beta v v')
    B = A - (A*v) * (beta * v)';
end % func

function beta = beta_from_v(v)
    beta = 2/(v'*v);
end% function

function v = from_essential_v(essential)
    v = [zeros(j-1,1); 1; essential];
end% function

function [W, Y] = wy(QF)
    % Let Q be stored in the factored form Q = Q_1 * ... * Q_r
    % where Q_i = I - beta * v * v'
    % This algorithm computes W and Y such that
    % Q = I_r - W * Y';
    % GVL4: algorithm 5.1.2
    [m,r] = size(QF);
    for j=1:r
        % extract j-th v from the factors array
        % The essential part of v
        essential = QF(j+1:m,j);
        % Insert additional zeros and one to complete v
        v = [zeros(j-1,1); 1; essential];
        % Compute beta from v [it is not stored in factors array]
        beta = 2/(v'*v);
        if j==1
            % Initialize W and Y
            Y = v;
            W = beta*v;
        else
            % Update W and Y
            z = beta*(v - W*(Y(j:m,:)'*v(j:m)));
            Y = [Y v];
            W = [W z];
        end
    end
end% function

function B = wy_premul(A, W, Y)
    % Efficiently computes B = (I - WY') A
    B = A - W*(Y'*A);
end % func

function B = wy_postmul(A, W, Y)
    % Efficiently computes B = A (I - WY')
    B = A - (A*W)*Y';
end % func

function [QF, R] = qr(A)
    % Performs A = QR factorization
    % using Householder reflections
    % GVL4: Algorithm 5.2.1
    import spx.la.house;
    [m,n] = size(A);
    for j=1:min(n,m-1)
        % Compute the jth Householder matrix Q{j}...
        [v,beta] = house.gen(A(j:m,j));
        % Update... A = Q_j A
        A(j:m,j:n) = house.premul(A(j:m,j:n), v, beta);
        % Save the essential part of householder vector...
        A(j+1:m,j) = v(2:m-j+1);
    end
    % extract the R component
    R = triu(A(1:n,1:n));
    % extract the Q component in factored form
    QF = tril(A,-1);
end% function

function [QF, R, P] = qr_pivot(A)
    % Householder QR with column pivoting
    % Achieves factorization AP = QR
    % GVL4 : algorithm 5.4.1
    import spx.la.house;
    [m,n] = size(A);
    % Space for keeping squared norms of columns
    c = zeros(1,n);
    % Compute squared norms of each column
    for j=1:n
        c(j) = A(:,j)'*A(:,j);
    end
    % space for the permutation matrix
    P = 1:n;
    % index of column being processed
    r = 0;
    % Find the index of maximum norm column
    [tau,k] = max(c);
    % The tau > 0 check ensures that we process only till
    % rank of A
    while tau>1e-10 && r<min(m,n)
        r = r+1;
        % Column interchange between current column and max norm column
        A(:,[r k]) = A(:,[k r]);
        % Corresponding column interchange in the permutation matrix
        P([r k]) = P([k r]);
        % Corresponding interchange in the squared norm vector
        c([r k]) = c([k r]);
        % Compute the r-th Householder vector
        [v,beta] = house.gen(A(r:m,r));
        % Update by premultiplication A = Q_r A
        A(r:m,r:n) = A(r:m,r:n) - (beta*v)*(v'*A(r:m,r:n));
        % Save the essential part of householder vector.
        A(r+1:m,r) = v(2:m-r+1);
        if r<min(m,n)
            % update the squared norms
            c(r+1:n) = c(r+1:n)- A(r,r+1:n).^2;
            [tau k] = max(c(r+1:n));
            k = k+r;
        end
    end
    % extract the R component
    R = triu(A(1:n,1:n));
    % extract the Q component in factored form below sub-diagonal
    QF = tril(A,-1);
end % function

function Q = q_back_accum_thin(QF)
    % Explicit formation of an orthogonal matrix from its factored form
    %   representation. Uses backward accumulation.
    % GVL4: Section 5.2.2
    % Produces a "thin", m x n Q
    % with orthonormal columns, the first n columns of Q_{1}...Q_{n}.
    [m,n] = size(QF);
    % start with an empty matrix
    Q = [];
    % iterate over columns backwards
    for j=n:-1:1
        % extract the essential part of the j-th householder vector
        essential = QF(j+1:m,j);
        % expand it
        v = [1;essential];
        % Compute beta from the vector
        beta = 2/(v'*v);
        % Expand the Q matrix with zeros on top right and bottom left
        k = m-j;
        Q = [1 zeros(1,n-j);
             zeros(m-j,1) Q];
        % check if the essential part was non-zero
        if norm(essential)>0
            % premultiply Q with the householder projector
            Q = Q - (beta*v)*(v'*Q);
        end
    end
end% function

function Q = q_back_accum_full(QF)
    % Explicit formation of an orthogonal matrix from its factored form
    %   representation. Uses backward accumulation.
    % QF is m x n and
    % Produces an m x m Q that is the product of of Q_{1}...Q_{n}.
    % GVL4: Section 5.2.2
    [m,n] = size(QF);
    % We start with a matrix of size (m-n) x (m -n)
    % If m <= n, then we start with a matrix of size 1 x 1
    Q = eye(max(m-n,1),max(m-n,1));
    % We iterative over columns backwards
    for j=min(n,m-1):-1:1
        % Q_fact(j+1:m,j) is the essential part of the
        % j-th Householder matrix Q_{j}.
        % Get the essential part of householder vector in this column
        essential = QF(j+1:m,j);
        % Extend the vector with 1
        v = [1;essential];
        % Compute beta from the householder vector
        beta = 2/(v'*v);
        % Expand Q with zeros
        k = m-j;
        Q = [1 zeros(1,k);
             zeros(k,1) Q];
        % check for zero changes.
        if norm(essential)>0
            % premultiply with Householder projector
            Q = Q - (beta*v)*(v'*Q);
        end
   end
end % function

function [UF, B, VF] = bidiag(A)
    % Performs bidiagonalization of A = U B V'.
    % U and V are stored in factored forms.
    % GVL4: algorithm 5.4.2
    import spx.la.house;
    [m,n] = size(A);
    % Iterate over columns of A from first to last column.
    % If A is tall, then we can process all columns.
    % If A is square, then we will process m -1 columns
    % If A is wide, we can process all columns.
    for j=1:min(n,m-1)
        % zero out the j-th column
        % Compute the jth Householder vector  for U_j
        [u,beta] = house.gen(A(j:m,j));
        % Update... A = U_j A
        A(j:m,j:n) = A(j:m,j:n) - (beta*u)*(u'*A(j:m,j:n));
        % Save the essential part of householder vector...
        A(j+1:m,j) = u(2:m-j+1);
        if j+2<=n
            % zero out the j-th row now
            % Compute the j-th Householder vector for V_j
            [v,beta] = house.gen(A(j,j+1:n)');
            % Update... A = A V_j
            A(j:m,j+1:n) = A(j:m,j+1:n) - (A(j:m,j+1:n)*v)*(beta*v)';
            % Save the essential Householder vector in j-th row
            A(j,j+2:n) = v(2:n-j)';
        end
    end
    % Extract the diagonal and first super-diagonal
    B = tril(triu(A),1);
    % Extract the subdiagonal lower triangular part
    UF = tril(A,-1);
    % Space for V factors
    VF = zeros(n,n);
    % Extract the factors row by row
    for j=1:n-2
        % j-th row householder vector
        VF(j+2:n,j+1) = A(j,j+2:n)';
    end
end % function

end % methods

end % classdef


function c=ref_dgt_3(f,g,a,M)
%REF_DGT_3  DGT algorithm 3

L=size(g,1);
N=L/a;
b=L/M;

[c,h_a,h_m]=gcd(-a,M);
p=a/c;
q=M/c;
d=N/q;

w=zeros(M,N);

if 0

  % This version uses the definition.

  F=zeros(c,d,p,q);
  G=zeros(c,d,p,q);

  for r=0:c-1
    for s=0:d-1
      for k=0:p-1
	for l=0:q-1
	  for st=0:d-1
	    F(r+1,s+1,k+1,l+1)=F(r+1,s+1,k+1,l+1)+f(mod(r+k*M+st*p*M-l*h_a*a,L)+1)*exp(-2*pi*i*s*st/d);
	    G(r+1,s+1,k+1,l+1)=G(r+1,s+1,k+1,l+1)+g(mod(r+k*M-l*a+st*p*M,L)+1)*exp(-2*pi*i*s*st/d);
	  end;
	end;
      end;
    end;
  end;

 for r=0:c-1
    for l=0:q-1
      for u=0:q-1
	for s=0:d-1
	  for v=0:d-1
	    for k=0:p-1
	      w(r+l*c+1,mod(u+s*q-l*h_a,N)+1)=w(r+l*c+1,mod(u+s*q-l*h_a,N)+1)+...
		  1/d*F(r+1,v+1,k+1,l+1)*conj(G(r+1,v+1,k+1,u+1))*exp(2*pi*i*v*s/d);
	    end;
	  end;
	end;
      end;
    end;
  end;

else

  % This version uses matrix-vector products and ffts

  F=zeros(c,d,p,q);
  G=zeros(c,d,p,q);
  C=zeros(c,d,q,q);

  % Set up the matrices
  for r=0:c-1
    for s=0:d-1
      for k=0:p-1
	for l=0:q-1
	  F(r+1,s+1,k+1,l+1)=f(mod(r+k*M+s*p*M-l*h_a*a,L)+1);
	  G(r+1,s+1,k+1,l+1)=sqrt(M*d)*g(mod(r+k*M-l*a+s*p*M,L)+1);
	end;
      end;
    end;
  end;

  % fft them
  F=dft(F,[],2);
  G=dft(G,[],2);

  % Multiply them
  for r=0:c-1
    for s=0:d-1
      GM=reshape(G(r+1,s+1,:,:),p,q);
      FM=reshape(F(r+1,s+1,:,:),p,q);
      C(r+1,s+1,:,:)=GM'*FM;
    end;
  end;

  % Inverse fft
  C=idft(C,[],2);

  % Place the result
  for r=0:c-1
    for l=0:q-1
      for u=0:q-1
	for s=0:d-1
	  w(r+l*c+1,mod(u+s*q-l*h_a,N)+1)=C(r+1,s+1,u+1,l+1);
	end;
      end;
    end;
  end;

end;

c=dft(w);


function [pos, tri] = mesh_icosahedron

% MESH_ICOSAHEDRON returns the vertices and triangle of a 12-vertex icosahedral
% mesh.
%
% Use as
%   [pos, tri] = mesh_icosahedron
%
% See also MESH_TETRAHEDRON, MESH_OCTAHEDRON, MESH_SPHERE

% Copyright (C) 2002, Robert Oostenveld
% Copyright (C) 2019, Robert Oostenveld and Jan-Mathijs Schoffelen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%

tri = [
  1   2   3
  1   3   4
  1   4   5
  1   5   6
  1   6   2
  2   8   3
  3   9   4
  4  10   5
  5  11   6
  6   7   2
  7   8   2
  8   9   3
  9  10   4
  10  11   5
  11   7   6
  12   8   7
  12   9   8
  12  10   9
  12  11  10
  12   7  11
  ];

pos = zeros(12, 3);

rho = 0.4*sqrt(5);
phi = 2*pi*(0:4)/5;

pos( 1, :) = [0 0  1];          % top point

pos(2:6, 1) = rho*cos(phi)';
pos(2:6, 2) = rho*sin(phi)';
pos(2:6, 3) = rho/2;

pos(7:11, 1) = rho*cos(phi - pi/5)';
pos(7:11, 2) = rho*sin(phi - pi/5)';
pos(7:11, 3) = -rho/2;

pos(12, :) = [0 0 -1];          % bottom point

% scale all vertices to the unit sphere
pos = pos ./ repmat(sqrt(sum(pos.^2,2)), 1,3);


function [DVal,totalError,exitCode]=KLDistApproxGaussMix(w1,mu1,P1,w2,mu2,P2,algorithm,param8,w)
%%KLDISTAPPROXGAUSSMIX Approximate the Kullback-Leibler (KL) divergence
%          (also known as the KL distance or relative entropy) between two
%          multivariate Gaussian mixture distributions. Whereas an explicit
%          solution is available for the KL distance between two Gaussian
%          PDFs, no explicit solution exists between a Gaussian PDF and a
%          Gaussian mixture. Thus, various approximations are necessary.
%          As defined in Chapter 8.5 of [4], the relative entropy between
%          two continuous PDFs, f and g, is
%          D(f||g)=integral_{x} f(x) log(f(x)/g(x)) dx
%          The integral is over an appropriate domain of x, in this case
%          the d-dimensional real space for an n-dimensional state. Note
%          that changing the order of f and g changes the result (the
%          operation is not commutative).
%
%INPUTS: w1 The 1Xn1 or n1X1 set of weights of the first Gaussian mixture's
%           components such that all w>=0 and sum(w)=1.
%       mu1 The dXn1 set of mean values of the first Gaussian mixture's
%           components.
%        P1 The dXxdXn1 set of covariance matrices of the first Gaussian
%           mixture's components.
% w2,mu2,P2 The equivalent of w1, mu1 and P1 for the second mixtures
%           components. There length-n2, dXn2, and dXdXn2 in size.
% algorithm An optional parameter specifying the algorithm used to obtain
%           the approximation of the KL divergence. Possible values are
%           for:
%           0 Use adaptive numeric integration to directly evaluate the
%             Kullback-Leiberler divergence integral over a rectangular
%             region that is standard deviations from the means of the
%             distributions. The default number of standard deviations in
%             each direction is 4.5 and can be adjusted by passing a member
%             of param8 named 'numStdDev'. In 1D, the function
%             integral1DAdaptiveCC is used for the integration and the
%             inputs to that function 'RelTol', 'AbsTol', 'NPowMin', and
%             'NPowMax' can be passed as members of param8 if one wishes to
%             not use the defaults. If higher dimensions, the function
%             integrateUnifCub57Adaptive is used for the numeric
%             integration and the inputs to that function 'maxSearchReg',
%             'AbsTol', and 'RelTol' can be passed if one wishes to not use
%              the defaults. This approximation can produce negative
%              values, though it is often good.
%           1 Monte Carlo sampling from Equation 4 of Section 2 of [3].
%             This uses param8 as the number of samples (if given). The
%             default if param8 is not given is 3e3. As the number of
%             samples increases, this method should become increasingly
%             accurate. This approximation can produce negative values.
%           2 (The default if omitted or an empty matrix is passed) The
%             cubature integration approximation from Equation 8 in Section
%             3 of [3]. This uses param8 as the cubature points and w as
%             the weights, if provided. If not provided, then
%             fifthOrderCubPoints is used to obtain fifth-order xDim-
%             dimensional cubature points. This approximation can produce
%             negative values.
%           3 Use the variational upper bound from Section 8 of [3]. This
%             approximation should always be positive. 10 iterations are
%             performed. This bound is typically not zero when the two
%             input PDFs are equal.
%           4 The variational approximation from Section 7 of [3] (also
%             given in [1]). This approximation can produce negative
%             values.
%           5 The product of Gaussians approximation in [1] and Section 5
%             of [3]. This approximation can produce negative values.
%           6 Take the average of algorithms 5 and 4, which tend to be
%             upper and lower bounds.
%           7 Goldberger's approximation from Equation 2 of [2]. This is
%             also given in Section 6 of [3] as the matched bound
%             approximation. This approximation can produce negative
%             values and is often quite bad with certain classes of
%             distributions.
%    param8 An optional parameter. If algorithm=0, then this is a structure
%           that can take members as described above for algorithm 0. If
%           algorithm=1, then this is the number of samples to use. If
%           algorithm=2, then this is a set of cubature points as a
%           dXnumPoints matrix. For other values of algorithm, this input
%           is not used.
%         w This input is only used if algorithm=2, in which case it is the
%           set of cubature weights (sum(w)=1). that are associated with
%           the cubature points in param8. If param8 is omitted or an empty
%           matrix is passed, then w will be the weights associated with
%           the points from the fifthOrderCubPoints function.
%
%%OUTPUTS: val The approximate value of the Kullback-Leibler divergence
%              between the first and the second Gaussian mixture
%              distributions.
%
%Some approximations can produce negative values (the true KL divergence
%can never be negative).
%
%EXAMPLE 1:
%Though many of the algorithms are often cited in the literature, many of
%the approximations can be quite bad. Here, we consider a 1D example and
%then the mixture missing a few components and also the moment-matched
%Gaussian approximation.
% w1=[0.03,0.18,0.12,0.19,0.02,0.16,0.06,0.1,0.08,0.06];
% n1=length(w1);
% mu1=[1.45,2.20,0.67,0.48,1.49,0.91,1.01,1.42,2.77,0.89];
% P1=[0.0487,0.0305,0.1171,0.0174,0.0295,0.0102, 0.0323, 0.0380, 0.0115, 0.0679];
% P1=reshape(P1,[1,1,n1]);
%
% %The second PDF is the first with the five least-weight components deleted.
% w2=[0.18,0.12,0.19,0.16,0.1,0.08];
% w2=w2/sum(w2);
% n2=length(w2);
% mu2=[2.20,0.67,0.48,0.91,1.42,2.77];
% P2=[0.0305,0.1171,0.0174,0.0102,0.0380,0.0115];
% P2=reshape(P2,[1,1,n2]);
%
% %The third PDF is the second PDF with two more components deleted.
% w3=[0.18,0.12,0.19,0.16];
% w3=w3/sum(w3);
% n3=length(w3);
% mu3=[2.20,0.67,0.48,0.917];
% P3=[0.0305,0.1171,0.0174,0.0102];
% P3=reshape(P3,[1,1,n3]);
%
% %The fourth PDF is just the Gaussian matching the first two moments of the
% %original PDF.
% w4=1;
% [mu4,P4]=calcMixtureMoments(mu1,w1,P1);
%
% DVal=zeros(8,3);
% for curAlg=0:7
%     DVal(curAlg+1,1)=KLDistApproxGaussMix(w1,mu1,P1,w2,mu2,P2,curAlg);
%     DVal(curAlg+1,2)=KLDistApproxGaussMix(w1,mu1,P1,w3,mu3,P3,curAlg);
%     DVal(curAlg+1,3)=KLDistApproxGaussMix(w1,mu1,P1,w4,mu4,P4,curAlg);
% end
% RelErr=abs(bsxfun(@rdivide,bsxfun(@minus,DVal,DVal(1,:)),DVal(1,:)))
% %The rows of RelErr are the algorithms and the PDF chosen for comparison
% %against the original PDF is given by the column. The value obtained
% %through the numeric integration (algorithm 0) is nearly exact, so it is
% %taken as a basis of comparison for the relative error of the
% %approximations. It can be seen that some approximations are much worse
% %than others.
% %We can visualize the above PDFs.
% numPoints=500;
% xVals=linspace(0,3,numPoints);
% PDFVals1=GaussianMixtureD.PDF(xVals,w1,mu1,P1);
% PDFVals2=GaussianMixtureD.PDF(xVals,w2,mu2,P2);
% PDFVals3=GaussianMixtureD.PDF(xVals,w3,mu3,P3);
% PDFVals4=GaussianMixtureD.PDF(xVals,w4,mu4,P4);
% figure(1)
% clf
% hold on
% plot(xVals,PDFVals1,'-k','linewidth',4)
% plot(xVals,PDFVals2,'-r','linewidth',2)
% plot(xVals,PDFVals3,'--g','linewidth',2)
% plot(xVals,PDFVals4,'-.b','linewidth',2)
% legend('10 Elements', '5 Elements', '3 Elements','Matched Gaussian')
%
%EXAMPLE 2:
%Here is an example with bivariare Gaussian mixtures.
% w1=[0.25;0.5;0.25];
% mu1=zeros(2,2);
% mu1(:,1)=[1;-1];
% mu1(:,2)=[-1;1];
% mu1(:,3)=[0;0];
% P1=zeros(2,2,2);
% P1(:,:,1)=[4/9,  14/45;
%            14/45,4/9];
% P1(:,:,2)=[4/9, 0;
%            0, 4/9];
% P1(:,:,3)=[2/9, -1/9;
%           -1/9, 3/9];
%
% %The second distribution just throws out the first component.
% w2=w1(2:3);
% w2=w2/sum(w2);
% mu2=mu1(:,2:3);
% P2=P1(:,:,2:3);
%
% %The third distribution replaces the original distribution with a single
% %moment-matched Gaussian.
% w3=1;
% [mu3,P3]=calcMixtureMoments(mu1,w1,P1);
%
% DVal=zeros(8,2);
% for curAlg=0:7
%     DVal(curAlg+1,1)=KLDistApproxGaussMix(w1,mu1,P1,w2,mu2,P2,curAlg);
%     DVal(curAlg+1,2)=KLDistApproxGaussMix(w1,mu1,P1,w3,mu3,P3,curAlg);
% end
% RelErr=abs(bsxfun(@rdivide,bsxfun(@minus,DVal,DVal(1,:)),DVal(1,:)))
% %The rows of RelErr are the algorithms and the PDF chosen for comparison
% %against the original PDF is given by the column. The value obtained
% %through the numeric integration (algorithm 0) has a few digits of
% %accuracy, so it is taken as a basis of comparison for the relative
% %error of the approximations. It can be seen that some approximations are
% %much worse than others.
% %We can visualize the above PDFs.
% figure(1)
% clf
% subplot(3,1,1);
% numPoints=250;
% vals=linspace(-3,3,numPoints);
% [X,Y]=meshgrid(vals,vals);
% points=[X(:)';Y(:)'];
% PDFVals=GaussianMixtureD.PDF(points,w1,mu1,P1);
% PDFVals=reshape(PDFVals,numPoints,numPoints);
% surf(X,Y,PDFVals,'EdgeColor','none')
% title('Full PDF')
% %Plot the reduced PDF
% subplot(3,1,2);
% numPoints=250;
% vals=linspace(-3,3,numPoints);
% [X,Y]=meshgrid(vals,vals);
% points=[X(:)';Y(:)'];
% PDFVals=GaussianMixtureD.PDF(points,w2,mu2,P2);
% PDFVals=reshape(PDFVals,numPoints,numPoints);
% surf(X,Y,PDFVals,'EdgeColor','none')
% title('Missing 1 Component')
% %Plot the Gaussian approximation.
% subplot(3,1,3);
% numPoints=250;
% vals=linspace(-3,3,numPoints);
% [X,Y]=meshgrid(vals,vals);
% points=[X(:)';Y(:)'];
% PDFVals=GaussianMixtureD.PDF(points,w3,mu3,P3);
% PDFVals=reshape(PDFVals,numPoints,numPoints);
% surf(X,Y,PDFVals,'EdgeColor','none')
% title('Gaussian Approximation')
%
%REFERENCES:
%[1] J. L. Durrieu, J. P. Thiran, and F. Kelly, "Lower and upper bounds for
%    approximation of Kullback-Leibler divergence between Gaussian mixture
%    models," in Proceedings of the IEEE International Conference on
%    Acoustics, Speech and Signal Processing, Kyoto, Japan, 25-30 Mar.
%    2012, pp. 4833-4836.
%[2] J. Goldberger, S. Gordon, and H. Greenspan, "An efficient image
%    similarity measure based on approximations of KL-divergence between
%    two Gaussian mixtures," in Proceedings of the Ninth IEEE International
%    Conference on Computer Vision, Nice, France, 2003.
%[3] J. R. Hershey and P. A. Olden, "Approximating the Kullback Leibler
%    divergence between Gaussian mixture models," in Proceedings of the
%    IEEE International Conference on Acoustics, Speech and Signal
%    Processing, Honolulu, HI, 15-20 Apr. 2007.
%[4] T. M. Cover and J. A. Thomas, Elements of Information Theory, 2nd ed.
%    Hoboken, NJ: Wiley-Interscience, 2006.
%
%July 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<7||isempty(algorithm))
    algorithm=2;
end

d=size(mu1,1);
n1=length(w1);
n2=length(w2);

totalError=[];
exitCode=0;
switch(algorithm)
    case 0%Numeric integration over a bounded region.
        if(nargin>7&&~isempty(param8))
            if(isfield(param8,'numStdDev'))
                numStdDev=param8.numStdDev;
            end
        else
            numStdDev=4.5;
        end

        [mu1Merged,P1Merged]=calcMixtureMoments(mu1,w1,P1);
        [mu2Merged,P2Merged]=calcMixtureMoments(mu2,w2,P2);

        stdDevStep1=numStdDev*sqrt(diag(P1Merged));
        stdDevStep2=numStdDev*sqrt(diag(P2Merged));

        P1Inv=zeros(d,d,n1);
        P1InvDet=zeros(n1,1);
        P2Inv=zeros(d,d,n2);
        P2InvDet=zeros(n2,1);
        for k=1:n1
            P1Inv(:,:,k)=inv(P1(:,:,k));
            P1InvDet(k)=det(P1Inv(:,:,k));
        end
        for k=1:n2
            P2Inv(:,:,k)=inv(P2(:,:,k));
            P2InvDet(k)=det(P2Inv(:,:,k));
        end

        upperBounds=max(mu1Merged+stdDevStep1,mu2Merged+stdDevStep2);
        lowerBounds=min(mu1Merged-stdDevStep1,mu2Merged-stdDevStep2);

        f=@(x)KLArgGaussMix(x,w1,mu1,P1Inv,P1InvDet,w2,mu2,P2Inv,P2InvDet);

        if(d==1)
            %A different integration routine is used for 1D veruss
            %arbitrary dimensional integrals.
            bounds=[lowerBounds;upperBounds];

            RelTol=[];
            AbsTol=[];
            NPowMin=[];
            NPowMax=[];
            if(nargin>7&&~isempty(param8))
                if(isfield(param8,'RelTol'))
                    RelTol=param8.RelTol;
                end

                if(isfield(param8,'AbsTol'))
                    AbsTol=param8.AbsTol;
                end

                if(isfield(param8,'NPowMin'))
                    NPowMin=param8.NPowMin;
                end

                if(isfield(param8,'NPowMax'))
                    NPowMax=param8.NPowMax;
                end
            end

            [DVal,totalError,exitCode]=integral1DAdaptiveCC(f,bounds,RelTol,AbsTol,NPowMin,NPowMax);
        else
            maxSearchReg=[];
            AbsTol=[];
            RelTol=[];
            if(nargin>7&&~isempty(param8))
                if(isfield(param8,'RelTol'))
                    RelTol=param8.RelTol;
                end

                if(isfield(param8,'AbsTol'))
                    AbsTol=param8.AbsTol;
                end

                if(isfield(param8,'maxSearchReg'))
                    maxSearchReg=param8.maxSearchReg;
                end
            end

            [DVal,totalError,exitCode]=integrateUnifCub57Adaptive(f,lowerBounds,upperBounds,maxSearchReg,AbsTol,RelTol);
        end
    case 1%Monte Carlo sampling from Equation 4 of Section 2 of [3].
        if((nargin<8||isempty(param8)))
            numSamp=2e3;
        else
            numSamp=param8;
        end

        S1=zeros(d,d,n1);
        S2=zeros(d,d,n2);
        for k=1:n1
            S1(:,:,k)=chol(P1(:,:,k),'lower');
        end
        for k=1:n2
            S2(:,:,k)=chol(P2(:,:,k),'lower');
        end

        x1Samp=GaussianMixtureD.randS(numSamp,w1,mu1,S1);
        PDFVals1=GaussianMixtureD.PDFS(x1Samp,w1,mu1,S1);

        logVals=log(PDFVals1)-log(GaussianMixtureD.PDFS(x1Samp,w2,mu2,S2));
        %Get rid of points where PDFVals1==0 due to finite precision
        %errors. This probably won't occur. However, eliminating them also
        %gets rid of any unlikely points where both PDFs evaluate to 0 and
        %thus avoids returning NaN values.
        sel=(PDFVals1==0);
        logVals(sel)=0;

        DVal=sum(logVals)/numSamp;
    case 2%The cubature integration approximation from Equation 8 in
          %Section 3 of [3].
        if(nargin<8||isempty(param8))
            [xi,w]=fifthOrderCubPoints(d);
        else
            xi=param8;
        end

        S1=zeros(d,d,n1);
        S2=zeros(d,d,n2);
        for k=1:n1
            S1(:,:,k)=chol(P1(:,:,k),'lower');
        end
        for k=1:n2
            S2(:,:,k)=chol(P2(:,:,k),'lower');
        end

        w=w(:).';
        DVal=0;
        for a=1:n1
            %The component over which the expected value is taken.
            xiCur=transformCubPoints(xi,mu1(:,a),S1(:,:,a));

            logVals=log(GaussianMixtureD.PDFS(xiCur,w1,mu1,S1))-log(GaussianMixtureD.PDFS(xiCur,w2,mu2,S2));
            %Get rid of NaN values.
            %logVals(isnan(logVals))=0;

            DVal=DVal+w1(a)*sum(logVals.*w);
        end
    case 3%The variational upper bound from Section 8 of [3].
        psi=zeros(n1,n2);
        phi=zeros(n2,n1);

        %We start with psi and phi being the same and equal to what would
        %produce the convexity bound:
        for a=1:n1
            for b=1:n2
                psi(a,b)=w1(a)*w2(b);
                phi(b,a)=psi(a,b);
            end
        end

        %Get all pairwise KL-divergences.
        DKLg=zeros(n1,n2);
        for a=1:n1
            for b=1:n2
                DKLg(a,b)=KLDistGauss(mu1(:,a),P1(:,:,a),mu2(:,b),P2(:,:,b));
            end
        end
        expnDKLg=exp(-DKLg);

        numIter=10;
        for curIter=1:numIter
            %Equation 24 for phi.
            for a=1:n1
                denom=sum(psi(a,:).*expnDKLg(a,:));
                for b=1:n2
                    phi(b,a)=w1(a)*psi(a,b)*expnDKLg(a,b)/denom;
                end
            end

            %Equation 23 for psi
            for b=1:n2
                denom=sum(phi(b,:));
                for a=1:n1
                    psi(a,b)=w2(b)*phi(b,a)/denom;
                end
            end
        end

        %The first term in Equation 22
        term1=0;
        term2=0;
        for a=1:n1
            for b=1:n2
                val=phi(b,a)*log(phi(b,a)/psi(a,b));
                if(isnan(val))
                    val=0;
                end
                term1=term1+val;

                term2=term2+phi(b,a)*DKLg(a,b);
            end
        end

        DVal=term1+term2;
    case 4%The variational approximation from Section 2.3 of [1] and
          %Section 7 of [3].

        %Compute the necessary KL divergences.
        DKLf=zeros(n1,n1);
        DKLg=zeros(n1,n2);
        for a=1:n1
            DKLf(a,a)=KLDistGauss(mu1(:,a),P1(:,:,a),mu1(:,a),P1(:,:,a));
            for ap=(a+1):n1
                DKLf(a,ap)=KLDistGauss(mu1(:,a),P1(:,:,a),mu1(:,ap),P1(:,:,ap));
                DKLf(ap,a)=KLDistGauss(mu1(:,ap),P1(:,:,ap),mu1(:,a),P1(:,:,a));
            end

            for b=1:n2
                DKLg(a,b)=KLDistGauss(mu1(:,a),P1(:,:,a),mu2(:,b),P2(:,:,b));
            end
        end

        %Evaluate the sum in Equation 18 of [1], which is the same as
        %Equation 20 of [3].
        DVal=0;
        w1=w1(:).';
        w2=w2(:).';
        for a=1:n1
            DVal=DVal+w1(a)*(log(sum(w1.*exp(-DKLf(a,:))))-log(sum(w2.*exp(-DKLg(a,:)))));
        end
    case 5%The product of Gaussians approximation in [1] and Section 5 of
          %[3].

        P1Inv=zeros(d,d,n1);
        P2Inv=zeros(d,d,n2);
        for k=1:n1
            P1Inv(:,:,k)=inv(P1(:,:,k));
        end
        for k=1:n2
            P2Inv(:,:,k)=inv(P2(:,:,k));
        end

        %We have to first compute the z and t terms. The logarithms of the z
        %and t terms are given in Equation 22 in [1]. However, that
        %expression is incorrect; the corrected expression is used here

        zaap=zeros(n1,n1);
        tab=zeros(n1,n2);
        for a=1:n1
            for ap=a:n1
                diff=mu1(:,a)-mu1(:,ap);
                Sigmad=inv(P1Inv(:,:,a)+P1Inv(:,:,ap));
                const=sqrt(det(2*pi*Sigmad))/(sqrt(det(2*pi*P1(:,:,a))*det(2*pi*P1(:,:,ap))));

                B=P1Inv(:,:,a)*Sigmad*P1Inv(:,:,ap);
                zaap(a,ap)=const*exp(-(1/2)*diff'*B*diff);
                zaap(ap,a)=zaap(a,ap);
            end

            for b=1:n2
                diff=mu1(:,a)-mu2(:,b);
                Sigmad=inv(P1Inv(:,:,a)+P2Inv(:,:,b));
                const=sqrt(det(2*pi*Sigmad))/(sqrt(det(2*pi*P1(:,:,a))*det(2*pi*P2(:,:,b))));

                B=P1Inv(:,:,a)*Sigmad*P2Inv(:,:,b);
                tab(a,b)=const*exp(-(1/2)*diff'*B*diff);
            end
        end

        %Evaluate the sum in Equation 12 of [1], which is the same as
        %Equation 12 of [3].
        DVal=0;
        w1=w1(:).';
        w2=w2(:).';
        for a=1:n1
            DVal=DVal+w1(a)*(log(sum(w1.*zaap(a,:)))-log(sum(w2.*tab(a,:))));
        end
    case 6
        DProd=KLDistApproxGaussMix(w1,mu1,P1,w2,mu2,P2,5);
        DVar=KLDistApproxGaussMix(w1,mu1,P1,w2,mu2,P2,4);
        DVal=(DProd+DVar)/2;
    case 7%Goldberger's approximation from Equation 2 of [2].
        DKLg=zeros(n1,n2);
        for a=1:n1
            for b=1:n2
                DKLg(a,b)=KLDistGauss(mu1(:,a),P1(:,:,a),mu2(:,b),P2(:,:,b));
            end
        end

        w2=w2(:).';

        %Perform the minimization in Equation 1 of [2], which is also given
        %in Equation 14 of [3] and simultaneously evaluate Equation 2 of
        %[2], which is the same as Equation 15 in [3].
        DVal=0;
        logw2=log(w2);
        for a=1:n1
            minVal=min(DKLg(a,:)-logw2);
            DVal=DVal+w1(a)*(log(w1(a))+minVal);
        end
    otherwise
        error('Unknown algorithm specified.')
end
end

function val=KLArgGaussMix(x,w1,mu1,P1Inv,P1InvDet,w2,mu2,P2Inv,P2InvDet)
%%KLSAMPGAUSSMIX The KL divergence is an integral. This evaluates the
%                argument of the integral for the KL divergences between
%                two Gaussian mixtures.
%
%July 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.

PDF1=GaussianMixtureD.PDFI(x,w1,mu1,P1Inv,P1InvDet);
PDF2=GaussianMixtureD.PDFI(x,w2,mu2,P2Inv,P2InvDet);

val=PDF1.*(log(PDF1)-log(PDF2));
val(isnan(val))=0;

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 r = gedrnd(v,varargin)
% Generate random variables from a Generalized Error Distribution (GED) with V degrees of freedom
%
% USAGE:
%   R=gedrnd(V)
%   R=gedrnd(V,S1)
%   R=gedrnd(V,S1,S2,S3,...,SN)
%   R=gedrnd(V,[S1 S2 S3 ... SN])
%
% INPUTS:
%   V     - Degree of freedom parameter.  Either scalar or matrix.
%   Sx    - [OPTIONAL] Size of output.
%           R will be:
%               1 by 1 if V is scalar and no S are entered
%               size(V) if V is non-scalar and no S are entered
%               S1 by S1 if V is scalar and only S1 is entered
%               [S1 S2 ... SN] otherwise
%               If V is non-scalar and S are provided, size(V) must equal [S1 S2 ... SN]
%
% OUTPUTS:
%   R     - GED distributed random variables
%
% COMMENTS:
%   V>=1
%   A scalar GED r.v. with variance normalized to 1 has probability
%   density given by:
%       f(x,v) = [v/(lda*2^(1+1/v)*gamma(1/v))]*exp(-0.5*|x/lda|^v)
%       lda = [2^(-2/v)*gamma(1/v)/gamma(3/v)]^0.5
%   If X~GED(v) then abs(X)^v = Y~Gamma(1/v) (cf. Tadikamalla 1980).
%   GAMRND does the computational work.
%
% REFERENCES:
%   [1] Tadikamalla (1980), J.Am.Stat.Assoc. (75)
%   [2] Nelson (1991), Econometrica
%
% See also GEDPDF, GEDCDF, GEDINV, GEDLL, GAMRND

% Copyright: Ivana Komunjer
% komunjer@hss.caltech.edu
% Modifications Copyright:
% Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 9/1/2005

if nargin < 1,
    error('Requires at least one input argument.');
end

v(v<1)=NaN;

[err, errtext, sizeOut] = iscompatible(1,v,varargin{:});

if err
    error(errtext)
end

%   Return NaN if the argument V is outside its limit.
r = gamrnd(1./v,1,sizeOut);
r = r.^(1./v);
rndsgn = 2*((rand(sizeOut)>0.5)-0.5);  % generates a random sign -1 or +1
r = r.*rndsgn;

scalex = (gamma(3./v)./gamma(1./v)).^0.5; % standardizes the obtained values
r = r./scalex;


function [M, Mx, My, Mz] = convectionTerm3D(u)
% This function uses the central difference scheme to discretize a 2D
% convection term in the form \grad (u \phi) where u is a face vactor
% It also returns the x and y parts of the matrix of coefficient.
%
% SYNOPSIS:
%   [M, Mx, My, Mz] = convectionTerm3D(u)
%
% PARAMETERS:
%   u   - FaceVariable
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% extract data from the mesh structure
Nx = u.domain.dims(1);
Ny = u.domain.dims(2);
Nz = u.domain.dims(3);
G=reshape((1:(Nx+2)*(Ny+2)*(Nz+2)), Nx+2, Ny+2, Nz+2);
DXe = repmat(u.domain.cellsize.x(3:end), 1, Ny, Nz);
DXw = repmat(u.domain.cellsize.x(1:end-2), 1, Ny, Nz);
DXp = repmat(u.domain.cellsize.x(2:end-1), 1, Ny, Nz);
DYn = repmat(u.domain.cellsize.y(3:end)', Nx, 1, Nz);
DYs = repmat(u.domain.cellsize.y(1:end-2)', Nx, 1, Nz);
DYp = repmat(u.domain.cellsize.y(2:end-1)', Nx, 1, Nz);
DZ = zeros(1,1,Nz+2);
DZ(1,1,:) = u.domain.cellsize.z;
DZf=repmat(DZ(1,1,3:end), Nx, Ny, 1);
DZb=repmat(DZ(1,1,1:end-2), Nx, Ny, 1);
DZp=repmat(DZ(1,1,2:end-1), Nx, Ny, 1);

% define the vectors to stores the sparse matrix data
iix = zeros(3*(Nx+2)*(Ny+2)*(Nz+2),1);
jjx = zeros(3*(Nx+2)*(Ny+2)*(Nz+2),1);
sx = zeros(3*(Nx+2)*(Ny+2)*(Nz+2),1);
iiy = zeros(3*(Nx+2)*(Ny+2)*(Nz+2),1);
jjy = zeros(3*(Nx+2)*(Ny+2)*(Nz+2),1);
sy = zeros(3*(Nx+2)*(Ny+2)*(Nz+2),1);
iiz = zeros(3*(Nx+2)*(Ny+2)*(Nz+2),1);
jjz = zeros(3*(Nx+2)*(Ny+2)*(Nz+2),1);
sz = zeros(3*(Nx+2)*(Ny+2)*(Nz+2),1);
mnx = Nx*Ny*Nz;	mny = Nx*Ny*Nz;   mnz = Nx*Ny*Nz;

% reassign the east, west, north, and south velocity vectors for the
% code readability
ue = u.xvalue(2:Nx+1,:,:)./(DXp+DXe);
uw = u.xvalue(1:Nx,:,:)./(DXp+DXw);
vn = u.yvalue(:,2:Ny+1,:)./(DYp+DYn);
vs = u.yvalue(:,1:Ny,:)./(DYp+DYs);
wf = u.zvalue(:,:,2:Nz+1)./(DZp+DZf);
wb = u.zvalue(:,:,1:Nz)./(DZp+DZb);

% calculate the coefficients for the internal cells
AE = reshape(ue,mnx,1);
AW = reshape(-uw,mnx,1);
AN = reshape(vn,mny,1);
AS = reshape(-vs,mny,1);
AF = reshape(wf,mnz,1);
AB = reshape(-wb,mnz,1);
APx = reshape((ue.*DXe-uw.*DXw)./DXp,mnx,1);
APy = reshape((vn.*DYn-vs.*DYs)./DYp,mny,1);
APz = reshape((wf.*DZf-wb.*DZb)./DZp,mnz,1);

% build the sparse matrix based on the numbering system
rowx_index = reshape(G(2:Nx+1,2:Ny+1,2:Nz+1),mnx,1); % main diagonal x
iix(1:3*mnx) = repmat(rowx_index,3,1);
rowy_index = reshape(G(2:Nx+1,2:Ny+1,2:Nz+1),mny,1); % main diagonal y
iiy(1:3*mny) = repmat(rowy_index,3,1);
rowz_index = reshape(G(2:Nx+1,2:Ny+1,2:Nz+1),mnz,1); % main diagonal z
iiz(1:3*mnz) = repmat(rowz_index,3,1);
jjx(1:3*mnx) = [reshape(G(1:Nx,2:Ny+1,2:Nz+1),mnx,1); reshape(G(2:Nx+1,2:Ny+1,2:Nz+1),mnx,1); reshape(G(3:Nx+2,2:Ny+1,2:Nz+1),mnx,1)];
jjy(1:3*mny) = [reshape(G(2:Nx+1,1:Ny,2:Nz+1),mny,1); reshape(G(2:Nx+1,2:Ny+1,2:Nz+1),mny,1); reshape(G(2:Nx+1,3:Ny+2,2:Nz+1),mny,1)];
jjz(1:3*mnz) = [reshape(G(2:Nx+1,2:Ny+1,1:Nz),mnz,1); reshape(G(2:Nx+1,2:Ny+1,2:Nz+1),mnz,1); reshape(G(2:Nx+1,2:Ny+1,3:Nz+2),mnz,1)];
sx(1:3*mnx) = [AW; APx; AE];
sy(1:3*mny) = [AS; APy; AN];
sz(1:3*mnz) = [AB; APz; AF];

% build the sparse matrix
kx = 3*mnx;
ky = 3*mny;
kz = 3*mnz;
Mx = sparse(iix(1:kx), jjx(1:kx), sx(1:kx), (Nx+2)*(Ny+2)*(Nz+2), (Nx+2)*(Ny+2)*(Nz+2));
My = sparse(iiy(1:ky), jjy(1:ky), sy(1:ky), (Nx+2)*(Ny+2)*(Nz+2), (Nx+2)*(Ny+2)*(Nz+2));
Mz = sparse(iiz(1:kz), jjz(1:kz), sz(1:kz), (Nx+2)*(Ny+2)*(Nz+2), (Nx+2)*(Ny+2)*(Nz+2));
M = Mx + My + Mz;


% An example of nonnegative CP decomposition from partial observations
%% Generate synthetic 4-order tensor
rand('seed',0); randn('seed',0);
N1 = 30; N2 = 30; N3 = 30; N4 = 30; % tensor dimension
R = 10; % tensor rank

% randomly generate factor matrices
A1 = max(0,randn(N1,R));
A2 = max(0,randn(N2,R));
A3 = max(0,randn(N3,R));
A4 = max(0,randn(N4,R));
% generate tensor M using the above factor matrices
M = ktensor({A1,A2,A3,A4});
M = full((arrange(M)));

sr = 0.4; % percentage of samples
% randomly choose samples
known = randsample(N1*N2*N3*N4,round(sr*N1*N2*N3*N4));
data = M.data(known);

sn = 60; % signal to noise
% -- add noise --
Noise = max(0,randn(size(data)));
data = data + 10^(-sn/20)*norm(data)/norm(Noise)*Noise;
ndim = [N1,N2,N3,N4];

%% Solve problem
% exact rank works but a rough rank overestimate is also fine
esr = round(1.25*R); % overestimate of rank
opts.tol = 1e-4; opts.maxit = 1000;
t0 = tic;
[A,Out] = ncpc(data,known,ndim,esr,opts);
time = toc(t0);

%% Reporting
relerr = norm(full(A)-M)/norm(M);
fprintf('time = %4.2e, ',time);
fprintf('solution relative error = %4.2e\n\n',relerr);

figure;
semilogy(1:Out.iter, Out.hist_obj,'k-','linewidth',2);
xlabel('iteration','fontsize',12);
ylabel('objective value','fontsize',12)

figure;
semilogy(1:Out.iter, Out.hist_rel(2,:),'k-','linewidth',2);
xlabel('iteration','fontsize',12);
ylabel('relative residual','fontsize',12)

%% <http://www.caam.rice.edu/~optimization/bcu/ncpc/example_ncpc.m Download this m-file>


function [E, V] = VBA_dirichlet_moments (a)
% // VBA toolbox //////////////////////////////////////////////////////////
%
% [E, V] = VBA_dirichlet_moments (a)
% Derives the first- and second-order moments of a Dirichlet density
%
% IN:
%   - a: parameters of the Dirichlet distribution (pseudo-counts)
% OUT:
%   - E: first-order moment (expectation)
%   - V: second-order moment (variance)
%
% /////////////////////////////////////////////////////////////////////////

% Let X = (X_1, ..., X_k) ~ Dir(a)

% shorthand
a0 = sum(a);

% First-order moment E[X]
E = a./a0;

% second order moment Var[X]
V = (a .* (a0-a)) ./ (a0.^2 * (a0+1));


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

% Author : Nathanael Perraudin
% Testing: test_ngwft

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

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

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

    C=Frame'*f;

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

end


% CAMCALD Camera calibration from data points
%
% C = CAMCALD(D) is the camera matrix (3x4) determined by least squares
% from corresponding world and image-plane points.  D is a table
% of points with rows of the form [X Y Z U V] where (X,Y,Z) is the
% coordinate of a world point and [U,V] is the corresponding image
% plane coordinate.
%
% [C,E] = CAMCALD(D) as above but E is the maximum residual error after
% back substitution [pixels].
%
% Notes:
% - This method assumes no lense distortion affecting the image plane
%   coordinates.
%
% See also CentralCamera.

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB 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.
%
% MVTB 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 MVTB.  If not, see <http://www.gnu.org/licenses/>.

function [C, resid] = camcald(XYZ, uv)

    if numcols(XYZ) ~= numcols(uv)
        error('must have same number of world and image-plane points');
    end

    n = numcols(XYZ);
    if n < 6,
        error('At least 6 points required for calibration');
    end
%
% build the matrix as per Ballard and Brown p.482
%
% the row pair are one row at this point
%
    A = [ XYZ' ones(n,1) zeros(n,4) -repmat(uv(1,:)', 1,3).*XYZ' ...
          zeros(n,4) XYZ' ones(n,1) -repmat(uv(2,:)', 1,3).*XYZ'  ];
%
% reshape the matrix, so that the rows interleave
%
    A = reshape(A',11, n*2)';
    if rank(A) < 11,
        error('Rank deficient,  perhaps points are coplanar or collinear');
    end

    B = reshape( uv, 1, n*2)';

    C = A\B;    % least squares solution
    resid = max(max(abs(A * C - B)));
    if resid > 1,
        warning('Residual greater than 1 pixel');
    end
    fprintf('maxm residual %f pixels.\n', resid);
    C = reshape([C;1]',4,3)';


function [kden,N,K] = density_dir(CIJ)
%DENSITY        Density
%
%   kden = density_dir(CIJ);
%   [kden,N,K] = density_dir(CIJ);
%
%   Density is the fraction of present connections to possible connections.
%
%   Input:      CIJ,    directed (weighted/binary) connection matrix
%
%   Output:     kden,   density
%               N,      number of vertices
%               K,      number of edges
%
%   Notes:  Assumes CIJ is directed and has no self-connections.
%           Weight information is discarded.
%
%
%   Olaf Sporns, Indiana University, 2002/2007/2008

N = size(CIJ,1);
K = nnz(CIJ);
kden = K/(N^2-N);


function c = tapas_unitsq_sgm_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the unit square sigmoid observation model for binary responses
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The unit square sigmoid (ussgm) is the function
%
% f(x) = x^zeta/(x^zeta + (1-x)^zeta) = ustapas_sgm(x; zeta),
%
% where x is in the unit interval, and zeta > 0 is a parameter that determines the shape of the
% sigmoid. Since both its argument and value are always in the unit interval, its graph is
% restricted to the unit square, hence the name unit square sigmoid.
%
% In the application here, the ussgm is the probability of observing a decision y=1 (rather than
% the only alternative y=0) given the current probability mu1hat (or value) of input u=1:
%
% p(y=1|mu1hat) = ustapas_sgm(mu1hat; zeta)
%
% The parameter zeta regulates the steepness of the sigmoid such that it forms the diagonal of
% the unit square for zeta=1 and approaches a step function at 0.5 as zeta approaches infinity.
% Values of 0 < zeta < 1 lead to sigmoids with reverse concavity than usual, but they still
% represent valid observation models.
%
% Zeta can be interpreted as inverse decision noise. To have a shrinkage prior on this, choose a
% high value. It is estimated log-space since it has a natural lower bound at zero.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Is the decision based on predictions or posteriors? Comment as appropriate.
c.predorpost = 1; % Predictions
%c.predorpost = 2; % Posteriors

% Model name
c.model = 'tapas_unitsq_sgm';

% Sufficient statistics of Gaussian parameter priors

% Zeta
c.logzemu = log(48);
c.logzesa = 1;

% Gather prior settings in vectors
c.priormus = [
    c.logzemu,...
         ];

c.priorsas = [
    c.logzesa,...
         ];

% Model filehandle
c.obs_fun = @tapas_unitsq_sgm;

% Handle to function that transforms observation parameters to their native space
% from the space they are estimated in
c.transp_obs_fun = @tapas_unitsq_sgm_transp;

return;


function [v,r] = dquat2line(dq)

% DQUAT2LINE Transforms a line position dual quaternion into
%            its vector representation (orientation + position).
%
%     [V,R] = DQUAT2LINE(DQ) transforms the double quaternion
%         representation of a line position (in the euclidean space) into a
%         vector V which represents the line orientation (unitary vector)
%         and a vector R which is the coordinates of the nearest point from
%         the origin belonging to the line (it could be another r, it is
%         NOT unique).
%         DQ is either a vector of size 8 or an array of size 8*N (each
%         column represents a line position dual quaternion) where N is the
%         number of lines. V and R have the same size. They are either a
%         vector of size 3 or an array of size 3*N depending on the input
%         format.
%
% See also DQUAT2POS, DQUAT2VEL, DQUAT2LINEVEL, LINE2DQUAT

if nargout < 2 || nargout > 2
    error('DualQuaternion:dquat2line:wrongNumberOfOutputs',...
        'You specified %d outputs. It should be 2 (see documentation).',nargout);
end

sdq = size(dq);
if sdq == [1 8], dq = dq'; sdq = size(dq); end

% wrong format
if sdq(1) ~= 8
    error('DualQuaternion:dquat2line:wrongsize',...
        '%d rows in the DQ array. It should be 8.',sdq(1));
end

% check line position dual quaternion
tol = 1e-6;
[maxval,imax] = max(max(abs(dq([1 5],:))));
if maxval > tol
   warning('DualQuaternion:dquat2line:wrongFormat',...
        'At least one dual quaternion is not a line position dual quaternion (tol = %.1e).\n Indices of max values: %d \n Max value = %.2e',...
        tol,imax,maxval);
end

% extraction of the line position parameters, v and r
v = dq(2:4,:);
w = dq(6:8,:);

% check if the norm of v is unitary (it should be)
normv2 = sum(v.^2);
[maxval2,imax2] = max(normv2);
[minval2,imin2] = min(normv2);
i2 = union(imax2,imin2);
if maxval2 > 1+tol || minval2 < 1-tol
       warning('DualQuaternion:dquat2line:wrongFormatNotUnitary',...
        'At least one dual quaternion has not a unitary orientation ( is it a line position dual quaternion?) (tol = %.1e).\n Indices of max values: %d \n Min and Max value = [%.2e %.2e]',...
        tol,i2,minval2,maxval2);
end
% r is the position vector of the point belonging to the line such that
% this point is the nearest point from the origin (it is orthogonal to the
% line orientation)
normv2 = repmat(normv2,3,1);
r = cross(v,w)./normv2;


clear all
clf

projV=@(x,A)A*inv(A'*A)*A'*x;
V1=[1 0 0]';
V2=[1 1 0]';

% Alternating projections.
x=[0.5 1 1]';
subplot(121)
plot3([0 V1(1)],[0 V1(2)],[0 V1(3)],'-b','LineWidth',2);hold on
plot3([0 V2(1)],[0 V2(2)],[0 V2(3)],'-b','LineWidth',2);
plot3(x(1),x(2),x(3),'.k','MarkerSize',20);
for i=1:5
  xV1=projV(x,V1);
  h=arrow(x,xV1,'Length',5);
  set(h,'LineStyle',':','EdgeColor','r','FaceColor','r');
  text(xV1(1),xV1(2),xV1(3),['P_{V_1}(x)'])
  x=projV(xV1,V2);
  h=arrow(xV1,x,'Length',5);
  text(x(1),x(2),x(3),['P_{V_2}(P_{V_1}(x))'])
  plot3(x(1),x(2),x(3),'.k','MarkerSize',20);
end
axis([0 1 0 1 0 1]);box on;axis equal;axis tight
title('Alternating projections');

% Average Alternating Reflections/DR.
x=[0.5 1 1]';
subplot(122)
plot3([-1 V1(1)],[0 V1(2)],[0 V1(3)],'-b','LineWidth',2);hold on
plot3([-1 V2(1)],[-1 V2(2)],[0 V2(3)],'-b','LineWidth',2);
plot3(x(1),x(2),x(3),'.k','MarkerSize',20);
for i=1:5
  xV1=2*projV(x,V1)-x;
  h=arrow(x,xV1,'Length',5);
  set(h,'LineStyle',':','EdgeColor','r','FaceColor','r');
  text(xV1(1),xV1(2),xV1(3),['R_{V_1}(x)'])
  xV2=2*projV(xV1,V2)-xV1;
  h=arrow(xV1,xV2,'Length',5);
  set(h,'LineStyle',':','EdgeColor','r','FaceColor','r');
  text(xV2(1),xV2(2),xV2(3),['R_{V_2}(R_{V_1}(x))'])
  x=(xV2+x)/2;
  arrow(xV2,x,'Length',5);
  text(x(1),x(2),x(3),['(R_{V_2}(R_{V_1}(x))+x)/2'])
  plot3(x(1),x(2),x(3),'.k','MarkerSize',20);
end
axis([-1 1 -1 1 -1 1]);box on;axis equal;axis tight
title('Average Alternating Reflections/DR');


% Signal Processing Q17734
% https://dsp.stackexchange.com/questions/17734
% Estimate the Discrete Fourier Series of a Signal with Missing Samples
% References:
%   1.  aa
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.001     17/01/2021  Royi Avital     RoyiAvital@yahoo.com
%   *   First release.
% - 1.0.000     20/07/2017
%   *   First release.

%% General Parameters

subStreamNumberDefault = 79;
run('InitScript.m');

figureIdx           = 0;
figureCounterSpec   = '%04d';

generateFigures = OFF;

%% Simulation Parameters

numSamples      = 150;
numGivenSamples = 120; %<! Must be not greater than 'numSamples'
filterNumCoeff  = 15;

%% Generate Data and Samples

vFilterCoeff = rand([filterNumCoeff, 1]);
vFilterCoeff = vFilterCoeff / sum(vFilterCoeff);

% Generating the signal
vX = randn([numSamples, 1]);
vX = conv2(vX, vFilterCoeff, 'same');

% Generating the given data
vGivenIdx       = sort(randperm(numSamples, numGivenSamples));
vXX             = vX(vGivenIdx);

% Generating the DFT Matrix with the respected rows
% Pay attention to scaling in order to make it Unitary
mF = dftmtx(numSamples) / sqrt(numSamples);
mA = mF(:, vGivenIdx);

%% Estimation

% This is equivalent to padding with zeros all the samples which are not
% given.

% vY = (mA * mA') \ (mA * vXX);
vY = pinv(mA') * vXX;

%% Display Results

refDftIdx   = floor((numSamples + 1) / 2);
vDftIdx     = [0:(refDftIdx - 1)];

vXDftEstimated  = vY(1:refDftIdx);
vXDft           = mF * vX;
vXDft           = vXDft(1:refDftIdx);

hFigure     = figure('Position', figPosLarge);
hAxes       = axes();
hLineSeries = plot(vDftIdx, abs([vXDft, vXDftEstimated]));
set(hLineSeries, 'LineWidth', lineWidthNormal);
set(get(hAxes, 'Title'), 'String', ['Estimating the DFT Given Missing Samples'], ...
    'FontSize', fontSizeTitle);
set(get(hAxes, 'XLabel'), 'String', 'DFT Sample Index [n]', ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', 'Amplitude', ...
    'FontSize', fontSizeAxis);
hLegend = ClickableLegend({['The DFT of the Complete Signal'], ['Estimated DFT from Partial Signal']});

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function [X, iter] = fista(grad, Xinit, L, lambda, opts, calc_F)
% function [X, iter] = fista(grad, Xinit, L, lambda, opts, calc_F)
% * A Fast Iterative Shrinkage-Thresholding Algorithm for
% Linear Inverse Problems.
% * Solve the problem: `X = arg min_X F(X) = f(X) + lambda||X||_1` where:
%   - `X`: variable, can be a matrix.
%   - `f(X)` is a smooth convex function with continuously differentiable
%       with Lipschitz continuous gradient `L(f)` (Lipschitz constant of
%       the gradient of `f`).
% * Syntax: `[X, iter] = FISTA(calc_F, grad, Xinit, L, lambda,  opts)` where:
%   - INPUT:
%     + `grad`: a _function_ calculating gradient of `f(X)` given `X`.
%     + `Xinit`: initial guess.
%     + `L`: the Lipschitz constant of the gradient of `f(X)`.
%     + `lambda`: a regularization parameter, can be either a scalar or
%       a weighted matrix.
%     + `opts`: a _structure_ variable describing the algorithm.
%       * `opts.max_iter`: maximum iterations of the algorithm.
%           Default `300`.
%       * `opts.tol`: a tolerance, the algorithm will stop if difference
%           between two successive `X` is smaller than this value.
%           Default `1e-8`.
%       * `opts.show_progress`: showing `F(X)` after each iteration or not.
%           Default `false`.
%     + `calc_F`: optional, a _function_ calculating value of `F` at `X`
%       via `feval(calc_F, X)`.
% -------------------------------------
% Author: Tiep Vu, thv102, 4/6/2016
% (http://www.personal.psu.edu/thv102/)
% -------------------------------------
    opts = initOpts(opts);
    Linv = 1/L;
    lambdaLiv = lambda*Linv;
    x_old = Xinit;
    y_old = Xinit;
    t_old = 1;
    iter = 0;
    cost_old = 1e10;
    %% MAIN LOOP
    while  iter < opts.max_iter
        iter = iter + 1;
        x_new = shrinkage(y_old - Linv*feval(grad, y_old), lambdaLiv);
        t_new = 0.5*(1 + sqrt(1 + 4*t_old^2));
        y_new = x_new + (t_old - 1)/t_new * (x_new - x_old);
        %% check stop criteria
        e = norm1(x_new - x_old)/numel(x_new);
        if e < opts.tol
            break;
        end
        %% update
        x_old = x_new;
        t_old = t_new;
        y_old = y_new;
        %% show progress
        if opts.verbose
            if nargin ~= 0
                cost_new = feval(calc_F, x_new);
%                 if cost_new <= cost_old
%                     stt = 'YES.';
%                 else
%                     stt = 'NO, check your code.';
%                 end
%                 fprintf('iter = %3d, cost = %f, cost decreases? %s\n', ...
%                     iter, cost_new, stt);
                fprintf('iter = %3d, cost = %f\n', iter, cost_new);
                cost_old = cost_new;
            else
                if mod(iter, 5) == 0
                    fprintf('.');
                end
                if mod(iter, 10) == 0
                   fprintf('%d', iter);
                end
            end
        end
    end
    X = x_new;
end


function   v = eigenvector_centrality_und(CIJ)
%EIGENVECTOR_CENTRALITY_UND      Spectral measure of centrality
%
%   v = eigenvector_centrality_und(CIJ)
%
%   Eigenector centrality is a self-referential measure of centrality:
%   nodes have high eigenvector centrality if they connect to other nodes
%   that have high eigenvector centrality. The eigenvector centrality of
%   node i is equivalent to the ith element in the eigenvector
%   corresponding to the largest eigenvalue of the adjacency matrix.
%
%   Inputs:     CIJ,        binary/weighted undirected adjacency matrix.
%
%   Outputs:      v,        eigenvector associated with the largest
%                           eigenvalue of the adjacency matrix CIJ.
%
%   Reference: Newman, MEJ (2002). The mathematics of networks.
%
%   Contributors:
%   Xi-Nian Zuo, Chinese Academy of Sciences, 2010
%   Rick Betzel, Indiana University, 2012
%   Mika Rubinov, University of Cambridge, 2015

%   MODIFICATION HISTORY
%   2010/2012: original (XNZ, RB)
%   2015: ensure the use of leading eigenvector (MR)

n = length(CIJ);
if n < 1000
    [V,D] = eig(CIJ);
else
    [V,D] = eigs(sparse(CIJ));
end
[~,idx] = max(diag(D));
ec = abs(V(:,idx));
v = reshape(ec, length(ec), 1);


function alpha = meshDihedralAngles(vertices, edges, faces)
%MESHDIHEDRALANGLES Dihedral at edges of a polyhedal mesh
%
%   ALPHA = meshDihedralAngles(V, E, F)
%   where V, E and F represent vertices, edges and faces of a mesh,
%   computes the dihedral angle between the two adjacent faces of each edge
%   in the mesh. ALPHA is a column array with as many rows as the number of
%   edges. The i-th element of ALPHA corresponds to the i-th edge.
%
%   Note: the function assumes that the faces are correctly oriented. The
%   face vertices should be indexed counter-clockwise when considering the
%   supporting plane of the face, with the outer normal oriented outwards
%   of the mesh.
%
%   Example
%   [v, e, f] = createCube;
%   rad2deg(meshDihedralAngles(v, e, f))
%   ans =
%       90
%       90
%       90
%       90
%       90
%       90
%       90
%       90
%       90
%       90
%       90
%       90
%
%   See also
%   meshes3d, polyhedronMeanBreadth, trimeshMeanBreadth, dihedralAngle, meshEdgeFaces
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2010-10-04,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% compute normal of each face
normals = meshFaceNormals(vertices, faces);

% indices of faces adjacent to each edge
edgeFaces = meshEdgeFaces(vertices, edges, faces);

% allocate memory for resulting angles
Ne = size(edges, 1);
alpha = zeros(Ne, 1);

% iterate over edges
for i = 1:Ne
    % indices of adjacent faces
    indFace1 = edgeFaces(i, 1);
    indFace2 = edgeFaces(i, 2);

    % normal vector of adjacent faces
    normal1 = normals(indFace1, :);
    normal2 = normals(indFace2, :);

    % compute dihedral angle of two vectors
    alpha(i) = vectorAngle3d(normal1, normal2);
end


function varargout = gradientSpect(f, dn, dim, deriv_order)
%GRADIENTSPECT Calculate the gradient using a Fourier spectral method.
%
% DESCRIPTION:
%       gradientSpect calculates the gradient of an n-dimensional input
%       matrix using the Fourier collocation spectral method. The gradient
%       for singleton dimensions is returned as 0.
%
%       Examples:
%       1. 1D Sinusoid:
%
%           % compute gradient of a 2 period sinusoid
%           x = pi/20:pi/20:4*pi;
%           y = sin(x);
%           dydx = gradientSpect(y, pi/20);
%
%           % plot gradient and error compared to analytical solution
%           subplot(2, 1, 1), plot(x, cos(x), 'k-', x, dydx, 'bx');
%           axis tight;
%           title('dy/dx');
%           subplot(2, 1, 2), plot(x, cos(x) - dydx, 'k-');
%           axis tight;
%           title('Relative Error');
%
%       2. Modified 2D Mathworks gradient example (x and y reversed):
%
%           [x, y] = meshgrid(-2:.2:2, -2:.2:2);
%           z = x .* exp(-x.^2 - y.^2);
%           [px,py] = gradientSpect(z, [.2, .2]);
%           contour(z), hold on, quiver(py, px), hold off;
%
% USAGE:
%       fx = gradientSpect(f, dx)
%       fx = gradientSpect(f, dx, [], deriv_order)
%       fn = gradientSpect(f, dn, dim)
%       fn = gradientSpect(f, dn, dim, deriv_order)
%       [fx, fy] = gradientSpect(f, dn)
%       [fx, fy] = gradientSpect(f, dn, [], deriv_order)
%       [fx, fy, fz, ...] = gradientSpect(f, dn)
%       [fx, fy, fz, ...] = gradientSpect(f, dn, [], deriv_order)
%
% INPUTS:
%       f           - matrix or vector to find the gradient of
%       dn          - array of values for the grid point spacing in each
%                     dimension. If a value for dim is given, dn is the
%                     spacing in dimension dim.
%
% OPTIONAL INPUTS:
%       dim         - optional input to specify a single dimension over
%                     which to compute the gradient for n-dimension input
%                     functions
%       deriv_order - order of the derivative to compute, e.g., use 1 to
%                     compute df/dx, 2 to compute df^2/dx^2, etc.
%                     (default = 1)
%
%
% OUTPUTS:
%       fx, fy, ... - gradient in the each dimension, where x corresponds
%                     to dim = 1, y corresponds to dim = 2 etc
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 25th January 2012
%       last update - 27th August 2012
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox
%
% See also gradient, gradientFD

% This file is part of k-Wave. k-Wave 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.
%
% k-Wave is distributed in the hope that it will be useful, but WITHOUT ANY
% WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% check for user input for deriv_order
if nargin < 4  || isempty(deriv_order)
    deriv_order = 1;
elseif ~((deriv_order > 0) && (deriv_order == round(deriv_order)))
    error('input for deriv_order must be an integer > 0');
end

% check for user input for dim
if (nargin < 3) || isempty(dim)
    dim = 0;
end

% get size of the input function
sz = size(f);

% check if input is 1D or user defined input dimension is given
if dim || (nargout == 1 && (max(sz) == prod(sz)))

    % check if a single dn value is given, if not, extract the required
    % value
    if numel(dn) ~= 1
        dn = dn(dim);
    end

    % get the grid size along the specified dimension, or the longest
    % dimension if 1D
    if nargout == 1 && (max(sz) == prod(sz))
        [Nx, dim] = max(sz);
    else
        Nx = sz(dim);
    end

    % get the wavenumbers
    kx = getWavenumbers(Nx, dn, dim);

    % calculate derivative and assign output
    varargout{1} = ifft( bsxfun(@times, (1i*kx).^deriv_order, fft(f, [], dim)) , [], dim, 'symmetric');

else

    % check for the correct number of dn values
    if length(dn) ~= length(sz)
        error([num2str(length(sz)) ' values for dn must be specified for a ' num2str(length(sz)) '-dimensional input matrix']);
    end

    % check for the correct number of outputs
    if nargout ~= length(sz)
        error(['Incorrect number of output arguments for ' num2str(length(sz)) '-dimensional input matrix']);
    end

    % calculate the gradient for each non-singleton dimension
    for dim = 1:ndims(f)
        if sz(dim) > 1

            % get the wavenumbers
            kx = getWavenumbers(sz(dim), dn(dim), dim);

            % calculate derivative and assign output
            varargout{dim} = ifft( bsxfun(@times, (1i*kx).^deriv_order, fft(f, [], dim)) , [], dim, 'symmetric');

        else
            % assign the derivate for singleton dimensions to be 0
            varargout{dim} = 0;
        end
    end
end

function kx = getWavenumbers(Nx, dx, dim)
% subfuction to compute the wavenumber vector

% calculate the wavenumbers
if rem(Nx, 2) == 0
    % grid dimension has an even number of points
    nx = ((-Nx/2:Nx/2-1)/Nx).';
else
    % grid dimension has an odd number of points
    nx = ((-(Nx-1)/2:(Nx-1)/2)/Nx).';
end
kx = ifftshift((2*pi/dx).*nx);

% permute to be in the correction direction for use with bsxfun
if dim == 1
    kx = reshape(kx, Nx, 1);
else
    kx = reshape(kx, [ones(1, dim - 1), Nx]);
end


function y = huber_pos( x, M, t )

%HUBER_POS   Monotonic huber penalty function.
%   For a vector X, HUBER_POS(X) computes the monotonic Huber-style function
%
%                      0     if 0>=X
%       HUBER_POS(X) = X^2   if 0<=X<=1
%                      2*X-1 if    X>=1
%
%   HUBER_POS(X,M) is the monotonic Huber-style penalty function of
%   halfwidth M, M.^2.*HUBER_POS(X./M). M must be real and positive.
%
%   HUBER_POS(X,M,T) computes the monotonic Huber-style penalty function
%   with halfwidth M and concomitant scale T:
%
%       HUBER_POS(X,M,T) = T.*HUBER_POS(X./T,M) if T > 0
%                          +Inf                 if T <= 0
%
%   See the help file for HUBER for information about this usage.
%
%   For matrices and N-D arrays, the penalty function is applied to each
%   element of X independently. M and T must be compatible with X in the same
%   sense as .*: one must be a scalar, or they must have identical size.
%
%   Disciplined convex programming information:
%       HUBER_POS is jointly convex in X and T. It is nondecreasing in X and
%       nonincreasing in T. Therefore, when used in CVX specifications, X
%       must be convex and T must be concave (or affine). Both must be real.

%
% Check arguments
%

narginchk(1,3);
if ~isreal( x ),
    error( 'First argument must be real.' );
end
if nargin < 2,
    M = 1;
elseif ~isreal( M ) || any( M( : ) <= 0 ),
    error( 'Second argument must be real and positive.' );
end
if nargin < 3,
    t = 1;
elseif ~isreal( t ),
    error( 'Third argument must be real.' );
end
sz = cvx_size_check( x, M, t );
if isempty( sz ),
    error( 'Sizes are incompatible.' );
end

%
% Compute result
%

y = max( x, 0 );
z = min( y, M );
y = t .* z .* ( 2 * y - z );
q = t <= 0;
if nnz( q ),
    if length( t ) == 1,
        y = Inf * ones( sy );
    else
        y( q ) = Inf;
    end
end

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function a = pascal3_inverse ( n, alpha )

%*****************************************************************************80
%
%% PASCAL3_INVERSE returns the inverse of the PASCAL3 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real ALPHA, the parameter.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n

      if ( i == 1 )

        if ( j == 1 )
          a(i,j) = 1.0;
        else
          a(i,j) = 0.0;
        end

      elseif ( j == 1 )

        a(i,j) = - alpha * a(i-1,j);

      else

        a(i,j) = a(i-1,j-1) - alpha * a(i-1,j);

      end

    end
  end

  return
end


function op = prox_l1_deadzone( q, epsilon )

%PROX_L1_DEADZONE    L1 norm with deadzone [-eps,eps]
%    OP = PROX_L1_DEADZONE( q, eps ) implements the nonsmooth function
%        OP(X) = q*sum_i max(0,x-eps)+max(0,-x-eps) = q*sum_i
%        max(0,|x|-eps)
%    Q is optional; if omitted, Q=1 is assumed. But if Q is supplied,
%    then it must be a positive real scalar (or must be same size as X).
%

% New March 2 2016

if nargin == 0,
	q = 1;
elseif ~isnumeric( q ) || ~isreal( q ) ||  any( q(:) < 0 ) || all(q(:)==0) %|| numel( q ) ~= 1
    if q==0
        op = prox_0;
        warning('TFOCS:zeroQ','q=0 so returning the proximal operator for the zero function');
        return;
    else
        error( 'Argument must be positive.' );
    end
end

% This is Matlab and Octave compatible code
op = tfocs_prox( @(x)f(q, epsilon,x), @(x,t)prox_f(q, epsilon,x,t) , 'vector' );
end

% These are now subroutines, that are NOT in the same scope
function v = f(qq,epsilon, x)
    v = sum( qq(:).*max(0,abs(x(:))-epsilon), 1 );
end

function x = prox_f(qq,epsilon, x,t)
% p = x if |x|<eps,
% p = sign(x)*eps if eps < |x| < tq
% p = sign(x)*( |x| - (eps+tq) ) if |x| > tq
% i.e., if |x|>eps,  p = sign(x).*max( |x|-tq, eps )
    tq = t .* qq;
    x  = sign(x).*( abs(x).*( abs(x)<epsilon ) + max(abs(x)-tq, epsilon).*( abs(x)>= epsilon) );
end

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


function [ n_data, c, x, fx ] = student_cdf_values ( n_data )

%*****************************************************************************80
%
%% STUDENT_CDF_VALUES returns some values of the Student CDF.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      Needs["Statistics`ContinuousDistributions`"]
%      dist = StudentTDistribution [ df ]
%      CDF [ dist, x ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 November 2005
%
%  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 C, is usually called the number of
%    degrees of freedom of the distribution.  C is typically an
%    integer, but that is not essential.  It is required that
%    C be strictly positive.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 13;

  c_vec = [ ...
    1.0, 2.0, 3.0, 4.0, ...
    5.0, 2.0, 5.0, 2.0, ...
    5.0, 2.0, 3.0, 4.0, ...
    5.0 ];

  fx_vec = [ ...
     0.6000231200328521, ...
     0.6001080279134390, ...
     0.6001150934648930, ...
     0.6000995134721354, ...
     0.5999341989834830, ...
     0.7498859393137811, ...
     0.7500879487671045, ...
     0.9500004222186464, ...
     0.9499969138365968, ...
     0.9900012348724744, ...
     0.9900017619355059, ...
     0.9900004567580596, ...
     0.9900007637471291 ];

  x_vec = [ ...
     0.325, ...
     0.289, ...
     0.277, ...
     0.271, ...
     0.267, ...
     0.816, ...
     0.727, ...
     2.920, ...
     2.015, ...
     6.965, ...
     4.541, ...
     3.747, ...
     3.365 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end