%DEMEV1	Demonstrate Bayesian regression for the MLP.
%
%	Description
%	The problem consists an input variable X which sampled from a
%	Gaussian distribution, and a target variable T generated by computing
%	SIN(2*PI*X) and adding Gaussian noise. A 2-layer network with linear
%	outputs is trained by minimizing a sum-of-squares error function with
%	isotropic Gaussian regularizer, using the scaled conjugate gradient
%	optimizer. The hyperparameters ALPHA and BETA are re-estimated using
%	the function EVIDENCE. A graph  is plotted of the original function,
%	the training data, the trained network function, and the error bars.
%
%	See also
%	EVIDENCE, MLP, SCG, DEMARD, DEMMLP1
%

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

clc;
disp('This demonstration illustrates the application of Bayesian')
disp('re-estimation to determine the hyperparameters in a simple regression')
disp('problem. It is based on a local quadratic approximation to a mode of')
disp('the posterior distribution and the evidence maximization framework of')
disp('MacKay.')
disp(' ')
disp('First, we generate a synthetic data set consisting of a single input')
disp('variable x sampled from a Gaussian distribution, and a target variable')
disp('t obtained by evaluating sin(2*pi*x) and adding Gaussian noise.')
disp(' ')
disp('Press any key to see a plot of the data together with the sine function.')
pause;

% Generate the matrix of inputs x and targets t.

ndata = 16;			% Number of data points.
noise = 0.1;			% Standard deviation of noise distribution.
randn('state', 0);
x = 0.25 + 0.07*randn(ndata, 1);
t = sin(2*pi*x) + noise*randn(size(x));

% Plot the data and the original sine function.
h = figure;
nplot = 200;
plotvals = linspace(0, 1, nplot)';
plot(x, t, 'ok')
xlabel('Input')
ylabel('Target')
hold on
axis([0 1 -1.5 1.5])
fplot('sin(2*pi*x)', [0 1], '-g')
legend('data', 'function');

disp(' ')
disp('Press any key to continue')
pause; clc;

disp('Next we create a two-layer MLP network having 3 hidden units and one')
disp('linear output. The model assumes Gaussian target noise governed by an')
disp('inverse variance hyperparmeter beta, and uses a simple Gaussian prior')
disp('distribution governed by an inverse variance hyperparameter alpha.')
disp(' ');
disp('The network weights and the hyperparameters are initialised and then')
disp('the weights are optimized with the scaled conjugate gradient')
disp('algorithm using the SCG function, with the hyperparameters kept')
disp('fixed. After a maximum of 500 iterations, the hyperparameters are')
disp('re-estimated using the EVIDENCE function. The process of optimizing')
disp('the weights with fixed hyperparameters and then re-estimating the')
disp('hyperparameters is repeated for a total of 3 cycles.')
disp(' ')
disp('Press any key to train the network and determine the hyperparameters.')
pause;

% Set up network parameters.
nin = 1;		% Number of inputs.
nhidden = 3;		% Number of hidden units.
nout = 1;		% Number of outputs.
alpha = 0.01;		% Initial prior hyperparameter.
beta_init = 50.0;	% Initial noise hyperparameter.

% Create and initialize network weight vector.
net = mlp(nin, nhidden, nout, 'linear', alpha, beta_init);

% Set up vector of options for the optimiser.
nouter = 3;			% Number of outer loops.
ninner = 1;			% Number of innter loops.
options = zeros(1,18);		% Default options vector.
options(1) = 1;			% This provides display of error values.
options(2) = 1.0e-7;		% Absolute precision for weights.
options(3) = 1.0e-7;		% Precision for objective function.
options(14) = 500;		% Number of training cycles in inner loop.

% Train using scaled conjugate gradients, re-estimating alpha and beta.
for k = 1:nouter
  net = netopt(net, options, x, t, 'scg');
  [net, gamma] = evidence(net, x, t, ninner);
  fprintf(1, '\nRe-estimation cycle %d:\n', k);
  fprintf(1, '  alpha =  %8.5f\n', net.alpha);
  fprintf(1, '  beta  =  %8.5f\n', net.beta);
  fprintf(1, '  gamma =  %8.5f\n\n', gamma);
  disp(' ')
  disp('Press any key to continue.')
  pause;
end

fprintf(1, 'true beta: %f\n', 1/(noise*noise));

disp(' ')
disp('Network training and hyperparameter re-estimation are now complete.')
disp('Compare the final value for the hyperparameter beta with the true')
disp('value.')
disp(' ')
disp('Notice that the final error value is close to the number of data')
disp(['points (', num2str(ndata),') divided by two.'])
disp(' ')
disp('Press any key to continue.')
pause; clc;
disp('We can now plot the function represented by the trained network. This')
disp('corresponds to the mean of the predictive distribution. We can also')
disp('plot ''error bars'' representing one standard deviation of the')
disp('predictive distribution around the mean.')
disp(' ')
disp('Press any key to add the network function and error bars to the plot.')
pause;

% Evaluate error bars.
[y, sig2] = netevfwd(mlppak(net), net, x, t, plotvals);
sig = sqrt(sig2);

% Plot the data, the original function, and the trained network function.
[y, z] = mlpfwd(net, plotvals);
figure(h); hold on;
plot(plotvals, y, '-r')
xlabel('Input')
ylabel('Target')
plot(plotvals, y + sig, '-b');
plot(plotvals, y - sig, '-b');
legend('data', 'function', 'network', 'error bars');

disp(' ')
disp('Notice how the confidence interval spanned by the ''error bars'' is')
disp('smaller in the region of input space where the data density is high,')
disp('and becomes larger in regions away from the data.')
disp(' ')
disp('Press any key to end.')
pause; clc; close(h);
%clear all


% function [x1,x2,P1,P2] = make_synthetic_data(N,range_model,range_image,verbose)
% makes a synthetic 3D model and makes two projective images of the model
% using arbitrary projective cameras Pi=[M|t]. The second cameras are normalized
% to Pi = Pi / Pi_{34}.
% inputs:
%               N               1x1     (optional) the maximum number of correspondences to generate.
%               range_model     2x1     (optional) defines a bounding box (starting from origin) containing the model
%               range_image     2x1     (optional) the size of the output image(scaling on correspondences)
%               verbose         1x1     (optional) plots the generated model and images
% outputs:
%               x1              3x1     homogeneous coordinates of the correspondences in image 1
%               x2              3x1     homogeneous coordinates of the correspondences in image 2
%               P1              3x4     the camera that generates x1(up to an affine transformation)
%               P2              3x4     the camera that generates x2(up to an affine transformation)
%
% Author: Omid Aghazadeh, KTH(Royal Institute of Technology), 2010/05/09
function [x1,x2,P1,P2] = make_synthetic_data(N,range_model,range_image,verbose)
if nargin < 1, N = 100; end
if nargin < 2, range_model = [10;10;10]; end
if nargin < 3, range_image = [640;480]; end
if nargin < 4, verbose = 1; end
X = rand(3,N); X = (X - repmat(min(X,[],2),1,N)) .* repmat(range_model ./ (max(X,[],2) - min(X,[],2)),1,N);
X = [X; ones(1,N)];
P1 = rand(3,4); P1(end) = 1;
P2 = rand(3,4); P2(end) = 1; %avoid cameras at infinity
x1 = P1*X; x1 = x1./repmat(x1(3,:),3,1); x2 = P2*X; x2 = x2./repmat(x2(3,:),3,1);
ixinv = sum(isinf(x1) | isnan(x1) | isinf(x2) | isnan(x2) | abs(x1) > 1e3 | abs(x2) > 1e3,1 )>0 ;
x1 = x1(:,~ixinv); x2 = x2(:,~ixinv); N = size(x1,2);
x1 = (x1-repmat([min(x1(1:2,:),[],2);0],1,N)).* repmat([range_image./(max(x1(1:2,:),[],2) - min(x1(1:2,:),[],2));1],1,N);

x2 = (x2-repmat([min(x2(1:2,:),[],2);0],1,N)).* repmat([range_image./(max(x2(1:2,:),[],2) - min(x2(1:2,:),[],2));1],1,N);
if verbose
    figure(1); subplot(1,3,1); plot3(X(1,:),X(2,:),X(3,:),'b.'); title('generated 3D model');
    subplot(1,3,2); plot(x1(1,:),x1(2,:),'rx'); title('generated first image');
    subplot(1,3,3); plot(x2(1,:),x2(2,:),'gx'); title('generated second image');
end
end


function test03 ( dim_num, level_max )

%*****************************************************************************80
%
%% TEST03 call SPARSE_GRID_GL to create a Gauss-Legendre sparse grid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 September 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, the level.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST03:\n' );
  fprintf ( 1, '  SPARSE_GRID_GL makes a sparse Gauss-Legendre grid.\n' );

  level_min = max ( 0, level_max + 1 - dim_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  LEVEL_MIN = %d\n', level_min );
  fprintf ( 1, '  LEVEL_MAX = %d\n', level_max );
  fprintf ( 1, '  Spatial dimension DIM_NUM = %d\n', dim_num );
%
%  Determine the number of points.
%
  point_num = sparse_grid_gl_size ( dim_num, level_max );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of unique points in the grid = %d\n', point_num );
%
%  Compute the weights and points.
%
  [ grid_weight, grid_point ] = sparse_grid_gl ( dim_num, level_max, point_num );
%
%  Print them out.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Grid weights:\n' );
  fprintf ( 1, '\n' );
  for point = 1 : point_num
    fprintf ( 1, '  %4d  %10f\n', point, grid_weight(point) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Grid points:\n' );
  fprintf ( 1, '\n' );
  for point = 1 : point_num
    fprintf ( 1, '  %4d', point );
    for dim = 1 : dim_num
      fprintf ( 1, '%10f', grid_point(dim,point) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function res = parallelEdge(edge, dist)
%PARALLELEDGE Edge parallel to another edge.
%
%   EDGE2 = parallelEdge(EDGE, DIST)
%   Computes the edge parallel to the input edge EDGE and located at the
%   given signed distance DIST.
%
%   Example
%     obox = [30 40 80 40 30];
%     figure; hold on; axis equal;
%     drawOrientedBox(obox, 'LineWidth', 2);
%     edge1 = centeredEdgeToEdge(obox([1 2 3 5]));
%     edge2 = centeredEdgeToEdge(obox([1 2 4 5])+[0 0 0 90]);
%     drawEdge(edge1, 'LineWidth', 2, 'color', 'g');
%     drawEdge(edge2, 'LineWidth', 2, 'color', 'g');
%     drawEdge(parallelEdge(edge1, -30), 'LineWidth', 2, 'color', 'k');
%     drawEdge(parallelEdge(edge2, -50), 'LineWidth', 2, 'color', 'k');
%
%   See also
%     edges2d, parallelLine, drawEdge, centeredEdgeToEdge, edgeToLine

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2012-07-31, using Matlab 7.9.0.529 (R2009b)
% Copyright 2012-2022 INRA - Cepia Software Platform

% compute the line parallel to the supporting line of edge
line = parallelLine(edgeToLine(edge), dist);

% result edge is given by line positions 0 and 1.
res = [line(:, 1:2) line(:, 1:2)+line(:, 3:4)];


function sparse_test02 ( )

%*****************************************************************************80
%
%% SPARSE_TEST02 demonstrates a simple use of the SPARSE matrix facility.
%
%  Discussion:
%
%    This is a nice but very simple example.
%
%    Note in this example that we define three sparse matrices,
%    SUP, DIAG, and SUB, and then define A as the sum of those matrices.
%
%    However, again, each time we define a sparse matrix, we are assuming
%    we have all the information available at one time.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 August 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    George Lindfield, John Penny,
%    Numerical Methods Using MATLAB,
%    Prentice Hall, 1999
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_TEST02:\n' );
  fprintf ( 1, '  Demonstrate the use of MATLAB''s SPARSE facility\n' );
  fprintf ( 1, '  to define a sparse matrix, and solve an associated\n' );
  fprintf ( 1, '  linear system.\n' );

  n = 100;
%
%  We set up the three diagonals of the -1, 2, -1 matrix.
%
  sup  = sparse ( 1:n-1, 2:n,   -1.0, n, n );
  diag = sparse ( 1:n,   1:n,    2.0, n, n );
  sub  = sparse ( 2:n,   1:n-1, -1.0, n, n );
%
%  A is the matrix whose superdiagonal, diagonal, and subdiagonal
%  have been set above.  Because SUP, DIAG and SUB are sparse,
%  A will "inherit" this property.
%
  A = sup + diag + sub;
%
%  Set up a right hand side b that defines a linear system whose
%  solution is [ 1, 2, 3, ..., n ].
%
  b(1:n-1,1) = 0.0;
  b(n,1) = n + 1;
%
%  Use MATLAB's backslash command to solve the linear system.
%  Because MATLAB "knows" that A is a sparse matrix, it will
%  automatically do the efficient thing.
%
  x = A \ b;
%
%  Print the solution.
%
  x(1:n)

  return
end


% OZDEMO
% This program creates the Ornstein-Zernike example in Chapter 3.
% [H,C]=OZDEMO returns the solution on a grid with a mesh
% spacing of 1/256.
%
function [h,c]=ozdemo
global L U rho
n=257;
epsilon=.1; sigma=2.0; rho=.2; beta=10; L=9;
dx=L/(n-1); r=0:dx:L; r=r';
%
% Compute the potential and store it in a global variable.
%
U=elj(r,sigma,epsilon,beta);
%
tol=[1.d-8,1.d-8];
x=zeros(2*n,1);
parms=[40,80,-.1];
[sol, it_hist, ierr] = nsoli(x,'oz',tol);
%
% Unpack h and c.
%
h=sol(1:n); c=sol(n+1:2*n);
%
% Plot the solution.
%
figure(1)
subplot(1,2,1)
plot(r,h,'-');
ylabel('h','Rotation',1);
xlabel('r');
subplot(1,2,2)
plot(r,c,'-');
ylabel('c','Rotation',1);
xlabel('r');
%
% Do a second solve with constant forcing terms.
% Are you getting the same results each time?
%
fb=it_hist(1,1);
parms=[40,80,-.1]; x=zeros(2*n,1);
[sola, it_hist1, ierr] = nsoli(x,'oz',tol,parms);
norm(sola-sol)
%
% plot residual vs iteration counter
%
figure(2)
ni=length(it_hist(:,1));
n1=length(it_hist1(:,1));
semilogy(0:ni-1,it_hist(:,1)/fb,'-',...
0:n1-1,it_hist1(:,1)/fb,'--');
legend('default','.1');
xlabel('Nonlinear iterations');
ylabel('Relative residual');
%
% plot residual vs function counter
%
figure(3)
semilogy(it_hist(:,2),it_hist(:,1)/fb,'-',...
it_hist1(:,2),it_hist1(:,1)/fb,'--');
xlabel('Function evaluations');
ylabel('Relative residual');
legend('default','.1');

%
function u=elj(r,sigma,epsilon,beta)
n2=length(r);
ra=r(2:n2);
r12=(sigma./ra).^12; r6=(sigma./ra).^6;
ua=exp(-4*beta*epsilon*(r12-r6));
u=[0,ua']';


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

%*****************************************************************************80
%
%% CN_LEG_03_XIU implements the Xiu precision 3 rule for region CN_LEG.
%
%  Discussion:
%
%    The rule has order
%
%      O = 2 * N.
%
%    The rule has precision P = 3.
%
%    CN_LEG is the cube [-1,+1]^N with the Legendre weight function
%
%      w(x) = 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Dongbin Xiu,
%    Numerical integration formulas of degree two,
%    Applied Numerical Mathematics,
%    Volume 58, 2008, pages 1515-1520.
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%
%    Input, integer O, the order.
%
%    Output, real X(N,O), the abscissas.
%
%    Output, real W(O), the weights.
%
  o = 2 * n;

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

  expon = 0;
  volume = c1_leg_monomial_integral ( expon );
  volume = volume ^ n;

  for j = 1 : o

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

    if ( i < n )
      i = i + 1;
      x(i,j) = sqrt ( 2.0 ) * r8_mop ( j ) / sqrt ( 3.0 );
      if ( n == 1 )
        x(i,j) = x(i,j) / sqrt ( 2.0 );
      end
    end

  end

  w(1:o) = volume / o;

  return
end


% THIS PROGRAM IS FOR IMPLEMENTATION OF DISCRETE TIME PROCESS UNSCENTED KALMAN FILTER
% FOR GAUSSIAN AND LINEAR STOCHASTIC DIFFERENCE EQUATION.
% (19 JULY 2005).
% UNSCENTED KALMAN FILTER (UKF) AT ITS BEST.
%(Under Nonlinear conditions,UNSCENTED KALMAN FILTER
% performs to a much better extent as compared to EXTENDED KALMAN FILTER).

clc;  close all;  clear all;

format long g;
Xo = [1; 0; 0; 0; 0; 0; 0; 0; 0; 0];
nx = length(Xo);
beta = 2;
elfa = 1*(10^-1);
lambda = (((elfa^2)*(nx)) - nx);
mn1 = mean(Xo);                                   %FIRST STEP
CV1 = (Xo-mn1)*(Xo-mn1)';
%CV1 = (Xo)*(Xo)';
PO = CV1;
FX = size(CV1);
VTt = [1 0 0 0 0 0 0 0 0 0]';
ADP1 = randn(1,200);
mn2 = mean(VTt);
NTt = [1 0 0 0 0 0 0 0 0 0]';
ADP2 = randn(1,200);
mn3 = mean(NTt);
Qo = (VTt-mn2)*(VTt-mn2)';
Ro = (NTt-mn3)*(NTt-mn3)';
%Qo = (VTt)*(VTt)';
%Ro = (NTt)*(NTt)';
FX = zeros(10,10);
PAO = [PO FX FX; FX Qo FX;FX FX Ro];
Xao = [mn1 0 0]';
lam = sqrt((elfa^2)*(nx));

Wmo = lambda/(nx+lambda);
Wmi = 1/(2*(nx+lambda));
Wmin = 1/(2*(nx+lambda));

Wco =  (lambda/(nx+lambda)) + (1-(elfa^2)+ beta);
Wci = Wmi;
Wcin = Wmin;

%CALCULATION OF SIGMA POINTS
[Sg11,Sg12,Sg13,Sg21,Sg22,Sg23,Sg31,Sg32,Sg33] = sigmacal(mn1,CV1,mn2,Qo,mn3,Ro,lam);

%TIME UPDATE EQUATIONS.
[Xinew1,Xinew2,Xinew3,Yinew1,Yinew2,Yinew3,Xbark,Ybark,Pbark] = TMUPDT(Wmo,Wmi,Wmin,Wco,Wci,Wcin,Sg11,Sg12,Sg13,Sg21,Sg22,Sg23,Sg31,Sg32,Sg33);

%MEASUREMENT UPDATE EQUATIONS.
[KGain,XNEW,PT1,PT2,PT3] = MSMTUPDT(Xinew1,Xinew2,Xinew3,Yinew1,Yinew2,Yinew3,Xbark,Ybark,Pbark,Wco,Wci,Wcin,Xo,VTt);

for ii = 1:1:100

    VTt = [ADP1(ii+1) 0 0 0 0 0 0 0 0 0]';
    NTt = [ADP2(ii+1) 0 0 0 0 0 0 0 0 0]';

    %CV1 = (XNEW)*(XNEW)';
    mn1 = mean(XNEW);
    mn2 = mean(VTt);
    mn3 = mean(NTt);
    % Qo = (VTt)*(VTt)';
    % Ro = (NTt)*(NTt)';

    CV1 = (XNEW-mn1)*(XNEW-mn1)';
    Qo = (VTt-mn2)*(VTt-mn2)';
    Ro = (NTt-mn3)*(NTt-mn3)';

    lam = sqrt((elfa^2)*(nx));

    [Sg11,Sg12,Sg13,Sg21,Sg22,Sg23,Sg31,Sg32,Sg33] = sigmacal(mn1,CV1,mn2,Qo,mn3,Ro,lam);

    [Xinew1,Xinew2,Xinew3,Yinew1,Yinew2,Yinew3,Xbark,Ybark,Pbark] = TMUPDT(Wmo,Wmi,Wmin,Wco,Wci,Wcin,Sg11,Sg12,Sg13,Sg21,Sg22,Sg23,Sg31,Sg32,Sg33);

    [KGain,XNEW,PT1,PT2,PT3,YNEW] = MSMTUPDT(Xinew1,Xinew2,Xinew3,Yinew1,Yinew2,Yinew3,Xbark,Ybark,Pbark,Wco,Wci,Wcin,XNEW,VTt);

    INP1(ii) = XNEW(1,1);
    INP2(ii) = YNEW(1,1);

end

T = 1:1:100;
figure(1);     subplot(211);    plot(real(INP1));   title('ORIGINAL SIGNAL');
subplot(212);  plot(real(INP2));   title('ESTIMATED SIGNAL (UNDER Nonlinear MODEL)');

figure(2);    plot(T,abs(INP1),T,abs(INP2));  title('Combined plot'); legend('original','estimated');


function [Pw] = RFT_Pval(u,k,c,fwhm,L,type,dof)
% computes Pval according to Friston et al. (1996), Eq. 1-4.
% [Pw] = RFT_Pval(u,k,c,fwhm,L)
% This function ca be called to derive the p-value at different levels of
% inference, i.e.:
% - P_peak = RFT_Pval(u,0,1,fwhm,L)
% - P_cluster = RFT_Pval(U,k,1,fwhm,L), where U was the set-inducing
% threshold, and k is the observed spatial extent of the cluster.
% - P_set = RFT_Pval(U,K,c,fwhm,L), where U and K were the set-inducing
% thresholds, and c was the number of observed upcrossing clusters
% IN:
%   - u: RF value
%   - k: cluster's spatial extent
%   - c: number of upcrossing clusters
%   - fwhm: estimated FWHM
%   - L: search volume
%   - type: type of RF. Can be set to 'norm' (normal, default), 't'
%   (Student) or 'F' (Fisher).
%   - dof: degrees of freedom (only relevant for 't' or 'F' fields).
% OUT:
%   - Pw: the ensuing p-value

EC = RFT_expectedTopo(u,L,fwhm,1,type,dof);
switch type
    case 'norm'
        P0 = 1-VBA_spm_Ncdf(u,0,1);
    case 't'
        P0 = 1-VBA_spm_Tcdf(u,dof);
    case 'F'
        P0 = 1-VBA_spm_Fcdf(u,dof(1),dof(2));
end
beta = (gamma(3/2).*EC./(L.*P0)).^2;
Pnk = exp(-beta.*k.^2);
if k == 0, Pnk = 1; end; % solves the numerical issue when P0=0
Pw = 1;
for i=0:c-1
    Pw = Pw - myPoissonPMF(i,EC.*Pnk);
end

function p = myPoissonPMF(x,Ex)
p = (Ex.^x).*exp(-Ex)./factorial(x);

% function p = myPoissonCDF(x,Ex)
% p = 1 - gammainc(Ex,x+1);


function euler = rotMat2euler(R)
%ROTMAT2EULER Converts a rotation matrix orientation to ZYX Euler angles
%
%   euler = rotMat2euler(R)
%
%   Converts a rotation matrix orientation to ZYX Euler angles where phi is
%   a rotation around X, theta around Y and psi around Z.
%
%   For more information see:
%   http://www.x-io.co.uk/node/8#quaternions
%
%   Date          Author          Notes
%   27/09/2011    SOH Madgwick    Initial release

    phi = atan2(R(3,2,:), R(3,3,:) );
    theta = -atan(R(3,1,:) ./ sqrt(1-R(3,1,:).^2) );
    psi = atan2(R(2,1,:), R(1,1,:) );

    euler = [phi(1,:)' theta(1,:)' psi(1,:)'];
end


options.filter = 'linear';
options.filter = '9-7';

%% 1D %%
n = 1024;
x = linspace(0,1,n)';
x = cos(10*pi*x) + (x>.4);

Jmin = 6;

options.ti = 0;
y = perform_lifting_transform(x, Jmin, +1, options);
x1 = perform_lifting_transform(y, Jmin, -1, options);
disp(['Error(should be 0)=' num2str(norm(x-x1)/norm(x))]);

options.ti = 1;
y = perform_lifting_transform(x, Jmin, +1, options);
x1 = perform_lifting_transform(y, Jmin, -1, options);
disp(['Error(should be 0)=' num2str(norm(x-x1)/norm(x))]);

%% test for RWT
options.ti = 1;
options.use_mex = 1;
options.wavelet_vm = 3;
options.wavelet_type = 'daubechies';
y = perform_wavelet_transform(x, Jmin, +1, options);
x1 = perform_wavelet_transform(y, Jmin, -1, options);
disp(['Error(should be 0)=' num2str(norm(x-x1)/norm(x))]);

%% test for wavelab
options.ti = 1;
options.use_mex = 0;
options.wavelet_vm = 3;
options.wavelet_type = 'daubechies';
y = perform_wavelet_transform(x, Jmin, +1, options);
x1 = perform_wavelet_transform(y, Jmin, -1, options);
disp(['Error(should be 0)=' num2str(norm(x-x1)/norm(x))]);

return;

%% 2D %%
n = 256;
Jmin = 3;
x = load_image('lena', n);
x = rescale(x);

options.ti = 0;
y = perform_lifting_transform(x, Jmin, +1, options);
x1 = perform_lifting_transform(y, Jmin, -1, options);
disp(['Error(should be 0)=' num2str(norm(x-x1)/norm(x))]);

options.ti = 1;
y = perform_lifting_transform(x, Jmin, +1, options);
x1 = perform_lifting_transform(y, Jmin, -1, options);
disp(['Error(should be 0)=' num2str(norm(x-x1)/norm(x))]);


function [l, u] = ComputeDistanceExtremes(X, a, b, M)
% [l, u] = ComputeDistanceExtremes(X, a, b, M)
%
% Computes sample histogram of the distances between rows of X and returns
% the value of these distances at the a^th and b^th percentils.  This
% method is used to determine the upper and lower bounds for
% similarity / dissimilarity constraints.
%
% X: (n x m) data matrix
% a: lower bound percentile between 1 and 100
% b: upper bound percentile between 1 and 100
% M: Mahalanobis matrix to compute distances
%
% Returns l: distance corresponding to a^th percentile
% u: distance corresponding the b^th percentile

if (a < 1 || a > 100),
    error('a must be between 1 and 100')
end
if (b < 1 || b > 100),
    error('b must be between 1 and 100')
end

n = size(X, 1);

num_trials = min(100, n*(n-1)/2);

% we will sample with replacement
dists = zeros(num_trials, 1);
for (i=1:num_trials),
    j1 = ceil(rand(1)*n);
    j2 = ceil(rand(1)*n);
    dists(i) = (X(j1,:) - X(j2,:))*M*(X(j1,:) - X(j2,:))';
end

[f, c] = hist(dists, 100);
l = c(floor(a));
u = c(floor(b));


function [in2] = ft22in2(ft2)
% Convert area from square feet to square inches.
% Chad A. Greene 2012
in2 = ft2*144;


function [EV, EVal] = ncuts(A, n_ev)
% Computes the n_ev smallest (non-zero) eigenvectors and eigenvalues of the
% of the Laplacian of A

D = sparse(1:size(A,1), 1:size(A,1), full(sum(A, 1)), size(A,1), size(A,2));

opts.issym = 0;
opts.isreal = 1;
opts.disp = 0;
nvec = n_ev+1;

[EV, EVal] = eigs((D - A) + (10^-10) * speye(size(D)), D, nvec, 'sm',opts);

[junk, sortidx] = sort(diag(EVal), 'descend');
EV = EV(:,sortidx(end-1:-1:1));
v = diag(EVal);
EVal = v(sortidx(end-1:-1:1));

EV = bsxfun(@rdivide, EV, sqrt(sum(EV.^2,1))); % makes the eigenvectors unit norm


function f = fx3 ( n, x )

%*****************************************************************************80
%
%% FX3 is the third 1D example.
%
%  Discussion:
%
%    The function should be plotted over [-1.0,+1.0].
%
%    Internally, this range is mapped to [-3.0,+3.0].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 May 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Rick Archibald, Anne Gelb, Jungho Yoon,
%    Polynomial fitting for edge detection in irregularly sampled signals
%    and images,
%    SIAM Journal on Numerical Analysis,
%    Volume 43, Number 1, 2006, pages 259-279.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N), the arguments.
%
%    Output, real F(N,1), the function values.
%

%
%  Destroy all row vectors!
%
  x = x ( : );
%
%  Map from the convenient range [-1,+1] to the physical range [-3,+3].
%
  x = ( ( 1.0 - x ) * ( -3.0 )   ...
      + ( 1.0 + x ) * ( +3.0 ) ) ...
      /   2.0;

  f = zeros ( n, 1 );

  i = find ( -2.0 <= x & x <= -1.0 );
  f(i) = 1.0;

  j = find ( -0.5 <= x & x <= 0.5 );
  f(j) = 0.5 + 4.0 * ( x(j) + 0.5 ).^2;

  k = find ( 1.25 <= x & 3.0 * x <= 7.0 );
  f(k) = 3.0 * ( 2.0 - x(k) );

  return
end


function vectorSez = rotVectToSEZCoords(rVectorECEF, vectorECEF)
    numVectors = size(rVectorECEF,2);

    kHat = repmat([0;0;1], 1, numVectors);
    zHat = vect_normVector(rVectorECEF);
    eHat = vect_normVector(cross(kHat, zHat));
    sHat = vect_normVector(cross(eHat, zHat));

    rotMat = [reshape(sHat, [1, 3, numVectors]);
              reshape(eHat, [1, 3, numVectors]);
              reshape(zHat, [1, 3, numVectors])];

    vectorECEF = reshape(vectorECEF, [3, 1, numVectors]);
    vectorSez = pagemtimes(rotMat, vectorECEF);
    vectorSez = squeeze(vectorSez);
end
%     kHat = [0;0;1];
%     zHat = normVector(rVectorECEF);
%     eHat = normVector(cross(kHat, zHat));
%     sHat = normVector(cross(eHat, zHat));
%
%     rotMat = [sHat'; eHat'; zHat'];
%
%     vectorSez = rotMat * vectorECEF;


function [dop_buf] = rayleigh_dop(in_val)
% Function to generate Doppler-filtered Rayleigh fading simulator

% Doppler parameters
vo = (1.2*1000/3600);    % vo = 1.2 km/h  ->  0.333 m/s
lambda = 3e8 / 5.4e9;    % lambda = c / fc;
fd = vo/lambda;          % Doppler Spread (around 6 Hz)

% Generate Doppler Sf (256 points)
f_rng = 10*fd;
f = -f_rng/2 : f_rng/255 : f_rng/2;
dop_Sf = 1./(1+9*(f/fd).^2);

% Generate 256 samples of Doppler-filtered, Rayleigh fading sim.
dop_buf1(129:256) = randn(1,128)+j*randn(1,128);
dop_buf1(  1:128) = conj(dop_buf1(256:-1:129));
dop_buf2(129:256) = randn(1,128)+j*randn(1,128);
dop_buf2(  1:128) = conj(dop_buf2(256:-1:129));
dop_buf1 = ifft(dop_buf1 .* dop_Sf);
dop_buf2 = ifft(dop_buf2 .* dop_Sf);
dop_buf = sqrt(dop_buf1.^2 + dop_buf2.^2);


%% with mandrill
x = mandrill;

wavelet = wavelet_factory_2d(size(x));
Sx = scat(x, wavelet);
sx = format_scat(Sx);

%% with 32,32
x = rand(32,32);

filt_opt.J = 3;
filt_opt.L = 6;
scat_opt.oversampling = 2;
[wavelet, filters] = wavelet_factory_2d(size(x), filt_opt, scat_opt);

Sx = scat(x, wavelet);
[s , meta] = format_scat(Sx);


function coef=ref_dgtns_1(f,gamma,V)
%REF_DGT_1 Reference DGTNS using P.Prinz algorithm
%   Usage:  c=ref_dgtns_1(f,gamma,V);
%

a=V(1,1);
b=V(2,2);
r=-V(2,1);
L=size(gamma,1);
M=L/b;
N=L/a;
W=size(f,2);

c=gcd(a,M);
p=a/c;
q=M/c;
d=N/q;

if r==0
  % The grid is rectangular. Call another reference algorithm.
  coef=zeros(M*N,W);
  coef(:)=ref_dgt(f,gamma,a,M);

  return;
end;

% We can now assume that the grid is truly nonseperable,
% and so d>1.

% Conjugate.
gammac=conj(gamma);

% Level 2: Block diagonalize, and use that some blocks are
% the same up to permutations.

p1=stridep(M,L);
p2=stridep(N,M*N);

% Get shift offsets for stage 2 of the algorithm.
[mll,all]=shiftoffsets(a,M);

% Step 1: Permute
s1 = f(p1,:);

% Step 2: Multiply by DG'
s2=zeros(M*N,W);

% Do interpreter-language-optimized indexing.
[n_big,m_big]=meshgrid(0:N-1,0:b-1);
base=m_big*M-n_big*a+L;
base=base.';

% Work arrays.
work=zeros(b,M/c*W);
wk=zeros(N,b);
wkrect=zeros(N,b);

% Create fixed modulation matrix (Does not change with k)
fixedmod=zeros(N,b);
for n=0:N-1
  fixedmod(n+1,:)=exp(2*pi*i*r*n/L*(0:M:L-1));
end;

% This loop iterates over the number of truly different wk's.
for ko=0:c-1

  % Create the wk of the rectangular-grid case.
  wkrect(:)=gammac(mod(base+ko,L)+1);

  % Create wk of skewed case.
  wk=(fixedmod.*wkrect);

  % Setup work array.
  for l=0:M/c-1
    k=ko+l*c;
    rowmod=exp(2*pi*i*r*(0:b-1)/b*all(l+1)).';

    work(:,l*W+1:(l+1)*W)=circshift(rowmod.*s1(1+(ko+l*c)*b:(ko+l*c+1)*b,:),-mll(l+1));
  end;

  % Do the actual multiplication,
  work2=wk*work;

  % Place the result correctly.
  for l=0:M/c-1
    k=ko+l*c;
    kmod=exp(2*pi*i*r*(0:N-1)*k/L).';
    colmod=exp(2*pi*i*r*(0:N-1)/b*mll(l+1)).';
    doublefac=exp(-2*pi*i*r/b*all(l+1)*mll(l+1));

    s2(1+(ko+l*c)*N:(ko+l*c+1)*N,:)=doublefac*colmod.*kmod.*circshift(work2(:,l*W+1:(l+1)*W),all(l+1));
  end;

end;

% Step 3: Permute again.
coef = s2(p2,:);

% Apply fft.
for n=1:N
  coef((n-1)*M+1:n*M,:)=fft(coef((n-1)*M+1:n*M,:));
end;


function [beta_median beta_std beta_lbound beta_ubound sigma_median sigma_t_median sigma_t_lbound sigma_t_ubound gamma_median]=stvol2estimates(beta_gibbs,sigma_gibbs,sigma_t_gibbs,gamma_gibbs,n,T,cband)

% compute the median, variance, and credibility intervals for the posterior distribution of beta
beta_median=quantile(beta_gibbs,0.5,2);
beta_std=std(beta_gibbs,0,2);
beta_lbound=quantile(beta_gibbs,(1-cband)/2,2);
beta_ubound=quantile(beta_gibbs,1-(1-cband)/2,2);

% compute the results for sigma (long-run value)
sigma_median=reshape(quantile(sigma_gibbs,0.5,2),n,n);

% compute the rsults for sigma (sample values)
sigma_t_median=cell(n,n);
sigma_t_lbound=cell(n,n);
sigma_t_ubound=cell(n,n);
% loop over periods and entries
for ii=1:T
   for jj=1:n
      for kk=1:jj
      sigma_t_median{jj,kk}(ii,1)=quantile(sigma_t_gibbs{ii,1}(jj,kk,:),0.5,3);
      sigma_t_lbound{jj,kk}(ii,1)=quantile(sigma_t_gibbs{ii,1}(jj,kk,:),(1-cband)/2,3);
      sigma_t_ubound{jj,kk}(ii,1)=quantile(sigma_t_gibbs{ii,1}(jj,kk,:),1-(1-cband)/2,3);
      end
   end
end

% compute the estimates for gamma
gamma_median=quantile(gamma_gibbs,0.5,1);


function [rows, cols, vals] = rcvize_matrix(M, i, j)
%function [rows, cols, vals] = rcvize_matrix(M, i, j)
%
% Given a DxD matrix M that we wish to insert into the (i,j)th DxD
% block of a sparse matrix, this function computes and returns the
% corresponding {row, col, val} vectors describing this block

% Copyright (C) 2016 by David M. Rosen

D = size(M, 1);

vals = reshape(M, [1, D^2]);  %Vectorize M by concatenating its columns

rows = repmat( [D*(i-1) + 1 : D*(i-1) + D], [1,D]);
cols = kron( [D*(j-1) + 1 : D*(j-1) + D], ones(1, D));

end


function fem2d_bvp_linear_test01 ( )

%*****************************************************************************80
%
%% FEM2D_BVP_LINEAR_TEST01 carries out test case #1.
%
%  Discussion:
%
%    Use A1, C1, F1, EXACT1, EXACT_UX1, EXACT_UY1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 June 2014
%
%  Author:
%
%    John Burkardt
%
  nx = 3;
  ny = 3;

% nx = 5;
% ny = 5;

% nx = 9;
% ny = 9;

% nx = 17;
% ny = 17;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM2D_BVP_LINEAR_TEST01\n' );
  fprintf ( 1, '  Solve - del ( A del U ) + C U = F \n' );
  fprintf ( 1, '  on the unit square with zero boundary conditions.\n' );
  fprintf ( 1, '  A1(X,Y) = 1.0\n' );
  fprintf ( 1, '  C1(X,Y) = 0.0\n' );
  fprintf ( 1, '  F1(X,Y) = 2*X*(1-X)+2*Y*(1-Y).\n' );
  fprintf ( 1, '  U1(X,Y) = X * ( 1 - X ) * Y * ( 1 - Y )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The grid uses %d by %d nodes.\n', nx, ny );
%
%  Geometry definitions.
%
  x = linspace ( 0.0, 1.0, nx );
  y = linspace ( 0.0, 1.0, ny );

  u = fem2d_bvp_linear ( nx, ny, @a1, @c1, @f1, x, y );

  if ( 0 )
    [ X, Y ] = meshgrid ( x, y );
    surf ( X, Y, u )
  end

  if ( nx * ny <= 25 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '     I     J    X         Y         U         Uexact    Error\n' );
    fprintf ( 1, '\n' );

    for j = 1 : ny
      for i = 1 : nx
        uexact = exact1 ( x(i), y(j) );
        fprintf ( 1, '  %4d  %4d  %8f  %8f  %8f  %8f  %8e\n', ...
          i, j, x(i), y(j), u(i,j), uexact, abs ( u(i,j) - uexact ) );
      end
    end

  end

  e1 = fem2d_l1_error ( nx, ny, x, y, u, @exact1 );
  e2 = fem2d_l2_error_linear ( nx, ny, x, y, u, @exact1 );
  h1s = fem2d_h1s_error_linear ( nx, ny, x, y, u, @exact_ux1, @exact_uy1 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  l1 error   = %g\n', e1 );
  fprintf ( 1, '  L2 error   = %g\n', e2 );
  fprintf ( 1, '  H1S error  = %g\n', h1s );

  return
end
function value = a1 ( x, y )

%*****************************************************************************80
%
%% A1 evaluates A function #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of A(X).
%
  value = 1.0;

  return
end
function value = c1 ( x, y )

%*****************************************************************************80
%
%% C1 evaluates C function #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of C(X).
%
  value = 0.0;

  return
end
function value = exact1 ( x, y )

%*****************************************************************************80
%
%% EXACT1 evaluates exact solution #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of the solution.
%
  value = x .* ( 1.0 - x ) .* y .* ( 1.0 - y );

  return
end
function value = exact_ux1 ( x, y )

%*****************************************************************************80
%
%% EXACT_UX1 evaluates the derivative dUdX of exact solution #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of dUdX.
%
  value = ( 1.0 - 2.0 * x ) .* ( y - y .* y );

  return
end
function value = exact_uy1 ( x, y )

%*****************************************************************************80
%
%% EXACT_UY1 evaluates the derivative dUdY of exact solution #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of dUdX.
%
  value = ( x - x .* x ) .* ( 1.0 - 2.0 * y );

  return
end
function value = f1 ( x, y )

%*****************************************************************************80
%
%% F1 evaluates right hand side function #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of the right hand side.
%
  value = 2.0 * x .* ( 1.0 - x ) ...
        + 2.0 * y .* ( 1.0 - y );

  return
end


function [ t, w ] = t_quadrature_rule ( n )

%*****************************************************************************80
%
%% T_QUADRATURE_RULE: quadrature rule for T(n,x).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the rule.
%
%    Output, real T(N,1), W(N,1), the points and weights of the rule.
%
  aj = zeros ( n, 1 );

  bj = 0.5 * ones ( n, 1 );
  bj(1) = sqrt ( 0.5 );

  w = zeros ( n, 1 );
  w(1,1) = sqrt ( pi );

  [ t, w ] = imtqlx ( n, aj, bj, w );

  w(1:n,1) = w(1:n,1).^2;

  return
end


function [x_g,w_g,phi,p_x,p_y,p_z] = threed_shapeiso(x_local,r,s,t,w)
%-----------------------------------------------------------------------
%  threed_shapeiso.m - computes test functions and derivatives on an
%                      element given element coordinates and Gauss points.
%                      Isoparametric coordinates are used (curved elements)
%
%  Copyright (c) 2002, Jeff Borggaard, Virginia Tech
%  Version: 1.0a
%
%  Usage:    [x_g,w_g,phi,p_x,p_y,p_z] = threed_shapeiso(x_local,r,s,t,w)
%
%  Variables:     x_local
%                        Coordinates of the element nodes
%                 (r,s,t)
%                        Coordinates of Gauss points in unit tetrahedron
%                 w
%                        Gauss weights associated with (r,s,t)
%
%                 x_g
%                        Coordinates of Gauss points in the element
%                 w_g
%                        Gauss weights scaled by the element Jacobian
%                 phi
%                        Value of element shape functions at x_g
%                 p_x
%                 p_y
%                 p_z
%                        First spatial derivatives of phi
%                 p_xx, etc.
%                        Second spatial derivatives of phi
%                        (only applicable with 2nd order or higher)
%                        (currently not implemented)
%-----------------------------------------------------------------------
  x      = x_local;
  [n,t1] = size(x);  % t1 had better be 3
  rule   = length(r);

  if (n == 10)
    % The following assumes isoparametric elements
    c0 =   x( 1,:);                                      % 1
    c1 =-3*x( 1,:) -   x( 2,:) + 4*x( 5,:)            ;  % r
    c2 =-3*x( 1,:) -   x( 3,:) + 4*x( 7,:)            ;  % s
    c3 =-3*x( 1,:) -   x( 4,:) + 4*x(10,:)            ;  % t
    c4 = 2*x( 1,:) + 2*x( 2,:) - 4*x( 5,:)            ;  % r^2
    c5 = 2*x( 1,:) + 2*x( 3,:) - 4*x( 7,:)            ;  % s^2
    c6 = 2*x( 1,:) + 2*x( 4,:) - 4*x(10,:)            ;  % t^2
    c7 = 4*x( 1,:) - 4*x( 5,:) + 4*x( 6,:) - 4*x( 7,:); % rs
    c8 = 4*x( 1,:) - 4*x( 5,:) + 4*x( 8,:) - 4*x(10,:); % rt
    c9 = 4*x( 1,:) - 4*x( 7,:) + 4*x( 9,:) - 4*x(10,:); % st

    x_g(:,1) = c0(1) + c1(1)*r + c2(1)*s + c3(1)*t + c4(1)*r.^2 ...
             + c5(1)*s.^2 + c6(1)*t.^2 + c7(1)*r.*s + c8(1)*r.*t ...
             + c9(1)*s.*t;
    xr = c1(1) + 2*c4(1)*r + c7(1)*s + c8(1)*t;
    xs = c2(1) + 2*c5(1)*s + c7(1)*r + c9(1)*t;
    xt = c3(1) + 2*c6(1)*t + c8(1)*r + c9(1)*s;

    x_g(:,2) = c0(2) + c1(2)*r + c2(2)*s + c3(2)*t + c4(2)*r.^2 ...
             + c5(2)*s.^2 + c6(2)*t.^2 + c7(2)*r.*s + c8(2)*r.*t ...
             + c9(2)*s.*t;
    yr = c1(2) + 2*c4(2)*r + c7(2)*s + c8(2)*t;
    ys = c2(2) + 2*c5(2)*s + c7(2)*r + c9(2)*t;
    yt = c3(2) + 2*c6(2)*t + c8(2)*r + c9(2)*s;

    x_g(:,3) = c0(3) + c1(3)*r + c2(3)*s + c3(3)*t + c4(3)*r.^2 ...
             + c5(3)*s.^2 + c6(3)*t.^2 + c7(3)*r.*s + c8(3)*r.*t ...
             + c9(3)*s.*t;
    zr = c1(3) + 2*c4(3)*r + c7(3)*s + c8(3)*t;
    zs = c2(3) + 2*c5(3)*s + c7(3)*r + c9(3)*t;
    zt = c3(3) + 2*c6(3)*t + c8(3)*r + c9(3)*s;

    % Compute the Jacobian of the (r,s,t) -> (x,y,z) transformation
    jac = ( xr.*ys.*zt + xs.*yt.*zr + xt.*yr.*zs ...
           -xr.*yt.*zs - xs.*yr.*zt - xt.*ys.*zr );

    w_g = jac.*w;

    % Invert derivatives of the mapping
    rx = ( ys.*zt-yt.*zs )./jac;
    ry = ( xt.*zs-xs.*zt )./jac;
    rz = ( xs.*yt-xt.*ys )./jac;

    sx = ( yt.*zr-yr.*zt )./jac;
    sy = ( xr.*zt-xt.*zr )./jac;
    sz = ( xt.*yr-xr.*yt )./jac;

    tx = ( yr.*zs-ys.*zr )./jac;
    ty = ( xs.*zr-xr.*zs )./jac;
    tz = ( xr.*ys-xs.*yr )./jac;

    % Compute shape function and derivatives at Gauss points
    u  = 1 - r  - s  - t ;
    ux =   - rx - sx - tx;
    uy =   - ry - sy - ty;
    uz =   - rz - sz - tz;

    phi = zeros(rule,n);
    phi(:,1)  = u - 2*r.*u - 2*s.*u - 2*t.*u;
    phi(:,2)  = r - 2*r.*u - 2*r.*s - 2*r.*t;
    phi(:,3)  = s - 2*r.*s - 2*s.*u - 2*s.*t;
    phi(:,4)  = t - 2*r.*t - 2*s.*t - 2*t.*u;
    phi(:,5)  = 4*r.*u;
    phi(:,6)  = 4*r.*s;
    phi(:,7)  = 4*s.*u;
    phi(:,8)  = 4*r.*t;
    phi(:,9)  = 4*s.*t;
    phi(:,10) = 4*t.*u;

    p_x = zeros(rule,n);
    p_x(:,1)  = ux - 2*rx.*u - 2*r.*ux - 2*sx.*u ...
                   - 2*s.*ux - 2*tx.*u - 2*t.*ux    ;
    p_x(:,2)  = rx - 2*rx.*u - 2*r.*ux - 2*rx.*s ...
                   - 2*r.*sx - 2*rx.*t - 2*r.*tx    ;
    p_x(:,3)  = sx - 2*rx.*s - 2*r.*sx - 2*sx.*u ...
                   - 2*s.*ux - 2*sx.*t - 2*s.*tx    ;
    p_x(:,4)  = tx - 2*rx.*t - 2*r.*tx - 2*sx.*t ...
                   - 2*s.*tx - 2*tx.*u - 2*t.*ux    ;
    p_x(:,5)  = 4*rx.*u + 4*r.*ux;
    p_x(:,6)  = 4*rx.*s + 4*r.*sx;
    p_x(:,7)  = 4*sx.*u + 4*s.*ux;
    p_x(:,8)  = 4*rx.*t + 4*r.*tx;
    p_x(:,9)  = 4*sx.*t + 4*s.*tx;
    p_x(:,10) = 4*tx.*u + 4*t.*ux;

    p_y = zeros(rule,n);
    p_y(:,1)  = uy - 2*ry.*u - 2*r.*uy - 2*sy.*u ...
                   - 2*s.*uy - 2*ty.*u - 2*t.*uy    ;
    p_y(:,2)  = ry - 2*ry.*u - 2*r.*uy - 2*ry.*s ...
                   - 2*r.*sy - 2*ry.*t - 2*r.*ty    ;
    p_y(:,3)  = sy - 2*ry.*s - 2*r.*sy - 2*sy.*u ...
                   - 2*s.*uy - 2*sy.*t - 2*s.*ty    ;
    p_y(:,4)  = ty - 2*ry.*t - 2*r.*ty - 2*sy.*t ...
                   - 2*s.*ty - 2*ty.*u - 2*t.*uy    ;
    p_y(:,5)  = 4*ry.*u + 4*r.*uy;
    p_y(:,6)  = 4*ry.*s + 4*r.*sy;
    p_y(:,7)  = 4*sy.*u + 4*s.*uy;
    p_y(:,8)  = 4*ry.*t + 4*r.*ty;
    p_y(:,9)  = 4*sy.*t + 4*s.*ty;
    p_y(:,10) = 4*ty.*u + 4*t.*uy;

    p_z = zeros(rule,n);
    p_z(:,1)  = uz - 2*rz.*u - 2*r.*uz - 2*sz.*u ...
                   - 2*s.*uz - 2*tz.*u - 2*t.*uz    ;
    p_z(:,2)  = rz - 2*rz.*u - 2*r.*uz - 2*rz.*s ...
                   - 2*r.*sz - 2*rz.*t - 2*r.*tz    ;
    p_z(:,3)  = sz - 2*rz.*s - 2*r.*sz - 2*sz.*u ...
                   - 2*s.*uz - 2*sz.*t - 2*s.*tz    ;
    p_z(:,4)  = tz - 2*rz.*t - 2*r.*tz - 2*sz.*t ...
                   - 2*s.*tz - 2*tz.*u - 2*t.*uz    ;
    p_z(:,5)  = 4*rz.*u + 4*r.*uz;
    p_z(:,6)  = 4*rz.*s + 4*r.*sz;
    p_z(:,7)  = 4*sz.*u + 4*s.*uz;
    p_z(:,8)  = 4*rz.*t + 4*r.*tz;
    p_z(:,9)  = 4*sz.*t + 4*s.*tz;
    p_z(:,10) = 4*tz.*u + 4*t.*uz;

  else
    error('Only quadratic isoparametric elements are supported\n')
  end


function imageData = whitebalance(imageData)
% WHITEBALANCE forces the average image color to be gray.
% Copyright 2013 The MathWorks, Inc.

% Find the average values for each channel.
avg_rgb = mean(mean(imageData));

% Find the average gray value and compute the scaling
% factor.
factors = max(mean(avg_rgb), 128)./avg_rgb;

% Adjust the image to the new gray value.
imageData(:,:,1) = uint8(imageData(:,:,1)*factors(1));
imageData(:,:,2) = uint8(imageData(:,:,2)*factors(2));
imageData(:,:,3) = uint8(imageData(:,:,3)*factors(3));


function [m,v,w,g,f,pp,gg]=v_gaussmix(x,c,l,m0,v0,w0,wx)
%V_GAUSSMIX fits a gaussian mixture pdf to a set of data observations [m,v,w,g,f]=(x,c,l,m0,v0,w0,wx)
%
% Usage:
%    (1) [m,v,w]=v_gaussmix(x,[],[],k);    % create GMM with k mixtures and diagonal covariances
%    (2) [m,v,w]=gaussmix(x,[],[],k,'v');    % create GMM with k mixtures and full covariances
%
% Inputs: n data values, k mixtures, p parameters, l loops
%
%     X(n,p)   Input data vectors, one per row.
%     C(1)     Minimum variance of normalized data (Use [] to take default value of 1/n^2)
%     L        The integer portion of l gives a maximum loop count. The fractional portion gives
%              an optional stopping threshold. Iteration will cease if the increase in
%              log likelihood density per data point is less than this value. Thus l=10.001 will
%              stop after 10 iterations or when the increase in log likelihood falls below
%              0.001.
%              As a special case, if L=0, then the first three outputs are omitted.
%              Use [] to take default value of 100.0001
%     M0       Number of mixtures required (or initial mixture means - see below)
%     V0       Initialization mode:
%                'f'    Initialize with K randomly selected data points [default]
%                'p'    Initialize with centroids and variances of random partitions
%                'k'    k-means algorithm ('kf' and 'kp' determine initialization)
%                'h'    k-harmonic means algorithm ('hf' and 'hp' determine initialization) [default]
%                's'    use unscaled data during initialization phase instead of scaling it first
%                'm'    M0 contains the initial centres
%                'v'    full covariance matrices
%              Mode 'hf' [the default] generally gives the best results but 'f' is faster and often OK
%     W0(n,1)  Data point weights
%
%     Alternatively, initial values for M0, V0 and W0 can be given  explicitly:
%
%     M0(k,p)  Initial mixture means, one row per mixture.
%     V0(k,p)  Initial mixture variances, one row per mixture.
%      or V0(p,p,k)  one full-covariance matrix per mixture
%     W0(k,1)  Initial mixture weights, one per mixture. The weights should sum to unity.
%     WX(n,1)  Data point weights
%
% Outputs: (Note that M, V and W are omitted if L==0)
%
%     M(k,p)   Mixture means, one row per mixture. (omitted if L==0)
%     V(k,p)   Mixture variances, one row per mixture. (omitted if L==0)
%       or V(p,p,k) if full covariance matrices in use (i.e. either 'v' option or V0(p,p,k) specified)
%     W(k,1)   Mixture weights, one per mixture. The weights will sum to unity. (omitted if L==0)
%     G       Average log probability of the input data points.
%     F        Fisher's Discriminant measures how well the data divides into classes.
%              It is the ratio of the between-mixture variance to the average mixture variance: a
%              high value means the classes (mixtures) are well separated.
%     PP(n,1)  Log probability of each data point
%     GG(l+1,1) Average log probabilities at the beginning of each iteration and at the end
%
% The fitting procedure uses one of several initialization methods to create an initial guess
% for the mixture centres and then uses the EM (expectation-maximization) algorithm to refine
% the guess. Although the EM algorithm is deterministic, the initialization procedures use
% random numbers and so the routine will not give identical answers if you call it multiple
% times with the same input data. See v_randvec() for generating GMM data vectors.

%  Bugs/Suggestions
%     (1) Allow processing in chunks by outputting/reinputting an array of sufficient statistics
%     (2) Other initialization options:
%              'l'    LBG algorithm
%              'm'    Move-means (dog-rabbit) algorithm
%     (3) Allow updating of weights-only, not means/variances
%     (4) Allow freezing of means and/or variances

%      Copyright (C) Mike Brookes 2000-2009
%      Version: $Id: v_gaussmix.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[n,p]=size(x); % n = number of training values, p = dimension of data vector
wn=ones(n,1);
memsize=v_voicebox('memsize');    % set memory size to use
if isempty(c)
    c=1/n^2;
else
    c=c(1);         % just to prevent legacy code failing
end
fulliv=0;           % initial variance is not full
if isempty(l)
    l=100+1e-4;         % max loop count + stopping threshold
end
if nargin<5 || isempty(v0) || ischar(v0)             % no initial values specified for m0, v0, w0
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %  No initialvalues given, so we must use k-means or equivalent
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if nargin<6
        if nargin<5 || isempty(v0)
            v0='hf';                 % default initialization mode: hf
        end
        wx=wn;                      % no data point weights
    else
        wx=w0(:);                   % data point weights
    end
    wx=wx/sum(wx);
    if any(v0=='m')
        k=size(m0,1);
    else
        k=m0;
    end
    fv=any(v0=='v');                % full covariance matrices requested
    mx0=wx'*x;         % calculate mean of input data in each dimension
    vx0=wx'*x.^2-mx0.^2; % calculate variance of input data in each dimension
    sx0=sqrt(vx0);
    sx0(sx0==0)=1;      % do not divide by zero when scaling
    if n<=k                         % each data point can have its own mixture
        xs=(x-mx0(wn,:))./sx0(wn,:);          % scale the data
        m=xs(mod((1:k)-1,n)+1,:);   % just include all points several times
        v=zeros(k,p);               % will be set to floor later
        w=zeros(k,1);
        w(1:n)=1/n;
        if l>0
            l=0.1;                  % no point in iterating
        end
    else                            % more points than mixtures
        if any(v0=='s')
            xs=x;                   % do not scale data during initialization
        else
            xs=(x-mx0(wn,:))./sx0(wn,:);  % else scale now
            if any(v0=='m')
                m=(m0-mx0(ones(k,1),:))./sx0(ones(k,1),:);  % scale specified means as well
            end
        end
        w=repmat(1/k,k,1);                      % all mixtures equally likely
        if any(v0=='k')                         % k-means initialization
            if any(v0=='m')
                [m,e,j]=v_kmeans(xs,k,m);
            elseif any(v0=='p')
                [m,e,j]=v_kmeans(xs,k,'p');
            else
                [m,e,j]=v_kmeans(xs,k,'f');
            end
        elseif any(v0=='h')                     % k-harmonic means initialization
            if any(v0=='m')
                [m,e,j]=v_kmeanhar(xs,k,[],4,m);
            else
                if any(v0=='p')
                    [m,e,j]=v_kmeanhar(xs,k,[],4,'p');
                else
                    [m,e,j]=v_kmeanhar(xs,k,[],4,'f');
                end
            end
        elseif any(v0=='p')                     % Initialize using a random partition
            j=ceil(rand(n,1)*k);                % allocate to random clusters
            j(v_rnsubset(k,n))=1:k;               % but force at least one point per cluster
            for i=1:k
                m(i,:)=mean(xs(j==i,:),1);
            end
        else
            if any(v0=='m')
                m=m0;                           % use specified centres
            else
                m=xs(v_rnsubset(k,n),:);          % Forgy initialization: sample k centres without replacement [default]
            end
            [e,j]=v_kmeans(xs,k,m,0);             % find out the cluster allocation
        end
        if any(v0=='s')
            xs=(x-mx0(wn,:))./sx0(wn,:);      % scale data now if not done previously
        end
        v=zeros(k,p);                   % diagonal covariances
        w=zeros(k,1);
        for i=1:k
            ni=sum(j==i);               % number assigned to this centre
            w(i)=(ni+1)/(n+k);          % weight of this mixture
            if ni
                v(i,:)=sum((xs(j==i,:)-repmat(m(i,:),ni,1)).^2,1)/ni;
            else
                v(i,:)=zeros(1,p);
            end
        end
    end
else
    %%%%%%%%%%%%%%%%%%%%%%%%
    % use initial values given as input parameters
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if nargin<7
        wx=wn;              % no data point weights
    end
    wx=wx(:)/sum(wx); % normalize weights and force a column vector
    mx0=wx'*x;         % calculate mean of input data in each dimension
    vx0=wx'*x.^2-mx0.^2; % calculate variance of input data in each dimension
    sx0=sqrt(vx0);
    sx0(sx0==0)=1;      % do not divide by zero when scaling
    [k,p]=size(m0);
    xs=(x-mx0(wn,:))./sx0(wn,:);          % scale the data
    m=(m0-mx0(ones(k,1),:))./sx0(ones(k,1),:);          % and the means
    v=v0;
    w=w0;
    fv=ndims(v)>2 || size(v,1)>k;                       % full covariance matrix is supplied
    if fv
        mk=eye(p)==0;                                    % off-diagonal elements
        fulliv=any(v(repmat(mk,[1 1 k]))~=0);            % check if any are non-zero
        if ~fulliv
            v=reshape(v(repmat(~mk,[1 1 k])),p,k)'./repmat(sx0.^2,k,1);   % just pick out and scale the diagonal elements for now
        else
            v=v./repmat(sx0'*sx0,[1 1 k]);              % scale the full covariance matrix
        end
    end
end
if length(wx)~=n
    error('%d datapoints but %d weights',n,length(wx));
end
lsx=sum(log(sx0));
xsw=xs.*repmat(wx,1,p); % weighted data points
if ~fulliv          % initializing with diagonal covariance
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Diagonal Covariance matrices  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    v=max(v,c);         % apply the lower bound
    xs2=xs.^2.*repmat(wx,1,p);          % square and weight the data for variance calculations

    % If data size is large then do calculations in chunks

    nb=min(n,max(1,floor(memsize/(8*p*k))));    % chunk size for testing data points
    nl=ceil(n/nb);                  % number of chunks
    jx0=n-(nl-1)*nb;                % size of first chunk

    im=repmat(1:k,1,nb); im=im(:);
    th=(l-floor(l))*n;
    sd=(nargout > 3*(l~=0)); % = 1 if we are outputting log likelihood values
    lp=floor(l)+sd;   % extra loop needed to calculate final G value

    lpx=zeros(1,n);             % log probability of each data point
    wk=ones(k,1);
    wp=ones(1,p);
    wnb=ones(1,nb);
    wnj=ones(1,jx0);

    % EM loop

    g=0;                            % dummy initial value for comparison
    gg=zeros(lp+1,1);
    ss=sd;                          % initialize stopping count (0 or 1)
    for j=1:lp
        g1=g;                       % save previous log likelihood (2*pi factor omitted)
        m1=m;                       % save previous means, variances and weights
        v1=v;
        w1=w;
        vi=-0.5*v.^(-1);                % data-independent scale factor in exponent
        lvm=log(w)-0.5*sum(log(v),2);   % log of external scale factor (excluding -0.5*p*log(2pi) term)

        % first do partial chunk (of length jx0)

        jx=jx0;
        ii=1:jx;                        % indices of data points in this chunk
        kk=repmat(ii,k,1);              % kk(jx,k): one row per data point, one column per mixture
        km=repmat(1:k,1,jx);            % km(jx,k): one row per data point, one column per mixture
        py=reshape(sum((xs(kk(:),:)-m(km(:),:)).^2.*vi(km(:),:),2),k,jx)+lvm(:,wnj); % py(k,jx) pdf of each point with each mixture
        mx=max(py,[],1);                % mx(1,jx) find normalizing factor for each data point to prevent underflow when using exp()
        px=exp(py-mx(wk,:));            % find normalized probability of each mixture for each datapoint
        ps=sum(px,1);                   % total normalized likelihood of each data point
        px=px./ps(wk,:);                % relative mixture probabilities for each data point (columns sum to 1)
        lpx(ii)=log(ps)+mx;
        pk=px*wx(ii);                   % pk(k,1) effective fraction of data points for each mixture (could be zero due to underflow)
        sx=px*xsw(ii,:);
        sx2=px*xs2(ii,:);
        for il=2:nl                     % process the data points in chunks
            ix=jx+1;
            jx=jx+nb;                   % increment upper limit
            ii=ix:jx;                   % indices of data points in this chunk
            kk=repmat(ii,k,1);
            py=reshape(sum((xs(kk(:),:)-m(im,:)).^2.*vi(im,:),2),k,nb)+lvm(:,wnb);
            mx=max(py,[],1);            % find normalizing factor for each data point to prevent underflow when using exp()
            px=exp(py-mx(wk,:));        % find normalized probability of each mixture for each datapoint
            ps=sum(px,1);               % total normalized likelihood of each data point
            px=px./ps(wk,:);            % relative mixture probabilities for each data point (columns sum to 1)
            lpx(ii)=log(ps)+mx;
            pk=pk+px*wx(ii);            % pk(k,1) effective fraction of data points for each mixture (could be zero due to underflow)
            sx=sx+px*xsw(ii,:);
            sx2=sx2+px*xs2(ii,:);
        end
        g=lpx*wx;                       % total log probability summed over all data points
        gg(j)=g;                        % save log prob at each iteration
        w=pk;                           % normalize to get the weights
        if pk                           % if all elements of pk are non-zero
            m=sx./pk(:,wp);             % calculate mixture means
            v=sx2./pk(:,wp);            % and variances
        else
            wm=pk==0;                   % mask indicating mixtures with zero weights
            nz=sum(wm);              	% number of zero-weight mixtures
            [vv,mk]=sort(lpx);          % find the lowest probability data points
            m=zeros(k,p);               % initialize means and variances to zero (variances are floored later)
            v=m;
            m(wm,:)=xs(mk(1:nz),:); 	% set zero-weight mixture means to worst-fitted data points
            w(wm)=1/n;               	% set these weights non-zero
            w=w*n/(n+nz);            	% normalize so the weights sum to unity
            wm=~wm;                 	% mask for non-zero weights
            m(wm,:)=sx(wm,:)./pk(wm,wp);  % recalculate means and variances for mixtures with a non-zero weight
            v(wm,:)=sx2(wm,:)./pk(wm,wp);
        end
        v=max(v-m.^2,c);                % apply floor to variances
        if g-g1<=th && j>1
            if ~ss, break; end  %  stop
            ss=ss-1;       % stop next time
        end
    end
    if sd && ~fv  % we need to calculate the final probabilities
        pp=lpx'-0.5*p*log(2*pi)-lsx;   % log of total probability of each data point
        gg=gg(1:j)-0.5*p*log(2*pi)-lsx;    % average log prob at each iteration
        g=gg(end);
        m=m1;       % back up to previous iteration
        v=v1;
        w=w1;
        mm=sum(m,1)/k;
        f=(m(:)'*m(:)-k*mm(:)'*mm(:))/sum(v(:));
    end
    if ~fv
        m=m.*sx0(ones(k,1),:)+mx0(ones(k,1),:);	% unscale means
        v=v.*repmat(sx0.^2,k,1);                % and variances
    else
        v1=v;
        v=zeros(p,p,k);
        mk=eye(p)==1;                           % mask for diagonal elements
        v(repmat(mk,[1 1 k]))=v1';              % set from v1
    end
end
if fv              % check if full covariance matrices were requested
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Full Covariance matrices  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    pl=p*(p+1)/2;
    lix=1:p^2;
    cix=repmat(1:p,p,1);
    rix=cix';
    lix(cix>rix)=[];                                        % index of lower triangular elements
    cix=cix(lix);                                           % index of lower triangular columns
    rix=rix(lix);                                           % index of lower triangular rows
    dix=find(rix==cix);
    lixi=zeros(p,p);
    lixi(lix)=1:pl;
    lixi=lixi';
    lixi(lix)=1:pl;                                        % reverse index to build full matrices
    v=reshape(v,p^2,k);
    v=v(lix,:)';                                            % lower triangular in rows

    % If data size is large then do calculations in chunks

    nb=min(n,max(1,floor(memsize/(24*p*k))));    % chunk size for testing data points
    nl=ceil(n/nb);                  % number of chunks
    jx0=n-(nl-1)*nb;                % size of first chunk
    %
    th=(l-floor(l))*n;
    sd=(nargout > 3*(l~=0)); % = 1 if we are outputting log likelihood values
    lp=floor(l)+sd;   % extra loop needed to calculate final G value
    %
    lpx=zeros(1,n);             % log probability of each data point
    wk=ones(k,1);
    wp=ones(1,p);
    wpl=ones(1,pl);             % 1 index for lower triangular matrix
    wnb=ones(1,nb);
    wnj=ones(1,jx0);

    % EM loop

    g=0;                        % dummy initial value for comparison
    gg=zeros(lp+1,1);
    ss=sd;                      % initialize stopping count (0 or 1)
    vi=zeros(p*k,p);            % stack of k inverse cov matrices each size p*p
    vim=zeros(p*k,1);       	% stack of k vectors of the form inv(v)*m
    mtk=vim;                  	% stack of k vectors of the form m
    lvm=zeros(k,1);
    wpk=repmat((1:p)',k,1);
    for j=1:lp
        g1=g;               	% save previous log likelihood (2*pi factor omitted)
        m1=m;                	% save previous means, variances and weights
        v1=v;
        w1=w;
        for ik=1:k

            % these lines added for debugging only
            %             vk=reshape(v(k,lixi),p,p);
            %             condk(ik)=cond(vk);
            %%%%%%%%%%%%%%%%%%%%
            [uvk,dvk]=eig(reshape(v(ik,lixi),p,p));	% convert lower triangular to full and find eigenvalues
            dvk=max(diag(dvk),c);                	% apply variance floor to eigenvalues
            vik=-0.5*uvk*diag(dvk.^(-1))*uvk';      % calculate inverse
            vi((ik-1)*p+(1:p),:)=vik;               % vi contains all mixture inverses stacked on top of each other
            vim((ik-1)*p+(1:p))=vik*m(ik,:)';       % vim contains vi*m for all mixtures stacked on top of each other
            mtk((ik-1)*p+(1:p))=m(ik,:)';           % mtk contains all mixture means stacked on top of each other
            lvm(ik)=log(w(ik))-0.5*sum(log(dvk));       % vm contains the weighted sqrt of det(vi) for each mixture
        end
        %
        %         % first do partial chunk
        %
        jx=jx0;
        ii=1:jx;
        xii=xs(ii,:).';
        py=reshape(sum(reshape((vi*xii-vim(:,wnj)).*(xii(wpk,:)-mtk(:,wnj)),p,jx*k),1),k,jx)+lvm(:,wnj);
        mx=max(py,[],1);                % find normalizing factor for each data point to prevent underflow when using exp()
        px=exp(py-mx(wk,:));            % find normalized probability of each mixture for each datapoint
        ps=sum(px,1);                   % total normalized likelihood of each data point
        px=px./ps(wk,:);                % relative mixture probabilities for each data point (columns sum to 1)
        lpx(ii)=log(ps)+mx;
        pk=px*wx(ii);                       % effective fraction of data points for each mixture (could be zero due to underflow)
        sx=px*xsw(ii,:);
        sx2=px*(xsw(ii,rix).*xs(ii,cix));	% accumulator for variance calculation (lower tri cov matrix as a row)
        for il=2:nl
            ix=jx+1;
            jx=jx+nb;        % increment upper limit
            ii=ix:jx;
            xii=xs(ii,:).';
            py=reshape(sum(reshape((vi*xii-vim(:,wnb)).*(xii(wpk,:)-mtk(:,wnb)),p,nb*k),1),k,nb)+lvm(:,wnb);
            mx=max(py,[],1);                % find normalizing factor for each data point to prevent underflow when using exp()
            px=exp(py-mx(wk,:));            % find normalized probability of each mixture for each datapoint
            ps=sum(px,1);                   % total normalized likelihood of each data point
            px=px./ps(wk,:);                % relative mixture probabilities for each data point (columns sum to 1)
            lpx(ii)=log(ps)+mx;
            pk=pk+px*wx(ii);                    % effective fraction of data points for each mixture (could be zero due to underflow)
            sx=sx+px*xsw(ii,:);             % accumulator for mean calculation
            sx2=sx2+px*(xsw(ii,rix).*xs(ii,cix));	% accumulator for variance calculation
        end
        g=lpx*wx;                   % total log probability summed over all data points
        gg(j)=g;                    % save convergence history
        w=pk;               	% w(k,1) normalize to get the column of weights
        if pk                       % if all elements of pk are non-zero
            m=sx./pk(:,wp);         % find mean and mean square
            v=sx2./pk(:,wpl);
        else
            wm=pk==0;                       % mask indicating mixtures with zero weights
            nz=sum(wm);                  % number of zero-weight mixtures
            [vv,mk]=sort(lpx);             % find the lowest probability data points
            m=zeros(k,p);                   % initialize means and variances to zero (variances are floored later)
            v=zeros(k,pl);
            m(wm,:)=xs(mk(1:nz),:);                % set zero-weight mixture means to worst-fitted data points
            w(wm)=1/n;                      % set these weights non-zero
            w=w*n/(n+nz);                   % normalize so the weights sum to unity
            wm=~wm;                         % mask for non-zero weights
            m(wm,:)=sx(wm,:)./pk(wm,wp);  % recalculate means and variances for mixtures with a non-zero weight
            v(wm,:)=sx2(wm,:)./pk(wm,wpl);
        end
        v=v-m(:,cix).*m(:,rix);                 % subtract off mean squared
        if g-g1<=th && j>1
            if ~ss, break; end  %  stop
            ss=ss-1;       % stop next time
        end
    end
    if sd  % we need to calculate the final probabilities
        pp=lpx'-0.5*p*log(2*pi)-lsx;   % log of total probability of each data point
        gg=gg(1:j)-0.5*p*log(2*pi)-lsx;    % average log prob at each iteration
        g=gg(end);
        %             gg' % *** DEBUG ONLY ***
        m=m1;                                           % back up to previous iteration
        v=zeros(p,p,k);                                 % reserve spave for k full covariance matrices
        trv=0;                                          % sum of variance matrix traces
        for ik=1:k                                      % loop for each mixture to apply variance floor
            [uvk,dvk]=eig(reshape(v1(ik,lixi),p,p));	% convert lower triangular to full and find eigenvectors
            dvk=max(diag(dvk),c);                       % apply variance floor to eigenvalues
            v(:,:,ik)=uvk*diag(dvk)*uvk';               % reconstitute full matrix
            trv=trv+sum(dvk);                           % add trace to the sum
        end
        w=w1;
        mm=sum(m,1)/k;
        f=(m(:)'*m(:)-k*mm(:)'*mm(:))/trv;
    else
        v1=v;                                           % lower triangular form
        v=zeros(p,p,k);                                 % reserve spave for k full covariance matrices
        for ik=1:k                                      % loop for each mixture to apply variance floor
            [uvk,dvk,]=eig(reshape(v1(ik,lixi),p,p));	% convert lower triangular to full and find eigenvectors
            dvk=max(diag(dvk),c);                       % apply variance floor
            v(:,:,ik)=uvk*diag(dvk)*uvk';               % reconstitute full matrix
        end
    end
    m=m.*sx0(ones(k,1),:)+mx0(ones(k,1),:);  % unscale means
    v=v.*repmat(sx0'*sx0,[1 1 k]);
end
if l==0         % suppress the first three output arguments if l==0
    m=g;
    v=f;
    w=pp;
end


function F = spm_Tcdf(x,v)
% Cumulative Distribution Function (CDF) of Students t distribution
% FORMAT p = spm_Tcdf(x,v)
%
% x - T-variate (Student's t has range (-Inf,Inf)
% v - degrees of freedom (v>0, non-integer d.f. accepted)
% F - CDF of Student's t-distribution with v degrees of freedom at points x
%__________________________________________________________________________
%
% spm_Tcdf implements the Cumulative Distribution of the Students t-distribution.
%
% Definition:
%--------------------------------------------------------------------------
% The CDF F(x) of the Student's t-distribution with v degrees of
% freedom is the probability that a realisation of a t random variable
% X has value less than x; F(x)=Pr{X<x} for X~G(h,c). Student's
% t-distribution is defined for real x and positive integer v (See
% Evans et al., Ch37).
%
% This implementation is not restricted to whole (positive integer) df
% v, rather it will compute for any df v>0.
%
% Variate relationships: (Evans et al., Ch37 & 7)
%--------------------------------------------------------------------------
% The Student's t distribution with 1 degree of freedom is the Standard
% Cauchy distribution, which has a simple closed form CDF.
%
% Algorithm:
%--------------------------------------------------------------------------
% The CDF of the Student's t-distribution with v degrees of freedom
% is related to the incomplete beta function by:
%       Pr(|X|<x) = betainc(v/(v+x^2),v/2,1/2)
% so
%              {     betainc(v/(v+x^2),v/2,1/2) / 2      for x<0
%       F(x) = |   0.5                                   for x=0
%              { 1 - betainc(v/(v+x^2),v/2,1/2) / 2      for x>0
%
% See Abramowitz & Stegun, 26.5.27 & 26.7.1; Press et al., Sec6.4 for
% definitions of the incomplete beta function. The relationship is
% easily verified by substituting for v/(v+x^2) in the integral of the
% incomplete beta function.
%
% MATLAB's implementation of the incomplete beta function is used.
%
%
% References:
%--------------------------------------------------------------------------
% Evans M, Hastings N, Peacock B (1993)
%       "Statistical Distributions"
%        2nd Ed. Wiley, New York
%
% Abramowitz M, Stegun IA, (1964)
%       "Handbook of Mathematical Functions"
%        US Government Printing Office
%
% Press WH, Teukolsky SA, Vetterling AT, Flannery BP (1992)
%       "Numerical Recipes in C"
%        Cambridge
%
%__________________________________________________________________________
% Copyright (C) 1992-2011 Wellcome Trust Centre for Neuroimaging

% Andrew Holmes
% $Id: spm_Tcdf.m 4182 2011-02-01 12:29:09Z guillaume $

%-Format arguments, note & check sizes
%--------------------------------------------------------------------------
if nargin<2, error('Insufficient arguments'), end

ad = [ndims(x);ndims(v)];
rd = max(ad);
as = [[size(x),ones(1,rd-ad(1))];...
      [size(v),ones(1,rd-ad(2))]];
rs = max(as);
xa = prod(as,2)>1;
if all(xa) && any(diff(as(xa,:)))
    error('non-scalar args must match in size');
end

%-Computation
%--------------------------------------------------------------------------
%-Initialise result to zeros
F = zeros(rs);

%-Only defined for strictly positive v. Return NaN if undefined.
md = ( ones(size(x))  &  v>0 );
if any(~md(:))
    F(~md) = NaN;
    warning('Returning NaN for out of range arguments');
end

%-Special case: f is 0.5 when x=0 (where betainc involves log of zero)
F( md  &  x==0 ) = 0.5;

%-Special case: Standard Cauchy distribution when v=1
ml = ( md  &  v==1 ); if xa(1), mlx=ml; else mlx=1; end
F(ml) = 0.5 + atan(x(mlx))/pi;

%-Compute where defined & not special cases
Q  = find( md  &  x~=0  &  v~=1 );
if isempty(Q), return, end
if xa(1), Qx=Q; else Qx=1; end
if xa(2), Qv=Q; else Qv=1; end

%-Compute
xQxPos = x(Qx)>0;
F(Q) = xQxPos -(xQxPos*2-1).*0.5.*betainc(v(Qv)./(v(Qv)+x(Qx).^2),v(Qv)/2,1/2);


function pass = test_jacobian( )
% Test jacobian

tol = 1e2*chebfunpref().cheb2Prefs.chebfun2eps;

% Check jacobian of an empty diskfunv is an empty diskfun.
u = diskfunv;
f = jacobian(u);
pass(1) = isempty(f) & isa(f,'diskfun');

% Check definition:
F = diskfunv(@(x,y) cos(pi*x.*y), @(x,y) sin(pi*y).*cos(pi*x));
Fx = diffx(F);
Fy = diffy(F);
pass(2) = ( norm(jacobian(F) - (Fx(1).*Fy(2)-Fy(1).*Fx(2)) ) < tol );

%check against true value
true = diskfun(@(x,y)-pi^2*y.*sin(pi*x.*y).*cos(pi*y).*cos(pi.*x)-...
    pi^2*x.*sin(pi*x.*y).*sin(pi*y).*sin(pi*x));
pass(3) = ( norm(jacobian(F)-true) < 4*tol ) ;

end


function [dir,x0,y0] = boundarydir(x,y,orderout)
%BOUNDARYDIR Determine the direction of a sequence of planar points.
%   DIR = BOUNDARYDIR(X,Y) determines the direction of travel of a
%   closed, nonintersecting sequence of planar points with coordinates
%   contained in column vectors X and Y. Values of DIR are 'cw'
%   (clockwise) and 'ccw' (counterclockwise). The direction of travel is
%   with respect to the image coordinate system defined in Chapter 2 of
%   the book.
%
%   [DIR,X0,Y0] = BOUNDARYDIR(X,Y,ORDEROUT) determines the direction DIR
%   of the input sequence, and also outputs the sequence with its
%   direction of travel as specified in ORDEROUT. Valid values of this
%   parameter as 'cw' and 'ccw'. The coordinates of the output sequence
%   are column vectors X0 and Y0.
%
%   The input sequence is assumed to be nonintersecting, and it cannot
%   have duplicate points, with the exception of the first and last
%   points possibly being the same, a condition often resulting from
%   boundary-following functions, such as bwboundaries.
%
%   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.
% Make sure coordinates are column vectors.
x = x(:);
y = y(:);

% If the first and last points are the same, delete the last point.
% The point will be restored later.
restore = false;
if x(1) == x(end) && y(1) == y(end)
   x = x(1:end-1);
   y = y(1:end-1);
   restore = true;
end
% Check for duplicate points.
if length([x y]) ~= length(unique([x y],'rows'))
   error('No duplicate points except first and last are allowed.')
end

% The topmost, leftmost point in the sequence is always a convex
% vertex.
x0 = x;
y0 = y;
cx = find(x0 == min(x0));
cy = find(y0 == min(y0(cx)));
x1 = x0(cx(1));
y1 = y0(cy(1));
% Scroll data so that the first point in the sequence is (x1, y1),
% the guaranteed convex point.
I = find(x0 == x1 & y0 == y1);
x0 = circshift(x0, [-(I - 1), 0]);
y0 = circshift(y0, [-(I - 1), 0]);

% Form the matrix needed to check for travel direction. Only three
% points are needed: (x1, y1), the point before it, and the point
% after it.
A = [x0(end) y0(end) 1; x0(1) y0(1) 1; x0(2) y0(2) 1];
dir = 'cw';
if det(A) > 0
   dir = 'ccw';
end

% Prepare outputs.
if nargin == 3
   x0 = x; % Reuse x0 and y0.
   y0 = y;
   if ~strcmp(dir,orderout)
      x0(2:end) = flipud(x0(2:end)); % Reverse order of travel.
      y0(2:end) = flipud(y0(2:end));
   end
   if restore
      x0(end + 1) = x0(1);
      y0(end + 1) = y0(1);
   end
end


function [res, diams] = imOrientedGranulo(img, angleList, granuloType, strelSizes, varargin)
% Gray level granulometry mean size for various orientations.
%
%   Compute typical size of bright or dark structures within images by
%   performing gray-level granulometries with horizontal structuring
%   elements and for various rotations of the input image.
%   The function "imDirectionalGranulo" is an alternative approach based on
%   line structuring elements with various orientations.
%
%   Usage
%   RES = imOrientedGranulo(IMG, ANGLES, TYPE, SIZES)
%   IMG should be a 2D image (binary, grayscale or color)
%   ANGLES is the list of angles to consider, in degrees, as a 1-by-N row
%     vector
%   GRTYPE can be one of {'opening', 'closing', 'erosion', 'dilation'}.
%   SIZES are given as radius in pixels. Diameters of strels are obtained
%     as 2*R+1.
%   The result GR is a 1-by-N array with as many columns as the number of
%     elements provided in ANGLES array.
%
%   [RES, DIAMS] = imOrientedGranulo(...)
%   Also returns the diameters used for each morphological filtering step.
%
%
%   Example
%   imOrientedGranulo
%
%   See also
%     imGranulometry, imDirectionalGranulo, imGranulo, imGranuloByRegion,
%     granuloMeanSize
%
%   Reference
%   The methodology is described in the following article:
%   "Exploring the microstructure of natural fibre composites by confocal
%   Raman imaging and image analysis", by Antoine Gallos, Gabriel Paës,
%   David Legland, Florent Allais, Johnny Beaugrand (2017).
%   Composites Part A: Applied Science and Manufacturing 94, p. 32-40.
%   doi: https://doi.org/10.1016/j.compositesa.2016.12.005
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2015-12-02,    using Matlab 8.6.0.267246 (R2015b)
% Copyright 2015 INRAE - Cepia Software Platform.

% how to interpolate images
if islogical(img)
    interp = 'nearest';
else
    interp = 'linear';
end

% convert sizes from radius to diameter
diams = 2 * strelSizes + 1;

% allocate result array
nAngles = length(angleList);
res = zeros(1, nAngles);

% iterate over the orientations
for iAngle = 1:nAngles
    angle = angleList(iAngle);
    imgr = imrotate(img, angle, interp);
    grCurve = imGranulo(imgr, granuloType, 'lineh', strelSizes);
    res(iAngle) = granuloMeanSize(grCurve, diams);
end


function [VELperm, VELsystemC, VELsystemCT, rhsbcUx, rhsbcUy] = ...
      CurvedINSViscousSetUp2D(dt, nu, g0, BCfunction)

% function [VELperm, VELsystemC, VELsystemCT, rhsbcUx, rhsbcUy] = ...
%      CurvedINSViscousSetUp2D(dt, nu, g0, BCfunction)
% Purpose: build velocity system and boundary forcing terms

Globals2D;

% choose order to integrate exactly
Nint = ceil(3*N/2);

% build cubature nodes for all elements
CubatureOrder = 2*(Nint+1); cub = CubatureVolumeMesh2D(CubatureOrder);

% build Gauss node data for all element faces
NGauss = (Nint+1); gauss = GaussFaceMesh2D(NGauss);

% Convert boundary conditions to Dirichlet & Neumann
saveBCType = BCType;
ids = find(saveBCType==In | saveBCType==Wall | saveBCType==Cyl);  BCType(ids) = Dirichlet;
ids = find(saveBCType==Out);                    BCType(ids) = Neuman;

% Form inhomogeneous boundary term for rhs data (assumes time independent forcing)
[bcUx,bcUy,bcPR] = feval(BCfunction, gauss.x, gauss.y, gauss.nx, gauss.ny, gauss.mapI, gauss.mapO, gauss.mapW, gauss.mapC, 0, nu);

% Build pressure boundary condition forcing vector
[VELsystemBC] = CurvedPoissonIPDGbc2D();
rhsbcUx = VELsystemBC*bcUx(:); rhsbcUy = VELsystemBC*bcUy(:);

% Build velocity system
[VELsystem, mm] = CurvedPoissonIPDG2D();
VELsystem = g0*mm/(dt*nu) + VELsystem;

% Restore original boundary types
BCType = saveBCType;

% Find Reverse-Cuthill-Mckee ordering to reduce bandwith of velocity system
VELperm    = symrcm(VELsystem);

% Apply row and column permutation to velocity matrix
VELsystem  = VELsystem(VELperm,VELperm);

% Compute Cholesky factorization of velocity matrix
VELsystemC  = chol(VELsystem); VELsystemCT = transpose(VELsystemC);
return


%  Figure 10.54      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
% fig10_54.m is a script to generate Fig. 10.54, the step response
% for the fuel/air ratio with a nonlinear sensor.  A (3,3)
% Pade approximant is used to approximate the delay.
clf;
% construct the F/A dynamics with the sensor time-constant too
f =[-50     0     0;
     0    -1     0;
    10    10   -10];

g =[25.0000;
    0.5000;
         0];
h =[0     0     1];
j=0;
[f3,g3,h3,j3 ]=pade(.2,3); %  the delay Pade model
% put the plant and the delay in series
[fp,gp,hp,jp]=series(f,g,h,j,f3,g3,h3,j3);
% construct the controller
nc=.1*[1 .3];
dc=[1 0]; % the PI controller in polynomial form
[fc,gc,hc,jc]= tf2ss(nc, dc); % put the controller in state space form
[fol,gol,hol,jol] = series(fc,gc,hc,jc,fp,gp,hp,jp);
% get a discrete model for Ts = .01
[phi,gam] = c2d(fol,gol,.01);
%
% form the closed-loop difference equation
x=[0 0 0 0 0 0 0]';
yout=[];

for t=0:.01:20;
 y=hol*x ;
 e=.4*sat(150*(.068-y));
 x=phi*x+gam*e;
 yout=[yout [e;y]];
end

t=0:.01:20;
subplot(211);
plot(t,yout(1,:));
axis( [0 20 -.6 .6]);
xlabel('Time (sec)');
ylabel('e');
title('Fig. 10.54 (b) Error plot for nonlinear control of F/A')
grid on;

subplot(212);
plot(t,yout(2,:));
axis([0 20 0 .09]);
xlabel('Time (sec)');
ylabel('F/A');
title('Fig. 10. 54 (c) Output F/A ratio for nonlinear control')
hold off;
%grid
nicegrid


function W = compute_mesh_weight(vertices,faces,type,options)

% compute_mesh_weight - compute a weight matrix
%
%   W = compute_mesh_weight(vertices,faces,type,options);
%
%   W is sparse weight matrix and W(i,j)=0 is vertex i and vertex j are not
%   connected in the mesh.
%
%   todo:
%       1. validate spring weights
%       2. add Mean_curvature weights
%   type is either
%       'combinatorial': W(i,j)=1 is vertex i is conntected to vertex j.
%       'distance': W(i,j) = 1/d_ij^2 where d_ij is distance between vertex
%           i and j.
%       'spring': W(i,j) = 1/d_ij where d_ij is distance between vertex
%           i and j.
%       'conformal' or 'dcp': W(i,j) = cot(alpha_ij)+cot(beta_ij) where alpha_ij and
%           beta_ij are the adjacent angle to edge (i,j). Refer to Skeleton
%           Extraction by Mesh Extraction_08, and Intrinsic Parameterizations of Surface Meshes_02.
%       'Laplace-Beltrami': W(i,j) = (cot(alpha_ij)+cot(beta_ij))/2 where alpha_ij and
%           beta_ij are the adjacent angle to edge (i,j). Refer to Computing discrete minimal surfaces
%           andtheir conjugates_93, Lemma 2 of On the convergence of metric and geometric properties of
%           polyhedral surfaces_06, and Characterizing Shape Using Conformal Factors_08, and ,
%       'Mean_curvature',W(i,j) = (1/area_i)*(cot(alpha_ij)+cot(beta_ij))/2 where alpha_ij and
%           beta_ij are the adjacent angle to edge (i,j), area_i is the
%           area of vertex i's Voroni vicinity. Refer to ??
%       'mvc': W(i,j) = [tan(/_kij/2)+tan(/_jil/2)]/d_ij where /_kij and /_jil
%           are angles at i
%
%   If options.ring is offered, the computation of it can be avoided.
%
%   Add spring and mvc weight (JJCAO, 2009)
%   Add options.ring (JJCAO, 2009)
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
[vertices,faces] = check_face_vertex(vertices,faces);

n = max(max(faces));

if isfield(options, 'verb')
    verb = options.verb;
else
    verb = n>5000;
end

if nargin<3
    type = 'dcp';
end

switch lower(type)
    case 'combinatorial'
        W = triangulation2adjacency(faces);
    case 'distance'
        W = my_euclidean_distance_2(triangulation2adjacency(faces),vertices);
        W(W>0) = 1./W(W>0);
    case 'spring'
        W = sqrt(my_euclidean_distance(triangulation2adjacency(faces),vertices));
        W(W>0) = 1./W(W>0);
    case {'conformal','dcp'} % conformal laplacian
        W = compute_mesh_weight_dcp(vertices, faces, verb);
    case 'laplace-beltrami' %
        W = compute_mesh_weight_dcp(vertices, faces, verb)*0.5;
    case 'mvc'% mvc laplacian
        if isfield(options, 'rings')
            rings = options.rings;
        else
            rings = compute_vertex_face_ring(faces);
        end
        W = sparse(n,n);
        for i = 1:n
            if verb
                progressbar(i,n);
            end
            for b = rings{i}
                % b is a face adjacent to a
                bf = faces(:,b);
                % compute complementary vertices
                if bf(1)==i
                    v = bf(2:3);
                elseif bf(2)==i
                    v = bf([1 3]);
                elseif bf(3)==i
                    v = bf(1:2);
                else
                    error('Problem in face ring.');
                end
                j = v(1); k = v(2);
                vi = vertices(:,i);
                vj = vertices(:,j);
                vk = vertices(:,k);
                % angles
                alpha = myangle(vi-vk,vi-vj);
                % add weight
                W(i,j) = W(i,j) + tan( 0.5*alpha )/sqrt(sum((vi-vj).^2));
                W(i,k) = W(i,k) + tan( 0.5*alpha )/sqrt(sum((vi-vk).^2));
            end
        end
    otherwise
        error('Unknown type.')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function W = compute_mesh_weight_dcp(verts, faces, verb)
n = size(verts,2);
W = sparse(n,n);
% new implementation
for i = 1:size(faces,2)
    c = faces(:,i);

    v1 = verts(:,c(1));
    v2 = verts(:,c(2));
    v3 = verts(:,c(3));

    cot1 = dot(v2-v1,v3-v1)/norm(cross(v2-v1,v3-v1));
    cot2 = dot(v3-v2,v1-v2)/norm(cross(v3-v2,v1-v2));
    cot3 = dot(v1-v3,v2-v3)/norm(cross(v1-v3,v2-v3));

    W(c(2),c(3)) = W(c(2),c(3)) + cot1;
    W(c(3),c(2)) = W(c(3),c(2)) + cot1;
    W(c(3),c(1)) = W(c(3),c(1)) + cot2;
    W(c(1),c(3)) = W(c(1),c(3)) + cot2;
    W(c(1),c(2)) = W(c(1),c(2)) + cot3;
    W(c(2),c(1)) = W(c(2),c(1)) + cot3;
end

%%
% old implementation
% for i = 1:n
%     if verb
%         progressbar(i,n);
%     end
%     for b = rings{i}
%         % b is a face adjacent to a
%         bf = faces(:,b);
%         % compute complementary vertices
%         if bf(1)==i
%             v = bf(2:3);
%         elseif bf(2)==i
%             v = bf([1 3]);
%         elseif bf(3)==i
%             v = bf(1:2);
%         else
%             error('Problem in face ring.');
%         end
%         j = v(1); k = v(2);
%         vi = verts(:,i);
%         vj = verts(:,j);
%         vk = verts(:,k);
%
%         u = vk-vi; v = vk-vj;
%         W(i,j) = W(i,j) + dot(u,v)/norm(cross(u,v));
%         u = vj-vi; v = vj-vk;
%         W(i,k) = W(i,k) + dot(u,v)/norm(cross(u,v));
%     end
% end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function beta = myangle(u,v)
du = sqrt( sum(u.^2) );
dv = sqrt( sum(v.^2) );
du = max(du,eps); dv = max(dv,eps);
beta = acos( sum(u.*v) / (du*dv) );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function W = my_euclidean_distance_2(A,vertex)
% square euclidean distance
if size(vertex,1)<size(vertex,2)
    vertex = vertex';
end

[i,j,s] = find(sparse(A));
d = sum( (vertex(i,:) - vertex(j,:)).^2, 2);
W = sparse(i,j,d);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function W = my_euclidean_distance(A,vertex)
% euclidean distance
if size(vertex,1)<size(vertex,2)
    vertex = vertex';
end

[i,j,s] = find(sparse(A));
d = sum( (vertex(i,:) - vertex(j,:)).^2, 2).^0.5;
W = sparse(i,j,d);


function [g,f] = octspec(B,A,Fs,Fc,s,n);
% OCTSPEC Plots an octave filter characteristics.
%    OCTSPEC(B,A,Fs,Fc) plots the attenuation of the filter defined by
%    B and A at sampling frequency Fs. Fc is the center frequency of
%    the octave filter. The plot covers one decade on both sides of Fc.
%
%    OCTSPEC(B,A,Fs,Fc,'ANSI',N) superposes the ANSI Order-N analog
%    specification for comparison. Default is N = 3.
%
%    OCTSPEC(B,A,Fs,Fc,'IEC',N) superposes the characteristics of the
%    IEC 61260 class N specification for comparison. Default is N = 1.
%
%    [G,F] = OCTSPEC(B,A,Fs,Fc) returns two 512-point vectors with
%    the gain (in dB) in G and logarithmically spaced frequencies in F.
%    The plot can then be obtained by SEMILOGX(F,G)
%
%    See also OCTDSGN, OCT3SPEC, OCT3DSGN.

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

% References:
%    [1] ANSI S1.1-1986 (ASA 65-1986): Specifications for
%        Octave-Band and Fractional-Octave-Band Analog and
%        Digital Filters, 1993.
%    [2] IEC 61260 (1995-08):  Electroacoustics -- Octave-Band and
%        Fractional-Octave-Band Filters, 1995.

if (nargin < 4) | (nargin > 6)
  error('Invalid number of input arguments.');
end

ansi = 0;
iec = 0;
if nargin > 4
  if strcmp(lower(s),'ansi')
    ansi = 1;
    if nargin == 5
      n = 3;
    end
  elseif strcmp(lower(s),'cei') | strcmp(lower(s),'iec')
    iec = 1;
    if nargin == 5
      n = 1
    end
    if (n < 0) | (n > 3)
      error('IEC class must be 0, 1, or 2');
    end
  end
end

N = 512;
pi = 3.14159265358979;
F = logspace(log10(Fc/10),log10(min(Fc*10,Fs/2)),N);
H = freqz(B,A,2*pi*F/Fs);
G = 20*log10(abs(H));

% Set output variables.
if nargout ~= 0
  g = G; f = F;
  return
end

% Generate the plot
if (ansi) 				% ANSI Order-n specification
  f = logspace(log10(Fc/10),log10(Fc*10),N);
  f1 = Fc/sqrt(2);
  f2 = Fc*sqrt(2);
  Qr = Fc/(f2-f1);
  Qd = (pi/2/n)/(sin(pi/2/n))*Qr;
  Af = 10*log10(1+Qd^(2*n)*((f/Fc)-(Fc./f)).^(2*n));
  semilogx(F,G,f,-Af,'--');
  legend('Filter',['ANSI order-' int2str(n)],0);
elseif (iec) 					% CEI specification
  semilogx(F,G);
  hold on
  if n == 0
    tolup =  [ .15 .15 .15 .15 .15 -2.3 -18.0 -42.5 -62 -75 -75 ];
    tollow = [ -.15 -.2 -.4 -1.1 -4.5 -realmax -inf -inf -inf -inf -inf ];
  elseif n == 1
    tolup =  [ .3 .3 .3 .3 .3 -2 -17.5 -42 -61 -70 -70 ];
    tollow = [ -.3 -.4 -.6 -1.3 -5 -realmax -inf -inf -inf -inf -inf ];
  elseif n == 2
    tolup =  [ .5 .5 .5 .5 .5 -1.6 -16.5 -41 -55 -60 -60  ];
    tollow = [ -.5 -.6 -.8 -1.6 -5.5 -realmax -inf -inf -inf -inf -inf ];
  end
  U = 2;
  f = Fc * U.^[ 0 1/8 1/4 3/8 1/2 1/2 1 2 3 4 NaN ];
  ff = Fc * U.^[ 0 -1/8 -1/4 -3/8 -1/2 -1/2 -1 -2 -3 -4 NaN ];
  f(length(f)) = realmax;
  ff(length(ff)) = realmin;
  semilogx(F,G,f,tolup,'--');
  semilogx(F,G,f,tollow,'--');
  semilogx(F,G,ff,tolup,'--');
  semilogx(F,G,ff,tollow,'--');
  hold off
  legend('Filter',['IEC class ' int2str(n)],0);
else
  semilogx(F,G);
end
xlabel('Frequency [Hz]'); ylabel('Gain [dB]');
title(['Octave filter: Fc =',int2str(Fc),' Hz, Fs = ',int2str(Fs),' Hz']);
axis([Fc/10 Fc*10 -80 5]);
grid on


function daub10_scale_plot ( n )

%*****************************************************************************80
%
%% DAUB10_SCALE_PLOT plots the DAUB10 scaling function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the recursion level.
%
  x = linspace ( 0.0, 10.0, 801 );

  y = daub10_scale ( n, x );

  plot ( x, y, 'LineWidth', 2 );

  grid on
  xlabel ( '<---X--->' );
  ylabel ( '<---Y--->' );
  title ( sprintf ( 'DAUB10 Scale Function, Recursion level n = %d', n ) );

  return
end


function x = r8sto_yw_sl ( n, b, x )

%*****************************************************************************80
%
%% R8STO_YW_SL solves the Yule-Walker equations for a R8STO matrix.
%
%  Discussion:
%
%    The R8STO storage format is used for a symmetric Toeplitz matrix.
%    It stores the N elements of the first row.
%
%    The matrix is also required to be positive definite.
%
%    This implementation of the algorithm assumes that the diagonal element
%    is 1.
%
%    The real symmetric Toeplitz matrix can be described by N numbers, which,
%    for convenience, we will label B(0:N-1).  We assume there is one more
%    number, B(N).  If we let A be the symmetric Toeplitz matrix whose first
%    row is B(0:N-1), then the Yule-Walker equations are:
%
%      A * X = -B(1:N)
%
%  Example:
%
%    To solve
%
%     1.0 0.5 0.2    x1   0.5
%     0.5 1.0 0.5 *  x2 = 0.2
%     0.2 0.5 1.0    x3   0.1
%
%    we input:
%
%      N = 3
%      B(1:3) = (/ 0.5, 0.2, 0.1 /)
%
%    with output:
%
%      X(1:3) = (/ -75, 12, -5 /) / 140
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gene Golub and Charles Van Loan,
%    Section 4.7.2, "Solving the Yule-Walker Equations",
%    Matrix Computations,
%    Third Edition,
%    Johns Hopkins, 1996.
%
%  Parameters:
%
%    Input, integer N, the order of the system.
%
%    Input, real B(N), defines the linear system.  The first entry of A is
%    a 1, followed by B(1) through B(N-1).  The right hand side of the
%    system is -B(1:N).
%
%    Output, real X(N), the solution of the linear system.
%
  x(1) = -b(1);
  beta = 1.0E+00;
  alpha = -b(1);

  for i = 1 : n-1
    beta = ( 1.0E+00 - alpha * alpha ) * beta;
    alpha = - ( b(i+1) + b(i:-1:1) * x(1:i)' ) / beta;
    x(1:i) = x(1:i) + alpha * x(i:-1:1);
    x(i+1) = alpha;
  end

  return
end


function kpu_sparse_test ( )

%*****************************************************************************80
%
%% KPU_SPARSE_TEST uses the KPU function to build a sparse grid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 April 2012
%
%  Author:
%
%    Original MATLAB version by Florian Heiss, Viktor Winschel.
%    This MATLAB version by John Burkardt.
%
%  Local parameters:
%
%    Local, integer D, the spatial dimension.
%
%    Local, integer MAXK, the maximum level to check.
%
  d = 10;
  maxk = 4;
  func = 'prod( exp(-(x/2).^2/2)/2/sqrt(2*pi), 2)';
  trueval = fu_integral ( d );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'KPU_SPARSE_TEST:\n' );
  fprintf ( 1, '  KPU sparse grid:\n' );
  fprintf ( 1, '  Sparse nested, unweighted quadrature over [0,1].\n' );
  fprintf ( 1, '  Exact integral is %g\n', trueval );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   D  Level   Nodes    SG error    MC error\n' );
  fprintf ( 1, '\n' );

  for k = 2 : maxk
%
%  Compute sparse grid estimate.
%
    [ x, w ] = nwspgr ( 'kpu', d, k );
    fx = eval ( func );
    SGappr = w' * fx;
    SGerror = sqrt((SGappr - trueval).^2)/trueval;
%
%  Average 1000 Monte Carlo estimates.
%
    numnodes = length ( w );
    sim = zeros(1000,1);
    for r = 1 : 1000
      x = rand ( numnodes, d );
      fx = eval ( func );
      sim(r) = mean ( fx );
    end
    simerror = sqrt ( mean( ( sim - trueval).^2) ) / trueval;

    fprintf( '  %2d     %2d  %6d  %10.5g  %10.5g\n', ...
      d, k, numnodes, SGerror, simerror )

  end

  return
end


%This Matlab script can be used to reproduce Figure 1.18 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.01 (Last edited: 2019-04-17)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Define the SNR
SNR = 1;

%Define betabar (strength of inter-cell interference)
betabar = 1e-1;

%Define the range of number of UEs
K = 1:20;

%Define range of antenna-UE ratios
c = [1 2 4 8];

%Extract the maximal number of UEs and BS antennas
Kmax = max(K);
Mmax = Kmax*max(c);

%Select number of Monte Carlo realizations for the line-of-sight (LoS)
%angles and of the non-line-of-sight (NLoS) Rayleigh fading
numberOfRealizations = 10000;

%Generate NLoS channels using uncorrelated Rayleigh fading
H_NLoS_desired = sqrt(1/2)*(randn(Mmax,Kmax,numberOfRealizations)+1i*randn(Mmax,Kmax,numberOfRealizations));
H_NLoS_interfering = sqrt(betabar/2)*(randn(Mmax,Kmax,numberOfRealizations)+1i*randn(Mmax,Kmax,numberOfRealizations));

%Preallocate matrices for storing the simulation results
SE_MMSE_NLoS_montecarlo = zeros(length(K),length(c));
SE_MMSE_NLoS_nonlinear = zeros(length(K),length(c));

%% Go through all Monte Carlo realizations
for n = 1:numberOfRealizations

    %Output simulation progress
    disp([num2str(n) ' realizations out of ' num2str(numberOfRealizations)]);

    %Go through the range of number of UEs
    for kindex = 1:length(K)

        %Go through the range of antenna-UE ratios
        for cindex = 1:length(c)

            %Compute the number of antennas
            M = K(kindex)*c(cindex);

            %Compute the SE with non-linear processing under NLoS propagation
            %for one realization of the Rayleigh fading. We use the classic
            %log-det formula for the uplink sum SE, when treating the
            %inter-cell interference as colored noise
            SE_MMSE_NLoS_nonlinear(kindex,cindex) = SE_MMSE_NLoS_nonlinear(kindex,cindex) + real(log2(det(eye(M) + SNR* H_NLoS_desired(1:M,1:K(kindex),n)*H_NLoS_desired(1:M,1:K(kindex),n)' +  SNR*H_NLoS_interfering(1:M,1:K(kindex),n)*H_NLoS_interfering(1:M,1:K(kindex),n)' )) - log2(det(eye(M)+SNR*H_NLoS_interfering(1:M,1:K(kindex),n)*H_NLoS_interfering(1:M,1:K(kindex),n)')))/numberOfRealizations;

            %Compute the SE with M-MMSE under NLoS propagation for one
            %realization of the Rayleigh fading

            %Compute the M-MMSE combining vectors
            MMMSEfilter = ( SNR* H_NLoS_desired(1:M,1:K(kindex),n)*H_NLoS_desired(1:M,1:K(kindex),n)' + SNR* H_NLoS_interfering(1:M,1:K(kindex),n)*H_NLoS_interfering(1:M,1:K(kindex),n)' + eye(M) ) \ (SNR*H_NLoS_desired(1:M,1:K(kindex),n));

            %Compute the intra-cell channel powers after M-MMSE combining
            channelgainsIntracell = abs(MMMSEfilter'*H_NLoS_desired(1:M,1:K(kindex),n)).^2;

            %Extract the desired signal power for each UE
            signalpowers = diag(channelgainsIntracell);

            %Extract and compute interference powers for each UE
            interferencepowers = sum(channelgainsIntracell,2) - signalpowers + sum(abs(MMMSEfilter'*H_NLoS_interfering(1:M,1:K(kindex),n)).^2,2);

            %Compute the effective 1/SNR after noise amplification
            scalednoisepower = (1/SNR)*sum(abs(MMMSEfilter').^2,2);

            %Compute the uplink SE with M-MMSE combining
            SE_MMSE_NLoS_montecarlo(kindex,cindex) = SE_MMSE_NLoS_montecarlo(kindex,cindex) + sum(log2(1 + signalpowers./(interferencepowers+scalednoisepower)))/numberOfRealizations;

        end

    end

end

%% Plot the simulation results
figure(1);
hold on; box on;

plot(K,SE_MMSE_NLoS_montecarlo(:,4)./SE_MMSE_NLoS_nonlinear(:,4),'r-','LineWidth',1);
plot(K,SE_MMSE_NLoS_montecarlo(:,3)./SE_MMSE_NLoS_nonlinear(:,3),'k-.','LineWidth',1);
plot(K,SE_MMSE_NLoS_montecarlo(:,2)./SE_MMSE_NLoS_nonlinear(:,2),'b--','LineWidth',1);
plot(K,SE_MMSE_NLoS_montecarlo(:,1)./SE_MMSE_NLoS_nonlinear(:,1),'k:','LineWidth',1);

xlabel('Number of UEs (K)');
ylabel('Fraction of non-linear performance');

legend('M/K=8','M/K=4','M/K=2','M/K=1','Location','SouthWest');
ylim([0.5 1]);


function value = r8_epsilon ( )

%*****************************************************************************80
%
%% R8_EPSILON returns the R8 roundoff unit.
%
%  Discussion:
%
%    The roundoff unit is a number R which is a power of 2 with the
%    property that, to the precision of the computer's arithmetic,
%      1 < 1 + R
%    but
%      1 = ( 1 + R / 2 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VALUE, the roundoff unit.
%
  value = eps;

  return
end