% Modelling Sensor Directivity in 3D Example
%
% This example demonstrates how the sensitivity of a large single element
% detector varies with the angular position of a point-like source. It is a
% 3D version of the Modelling Sensor Directivity in 2D example.
%
% author: Ben Cox
% date: 29th October 2010
% last update: 23rd February 2012
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

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

clear all;

% =========================================================================
% SIMULATION
% =========================================================================

% create the computational grid
Nx = 64;            % number of grid points in the x direction
Ny = 64;            % number of grid points in the y direction
Nz = 64;            % number of grid points in the z direction
dx = 100e-3/Nx;     % grid point spacing in the x direction [m]
dy = dx;            % grid point spacing in the y direction [m]
dz = dx;            % grid point spacing in the z direction [m]
kgrid = makeGrid(Nx, dx, Ny, dy, Nz, dz);

% define the properties of the propagation medium
medium.sound_speed = 1500;	% [m/s]

% create the time array
[kgrid.t_array, dt] = makeTime(kgrid, medium.sound_speed);
Nt = length(kgrid.t_array);

% define a large area detector
sensor.mask = zeros(Nx, Ny, Nz);
sensor.mask(Nx/2+1, (Ny/2-9):(Ny/2+11),(Nz/2-9):(Nz/2+11)) = 1;

% define equally spaced point sources lying on a circle centred at the
% centre of the detector face
Nangles = 11;
circle = makeCartCircle(25*dx, Nangles, [0,0], pi);
circle = [circle; zeros(1,Nangles)];

% find the binary sensor mask most closely corresponding to the cartesian
% points coordinates from makeCartCircle
circle3D = cart2grid(kgrid,circle);

% find the indices of the sources in the binary source mask
source_positions = find(circle3D == 1);

% define a time varying sinusoidal source
source_freq = 0.25e6;
source_mag = 1;
source.p = source_mag*sin(2*pi*source_freq*kgrid.t_array);

% filter the source to remove high frequencies not supported by the grid
source.p = filterTimeSeries(kgrid, medium, source.p);

% pre-allocate array for storing the output time series
single_element_data = zeros(Nt,length(source_positions));

% run a simulation for each of these sources to see the effect that the
% angle from the detector has on the measured signal
for source_loop = 1:length(source_positions)

    % select a point source
    source.p_mask = zeros(Nx,Ny,Nz);
    source.p_mask(source_positions(source_loop)) = 1;

    % create a display mask to display the transducer
    display_mask = source.p_mask + sensor.mask;

    % run the simulation
    input_args = {'PMLSize', 10, 'DisplayMask', display_mask, 'PlotScale', [-0.2 0.2], 'PlotFreq', 50,  'DataCast', 'single'};
    sensor_data = kspaceFirstOrder3D(kgrid, medium, source, sensor, input_args{:});

    % average the data recorded for each grid point to simulate the
    % measured signal from a large aperture, single element, detector
    single_element_data(:,source_loop) = sum(sum(sensor_data,1),1);

end

% =========================================================================
% VISUALISATION
% =========================================================================

% plot source points and sensor mask
voxelPlot(circle3D + sensor.mask);
view([-34 34])

% plot the time series recorded for each of the sources
figure;
plot(kgrid.t_array, single_element_data);
colormap(getColorMap);
xlabel('time [s]');
ylabel('pressure');
title('time series from each direction');

% calculate angle between source and centre of detector face
angles = atan((kgrid.y(source_positions))./kgrid.x(source_positions));

% plot the maximum amplitudes for each of the sources, showing that the
% detector sensitivity falls off at low angles as expected.
figure;
plot(angles,max(single_element_data),'o')
colormap(getColorMap);
xlabel('angle between source and centre of detector face');
ylabel('maximum detected pressure from each direction')


function adj = meshAdjacencyMatrix(faces, varargin)
%MESHADJACENCYMATRIX Compute adjacency matrix of a mesh from set of faces
%
%   ADJMAT = meshAdjacencyMatrix(FACES)
%   Returns a sparse NV-by-NV matrix (NV being the maximum vertex index)
%   containing vertex adjacency of the mesh represented by FACES.
%   FACES is either a NF-by-3, a NF-by-4 index array, or a Nf-by-1 cell
%   array.
%
%   Example
%     [v f] = createCube;
%     adj = meshAdjacencyMatrix(f);
%
%   See also
%     meshes3d, triangulateFaces, smoothMesh

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

% Ensures faces is a N-by-3 or N-by-4 array
if iscell(faces) || (isnumeric(faces) && size(faces, 2) > 4)
    faces = triangulateFaces(faces);
end

% forces faces to b efloating point array, for sparse function
if ~isfloat(faces)
    faces = double(faces);
end

% populate a sparse matrix
if size(faces, 2) == 3
    adj = sparse(...
        [faces(:,1); faces(:,1); faces(:,2); faces(:,2); faces(:,3); faces(:,3)], ...
        [faces(:,3); faces(:,2); faces(:,1); faces(:,3); faces(:,2); faces(:,1)], ...
        1.0);
elseif size(faces, 2) == 4
    adj = sparse(...
        [faces(:,1); faces(:,1); faces(:,2); faces(:,2); faces(:,3); faces(:,3); faces(:,4); faces(:,4)], ...
        [faces(:,4); faces(:,2); faces(:,1); faces(:,3); faces(:,2); faces(:,4); faces(:,3); faces(:,1)], ...
        1.0);
end

% remove double adjacencies
adj = min(adj, 1);


function lissajous ( )

%*****************************************************************************80
%
%% LISSAJOUS uses MATLAB to draw a closed planar curve.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LISSAJOUS:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Make a plane curve by connecting a series of points.\n' );

  xy = load ( 'lissajous.txt' );
%
%  Plot the data.
%
  plot ( xy(:,1), xy(:,2), 'LineWidth', 2, 'Color', 'm' );
  hold on
%
%  To avoid clutter, only plot every 10th point.
%
  plot ( xy(1:10:end,1), xy(1:10:end,2), 'k.' );
  grid on
  axis ( [ -1.2, +1.2, -1.2, +1.2 ] )
  axis equal
  xlabel ( '<--- X --->' );
  ylabel ( '<--- Y --->' );
  title ( 'Lissajous, x=sin(3t+pi/2), y=sin(4t)' );
  hold off

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LISSAJOUS:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end


 function [sinit, bodycoil] = ir_mri_sensemap_init(ykj, varargin)
%function [sinit, bodycoil] = ir_mri_sensemap_init(ykj, varargin)
%|
%| Form initial sensitivity map estimates from surface coils and bodycoil.
%|
%| in
%| ykj [(N) ncoil] surface coil data (2D or 3D)
%|
%| option
%| 'type' initialization method (def: '' = 'ratio')
%|		options: 'median', 'order1', 'order2', ...
%|		if numeric, then simply return as "sinit"
%| 'bodycoil' [(N)] body coil data (def: SSoS of ykj with phase of 1st coil)
%| 'thresh' fraction of max |bodycoil| that is "good" (def: 0.05)
%| 'mask' [(N)] optional mask to pick good values instead of thresholding
%| 'chat' 0|1 verbosity
%|
%| out
%| sinit [(N) ncoil] initial sensitivity map estimates
%| bodycoil [(N)] computed SSoS bodycoil image (if not provided)
%|
%| used in ir_mri_sensemap_admm - see that m-file for details.
%|
%| Michael Allison
%| 2015-08-10 Jeff Fessler revise args, add tests, support 3D

if nargin < 1, ir_usage(), end
if streq(ykj, 'test', 4), ir_mri_sensemap_init_test(ykj), return, end

arg.type = '';
arg.thresh = 0.05;
arg.mask = [];
arg.bodycoil = [];
arg.chat = false;
arg = vararg_pair(arg, varargin);

bodycoil = arg.bodycoil;
if isempty(bodycoil)
	if ndims(ykj) < 3, fail('ykj must be 3D or 4D to infer bodycoil'), end
	ndim = ndims(ykj) - 1; % 2 or 3
	bodycoil = sqrt(sum(abs(ykj).^2, ndim+1)); % sum-of-squares
        tmp = stackpick(ykj, 1); % ykj(:,...,:,1)
        tmp = angle(tmp); % crude estimate of phase of image f_j
        bodycoil = bodycoil .* exp(1i * tmp);
else
	ndim = ndims(bodycoil);
	dim = size(bodycoil);
end

if ~isempty(arg.type) && isnumeric(arg.type) % trivial case
	sinit = arg.type;
return
end

dim = size(bodycoil);
ncoil = numel(ykj) / prod(dim);
if round(ncoil) ~= ncoil, fail('size'), end

if ~isempty(arg.mask)
	jf_equal(size(arg.mask), dim)
	if arg.chat
		warn('using mask instead of threshold to determine good pixels')
	end
	good = mask;
end

if arg.chat
	tell = @(s) printm(s);
else
	tell = @(s) nop(s);
end

sinit = zeros([prod(dim) ncoil], 'single');
for ic = 1:ncoil
	% cannot use stackpick because of case of just one coil
	if ndim == 2
		zj = ykj(:,:,ic);
	elseif ndim == 3
		zj = ykj(:,:,:,ic);
	end

	tmp = div0(zj, bodycoil); % usual ratio

	% determine "good" pixels
	if isempty(arg.mask)
		good = abs(bodycoil) > arg.thresh * max(abs(bodycoil(:)));
	end

	switch arg.type
	case {'', 'ratio'}
		% set all uncertain map values to median of good ones
		tell('Using zero background.')
		tmp(~good) = 0;

	case 'median'
		% set all uncertain map values to median of good ones
		tell('Using median background.')
		tmp1 = median(abs(tmp(good)));
		tmp2 = median(angle(tmp(good))); % dubious
		tmp(~good) = tmp1 .* exp(1i*tmp2);

	case 'avg'
		% set all uncertain map values to mean of good ones
		tell('Using mean background.')
		tmp1 = mean(abs(tmp(good)));
		tmp2 = mean(angle(tmp(good))); % dubious
		tmp(~good) = tmp1 .* exp(1i*tmp2);

	case 'zeros'
		tmp = zeros(dim);

	case 'order1'
		tell('Using 1st-order fit for background.')
		expo = [0 0; 1 0; 0 1];
		tmp = ortho_init(dim, expo, zj, bodycoil, good);

	case 'order2'
		tell('Using 2nd order fit for background.')
		expo = [0 0; 1 0; 0 1; 1 1; 2 0; 0 2];
		tmp = ortho_init(dim, expo, zj, bodycoil, good);

	case 'order3'
		tell('Using 3rd order fit for background.')
		expo = [0 0; 1 0; 0 1; 1 1; 2 0; 2 1; 0 2; 1 2; ...
			3 0; 0 3];
		tmp = ortho_init(dim, expo, zj, bodycoil, good);

	case 'order4'
		tell('Using 4th order fit for background.')
		expo = [0 0; 1 0; 0 1; 1 1; 2 0; 2 1; 0 2; 1 2; ...
			3 0; 0 3; 2 2; 3 1; 1 3; 4 0; 0 4];
		tmp = ortho_init(dim, expo, zj, bodycoil, good);

	otherwise
		fail('Unknown initialization type "%s"', init)
	end

	sinit(:,ic) = single(tmp(:));
end % ic
sinit = reshape(sinit, [dim ncoil]);

end %

function nop(varargin)
end % nop

% ortho_init()
% initialize using orthogonal polynomial basis fit
function init = ortho_init(dim, expo, zj, bodycoil, good)
switch numel(dim)
case 2
	%xx = ndgrid_jf('mat', 0:nx-1, 0:ny-1);
	A = cheby2d(dim(1), dim(2), expo);
case 3
	nx = dim(1);
	ny = dim(2);
	ny = dim(2);
	A = cheby3d(dim(1), dim(2), dim(3), expo);
otherwise
	fail('#dim = %d not done', numel(dim))
end

A = reshape(A, [prod(dim), size(expo,1)]);
maskBodyI = good .* bodycoil;
Aw = repmat(maskBodyI(:), [1 size(expo,1)]) .* A;
theta = pinv(Aw) * zj(:);
clear Aw
init = A * theta; % not Aw
init = reshape(init, size(bodycoil));

end % ortho_init()

% cheby2d()
function polys = cheby2d(nx,ny,expo)
nbases = length(expo);
x = linspace(-1,1,nx);
y = linspace(-1,1,ny);
polys = zeros(nx,ny,nbases);
for n = 1:nbases
	tmp = polyval(ir_chebyshev_poly(expo(n,1)),x);
	tmp = tmp';
	X = repmat(tmp,[1 ny]);
	tmp = polyval(ir_chebyshev_poly(expo(n,2)),y);
	Y = repmat(tmp,[nx 1]);
	polys(:,:,n) = X .* Y;
end

end % cheby2d()

% cheby3d()
function polys = cheby3d(nx,ny,expo)
nbases = size(expo,1);
x = linspace(-1,1,nx);
y = linspace(-1,1,ny);
z = linspace(-1,1,nz);
polys = zeros(nx, ny, nz, nbases, 'single');
for n = 1:nbases
	tmp = polyval(ir_chebyshev_poly(expo(n,1)), x);
	X = repmat(tmp', [1 ny]);
	tmp = polyval(ir_chebyshev_poly(expo(n,2)), y);
	Y = repmat(tmp, [nx 1]);
	tmp = polyval(ir_chebyshev_poly(expo(n,3)), z);
	Z = repmat(tmp, [nx 1]);
	polys(:,:,:,n) = X .* Y .* Z;
end

end % cheby3d()

% ir_mri_sensemap_init_test2()
% 2D
function ir_mri_sensemap_init_test2
nx = 28; ny = 32;
ig = image_geom('nx', nx, 'ny', ny, 'fov', 22);
tmp = ellipse_im(ig, 'shepplogan-emis', 'oversample', 2);
%tmp = ones(nx,ny);
xtrue = tmp .* exp(1i * (tmp-2));
im(abs(xtrue))
ncoil = 4;
strue = ir_mri_sensemap_sim('nx', ig.nx, 'ny', ig.ny, 'dx', ig.dx, ...
	'ncoil', ncoil);
scale = 1 / sqrt(sum(abs(strue(end/2,end/2,:).^2)));
strue = strue * scale; % normalize so SSoS=1 near center
rng(0)
ykj = strue .* repmat(xtrue, [1 1 ncoil]) ...
	+ 2^-8 * (randn(size(strue)) + 1i * randn(size(strue)));
im plc 5 2
clim = minmax(abs(strue))';
im(1, 'row', 1, abs(strue), clim, 'true'), cbar
im(2, 'row', 1, abs(ykj)), cbar
sinit1 = ir_mri_sensemap_init(ykj);
im(3, 'row', 1, abs(sinit1), clim), cbar
slist = {'ratio', 'median', 'avg', 'order1', 'order2', 'order3', 'order4', 'zeros'};
for is=1:numel(slist)
	stype = slist{is};
	sinit2 = ir_mri_sensemap_init(ykj, 'type', stype);
	im(2+is, 'row', 1, abs(sinit2), clim, stype), cbar
	drawnow
end

end % ir_mri_sensemap_init_test2()

% ir_mri_sensemap_init_test3()
% 3D
function ir_mri_sensemap_init_test3
nx = 28; ny = 32; nz=6;
ig = image_geom('nx', nx, 'ny', ny, 'nz', nz, 'fov', 22, 'zfov', 10);
tmp = ellipsoid_im(ig, 'shepp-logan-e3d', 'oversample', 2);
%tmp = ig.ones;
xtrue = tmp .* exp(1i * tmp); % arbitrary phase
%im(abs(xtrue))
nring = 3;
ncoil = 4 * nring;
strue = ir_mri_sensemap_sim('nx', ig.nx, 'ny', ig.ny, 'nz', ig.nz, ...
	'dx', ig.dx, 'nring', nring, 'ncoil', ncoil);
%im('row', 3, abs(strue))
scale = 1 / sqrt(sum(abs(strue(end/2,end/2,end/2,:).^2)));
strue = strue * scale; % normalize so SSoS=1 near center
rng(0)
ykj = strue .* repmat(xtrue, [1 1 1 ncoil]) ...
	+ 1 * 2^-12 * (randn(size(strue)) + 1i * randn(size(strue)));
im plc 4 1
clim = minmax(abs(strue))';
im(1, 'row', nring, abs(strue), clim, 'true'), cbar
titlef('True smaps for 3D [%d,%d,%d] with %d coils (%d rings of %d coils)', ...
	nx, ny, nz, ncoil, nring, ncoil/nring)
im(2, 'row', nring, abs(ykj), 'surface coils'), cbar
sinit1 = ir_mri_sensemap_init(ykj);
im(3, 'row', nring, abs(sinit1), clim, 'ratio'), cbar
slist = {'median'}; % 'avg', 'order1', 'order2', 'order3', 'order4', 'zeros'};
for is=1:numel(slist)
	stype = slist{is};
	sinit2 = ir_mri_sensemap_init(ykj, 'type', stype);
	im(3+is, 'row', nring, abs(sinit2), clim, stype), cbar
	drawnow
end
end % ir_mri_sensemap_init_test3()

% ir_mri_sensemap_init_test()
function ir_mri_sensemap_init_test(arg)
switch arg
case 'test2'
	ir_mri_sensemap_init_test2
case 'test3'
	ir_mri_sensemap_init_test3
case 'test'
	ir_mri_sensemap_init_test2
	if im, prompt, end
	ir_mri_sensemap_init_test3
otherwise
	fail('unknown test "%s"', arg)
end
end % ir_mri_sensemap_init_test()


function Stat=clusterstat(S,partition,between)
%function Stat=clusterstat(S,partition,[between])
%
%PURPOSE
%
%To compute various intra- and extra-cluster statistics.
%
%INPUT
%
%[An argument in brackets is optional. If it isn't  given or it's
% an empty matrix/string, the function will use a default value.]
%
% S         (matrix) NxN similarity or distance matrix between N
%             objects. Must be symmetric.
% partition (1xN vector) partition vector (see explanation for
%             'partition vector' in function hcluster)
% [between] (scalar) 1: compute between-cluster similarity/distance matrix,
%                    0: (default) don't compute
%
%OUTPUT
%
% All fields are vectors of size 1xK, where K is the number of clusters
%
% Stat.N(i)             the number of objects in cluster i
%
% Stat.internal.min(i)  minimum/average/max internal similarity/distance
% Stat.internal.avg(i)  in cluster i
% Stat.internal.max(i)
%  Note: if there is only one item in the cluster, the internal statistics
%  are set to NaN
%
% Stat.external.min(i)  minimum/average/max external similarity/distance from
% Stat.external.avg(i)  objects of cluster i to the objects of other clusters
% Stat.external.max(i)
%
%DETAILS
%
%Partition vector divides items  into clusters: partition(i) is
%the label  of cluster that item i belongs to. Cluster labels must
%be integers 1,2,...,K where K is the number of clusters.
%
%"Internal" for cluster k refers to all pairwise
%distances/similarities between objects belonging to the same
%cluster, i.e., for objects i for which partition(i)==k.
%
%"External" for cluster k refers to all pairwise
%distances/similarities between the members of the cluster k and
%members of the other clusters, i.e., the whole similarity matrix
%excluding the internal similarities of cluster k.
%
%If 'between' is set to 1 the function computes also the following
%KxK matrices
%
%Stat.between.min(i,j)
%Stat.between.avg(i,j)
%Stat.between.max(i,j)
%
%These include distances/similarities  between clusters i and j
%defined as min/average/max pairwise distances between objects
%belonging to clusters i and j, respectively.
%
%SEE ALSO
% clusterquality
% icassoStability
% icassoRindex
% rindex

%COPYRIGHT NOTICE
%This function is a part of Icasso software library
%Copyright (C) 2003-2005 Johan Himberg
%
%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 any later version.
%
%This program is distributed in the hope that it will be useful,
%but WITHOUT ANY WARRANTY; without even the implied warranty of
%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%GNU General Public License for more details.
%
%You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software
%Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

% ver 1.2. 100105

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

if nargin<3|isempty(between)
  between=0;
end

% Number of clusters
Ncluster=max(partition);

%Initialize the struct
Stat.internal.sum(1:Ncluster,1)=NaN;
Stat.internal.min(1:Ncluster,1)=NaN;
Stat.internal.avg(1:Ncluster,1)=NaN;
Stat.internal.max(1:Ncluster,1)=NaN;
Stat.external.sum(1:Ncluster,1)=NaN;
Stat.external.min(1:Ncluster,1)=NaN;
Stat.external.avg(1:Ncluster,1)=NaN;
Stat.external.max(1:Ncluster,1)=NaN;
Stat.index = cell(1,Ncluster);

for cluster=1:Ncluster,
  thisPartition=(partition==cluster);
  Stat.index{cluster} = find(thisPartition);
  S_=S(thisPartition,thisPartition);
  Stat.N(cluster)=size(S_,1);
  S_(eye(size(S_))==1)=[];
  if ~isempty(S_),
    Stat.internal.sum(cluster)=sum(S_);
    Stat.internal.min(cluster)=min(S_);
    Stat.internal.avg(cluster)=mean(S_);
    Stat.internal.max(cluster)=max(S_);
  end
  if Ncluster>1,
    S_=S(thisPartition,~thisPartition);
    Stat.external.sum(cluster)=sum(S_(:));
    Stat.external.min(cluster)=min(S_(:));
    Stat.external.avg(cluster)=mean(S_(:));
    Stat.external.max(cluster)=max(S_(:));
  end
end

if between,
  Stat.between.min=zeros(Ncluster,Ncluster);
  Stat.between.max=zeros(Ncluster,Ncluster);
  Stat.between.avg=zeros(Ncluster,Ncluster);

  for i=1:Ncluster,
    Pi=find(i==partition);
    for j=i+1:Ncluster,
      Pj=find(j==partition);
      d_=S(Pi,Pj);
      Stat.between.min(i,j)=min(d_(:));
      Stat.between.avg(i,j)=mean(d_(:));
      Stat.between.max(i,j)=max(d_(:));		;
    end
  end

  Stat.between.min=Stat.between.min+ ...
    Stat.between.min';
  Stat.between.max=Stat.between.max+ ...
      Stat.between.max';
  Stat.between.avg=Stat.between.avg+ ...
      Stat.between.avg';
end


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nickabattista[at]gmail.com
% IB2d Created: May 27th, 2015
% Institution: TCNJ
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs or non-invariant beams*)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%   .
%   .
%   .
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nickabattista[at]gmail.co) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the beam attributes!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function beams_info = update_nonInv_Beams(dt,current_time,beams_info)

% beams_info:   col 1: 1ST PT.
%               col 2: MIDDLE PT. (where force is exerted)
%               col 3: 3RD PT.
%               col 4: beam stiffness
%               col 5: x-curvature
%               col 6: y-curvature

%IDs = beams_info(:,1);   % Gives Middle Pt.

% Time initialization
t1 = 0.25;                          % Period A->B (down-stroke)
t2 = 0.25;                          % Period B->A (up-stroke)
tR = 0.0;                           % Resting time between A->B and B->A
period = (t1+t2+tR);                % Time it takes to move to the right
t = rem(current_time,period);       % Recalculate Time (using modular arithmetic)

% Cubic Interpolation Information
p1 = 0.10;
p2 = 0.90;

% a COEFFICIENTS
a0 = 0;
a1 = 0;
a2 = 0;
a3 = 11.111111111111088;

% b COEFFICIENTS
b0 = 0.013888888888889;
b1 = -0.416666666666666;
b2 = 4.166666666666658;
b3 = -2.777777777777770;

% c COEFFICIENTS
c0 =  -10.111111111111073;
c1 =  33.333333333333222;
c2 = -33.333333333333222;
c3 =  11.111111111111073;

% Read In y_Pts for two Phases!
[xP1,yP1,yP2] = read_File_In('swimmer.phases'); % NOTE xP1 = xP2
xP2 = xP1;

%
% FIRST WE COMPUTE THE INTERPOLATE GEOMETRY BETWEEN BOTH PHASES
%

%
% START THE INTERPOLATING BETWEEN STATES!
%
if t <= t1 % STATE A -> STATE B

    % Scaling time for appropriate use in interp. function so tTilde\in[0,1]
    tTilde = (t/t1);

    % Evaluate Pieceise Cubic Interpolation Poly
    if ( tTilde<=p1 )
        gFUNC = a0 + a1*tTilde + a2*tTilde^2 + a3*tTilde^3;
    elseif ( (tTilde>p1) && (tTilde<=p2) )
        gFUNC = b0 + b1*tTilde + b2*tTilde^2 + b3*tTilde^3;
    else
        gFUNC = c0 + c1*tTilde + c2*tTilde^2 + c3*tTilde^3;
    end

    %xPts = xP1 + gFUNC*( xP2 - xP1 );
    yPts = yP1 + gFUNC*( yP2 - yP1 );

elseif ( t >= t1+tR ) % STATE B -> A

    % Scaling time for appropriate use in interp. function so tTilde\in[0,1]
    tTilde = (t-t1-tR)/(t2);

    % Evaluate Pieceise Cubic Interpolation Poly
    if tTilde<=p1
        gFUNC = a0 + a1*tTilde + a2*tTilde^2 + a3*tTilde^3;
    elseif tTilde<=p2
        gFUNC = b0 + b1*tTilde + b2*tTilde^2 + b3*tTilde^3;
    else
        gFUNC = c0 + c1*tTilde + c2*tTilde^2 + c3*tTilde^3;
    end

    %xPts = xP2 + gFUNC*( xP1 - xP2 );
    yPts = yP2 + gFUNC*( yP1 - yP2 );

end

%
% NOW WE UPDATE THE CURAVTURES APPROPRIATELY
%
%beams_info(:,5) = xPts(1:end-2)+xPts(3:end)-2*xPts(2:end-1);
beams_info(:,6) = yPts(1:end-2)+yPts(3:end)-2*yPts(2:end-1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: reads in info from file
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x1,y1,y2] = read_File_In(file_name)

filename = file_name;  %Name of file to read in

fileID = fopen(filename);

    % Read in the file, use 'CollectOutput' to gather all similar data together
    % and 'CommentStyle' to to end and be able to skip lines in file.
    C = textscan(fileID,'%f %f %f','CollectOutput',1);

fclose(fileID);        %Close the data file.

mat_info = C{1};   %Stores all read in data

%Store all elements in matrix
mat = mat_info(1:end,1:end);

x1 =  mat(:,1); %store xVals1/2
y1 =  mat(:,2); %store yVals1
y2 =  mat(:,3); %store yVals2


function [LL, LLS, ht] = tarch_likelihood(parameters, data, fdata, fIdata, p, o, q, error_type, tarch_type, back_cast, T, estim_flag)
% Log likelihood for TARCH(P,O,Q) estimation
%
% USAGE:
%   [LL, LLS, HT] = tarch_likelihood(PARAMETERS, DATA, FDATA, FIDATA, P, O, Q, ERROR_TYPE, TARCH_TYPE, BACK_CAST, T, ESTIM_FLAG)
%
% INPUTS:
%   PARAMETERS    - A vector of GARCH process parameters
%                   [omega alpha gamma beta [nu lambda]]
%   DATA          - Vector of mean zero residuals
%   FDATA         - Either abs(data) or data.^2, depending on tarch_type
%   FIDATA        - fdata times an indicator for negative, e.g. fdata.*(data<0)
%   P             - The lag order length for ARCH
%   O             - The lag order of asymmetric terms
%   Q             - The lag order length for GARCH
%   ERROR_TYPE    - The type of error being assumed, valid types are:
%                     1 if 'NORMAL'
%                     2 if 'STUDENTST'
%                     3 if 'GED'
%                     4 if 'SKEWT'
%   TARCH_TYPE    - 1 for absolute vale of return
%                 - 2 for squared returns (standard case)
%   BACK_CAST     - The value used for variance recursion
%   T             - Length of data
%   ESTIM_FLAG    - [OPTIONAL] Flag (0 or 1) to indicate if the function
%                   is being used in estimation.  If it is 1, then the parameters are
%                   transformed from unconstrained values to constrained by standard
%                   garch model constraints
%
% OUTPUTS:
%   LL             - Minus 1 times the log likelihood
%   LLS            - Time series of log likelihoods (Also multiplied by -1)
%   HT             - Time series of conditional variances
%
% COMMENTS:
%   See also TARCH

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

if nargin==12 && estim_flag
    %If for estimation, transform the parameters
    [parameters,nu,lambda]=tarch_itransform(parameters,p,o,q,error_type);
else
    %Otherwise the parameters simply must be parsed
    if error_type==2 || error_type==3
        %Seperate nu from the remaning parameters
        nu=parameters(p+o+q+2);
        parameters=parameters(1:1+p+o+q);
    elseif error_type==4
        lambda=parameters(p+o+q+3);
        nu=parameters(p+o+q+2);
        parameters=parameters(1:1+p+o+q);
    end
end

%Backcast length
m  =  max([p o q]);
%Compute the conditional variances
ht=tarch_core(fdata,fIdata,parameters,back_cast,p,o,q,m,T,tarch_type);

%Indices for the relevant opservations
t = (m + 1):T;
ht=ht(t);
data=data(t);
%Compute the log likelihoods
switch error_type
    case 1
        [LL, LLS] = normloglik(data,0,ht);
        LLS = -LLS;
        LL = -LL;
    case 2
        [LL, LLS] = stdtloglik(data,0,ht,nu);
        LLS = -LLS;
        LL = -LL;
    case 3
        [LL, LLS] = gedloglik(data,0,ht,nu);
        LLS = -LLS;
        LL = -LL;
    case 4
        [LL, LLS] = skewtloglik(data,0,ht,nu,lambda);
        LLS = -LLS;
        LL = -LL;
end


function [Estimation_X0,FirstLandmarks] = FEKF_update(Estimation_X0, CameraMeasurementThis, Sigma_OB , FirstLandmarks)
NumberOfLandmarksObInThisStep = size(CameraMeasurementThis,1)/3;

% initialise the IndexOfFeature if possible
if size(Estimation_X0.landmarks,2) > 0
    IndexOfFeature = Estimation_X0.landmarks(4,:)';
else
    IndexOfFeature = [];
end

IndexObservedAlreadyThis = [];
IndexObservedNew = [];
for i = 1:NumberOfLandmarksObInThisStep
    % check whether the feature is observed before or not
    M = find( IndexOfFeature== CameraMeasurementThis(3*i,2) );
    if isempty(M)
        IndexObservedNew = [IndexObservedNew;CameraMeasurementThis(3*i,2) ];
    else
        IndexObservedAlreadyThis = [IndexObservedAlreadyThis;CameraMeasurementThis(3*i,2)];
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% IndexObservedNew=[78; 97; 18] indicates that the
% robot firstly observes landmarks 78 97 18 in this step
% IndexObservedAlreadyThis=[19; 20; 53] indicates
% that the robot observes again landmarks 19 20 53 in this step
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

orientation = Estimation_X0.orientation;
position    = Estimation_X0.position;
cov         = Estimation_X0.cov;

NumberOfFeature = size( IndexOfFeature,1);
NumberOfOldFeatureInThisStep = size(IndexObservedAlreadyThis,1);
NumberOfNewFeatureInThisStep = size(IndexObservedNew,1);

% update state vector and covariance by considering
% new feature into state and covariance
if ~isempty(IndexObservedNew)

    orientation = Estimation_X0.orientation;
    position    = Estimation_X0.position;

   % copy previous covariance
    temp    = repmat({eye(3)}, NumberOfNewFeatureInThisStep, 1 );
    tempKK  = blkdiag(temp{:});
    Sigma   = blkdiag(Estimation_X0.cov,tempKK);
    KK      = eye(6+3*(NumberOfFeature+NumberOfNewFeatureInThisStep));

 %   add new features
    for i = 1:NumberOfNewFeatureInThisStep
        indNewf = IndexObservedNew(i);
        Estimation_X0.landmarks(4,NumberOfFeature+i) = indNewf;
        m2 = find( CameraMeasurementThis(:,2) == indNewf );
        nf = CameraMeasurementThis( m2, 1 );

        Estimation_X0.landmarks(1:3,NumberOfFeature+i) = orientation*nf+position;
        Newlandmark=[ orientation*nf+position;  indNewf  ];

        FirstLandmarks=[FirstLandmarks  Newlandmark];

%         KK( 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i,1:6 ) = [-skew(Estimation_X0.orientation*(nf)) eye(3)];
%         KK (6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i, 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i )=Estimation_X0.orientation;

         KK( 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i,1:6 ) = [-skew(orientation*(nf)) eye(3)];
        KK (6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i, 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i )=orientation;
                tempKK(3*i-2:3*i,3*i-2:3*i)=diag(nf.^2)*Sigma_OB^2;
    end
    Sigma   = blkdiag(Estimation_X0.cov,tempKK);
    Estimation_X0.cov = KK*Sigma*KK';

    NumberOfFeature=NumberOfFeature+NumberOfNewFeatureInThisStep;
end

orientation = Estimation_X0.orientation;
position    = Estimation_X0.position;
cov         = Estimation_X0.cov;

% update state and covariance
if ~isempty(IndexObservedAlreadyThis)
    Z = zeros( NumberOfOldFeatureInThisStep*3 , 1);
    Y = zeros( NumberOfOldFeatureInThisStep*3 , 1);
    H = zeros(3*NumberOfOldFeatureInThisStep, 6+3*NumberOfFeature);

    temp = repmat({eye(3)}, NumberOfOldFeatureInThisStep,1 );
    R = blkdiag(temp{:});

    % update old features
    for i = 1:NumberOfOldFeatureInThisStep
        ind = find(IndexOfFeature == IndexObservedAlreadyThis(i));

        fi  = Estimation_X0.landmarks(1:3,ind);
        Y(3*i-2:3*i,1) = ObservationModel( orientation, position, fi );

        ind2 = find(CameraMeasurementThis(:,2) == IndexObservedAlreadyThis(i));
        Z(3*i-2:3*i,1 ) = CameraMeasurementThis(ind2,1);

        FirstLandmark=FirstLandmarks(1:3,ind);

        H(3*i-2:3*i, 1:6) = [-orientation'*skew(FirstLandmark-position) orientation'];
        H(3*i-2:3*i, 6+3*ind-2:6+3*ind) = -orientation';
        R(3*i-2:3*i,3*i-2:3*i) = diag(CameraMeasurementThis(ind2,1).^2)*Sigma_OB^2;
    end

    % question @RomaTeng, different computaton scheme
    z = Z-Y;
    S = H*cov*H'+R;
    K = cov*H'*inv(S);
    s = K*z;

    Estimation_X0 = SpecialAdd(Estimation_X0,-s);
    cov = ( eye(6+3*NumberOfFeature) -K*H )*cov;
    Estimation_X0.cov = cov;

end


function [g_all, vv_all]=vifsub_est_M(org,dist, subbands, M);

% uses convolution for determining the parameters of the distortion channel
% Called by vifvec.m

tol = 1e-15; % tolernace for zero variance. Variance below this is set to zero, and zero is set to this value to avoid numerical issues.

for i=1:length(subbands)
    sub=subbands(i);
    y=org{sub};
    yn=dist{sub};

    % compute the size of the window used in the distortion channel estimation
    lev=ceil((sub-1)/6);
    winsize=2^lev+1; offset=(winsize-1)/2;
    win = ones(winsize);

    % force subband size to be multiple of M
    newsize=floor(size(y)./M)*M;
    y=y(1:newsize(1),1:newsize(2));
    yn=yn(1:newsize(1),1:newsize(2));

    % Correlation with downsampling. This is faster than downsampling after
    % computing full correlation.
    winstep=[M M];
    winstart=[1 1].*floor(M/2)+1;
    winstop=size(y)-ceil(M/2)+1;

    % mean
    mean_x = corrDn(y,win/sum(win(:)),'reflect1',winstep, winstart,winstop);
    mean_y = corrDn(yn,win/sum(win(:)),'reflect1',winstep, winstart,winstop);
    % cov
    cov_xy = corrDn(y.*yn, win, 'reflect1',winstep, winstart,winstop) - sum(win(:)).*mean_x.*mean_y;
    % var
    ss_x = corrDn(y.^2,win, 'reflect1',winstep, winstart,winstop) - sum(win(:)).*mean_x.^2;
    ss_y = corrDn(yn.^2,win, 'reflect1',winstep, winstart,winstop) - sum(win(:)).*mean_y.^2;

    % get rid of numerical problems, very small negative numbers, or very
    % small positive numbers, or other theoretical impossibilities.
    ss_x(ss_x<0)=0;
    ss_y(ss_y<0)=0;

    % Regression
    g = cov_xy./(ss_x+tol);

    % Variance of error in regression
    vv = (ss_y - g.*cov_xy)/(sum(win(:)));

    % get rid of numerical problems, very small negative numbers, or very
    % small positive numbers, or other theoretical impossibilities.
    g (ss_x < tol) = 0;
    vv (ss_x < tol) = ss_y (ss_x < tol);
    ss_x(ss_x<tol)=0;

    g (ss_y < tol) = 0;
    vv (ss_y < tol) = 0;

    % constrain g to be non-negative.
    vv(g<0)=ss_y(g<0);
    g(g<0)=0;

    % take care of numerical errors, vv could be very small negative
    vv( vv <= tol) = tol;

    g_all{i}=g;
    vv_all{i}=vv;

end


function Y = vl_nndwt2( X, dzdy, varargin )
% dwt multi_channel
% only support haart and db2 for 1-level transformation

opts.padding = 0 ;
opts.wavename = 'haart';
opts = vl_argparse(opts, varargin, 'nonrecursive') ;
padding = opts.padding;

if  nargin <= 1 || isempty(dzdy)
    sz = size(X);
    X = X/2;
    if size(X, 3) == 1
        sz(3) = 1;
    end
    if size(X, 4) == 1
        sz(4) = 1;
    end
            im_c1 = X(1:2:end, 1:2:end, :, :);
            im_c2 = X(1:2:end, 2:2:end, :, :);
            im_c3 = X(2:2:end, 1:2:end, :, :);
            im_c4 = X(2:2:end, 2:2:end, :, :);
            Y = zeros([sz(1)/2 sz(2)/2 sz(3)*4 sz(4)], 'like', X);
            Y(:,:,1:sz(3),:)           = im_c1 + im_c2 + im_c3 + im_c4;
            Y(:,:,sz(3)+1:sz(3)*2,:)   = -im_c1 - im_c2 + im_c3 + im_c4;
            Y(:,:,sz(3)*2+1:sz(3)*3,:) = -im_c1 + im_c2 - im_c3 + im_c4;
            Y(:,:,sz(3)*3+1:end,:)     = im_c1 - im_c2 - im_c3 + im_c4;
else
    sz = size(dzdy);
    if size(X, 4) == 1
        sz(4) = 1;
    end

            dzdy = dzdy/2;
            Y = zeros([sz(1)*2 sz(2)*2 sz(3)/4 sz(4)],'like',dzdy);
            Y(1:2:end, 1:2:end, : , :) = dzdy(:,:,1:sz(3)/4,:) - dzdy(:,:,sz(3)/4+1:sz(3)/2,:) - ...
                                         dzdy(:,:,sz(3)/2+1:3*sz(3)/4,:) + dzdy(:,:,sz(3)/4*3+1:end,:);
            Y(1:2:end, 2:2:end, : , :) = dzdy(:,:,1:sz(3)/4,:) - dzdy(:,:,sz(3)/4+1:sz(3)/2,:) + ...
                                         dzdy(:,:,sz(3)/2+1:3*sz(3)/4,:) - dzdy(:,:,sz(3)/4*3+1:end,:);
            Y(2:2:end, 1:2:end, : , :) = dzdy(:,:,1:sz(3)/4,:) + dzdy(:,:,sz(3)/4+1:sz(3)/2,:) - ...
                                         dzdy(:,:,sz(3)/2+1:3*sz(3)/4,:) - dzdy(:,:,sz(3)/4*3+1:end,:);
            Y(2:2:end, 2:2:end, : , :) = dzdy(:,:,1:sz(3)/4,:) + dzdy(:,:,sz(3)/4+1:sz(3)/2,:) + ...
                                         dzdy(:,:,sz(3)/2+1:3*sz(3)/4,:) + dzdy(:,:,sz(3)/4*3+1:end,:);

end


function [divI, I_dx, I_dy] = computeDivergence(I_gx, I_gy)
%
%
%       [divI, I_dx, I_dy] = computeDivergence(I_gx, I_gy)
%
%       Input:
%           -I_gx: an input image
%           -I_gx
%
%       Output:
%           -I_dx:
%           -I_dy:
%           -divI:
%
%     Copyright (C) 2011-15  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

if(~exist('I_gx', 'var') || ~exist('I_gy', 'var'))
    error('Gradients are needed to compute divergence');
end

kernelX = [0 0 0; -1 1 0; 0  0 0];
kernelY = [0 0 0;  0 1 0; 0 -1 0];

I_dx = imfilter(I_gx, kernelX, 'same');
I_dy = imfilter(I_gy, kernelY, 'same');

divI = RemoveSpecials(I_dx + I_dy);

end


function [ degrees, minutes, seconds] = latlonvec(decimal_degrees)
%LATLONVEC Convert decimal degrees into degrees/minutes/seconds
%
% Description:
% Return degrees, minutes, seconds given decimal degrees. Sign of
% coordinate will be returned in degrees portion of coordinate.
%
% Sample fuction call:
% [ degrees, minutes, seconds] = latlonvec(decimal_degrees)
%
% Author: Scott Lee
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

degrees = fix(abs(decimal_degrees));	% integer degrees
frac = abs(decimal_degrees) - degrees;	% fractional degrees, used to compute minutes
minutes = fix(frac*60);	    % integer minutes
frac = frac - minutes/60;	% fractional minutes, used to compute seconds
seconds = frac*3600;		% decimal seconds

% Handle sign.  Degrees portion will contain the sign of the coordinate.
% Minutes and seconds will always be positive.
% sign function returns -1, 0, +1 for x < 0, x == 0, x > 0, respectively
degrees = sign(decimal_degrees).*degrees;

if nargout<2
  degrees = [degrees minutes seconds];
end

end

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


function dist = codedist_bay(C1,C2,Py)
% compute the distance using a Bayesian metric between 'C1' and  'C2'
%
%    dist = codedist_bay(C1,C2,{Py})
%
% 'C1' contains the result code,
% 'C2' contains the codebooks prototype. '0' or  an infine small number 'eps'  represents the don't care
% 'Py' is a matrix containing the moderated outputs of the binary classifiers
%
%
% An example:
%  >> Ye = [1 1; 1 1];
%  >> codebook = [1 2 3];
%  >> old_codebook = [1 1 -1; 1 -1 -1];
%  >> Py = [.9 -.1; -.1 .13];
%  >> code(Ye, codebook, [],old_codebook,'codedist_bay',{Py})
% in this call, 'Ye' is not used explicitly.
%
% To use this distance measure in LS-SVMlab, the following
% procedure is to be followed, assume input data 'X' and multiclass
% output 'Y':
%
%  >> [Ycode,codebook,old_codebook] = code(Y,'code_MOC');
%  >> [alpha,b] = trainlssvm({X,Yc,'c',gam,sig2});
%  >> Yhc = simlssvm({X,Yc,'c',gam,sig2},{alpha,b},Xt);
%
%  The moderated output for the LS-SVM can be computed using the bayesian inference
%  framework for the LS-SVM:
%  >> Ymod = bay_modoutClass(model,Xt);
%  >> Yh = code(Yhc,old_codebook,[],codebook,'codedist_bay',{Ymod});
%
% see also:
%   bay_modoutClass, codedist_hamming, code_ECOC

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

% % encode for training
% >> model = initlssvm(X,Y,'classification',gam,sig2,'preprocess','RBF_kernel');
% >> model = changelssvm(model,'codetype','code_MOC');
% >> model = changelssvm(model,'codedist_fct','codedist_hamming');
% >> model = trainlssvm(model);
%
% % decode for simulating
% >> model = changelssvm(model,'codedist_fct','codedist_bay');
% >> model = changelssvm(model,'codedist_args',{bay_modoutClass(model,Xt)});
% >>   Yt  = simlssvm(model,Xt);

if nargin<3,
  error(['moderated output needed as function arguments' ...
	 '(for LS-SVM, model.codedist_args =' ...
	 ' bay_modoutClass(model,X)).']);
end

[nb,nbin] = size(Py);
[~,dim] = size(C2);
dist = zeros(nb,dim);

for d = 1:dim,
  for n= 1:nb,
    dist(n,d) = sum((1-Py(n,:).*C2(:,d)'))-sum(C2(:,d)==eps);
  end
end


function lik = lik_binomial(varargin)
%LIK_BINOMIAL  Create a Binomial likelihood structure
%
%  Description
%    LIK = LIK_BINOMIAL creates Binomial likelihood structure.
%
%    The likelihood is defined as follows:
%                  __ n
%      p(y|f, z) = || i=1 [ p_i^(y_i)*(1-p_i)^(z_i-y_i)) *
%                           gamma(z_i+1)/(gamma(y_i+1)*gamma(z_i-y_i+1))]
%    where p_i = exp(f_i)/ (1+exp(f_i)) is the succes probability,
%    which is a function of the latent variable f_i and z is a
%    vector of numbers of trials.
%
%    When using Binomial likelihood you need to give the vector z
%    as an extra parameter to each function that requires y also.
%    For example, you should call gp_optim as follows
%      gp_optim(gp, x, y, 'z', z)
%
%  See also
%    GP_SET, LIK_*
%
% Copyright (c) 2009-2010 Jaakko Riihimäki & Jarno Vanhatalo
% Copyright (c) 2010-2011 Aki Vehtari

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

  ip=inputParser;
  ip.FunctionName = 'LIK_BINOMIAL';
  ip.addOptional('lik', [], @isstruct);
  ip.parse(varargin{:});
  lik=ip.Results.lik;

  if isempty(lik)
    init=true;
    lik.type = 'Binomial';
  else
    if ~isfield(lik,'type') || ~isequal(lik.type,'Binomial')
      error('First argument does not seem to be a valid likelihood function structure')
    end
    init=false;
  end

  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_binomial_pak;
    lik.fh.unpak = @lik_binomial_unpak;
    lik.fh.ll = @lik_binomial_ll;
    lik.fh.llg = @lik_binomial_llg;
    lik.fh.llg2 = @lik_binomial_llg2;
    lik.fh.llg3 = @lik_binomial_llg3;
    lik.fh.tiltedMoments = @lik_binomial_tiltedMoments;
    lik.fh.predy = @lik_binomial_predy;
    lik.fh.predprcty = @lik_binomial_predprcty;
    lik.fh.invlink = @lik_binomial_invlink;
    lik.fh.recappend = @lik_binomial_recappend;
  end

end

function [w,s,h] = lik_binomial_pak(lik)
%LIK_BINOMIAL_PAK  Combine likelihood parameters into one vector.
%
%  Description
%    W = LIK_BINOMIAL_PAK(LIK) takes a likelihood structure LIK
%    and returns an empty verctor W. If Binomial likelihood had
%    parameters this would combine them into a single row vector
%    W (see e.g. likelih_negbin). This is a mandatory subfunction
%    used for example in energy and gradient computations.
%
%  See also
%    LIK_NEGBIN_UNPAK, GP_PAK

  w = []; s = {}; h=[];
end

function [lik, w] = lik_binomial_unpak(lik, w)
%LIK_BINOMIAL_UNPAK  Extract likelihood parameters from the vector.
%
%  Description
%    W = LIK_BINOMIAL_UNPAK(W, LIK) Doesn't do anything.
%
%    If Binomial likelihood had parameters this would extracts
%    them parameters from the vector W to the LIK structure.
%    This is a mandatory subfunction used for example in energy
%    and gradient computations.
%
%  See also
%    LIK_BINOMIAL_PAK, GP_UNPAK

  lik=lik;
  w=w;

end

function ll = lik_binomial_ll(lik, y, f, z)
%LIK_BINOMIAL_LL  Log likelihood
%
%  Description
%    LL = LIK_BINOMIAL_LL(LIK, Y, F, Z) takes a likelihood
%    structure LIK, succes counts Y, numbers of trials Z, and
%    latent values F. Returns the log likelihood, log p(y|f,z).
%    This subfunction is needed when using Laplace approximation
%    or MCMC for inference with non-Gaussian likelihoods. This
%    subfunction is also used in information criteria (DIC, WAIC)
%    computations.
%
%  See also
%    LIK_BINOMIAL_LLG, LIK_BINOMIAL_LLG3, LIK_BINOMIAL_LLG2, GPLA_E

  if isempty(z)
    error(['lik_binomial -> lik_binomial_ll: missing z!'...
           'Binomial likelihood needs the expected number of   '...
           'occurrences as an extra input z. See, for         '...
           'example, lik_binomial and gp_optim.             ']);
  end

  expf = exp(f);
  p = expf ./ (1+expf);
  N = z;
  ll =  sum(gammaln(N+1)-gammaln(y+1)-gammaln(N-y+1)+y.*log(p)+(N-y).*log(1-p));
end

function llg = lik_binomial_llg(lik, y, f, param, z)
%LIK_BINOMIAL_LLG    Gradient of the log likelihood
%
%  Description
%    LLG = LIK_BINOMIAL_LLG(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, succes counts Y, numbers of trials Z and
%    latent values F. Returns the gradient of the log likelihood
%    with respect to PARAM. At the moment PARAM can be 'param' or
%    'latent'. This subfunction is needed when using Laplace
%    approximation or MCMC for inference with non-Gaussian
%    likelihoods.
%
%  See also
%    LIK_BINOMIAL_LL, LIK_BINOMIAL_LLG2, LIK_BINOMIAL_LLG3, GPLA_E

  if isempty(z)
    error(['lik_binomial -> lik_binomial_llg: missing z!'...
           'Binomial likelihood needs the expected number of   '...
           'occurrences as an extra input z. See, for         '...
           'example, lik_binomial and gp_optim.             ']);
  end

  switch param
    case 'latent'
      expf = exp(f);
      N = z;

      llg = y./(1+expf) - (N-y).*expf./(1+expf);
  end
end

function llg2 = lik_binomial_llg2(lik, y, f, param, z)
%LIK_BINOMIAL_LLG2  Second gradients of the log likelihood
%
%  Description
%    LLG2 = LIK_BINOMIAL_LLG2(LIK, Y, F, PARAM) takes a
%    likelihood structure LIK, succes counts Y, numbers of trials
%    Z, and latent values F. Returns the Hessian of the log
%    likelihood with respect to PARAM. At the moment PARAM can be
%    only 'latent'. G2 is a vector with diagonal elements of the
%    Hessian matrix (off diagonals are zero). This subfunction
%    is needed when using Laplace approximation or EP for inference
%    with non-Gaussian likelihoods.
%
%  See also
%    LIK_BINOMIAL_LL, LIK_BINOMIAL_LLG, LIK_BINOMIAL_LLG3, GPLA_E

  if isempty(z)
    error(['lik_binomial -> lik_binomial_llg2: missing z!'...
           'Binomial likelihood needs the expected number of    '...
           'occurrences as an extra input z. See, for          '...
           'example, lik_binomial and gp_optim.              ']);
  end

  switch param
    case 'latent'
      expf = exp(f);
      N = z;

      llg2 = -N.*expf./(1+expf).^2;
  end
end

function llg3 = lik_binomial_llg3(lik, y, f, param, z)
%LIK_BINOMIAL_LLG3  Third gradients of the log likelihood
%
%  Description
%    LLG3 = LIK_BINOMIAL_LLG3(LIK, Y, F, PARAM) takes a
%    likelihood structure LIK, succes counts Y, numbers of trials
%    Z and latent values F and returns the third gradients of the
%    log likelihood with respect to PARAM. At the moment PARAM
%    can be only 'latent'. G3 is a vector with third gradients.
%    This subfunction is needed when using Laplace appoximation
%    for inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_BINOMIAL_LL, LIK_BINOMIAL_LLG, LIK_BINOMIAL_LLG2, GPLA_E, GPLA_G

  if isempty(z)
    error(['lik_binomial -> lik_binomial_llg3: missing z!'...
           'Binomial likelihood needs the expected number of    '...
           'occurrences as an extra input z. See, for          '...
           'example, lik_binomial and gp_optim.              ']);
  end

  switch param
    case 'latent'
      expf = exp(f);
      N = z;
      llg3 = N.*(expf.*(expf-1))./(1+expf).^3;
  end
end

function [logM_0, m_1, sigm2hati1] = lik_binomial_tiltedMoments(lik, y, i1, sigma2_i, myy_i, z)
%LIK_BINOMIAL_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_BINOMIAL_TILTEDMOMENTS(LIK, Y, I, S2,
%    MYY, Z) takes a likelihood structure LIK, succes counts Y,
%    numbers of trials Z, index I and cavity variance S2 and mean
%    MYY. Returns the zeroth moment M_0, mean M_1 and variance
%    M_2 of the posterior marginal (see Rasmussen and Williams
%    (2006): Gaussian processes for Machine Learning, page 55).
%    This subfunction is needed when using EP for inference with
%    non-Gaussian likelihoods.
%
%  See also
%    GPEP_E

%  if isempty(z)
%    error(['lik_binomial -> lik_binomial_tiltedMoments: missing z!'...
%           'Binomial likelihood needs the expected number of               '...
%           'occurrences as an extra input z. See, for                     '...
%           'example, lik_binomial and gp_optim.                         ']);
%  end

  yy = y(i1);
  N = z(i1);
  logM_0=zeros(size(yy));
  m_1=zeros(size(yy));
  sigm2hati1=zeros(size(yy));

  for i=1:length(i1)
    if isscalar(sigma2_i)
      sigma2ii = sigma2_i;
    else
      sigma2ii = sigma2_i(i);
    end

    % Create function handle for the function to be integrated
    % (likelihood * cavity) and useful integration limits
    [tf,minf,maxf]=init_binomial_norm(yy(i),myy_i(i),sigma2ii,N(i));

    % Integrate with quadrature
    RTOL = 1.e-6;
    ATOL = 1.e-10;
    [m_0, m_1(i), m_2] = quad_moments(tf,minf, maxf, RTOL, ATOL);
    if isnan(m_0)
      logM_0=NaN;
      return
    end
    sigm2hati1(i) = m_2 - m_1(i).^2;

    % If the second central moment is less than cavity variance
    % integrate more precisely. Theoretically for log-concave
    % likelihood should be sigm2hati1 < sigm2_i.
    if sigm2hati1(i) >= sigma2ii
      ATOL = ATOL.^2;
      RTOL = RTOL.^2;
      [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
      sigm2hati1(i) = m_2 - m_1(i).^2;
      %    if sigm2hati1 >= sigm2_i
      %      error('lik_binomial_tilted_moments: sigm2hati1 >= sigm2_i');
      %    end
    end
    logM_0(i) = log(m_0);
  end
end

function [lpy, Ey, Vary] = lik_binomial_predy(lik, Ef, Varf, yt, zt)
%LIK_BINOMIAL_PREDY  Returns the predictive mean, variance and density of y
%
%  Description
%    [LPY] = LIK_BINOMIAL_PREDY(LIK, EF, VARF YT, ZT)
%    Returns logarithm of the predictive density PY of YT, that is
%        p(yt | y, zt) = \int p(yt | f, zt) p(f|y) df.
%    This requires also the succes counts YT, numbers of trials ZT.
%    This subfunction is needed when computing posterior predictive
%    distributions for future observations.
%
%    [LPY, EY, VARY] = LIK_BINOMIAL_PREDY(LIK, EF, VARF) takes a
%    likelihood structure LIK, posterior mean EF and posterior
%    Variance VARF of the latent variable and returns the
%    posterior predictive mean EY and variance VARY of the
%    observations related to the latent variables. This subfunction
%    is needed when computing posterior predictive distributions for
%    future observations.
%
%
%  See also
%    GPEP_PRED, GPLA_PRED, GPMC_PRED

  if isempty(zt)
    error(['lik_binomial -> lik_binomial_predy: missing z!'...
           'Binomial likelihood needs the expected number of       '...
           'occurrences as an extra input z. See, for             '...
           'example, lik_binomial and gp_optim.                 ']);
  end

  if nargout > 1
  % Here we approximate the inverse logistic function by a linear combination of inverse probit function
  % logitinv(f) = a probit_inv(f/c1S) + (1 - a) probit_inv(f/c2s)
  % see Demidenko (2004). Mixed models: Theory and applications
      p1 = 0.4353;   p2 = 0.5647; % these are a and (1 - a)
      c1S = 2.2967^2;  c2S = 1.3017^2;

      z1 = Ef ./ sqrt(c1S + Varf);
      z2 = Ef ./ sqrt(c2S + Varf);

      % unconditional expectation
      p = (p1 .* normcdf(z1) + p2 .* normcdf(z2));
      Ey = zt .* p;

      % unconditional variance
      if any(zt ~= 1)
          nt = length(Ef);
          zz11 = zeros(nt, 1);
          zz12 = zeros(nt, 1);
          zz22 = zeros(nt, 1);

          for i = 1:length(Ef)
              zz11(i) = mvncdf([Ef(i) Ef(i)], [0 0], ...
                  [Varf(i) + c1S Varf(i); Varf(i) Varf(i) + c1S]);

              zz12(i) = mvncdf([Ef(i) Ef(i)], [0 0], ...
                  [Varf(i) + c1S Varf(i); Varf(i) Varf(i) + c2S]);

              zz22(i) = mvncdf([Ef(i) Ef(i)], [0 0], ...
                  [Varf(i) + c2S Varf(i); Varf(i) Varf(i) + c2S]);

          end
          Ey12 = p1^2 * zz11 + 2*p1*p2 * zz12 + p2^2 * zz22;

      else
          Ey12 = 0;

      end

      % unconditional variance
      Vary = zt .* (p - Ey12) + zt.^2 .* (Ey12 - p.^2);

  %     nt=length(Ef);
  %     Ey=zeros(nt,1);
  %     EVary = zeros(nt,1);
  %     VarEy = zeros(nt,1);
  %     for i1=1:nt
  %        ci = sqrt(Varf(i1));
  %       F  = @(x)zt(i1)./(1+exp(-x)).*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
  %        Ey(i1) = quadgk(F,Ef(i1)-6*ci,Ef(i1)+6*ci);
  %
  %       F2  = @(x)zt(i1)./(1+exp(-x)).*(1-1./(1+exp(-x))).*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
  %       EVary(i1) = quadgk(F2,Ef(i1)-6*ci,Ef(i1)+6*ci);
  %
  %       F3  = @(x)(zt(i1)./(1+exp(-x))).^2.*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
  %       VarEy(i1) = quadgk(F3,Ef(i1)-6*ci,Ef(i1)+6*ci) - Ey(i1).^2;
  %     end
  %     Vary = EVary+VarEy;
  end

  nt=length(yt);
  lpy=zeros(nt,1);
  if (size(Ef,2) > 1) && (size(Ef,2) > 1) && size(yt,2) == 1
    % Approximate integral with sum of grid points when using corrected
    % marginal posterior pf
    for i1=1:length(yt)
      py = arrayfun(@(f) exp(lik.fh.ll(lik, yt(i1), f, zt(i1))), Ef(i1,:));
      pf = Varf(i1,:)./sum(Varf(i1,:));
      lpy(i1) = log(sum(py.*pf));
    end
  else
    for i1=1:nt
      ci = sqrt(Varf(i1));
      F  = @(x)exp(gammaln(zt(i1)+1)-gammaln(yt(i1)+1)-gammaln(zt(i1)-yt(i1)+1) + yt(i1).*log(1./(1+exp(-x))) + (zt(i1)-yt(i1)).*log(1-(1./(1+exp(-x))))).*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
      lpy(i1) = log(quadgk(F,Ef(i1)-6*ci,Ef(i1)+6*ci));
    end
  end

end

function prctys = lik_binomial_predprcty(lik, Ef, Varf, zt, prcty)
%LIK_BINOMIAL_PREDPRCTY  Returns the percentiled of predictive density of y
%
%  Description
%    PRCTY = LIK_BINOMIAL_PREDPRCTY(LIK, EF, VARF YT, ZT)
%    Returns percentiles of the predictive density PY of YT, that is
%    This requires also the succes counts YT, numbers of trials ZT. This
%    subfunction is needed when using function gp_predprcty.
%
%  See also
%    GP_PREDPCTY

  if isempty(zt)
    error(['lik_binomial -> lik_binomial_predprcty: missing z!'...
           'Binomial likelihood needs the expected number of       '...
           'occurrences as an extra input z. See, for             '...
           'example, lik_binomial and gp_optim.                 ']);
  end

  opt=optimset('TolX',.5,'Display','off');
  nt=size(Ef,1);
  prctys = zeros(nt,numel(prcty));
  prcty=prcty/100;
  for i1=1:nt
    ci = sqrt(Varf(i1));
    for i2=1:numel(prcty)
      a=floor(fminbnd(@(a) (quadgk(@(f) binocdf(a,zt(i1),logitinv(f)).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)-prcty(i2)).^2,binoinv(prcty(i2),zt(i1),logitinv(Ef(i1)-1.96*ci)),binoinv(prcty(i2),zt(i1),logitinv(Ef(i1)+1.96*ci)),opt));
      if quadgk(@(f) binocdf(a,zt(i1),logitinv(f)).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)<prcty(i2)
        a=a+1;
      end
      prctys(i1,i2)=a;
    end
  end
end

function [df,minf,maxf] = init_binomial_norm(yy,myy_i,sigm2_i,N)
%INIT_BINOMIAL_NORM
%
%  Description
%    Return function handle to a function evaluating Binomial *
%    Gaussian which is used for evaluating (likelihood * cavity)
%    or (likelihood * posterior) Return also useful limits for
%    integration. This is private function for lik_binomial. This
%    subfunction is needed by subfunctions tiltedMoments and predy.
%
% See also
%   LIK_BINOMIAL_TILTEDMOMENTS, LIK_BINOMIAL_PREDY

% avoid repetitive evaluation of constant part
  ldconst = gammaln(N+1)-gammaln(yy+1)-gammaln(N-yy+1) - log(sigm2_i)/2 - log(2*pi)/2;
%   ldconst = log(factorial(N)/(factorial(yy)*factorial(N-yy))-log(sigm2_i)/2 -log(2*pi)/2;

 % Create function handle for the function to be integrated
  df = @binomial_norm;
 % use log to avoid underflow, and derivates for faster search
  ld = @log_binomial_norm;
  ldg = @log_binomial_norm_g;
  ldg2 = @log_binomial_norm_g2;

  % Set the limits for integration
  % Binomial likelihood is log-concave so the binomial_norm
  % function is unimodal, which makes things easier
  if yy==0 || yy==N
    % with yy==0 or yy==N the mode of the likelihood is not defined
    % use the mode of the Gaussian (cavity or posterior) as a first guess
    modef = myy_i;
  else
    % use precision weighted mean of the Gaussian approximation of the
    % binomial likelihood and Gaussian
    mean_app = log(yy./(N-yy));
    ld0=1/(1+exp(-mean_app));
    ld1=(1-ld0)*ld0;
    ld2=ld0-3*ld0^2+2*ld0^3;
    var_app=inv(-( yy*(ld2*ld0-ld1^2)/ld0^2 + (N-yy)*(ld2*(ld0-1)-ld1^2)/(ld0-1)^2 ));

    modef = (myy_i/sigm2_i + mean_app/var_app)/(1/sigm2_i + 1/var_app);
%     sigm_app = sqrt((1/sigm2_i + 1/var_app)^-1);
  end
  % find the mode of the integrand using Newton iterations
  % few iterations is enough, since the first guess in the right direction
  niter=3;       % number of Newton iterations
  mindelta=1e-6; % tolerance in stopping Newton iterations
  for ni=1:niter
      g = ldg(modef);
      h = ldg2(modef);
      delta=-g/h;
      modef=modef+delta;
      if abs(delta)<mindelta
          break
      end
  end
  if abs(delta)>1 || isinf(delta) || isnan(delta)
    % Newton algorithm didn't work properly so do binary search
    modef=myy_i;
    a=modef-5.*sqrt(sigm2_i); b=modef+5.*sqrt(sigm2_i); delta=1;
    while ldg(a)<0
      a=a-5.*sqrt(sigm2_i);
    end
    while ldg(b)>0
      b=b+5.*sqrt(sigm2_i);
    end
    while delta > 0.1
      modef=(a+b)/2;
      if ldg(modef) > 0
        a=modef;
      else
        b=modef;
      end
      delta=b-a;
    end
    h=ldg2(modef);
  end
  % integrand limits based on Gaussian approximation at mode
  modes=sqrt(-1/h);
  minf=modef-4*modes;
  maxf=modef+4*modes;
  modeld=ld(modef);
  iter=0;
  % check that density at end points is low enough
  lddiff=12; % min difference in log-density between mode and end-points
  minld=ld(minf);
  step=1;
  while minld>(modeld-lddiff)
    minf=minf-step*modes;
    minld=ld(minf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_binomial -> init_binomial_norm: ' ...
             'integration interval minimun not found ' ...
             'even after looking hard!'])
    end
  end
  maxld=ld(maxf);
  iter=0;
  step=1;
  while maxld>(modeld-lddiff)
    maxf=maxf+step*modes;
    maxld=ld(maxf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_binomial -> init_binomial_norm: ' ...
             'integration interval maximun not found ' ...
             'even after looking hard!'])
    end
  end

  function integrand = binomial_norm(f)
  % Logit * Gaussian
    integrand = exp(ldconst + yy*log(1./(1.+exp(-f)))+(N-yy)*log(1-1./(1.+exp(-f)))...
                   - 0.5 * (f-myy_i).^2./sigm2_i);
%     integrand = exp(ldconst ...
%                     +yy*log(x)+(N-yy)*log(1-x) ...
%                     -0.5*(f-myy_i).^2./sigm2_i);
    integrand(isnan(integrand)|isinf(integrand))=0;
  end

  function log_int = log_binomial_norm(f)
  % log(Binomial * Gaussian)
  % log_binomial_norm is used to avoid underflow when searching
  % integration interval

    log_int = ldconst + yy*log(1./(1.+exp(-f)))+(N-yy)*log(1-1./(1.+exp(-f)))...
                   - 0.5 * (f-myy_i).^2./sigm2_i;
%     log_int = ldconst ...
%               -log(1+exp(-yy.*f)) ...
%               -0.5*(f-myy_i).^2./sigm2_i;
    log_int(isnan(log_int)|isinf(log_int))=-Inf;
  end

  function g = log_binomial_norm_g(f)
  % d/df log(Binomial * Gaussian)
  % derivative of log_binomial_norm
    g = -(f-myy_i)./sigm2_i - exp(-f).*(N-yy)./((1+exp(-f)).^2.*(1-1./(1+exp(-f)))) ...
        + exp(-f).*yy./(1+exp(-f));
%     g = yy./(exp(f*yy)+1)...
%         + (myy_i - f)./sigm2_i;
  end

  function g2 = log_binomial_norm_g2(f)
  % d^2/df^2 log(Binomial * Gaussian)
  % second derivate of log_binomial_norm
    g2 = - (1+exp(2.*f)+exp(f).*(2+N*sigm2_i)./((1+exp(f))^2*sigm2_i));
%     a=exp(f*yy);
%     g2 = -a*(yy./(a+1)).^2 ...
%          -1/sigm2_i;
  end

end

function p = lik_binomial_invlink(lik, f, z)
%LIK_BINOMIAL_INVLINK  Returns values of inverse link function
%
%  Description
%    P = LIK_BINOMIAL_INVLINK(LIK, F) takes a likelihood structure LIK and
%    latent values F and returns the values of inverse link function P.
%    This subfunction is needed when using gp_predprctmu.
%
%     See also
%     LIK_BINOMIAL_LL, LIK_BINOMIAL_PREDY

  p = logitinv(f);
end

function reclik = lik_binomial_recappend(reclik, ri, lik)
%RECAPPEND  Append the parameters to the record
%
%  Description
%    RECLIK = GPCF_BINOMIAL_RECAPPEND(RECLIK, RI, LIK) takes a
%    likelihood record structure RECLIK, record index RI and
%    likelihood structure LIK with the current MCMC samples of
%    the parameters. Returns RECLIK which contains all the old
%    samples and the current samples from LIK. This subfunction
%    is needed when using MCMC sampling (gp_mc).
%
%  See also
%    GP_MC

  if nargin == 2
    reclik.type = 'Binomial';

    % Set the function handles
    reclik.fh.pak = @lik_binomial_pak;
    reclik.fh.unpak = @lik_binomial_unpak;
    reclik.fh.ll = @lik_binomial_ll;
    reclik.fh.llg = @lik_binomial_llg;
    reclik.fh.llg2 = @lik_binomial_llg2;
    reclik.fh.llg3 = @lik_binomial_llg3;
    reclik.fh.tiltedMoments = @lik_binomial_tiltedMoments;
    reclik.fh.invlink = @lik_binomial_invlink;
    reclik.fh.predprcty = @lik_binomial_predprcty;
    reclik.fh.predy = @lik_binomial_predy;
    reclik.fh.recappend = @likelih_binomial_recappend;
    return
  end

end


function [x, info] = lewisCenter(f, x, opts)
% x = LewisCenter(f, opts, x)
% compute the analytic center for the domain {Ax=b} intersect the domain of f
%
% Input:
%    f - a ConvexProgram
%    x - a feasible initial point
%    opts - a structure for options with the following properties (optional)
%       MaxIter - maximum number of iterations
%       Output - maximum number of iterations
%       CentralityTol, FeasibilityTol - stop the following are satisfied
%           ||(A' * lambda - grad f(x)) / sqrt(hess)||_inf < CentralityTol
%           ||A x - b||_inf < FeasibilityTol
%       p - the parameter for lp Lewis weight
%       JLDim - the number of dimensions used in estimating leverage score
%
% Output:
%  x - It outputs the analytic center of f
%  info - a structure containing centrality, feasibility and iter.

defaultOpts = struct('MaxIter', 100, 'Output', @disp, 'CentralityTol', 0.1, 'FeasibilityTol', 1e-12, 'JLDim', 10, 'p', 4);
if nargin >= 3
   opts = setField(defaultOpts, opts);
else
   opts = defaultOpts;
end
x = ddouble(x);

%% prepare the printout
output = TableDisplay('Iter', '5i', 'PredStep', '13.2e', 'CorrStep', '13.2e', 'Centrality', '13.2e', 'Feasibility', '13.2e');
output.output = opts.Output;
output.header();

%% initial parameters
assert(nargin >= 2 && all(f.distance(x) > 0), 'a feasible inital point is required.');

A = f.A; At = A'; b = f.b;
y = 0*b;
lastProgress = 0; % record the last iteration making progress
bestCentrality = 1e32;
d = size(f.A, 1); n = size(f.A, 2);
w = (n-d)/n * ones(n, 1);

%% find the central path
[rs, rx, h] = updateResidual(x, y, w);

for iter = 1:opts.MaxIter
   % Compute the cholesky decomposition
   cholErr = f.solver.factorize(1./h);
   w_new = max(double(1-f.solver.leverageScore(opts.JLDim)), 0);
   w = (w + w_new)/2;

   % Compute the direction
   if (cholErr < f.solver.cholTol)
      v = f.solver.solve([A*(rs./h) rx]); %rs = A^T y - g; %rx = Ax - b;
      Atv = At * v;
      y = y - v(:,1);

      % y = y - (R\(R'\(A*(rs./hess))))
      % dx = (rs + At * (R\(R'\(rx - A*(rs./hess)))))./hess;

      dx = (rs - Atv(:,1))./h - Atv(:,2)./h;
      t = stepSize(x, dx, 0.95); % 0.95*(distance from x to the closest boundart in direction dx)
      x = x + t * dx; % move in direction dx with distance of t (measured in dx)

      [rs, rx, h] = updateResidual(x, y, w);

      centrality = max(abs(rs./sqrt(h))); %||(A^T y - grad \phi)/sqrt(h)||_Inf
      feasibility = max(abs(rx)); %||Ax-b||_Inf
      if isempty(feasibility), feasibility = 0; end % Fix the case rx is []

      % Output the error
      o = struct('Iter', iter, 'PredStep', t, 'CorrStep', t, 'Centrality', centrality, 'Feasibility', feasibility);
      output.row(o);

      if (centrality < 0.9 * bestCentrality)
         bestCentrality = centrality;
         lastProgress = iter;
      end

      % Check stop criteria
      if centrality < opts.CentralityTol && feasibility < opts.FeasibilityTol
         break;
      end
   end

   if (iter > lastProgress + 10)
      break;
   end

   if (cholErr >= f.solver.cholTol)
      opts.Output('Failed due to numerical issue.');
      f.feasible = false;
      x = []; info = [];
      return;
   end
end

info = struct('centrality', centrality, 'feasibility', feasibility, 'iter', iter, 'hess', h);

function t = stepSize(x, dx, factor)
   t = min(double(factor*min(f.barrier.distance(x, dx))),1); % min(Nan,1) = 1 for double
end

function [rs, rx, h, wp] = updateResidual(x, y, w)
   wp = w.^(1-2/opts.p);

   [gb, hb] = f.barrier.derivatives(x); % obtain gradient and hessian of log-barrier ftn \phi(x) = -\sum log(x-l) -\sum log(u-x)
   % hp = diagonals of hessian matrix of log-barrier

   gb = gb.*wp;
   hb = hb.*wp;

   gc = zeros(size(gb), class(gb));
   hc = zeros(size(hb), class(hb));

   % Below if-end is skipped while running f.normalize() in sample.m (due to c=[])
   if ~isempty(f.c)
      gc = gc + f.c;
   end

   % Below if-end is skipped while running f.normalize() in sample.m (due to df=[])
   if ~isempty(f.df)
      gc_ = f.df(f.export(x));
      gc = gc + f.scale .* gc_(f.idx);

      hc_ = f.ddf(f.export(x));
      hc = hc + f.scale2 .* hc_(f.idx);
   end

   % Thus, g=gb and h=hb in f.normalize()
   g = gb + gc; h = hb + hc;

   rs = At * y - g;
   rx = A * x - b;
end

end


function [Iq,in_avg,ext_avg,in_min,ext_max]=icassoStability(sR,L,graphmode)
%function Iq=icassoStability(sR,L,graphmode)
%
%PURPOSE
%
%To compute and/or plot the stability (quality) indices of the ICA
%estimate-clusters.
%
% Iq=avg(intra-cluster similarity) - avg(extra-cluster similarity)
%
%See publication Himberg et al. (2004), "Validating the independent
%components of neuroimaging time-series via clustering and
%visualization". NeuroImage, 22:3(1214-1222). Ideally, each ICA
%estimate-cluster should have Iq=1. The smaller the value is, the
%less stable (compact and isolated) the estimate-cluster is.
%
%EXAMPLES OF BASIC USAGE
%
%   Iq=icassoStability(sR);
%
%returns the stability index for each estimate (centrotype) using
%the default number of estimate-clusters.
%
%   Iq=icassoStability(sR,13,'plotindex');
%
%...same as previous but also plots the values (in rank order) and
%uses 13 estimate clusters instead of default.
%
%INPUT
%
%[An argument in brackets is optional. If it isn't  given or it's
% an empty matrix/string, the function will use a default value.]
%
% sR          (struct) Icasso result data structure; clustering
%              must be readily computed.
% [L]         (scalar) number of clusters, default: (reduced) data
%              dimension
% [graphmode] (string) 'none' (default) | 'plotindex' | 'plotstat'
%
%OUTPUTS
%
% Iq  (Lx1 matrix) Iq(C) contains the index value for estimate C
% (actually estimate-cluster C)
%
%DETAILS
%
%The intra-cluster similarities for cluster C mean the mutual
%similarities between the estimates in C, and the extra-cluster
%similarities for C are the similarities between the estimates in C
%and the estimates not in C. The index is the average intra-cluster
%similarity subtracted by the average intra-cluster similarity.
%
%In default graph mode 'none'
% The function computes the index and returns it. No graphical
% output. Iq(C) contains the index for cluster C (That is, for
% estimates sR.cluster.partition(L,:)==C.
%
%In graph mode 'plotindex'
% The function computes the quality index as in mode 'none'
% and plots the index in the active axis (gca) as follows:
% The X-axis shows the value of the index for clusters. The clusters
% are ranked on the Y-axis in descending order according to the
% index. The Y-axis legend shows the cluster number (the same as in
% sR.cluster.partition(L,:)). The values are indicated by black
% dots connected into each other by a dotted line.
%
%In graph mode 'plotstat'
% As previous one but instead of the index,
% some cluster statistics are shown:
%  Light red  indicates the average intra-cluster similarity for a cluster
%  Light blue               average extra-cluster
%  Clear red                minimum intra-cluster
%  Clear blue               maximum extra-cluster
%
%SEE ALSO
% clusterquality
% icassoViz
% icassoShow

%COPYRIGHT NOTICE
%This function is a part of Icasso software library
%Copyright (C) 2003-2005 Johan Himberg
%
%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 any later version.
%
%This program is distributed in the hope that it will be useful,
%but WITHOUT ANY WARRANTY; without even the implied warranty of
%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%GNU General Public License for more details.
%
%You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software
%Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

% ver 1.2 100501 johan

if nargin<2|isempty(L),
  L=icassoGet(sR,'rdim');
  disp([sprintf('\n') 'Number of estimate-clusters not given: using (reduced)' ...
	   ' data dimension.']);
end

if nargin<3 | isempty(graphmode),
  graphmode='none';
end

clusternumber=1:L;
partition=sR.cluster.partition(L,:);

for i=1:L,
  NofEstimates(i)=sum(partition==i);
end

% compute cluster quality index

[Iq,in_avg,ext_avg]=clusterquality('mean',sR.cluster.similarity,partition);
[tmp,in_min,ext_max]=clusterquality('minmax',sR.cluster.similarity,partition);

% Sort entries according to score

[tmp,order]=sort(-Iq);
score=Iq(order);
in_avg=in_avg(order);
ext_avg=ext_avg(order);
in_min=in_min(order);
ext_max=ext_max(order);
clusternumber=clusternumber(order);
NofEstimates=NofEstimates(order);

% make tick mark labels
for i=1:L,
  ticklabel{i}=num2str(clusternumber(i));
end

% Plot
switch lower(graphmode)
 case 'none'
  return;
 case 'plotindex'
  cla reset;
  h_score=plot(score,1:L,'o:');
  set(h_score,'color','k','markersize',8,'markerfacecolor','k');
  set(gca,'ytick',1:L,'yticklabel',ticklabel,'ydir','reve');
  ax=axis;

  if ax(1)<0,
    axis([ax(1) 1  0.5 L+.5]);
  else
    axis([0 1  0.5 L+.5]);
  end

  grid on;
  xlabel('I_q=avg(S(i)_{int})-avg(S(i)_{ext})');
  title('Stability index (I_q) for ICA estimate-clusters');
 case 'plotstat'
  cla reset;

  h_in_avg=barh(in_avg,0.7); hold on
  h_ext_max=barh(-ext_max,0.35);
  h_in_min=barh(in_min,0.35);
  h_ext_avg=barh(-ext_avg,0.7);

  axis on;
  set(h_in_avg,'facecolor',[1 0.7 0.7]); hold on;
  set(h_in_min,'facecolor',[1 0 0]);
  set(h_ext_avg,'facecolor',[0.7 0.7 1]);
  set(h_ext_max,'facecolor',[0 0 1]);
  axis([-1 1 0.5 L+.5]);

  set(gca,'ytick',1:L,'yticklabel',ticklabel,...
	  'ydir','reve','xtick',[-1 -.5 0 .5 1],...
	  'xgrid','on',...
	  'xticklabel',{'1' '0.5 ' '0' '0.5' '1'});

  title('Statistics on within- and between-cluster similarities');
  xlabel('clusters are ordered according to I_q=avg(S(i)_{int})-avg(S(i)_{ext})');
  legend([h_in_avg(1),h_ext_avg(1),h_in_min(1),h_ext_max(1)],...
	 {'mean S_{in}' 'mean S_{ex}' 'min S_{in}' 'max S_{ex}'},-1);
 otherwise
  error('Graphmode must be ''none'',''plotindex'' or ''plotstat''.');
end

ylabel('Label')


function  p2 = project_3D_to_2D(p3)
d = distmatrix(p3);
p2_0 = p3(:,1:2);
fun = @(x)myfunc(x,d);

warnid = {'optim:fsolve:NonSquareSystem'};
for ii = 1:length(warnid)
    warning('OFF', warnid{ii});
end

p2 = fsolve(fun, p2_0, optimset('Display','off'));

for ii = 1:length(warnid)
    warning('ON', warnid{ii});
end

% -------------------------------------------------------
function  F = myfunc(p, d)
F = [];
kk = 1;
c = 1.2;
for ii = 1:size(d,1)
    for jj = 1:size(d,2)
        if d(ii,jj) == 0
            continue
        end
        F(kk) = sqrt((p(ii,1) - p(jj,1))^2 + (p(ii,2) - p(jj,2))^2) - d(ii,jj)*c;
        kk = kk+1;
    end
end

% -------------------------------------------------------------------
function d = distmatrix(varargin)

d=[];

v1 = double(varargin{1});
if size(v1,2) == 2
    v1 = [v1, zeros(size(v1,1),1)];
end

if length(varargin)==1

    n = size(v1,1);
    d = zeros(n);
    for i = 1:n
        for j = i+1:n
            d(i,j) = ((v1(i,1) - v1(j,1))^2 + ...
                      (v1(i,2) - v1(j,2))^2 + ...
                      (v1(i,3) - v1(j,3))^2)^0.5;
        end
    end

elseif length(varargin)==2

    v2 = double(varargin{2});
    m = size(v1,1);
    n = size(v2,1);
    d = zeros(m,n);
    for i = 1:m
        for j=1:n
            d(i,j) = ((v1(i,1) - v2(j,1))^2 + ...
                      (v1(i,2) - v2(j,2))^2 + ...
                      (v1(i,3) - v2(j,3))^2)^0.5;
        end
    end

end


function [r, pol, res, zer, zj, fj, wj, errvec, wt] = aaa(F, varargin)
%AAA   AAA and AAA-Lawson (near-minimax) real or complex rational approximation.
%   R = AAA(F, Z) computes the AAA rational approximant R (function handle) to
%   data F on the set of sample points Z.  F may be given by its values at Z,
%   or as a function handle or chebfun.  R = AAA(F, Z, 'degree', N) attempts to
%   compute the minimax approximation of degree N (i.e., rational type (N,N)).
%
%   [R, POL, RES, ZER] = AAA(F, Z) returns vectors of poles, residues, and zeros
%   of R.
%
%   [R, POL, RES, ZER, ZJ, FJ, WJ] = AAA(F, Z) also returns the vectors of
%   support points ZJ, approximation values FJ = r(ZJ), and weights WJ of the
%   barycentric representation of R.
%
%   [R, POL, RES, ZER, ZJ, FJ, WJ, ERRVEC] = AAA(F, Z) also returns the vector
%   of errors ||f-r||_infty in successive iterative steps of AAA.  Note that the
%   rational degrees are not 1,...,length(ERRVEC) but 0,...,length(ERRVEC)-1.
%
%   R = AAA(F, Z, NAME, VALUE) sets the following parameters:
%   - 'tol', TOL: relative tolerance (default TOL = 1e-13),
%   - 'degree', N: maximal degree (default N = 99).
%       The output rational approximant will be of degree at most N.  Like
%       'mmax', N+1, except that Lawson is turned on by default: see below.
%   - 'mmax', MMAX: maximal number of terms in the barycentric representation
%       (default MMAX = 100). R will be of degree at most MMAX-1.  Like
%       'degree', MMAX-1, except that Lawson is not turned on by default.
%   - 'dom', DOM: domain (default DOM = [-1, 1]).  No effect if Z is provided.
%   - 'cleanup', 'off' or 0: turns off automatic removal of Froissart doublets.
%   - 'cleanuptol', CLEANUPTOL: cleanup tolerance (default CLEANUPTOL = TOL).
%       Poles with residues less than this number times the geometric mean size
%       of F times the minimum distance to Z are deemed spurious by the cleanup
%       procedure.  If TOL = 0, then CLEANUPTOL defaults to 1e-13.
%   - 'lawson', NLAWSON: take NLAWSON iteratively reweighted least-squares steps
%       to bring approximation closer to minimax.  Specifying NLAWSON = 0
%       ensures there is no Lawson iteration.  See next paragraph.
%
%   If 'degree' is specified and 'lawson' is not, AAA attempts to find a minimax
%   approximant of degree N by AAA-Lawson iteration.  This will generally be
%   successful only if the minimax error is well above machine precision, and
%   is more reliable for complex problems than real ones.  If 'degree' and
%   'lawson' are both specified, then exactly NLAWSON Lawson steps are taken
%   (so NLAWSON = 0 corresponds to AAA approximation with no Lawson iteration).
%   The final weight vector WT of the Lawson iteration is available with
%   [R, POL, RES, ZER, ZJ, FJ, WJ, ERRVEC, WT] = AAA(F, Z).
%
%   Note that R may have fewer than N poles and zeros.  This may happen, for
%   example, if N is too large, or if F is even and N is odd, or if F is odd
%   and N is even.
%
%   One can also execute R = AAA(F), with no specification of a set Z.  If F is
%   a vector, this is equivalent to R = AAA(F, Z) with
%   Z = LINSPACE(-1, 1, LENGTH(F)).  If F is a function handle, AAA attempts
%   to resolve F on [-1,1] by default.
%
%   This standalone code works in GNU Octave as well as MATLAB.
%
% Examples:
%   r = aaa(@exp); xx = linspace(-1,1); plot(xx,r(xx)-exp(xx))
%
%   r = aaa(@exp,'degree',4); xx = linspace(-1,1); plot(xx,r(xx)-exp(xx))
%
%   X = linspace(-1,1,30); r = aaa(gamma(X),X);
%   fplot(r,[-5,5]), axis([-5 5 -15 15]), grid on
%
%   Z = exp(2i*pi*linspace(0,1,500));
%   [r,pol,res] = aaa(@tan,Z); disp([pol res])
%
%   X = linspace(-1,1,1000); F = tanh(20*X);
%   subplot(1,2,1)
%   r = aaa(F,X,'degree',15,'lawson',0); plot(X,F-r(X)), hold on
%   r = aaa(F,X,'degree',15); plot(X,F-r(X)), hold off
%
%   Z = exp(1i*pi*linspace(-1,1,1000)); G = exp(Z);
%   subplot(1,2,2)
%   r = aaa(G,Z,'degree',3,'lawson',0); plot(G-r(Z)), axis equal, hold on
%   r = aaa(G,Z,'degree',3); plot(G-r(Z)), axis equal, hold off
%
%   References on AAA and AAA-Lawson, respectively:
%
%   [1] Y. Nakatsukasa, O. Sete, and L. N. Trefethen, "The AAA algorithm
%   for rational approximation", SIAM J. Sci. Comp. 40 (2018), A1494-A1522.
%
%   [2] Y. Nakatsukasa and L. N. Trefethen, An algorithm for real and
%   complex rational minimax approximation, SIAM J. Sci. Comp. 42 (2020),
%   A3157-A3179.
%
% See also AAATRIG, CF, CHEBPADE, MINIMAX, PADEAPPROX, RATINTERP.

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

% Parse inputs:
[F, Z, M, dom, tol, mmax, cleanup_flag, cleanup_tol, needZ, mmax_flag, ...
    nlawson, degree_flag, degree] = parseInputs(F, varargin{:});

if ( needZ )
    % Z was not provided.  Try to resolve F on its domain.
    [r, pol, res, zer, zj, fj, wj, errvec] = ...
        aaa_autoZ(F, dom, tol, mmax, cleanup_flag, cleanup_tol, mmax_flag, ...
            nlawson, degree_flag, degree);
    return
end

% Remove infinite or NaN function values and repeated entries:
toKeep = ~isinf(F) & ~isnan(F);
F = F(toKeep); Z = Z(toKeep);
[Z, uni] = unique(Z,'stable'); F = F(uni);

% Initialization for AAA iteration:
M = length(Z);
abstol = tol*norm(F, inf);                 % Absolute tolerance
J = (1:M)';
zj = []; fj = []; C = []; A = [];
errvec = [];
R = mean(F)*ones(size(J));

% AAA iteration:
for m = 1:mmax
    % Introduce next support point:
    [~, jj] = max(abs(F(J) - R(J)));       % Select next support point
    zj = [zj; Z(J(jj))];                   % Update support points
    fj = [fj; F(J(jj))];                   % Update data values
    C = [C 1./(Z - Z(J(jj)))];             % Next column of Cauchy matrix
    J(jj) = [];                            % Update index vector
    A = [A, (F-fj(end)).*C(:,end)];        % Update Loewner matrix

    % Compute weights:
    if ( length(J) >= m )                  % The usual tall-skinny case
        [~, S, V] = svd(A(J,:), 0);        % Reduced SVD
        s = diag(S);
        mm = find( s == min(s) );          % Treat case of multiple min sing val
        nm = length(mm);
        wj = V(:,mm)*ones(nm,1)/sqrt(nm);  % Aim for non-sparse wt vector
    elseif ( length(J) >= 1 )
        V = null(A(J,:));                  % Fewer rows than columns
        nm = size(V,2);
        wj = V*ones(nm,1)/sqrt(nm);        % Aim for non-sparse wt vector
    else
        wj = ones(m,1)/sqrt(m);            % No rows at all (needed for Octave)
    end

    % Compute rational approximant:
    i0 = find(wj~=0);                      % Omit columns with wj = 0
    N = C(:,i0)*(wj(i0).*fj(i0));          % Numerator
    D = C(:,i0)*wj(i0);                    % Denominator
    R = N./D;
    Dinf = isinf(D);
    R(Dinf) = F(Dinf);                     % Interpolate at supp pts with wj~=0

    % Check if converged:
    maxerr = norm(F - R, inf);
    errvec = [errvec; maxerr];
    if ( maxerr <= abstol )
        break
    end
end
maxerrAAA = maxerr;                        % Error at end of AAA

% We now enter Lawson iteration: barycentric IRLS = iteratively reweighted
% least-squares if 'lawson' is specified with NLAWSON > 0 or 'mmax' is
% specified and 'lawson' is not.  In the latter case the number of steps
% is chosen adaptively.  Note that the Lawson iteration is unlikely to be
% successful when the errors are close to machine precision.

wj0 = wj; fj0 = fj;                        % Save params in case Lawson fails
wt = NaN(M,1); wt_new = ones(M,1);
if ( nlawson > 0 )                         % Lawson iteration

    maxerrold = maxerrAAA;
    maxerr = maxerrold;
    nj = length(zj);
    A = [];
    for j = 1:nj                           % Cauchy/Loewner matrix
        A = [A 1./(Z-zj(j)) F./(Z-zj(j))];
    end
    for j = 1:nj
        [i,~] = find(Z==zj(j));            % support pt rows are special
        A(i,:) = 0;
        A(i,2*j-1) = 1;
        A(i,2*j) = F(i);
    end
    stepno = 0;
    while ( (nlawson < inf) && (stepno < nlawson) ) || ...
          ( (nlawson == inf) && (stepno < 20) ) || ...
          ( (nlawson == inf) && (maxerr/maxerrold < .999) && (stepno < 1000) )
        stepno = stepno + 1;
        wt = wt_new;
        W = spdiags(sqrt(wt),0,M,M);
        [~,~,V] = svd(W*A,0);
        c = V(:,end);
        denom = zeros(M,1); num = zeros(M,1);
        for j = 1:nj
            denom = denom + c(2*j)./(Z-zj(j));
            num = num - c(2*j-1)./(Z-zj(j));
        end
        R = num./denom;
        for j = 1:nj
            [i,~] = find(Z==zj(j));        % support pt rows are special
            R(i) = -c(2*j-1)/c(2*j);
        end
        err = F - R; abserr = abs(err);
        wt_new = wt.*abserr; wt_new = wt_new/norm(wt_new,inf);
        maxerrold = maxerr;
        maxerr = max(abserr);
    end
    wj = c(2:2:end);
    fj = -c(1:2:end)./wj;
    % If Lawson has not reduced the error, return to pre-Lawson values.
    if ( (maxerr > maxerrAAA) && (nlawson == Inf) )
        wj = wj0; fj = fj0;
    end
end

% Remove support points with zero weight:
I = find(wj == 0);
zj(I) = []; wj(I) = []; fj(I) = [];

% Construct function handle and compute poles, residues and zeros:
r = @(zz) reval(zz, zj, fj, wj);
[pol, res, zer] = prz(zj, fj, wj);

if ( cleanup_flag == 1 && nlawson == 0 )      % Remove Froissart doublets
    [r, pol, res, zer, zj, fj, wj] = ...
        cleanup(r, pol, res, zer, zj, fj, wj, Z, F, cleanup_tol);
elseif ( cleanup_flag == 2 && nlawson == 0 )  % Alternative cleanup.  Currently
    a.zj = zj; a.fj = fj; a.wj = wj;          % an undocumented feature,
    a.Z = Z; a.F = F;                         % pending further investigation.
    a.cleanup_tol = max(cleanup_tol, eps);
    c = cleanup2(a);
    zj = c.zj; fj = c.fj; wj = c.wj;
    r = @(zz) reval(zz, zj, fj, wj);
    [pol, res, zer] = prz(zj, fj, wj);
end

end % of AAA()

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   PARSEINPUTS   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [F, Z, M, dom, tol, mmax, cleanup_flag, cleanup_tol, ...
    needZ, mmax_flag, nlawson, degree_flag, degree] = parseInputs(F, varargin)

% Check if F is empty:
if ( isempty(F) )
    error('AAA:emptyF', 'No function given.')
elseif ( isa(F, 'chebfun') )
    if ( size(F, 2) ~= 1 )
        error('AAA:nColF', 'Input chebfun must have one column.')
    end
end

% Sample points:
if ( ~isempty(varargin) && isfloat(varargin{1}) )
    % Z is given.
    Z = varargin{1};
    if ( isempty(Z) )
        error('AAA:emptyZ', ...
            'If sample set is provided, it must be nonempty.')
    end
    varargin(1) = [];
end

% Set defaults for other parameters:
tol = 1e-13;                   % Relative tolerance
mmax = 100;                    % Maximum number of terms
degree = NaN;                  % Specified degree
cleanup_tol = 1e-13;           % Cleanup tolerance
nlawson = Inf;                 % Number of Lawson steps (Inf means adaptive)
% Domain:
if ( isa(F, 'chebfun') )
    dom = F.domain([1, end]);
else
    dom = [-1, 1];
end
cleanup_flag = 1;              % Cleanup on
mmax_flag = 0;                 % Checks if mmax manually specified
degree_flag = 0;               % Checks if degree specified
cleanup_set = 0;               % Checks if cleanup_tol manually specified
while ( ~isempty(varargin) )   % Check if parameters have been provided
    if ( strncmpi(varargin{1}, 'tol', 3) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
            tol = varargin{2};
            if ( ~cleanup_set && tol > 0 ) % If not set, set cleanup_tol to tol
              cleanup_tol = tol;
            end
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'degree', 6) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
            if ( mmax_flag == 1 ) && ( mmax ~= varargin{2}+1 )
                error('AAA:degmmaxmismatch', ' mmax must equal degree+1.')
            end
            degree = varargin{2};
            mmax = degree + 1;
            mmax_flag = 1;
            degree_flag = 1;
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'mmax', 4) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
            if ( mmax_flag == 1 ) && ( mmax ~= varargin{2})
                error('AAA:degmmaxmismatch', ' mmax must equal degree+1.')
            end
            mmax = varargin{2};
            mmax_flag = 1;
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'lawson', 6) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
            nlawson = varargin{2};
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'dom', 3) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 2]) )
            dom = varargin{2};
        end
        varargin([1, 2]) = [];
        if ( isa(F, 'chebfun') )
            if ( ~isequal(dom, F.domain([1, end])) )
                warning('AAA:dom', ...
                    ['Given domain does not match that of the chebfun.\n', ...
                    'Results may be inaccurate.'])
            end
        end

    elseif ( strncmpi(varargin{1}, 'cleanuptol', 10) )
        if ( isfloat(varargin{2}) && isequal(size(varargin{2}), [1, 1]) )
          cleanup_tol = varargin{2};
          cleanup_set = 1;
        end
        varargin([1, 2]) = [];

    elseif ( strncmpi(varargin{1}, 'cleanup', 7) )
        if ( strncmpi(varargin{2}, 'off', 3) || ( varargin{2} == 0 ) )
            cleanup_flag = 0;
        elseif ( varargin{2} == 2 )     % Alternative cleanup
            cleanup_flag = 2;
        end
        varargin([1, 2]) = [];

    else
        error('AAA:UnknownArg', 'Argument unknown.')
    end
end

% Deal with Z and F:
if ( ~exist('Z', 'var') && isfloat(F) )
    % F is given as data values, pick same number of sample points:
    Z = linspace(dom(1), dom(2), length(F)).';
end

if ( exist('Z', 'var') )
    % Z is given:
    needZ = 0;

    % Work with column vector:
    Z = Z(:);
    M = length(Z);

    % Function values:
    if ( isa(F, 'function_handle') || isa(F, 'chebfun') )
        % Sample F on Z:
        F = F(Z);
    elseif ( isnumeric(F) )
        % Work with column vector and check that it has correct length.
        F = F(:);
        if ( length(F) ~= M )
            error('AAA:lengthFZ', ...
                'Inputs F and Z must have the same length.')
        end
    elseif ( ischar(F) )
        % F is given as a string input. Convert it to a function handle.
        F = inline(vectorize(F));
        F = F(Z);
    else
        error('AAA:UnknownF', 'Input for F not recognized.')
    end

else
    % Z was not given.  Set flag that Z needs to be determined.
    % Also set Z and M since they are needed as output.
    needZ = 1;
    Z = [];
    M = length(Z);
end

if ( ~degree_flag && (nlawson == Inf) )
    nlawson = 0;
end

end % End of PARSEINPUTS.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   CLEANUP   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [r, pol, res, zer, z, f, w] = ...
    cleanup(r, pol, res, zer, z, f, w, Z, F, cleanup_tol)
% Remove spurious pole-zero pairs.

% Find negligible residues:
if any(F)
    geometric_mean_of_absF = exp(mean(log(abs(F(F~=0)))));
else
    geometric_mean_of_absF = 0;
end
Zdistances = NaN(size(pol));
for j = 1:length(Zdistances)
    Zdistances(j) = min(abs(pol(j)-Z));
end
ii = find(abs(res)./Zdistances < cleanup_tol * geometric_mean_of_absF);
ni = length(ii);
if ( ni == 0 )
    return
elseif ( ni == 1 )
    warning('AAA:Froissart','1 Froissart doublet');
else
    warning('AAA:Froissart',[int2str(ni) ' Froissart doublets']);
end

% For each spurious pole find and remove closest support point:
for j = 1:ni
    azp = abs(z-pol(ii(j)));
    jj = find(azp == min(azp),1);

    % Remove support point(s):
    z(jj) = []; f(jj) = [];
end

% Remove support points z from sample set:
for jj = 1:length(z)
    F(Z == z(jj)) = [];
    Z(Z == z(jj)) = [];
end
m = length(z);
M = length(Z);

% Build Loewner matrix:
SF = spdiags(F, 0, M, M);
Sf = diag(f);
C = 1./(Z-z.');               % Cauchy matrix.
A = SF*C - C*Sf;              % Loewner matrix.

% Solve least-squares problem to obtain weights:
[~, ~, V] = svd(A, 0);
w = V(:,m);

% Build function handle and compute poles, residues and zeros:
r = @(zz) reval(zz, z, f, w);
[pol, res, zer] = prz(z, f, w);

end % End of CLEANUP.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   CLEANUP2   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function c = cleanup2(a)
%% Alternative cleanup procedure to remove spurious pole-zero pairs.
% This considers pole-zero distances.  Stefano Costa, August 2022.

z = a.zj; f = a.fj; w = a.wj;
[pol, res, zer] = prz(z, f, w);

cleanup_tol = a.cleanup_tol;

niter = 0;
while(true)
    niter = niter+1;
    Z = a.Z; F = a.F;
    ii = [];
    for jj = 1:length(pol)
        dz = min(abs(zer-pol(jj))); if isempty(dz), dz = 1e100; end
        dS = abs(Z-pol(jj));
        ds = min(dS);
        if any(F)
            q = 4*pi*abs(F).*dS;
            Q = mean(q);                % Arithmetic mean
        else
            Q = 0;
        end
        R = 8*cleanup_tol*Q/(4*pi);    % Equivalent residue value

        % Conditions to expunge poles
        % Expunge if either minimum distance is zero
        if (ds==0) || (dz==0)
            ii = [ii; jj];
        % Expunge if Z is a real interval
        elseif isreal(Z) && (abs(imag(pol(jj)))<eps) && ...
            (real(pol(jj))>=min(Z)) && (real(pol(jj))<=max(Z))
            ii = [ii; jj];
        % Expunge if Z is the unit disk
        elseif all(abs(Z)==1) && (abs(abs(pol(jj))-1)<eps)
            ii = [ii; jj];
        % Expunge if distance to closest zero is undetectable
        elseif (dz/ds<1) && (dz<max(cleanup_tol^2,eps))
            ii = [ii; jj];
        % Expunge if a nearby zero exists and residue is below the
        % equivalent value R. Two choices for real and complex F
        elseif ((dz/ds)<sqrt(cleanup_tol))
            if ( ~any(imag(F)) && (abs(real(res(jj))) < R) )
                ii = [ii; jj];
            elseif (abs(res(jj)) < R)
                ii = [ii; jj];
            end
        end
    end
    ii = unique(ii);

    ni = length(ii);
    if ( ni == 0 )
        % Nothing to do.
        break;
    elseif ( ni == 1 )
        warning('AAA:Froissart',...
            ['1 Froissart doublet, niter = ', int2str(niter)]);
    else
        warning('AAA:Froissart',...
            [int2str(ni) ' Froissart doublets, niter = ' int2str(niter)]);
    end

    % For each spurious pole find and remove closest support point:
    for j = 1:ni
        azp = abs(z-pol(ii(j)));
        jj = find(azp == min(azp),1);

        % Remove support point(s):
        z(jj) = [];
        f(jj) = [];
    end

    % Remove support points z from sample set:
    for jj = 1:length(z)
        F(Z == z(jj)) = [];
        Z(Z == z(jj)) = [];
    end
    m = length(z);
    M = length(Z);

    % Build Loewner matrix:
    SF = spdiags(F, 0, M, M);
    Sf = diag(f);
    C = 1./(Z-z.');             % Cauchy matrix.
    A = SF*C - C*Sf;            % Loewner matrix.

    % Solve least-squares problem to obtain weights:
    [~, ~, V] = svd(A, 0);
    w = V(:,m);

    % Compute poles, residues and zeros:
    [pol, res, zer] = prz(z, f, w);
end % End of while loop

c.zj = z; c.fj = f; c.wj = w;

end  % End of CLEANUP2.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   AAA_AUTOZ   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [r, pol, res, zer, zj, fj, wj, errvec] = ...
    aaa_autoZ(F, dom, tol, mmax, cleanup_flag, cleanup_tol, mmax_flag, ...
               nlawson, degree_flag, degree)
% Automated choice of sample set

% Flag if function has been resolved:
isResolved = 0;

% Main loop:
for n = 5:14
    % Sample points:
    % Next line enables us to do pretty well near poles
    Z = linspace(dom(1)+1.37e-8*diff(dom), dom(2)-3.08e-9*diff(dom), 1 + 2^n).';
    if degree_flag
       [r, pol, res, zer, zj, fj, wj, errvec] = aaa(F, Z, 'tol', tol, ...
          'mmax', mmax, 'cleanup', cleanup_flag, 'cleanuptol', cleanup_tol, ...
          'lawson', nlawson, 'degree', degree);
    else
       [r, pol, res, zer, zj, fj, wj, errvec] = aaa(F, Z, 'tol', tol, ...
          'mmax', mmax, 'cleanup', cleanup_flag, 'cleanuptol', cleanup_tol, ...
          'lawson', nlawson);
    end
    % Test if rational approximant is accurate:
    abstol = tol * norm(F(Z), inf);

    % On Z(n):
    err(1,1) = norm(F(Z) - r(Z), inf);

    Zrefined = linspace(dom(1)+1.37e-8*diff(dom), dom(2)-3.08e-9*diff(dom), ...
        round(1.5 * (1 + 2^(n+1)))).';
    err(2,1) = norm(F(Zrefined) - r(Zrefined), inf);
    if ( all(err < abstol) )
        % Final check that the function is resolved, inspired by sampleTest().
        % Pseudo random sample points in [-1, 1]:
        xeval = [-0.357998918959666; 0.036785641195074];
        % Scale to dom:
        xeval = (dom(2) - dom(1))/2 * xeval + (dom(2) + dom(1))/2;

        if ( norm(F(xeval) - r(xeval), inf) < abstol )
            isResolved = 1;
            break
        end
    end
end

if ( ( isResolved == 0 ) && ~mmax_flag )
    warning('AAA:notResolved', ...
        'Function not resolved using %d pts.', length(Z))
end

end % End of AAA_AUTOZ.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   PRZ   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [pol, res, zer] = prz(zj, fj, wj)
%   Compute poles, residues, and zeros of rational fun in barycentric form.

% Compute poles via generalized eigenvalue problem:
m = length(wj);
B = eye(m+1);
B(1,1) = 0;
E = [0 wj.'; ones(m, 1) diag(zj)];
pol = eig(E, B);
pol = pol(~isinf(pol));

% Compute residues via formula for res of quotient of analytic functions:
N = @(t) (1./(t-zj.')) * (fj.*wj);
Ddiff = @(t) -((1./(t-zj.')).^2) * wj;
res = N(pol)./Ddiff(pol);

% Compute zeros via generalized eigenvalue problem:
E = [0 (wj.*fj).'; ones(m, 1) diag(zj)];
zer = eig(E, B);
zer = zer(~isinf(zer));

end % End of PRZ.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   REVAL   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function r = reval(zz, zj, fj, wj)
%   Construct function handle to evaluate rational function in barycentric form.

zv = zz(:);                         % vectorize zz if necessary
CC = 1./(zv-zj.');                  % Cauchy matrix
r = (CC*(wj.*fj))./(CC*wj);         % vector of values

% Deal with input inf: r(inf) = lim r(zz) = sum(w.*f) / sum(w):
r(isinf(zv)) = sum(wj.*fj)./sum(wj);

% Deal with NaN:
ii = find(isnan(r));
for jj = 1:length(ii)
    if ( isnan(zv(ii(jj))) || ~any(zv(ii(jj)) == zj) )
        % r(NaN) = NaN is fine.
        % The second case may happen if r(zv(ii)) = 0/0 at some point.
    else
        % Clean up values NaN = inf/inf at support points.
        % Find the corresponding node and set entry to correct value:
        r(ii(jj)) = fj(zv(ii(jj)) == zj);
    end
end

% Reshape to input format:
r = reshape(r, size(zz));

end % End of REVAL.