function [hyp, flogtheta, i,simy,simse2] = trainGPoe(hyp, inf, mean, cov, lik, input, target, lag, simf, Nsamples)
% Function for the optimisation (training) of GP model hyperparameters
%
%% Syntax
% [hyp, flogtheta, i] = trainGPoe(hyp, inf, mean, cov, lik, input, target,
%                                 simf, lag, Nsamples);
%
%% Description
% Function for the optimisation (training) of GP model hyperparameters
% based on the training data via maximum marginal likelihood.
% Uses routines gp and minimize.
% Based on the work of C.E.Rasmussen.
%
% Input:
% * hyp      ... the structure of initial hyperparameters
% * inf      ... the function specifying the inference method
% * cov      ... the prior covariance function (see below)
% * mean     ... the prior mean function
% * lik      ... the likelihood function
% * input    ... the input part of the training data,  NxD matrix
% * target   ... the output part of the training data (ie. target), Nx1 vector
% * simf     ... the function handle of simulation function (e.g. @simulGPmc)
% * lag      ... the order of the model (number of used lagged outputs)
% * Nsamples ... the number of samples for MCMC simulation (optional)
% * minf     ... the function handle of the minimization method to be used
%                (optional, default=@minimize_and_return). The minimization
%				  method should return the following values:
%				  [X, fX, i,fout] = minimize(X, f, length, P1, P2, P3, ... ),
%				  where fout is the additional argument (along fitness and gradient)
%				  of the evaulation function f
%
% Output:
% * hyp       ... optimized hyperparameters
% * flogtheta ... the minus log likelihood for the different runs (init. to 0)
% * i         ... the number of iterations needed for the last optimization
%
% Examples:
% demo_example_gp_training.m
%
% See Also:
% gp, minimize, covFunctions, trainlgmp
%

if(nargin < 11)
  minf = @minimize;
end
if (nargin < 10)
  Nsamples = 100; % default value of Nsamples for MC simulation
end
if (nargin < 9)
  simf = @simulGPnaive; %choose the default simulation routine
end

if (nargin < 8)
  warning(['The number of dynamical system model order was not provided. ' ...
  'Guessing from the input data...']);
  lag=size(input,2)/2;
  assert(mod(lag,1)==0);
  fprintf('Guess: lag=%d\n',lag);
end

if (nargin < 7)
  error('Too few parameters are given.');
end

MIN_DIFF = 0.002;
flogtheta = [0,-1/eps];

% we reconstruct the input signal from the regressor matrix as
%  the two following commented lines show:
%%N = length(input,1) +    lag-1
%%[u(1) ... u(N-lag)]';	[u(N-lag+1) ...  u(N)]

u=[input(:,lag+1)      ;	input(end,lag+2:end);NaN];

simulated_y=[input(1:lag,1);target];

while (abs(flogtheta(end) - flogtheta(end-1))>MIN_DIFF)
	disp(' ');
	disp(strcat(['delta flogtheta: ', num2str(abs(flogtheta(end) - flogtheta(end-1)))]));
	disp(' ')
	[hyp, flogthetatmp,i] = feval(minf, hyp, @simLL, -100,...
				inf, mean, cov, lik, u, simulated_y, target, lag, simf);

	%in the last iteration of minimisation algorithm the simulation is evaluated again for obtaining
	% the simulated response 'simulated_y'
	disp('saving simulated response.')
    [~,~,simulated_y] = simLL(hyp,inf, mean, cov, lik, u, simulated_y, target, lag, simf);

    if isempty(flogthetatmp) % no improvement: at minimum
        disp('oops')
        break
    end
	flogtheta = [flogtheta flogthetatmp(end)];
end

function [nlZ,dnlZ,simulated_y]=simLL(hyp, inf, mean, cov, lik, u, simulated_y, target,lag,simf)
	% simLL simulates the response and forms the regressor vectors from the response. Despite the response signal in regressor vectors is the simulated one, the regressands are always the original measurements.
	%
	% log-likelihood is calculated from the gp model with input data from simulation (in regressor matrix) and measurements (as regressands).
	%
	% simulated_y is a NxS or 2NxS matrix where N is the signal length and S is the number of past iterations. The values from N+1 to 2N represent the predicted variances.
	%
	N=size(target,1)+lag;
	if size(simulated_y,1)==N
		simulated_y=[simulated_y;simulated_y*NaN];
	end

	simulated_input0 = construct(lag, u, simulated_y(1:N,end));

	[y_mu, se2] = feval(simf,hyp, inf, mean, cov, lik, simulated_input0, ...
	target, simulated_input0, lag);

	%y_mu_full combines measured initial values and simulated response:
	simulated_y(1:N,end+1)=[simulated_y(1:lag,end);y_mu(:)];
	simulated_y(N+1:2*N,end)=[zeros(lag,1);se2(:)];

	simulated_input1 = construct(lag, u, simulated_y(1:N,end));
	[nlZ,dnlZ]=gp(hyp,inf,mean,cov,lik,simulated_input1,target);
end

simy=simulated_y(1:end/2,:);
simse2=simulated_y(end/2:end,:);
end


I = imread('tire.tif');
figure(1); imshow(I); figure(2); imhist(I); figure(3); imshow(255-I); figure(4); imhist(255-I);
figure(5); imshow(I); figure(6); imhist(I); figure(7); histeq(I); figure(8); imhist(histeq(I));


function model = mvuOptimise(model, display, iters)

% MVUOPTIMISE Optimise an MVU model.
% FORMAT
% DESC optimises a maximum variance unfolding model.
% ARG model : the model to be optimised.
% RETURN model : the optimised model.
%
% SEEALSO : mvuCreate, modelOptimise
%
% COPYRIGHT : Neil D. Lawrence, 2009

% MLTOOLS

if(any(any(isnan(model.Y))))
  error('Cannot run MVU when missing data is present.');
end

[X, details] = mvu(distance(model.Y'), model.k, 'solver', model.solver);

model.X = X(1:1:model.q,:)';
model.lambda = details.D/sum(details.D);

function D = distance(Y)

  D = sqrt(dist2(Y', Y'));
return


function sensor_data = kspaceSecondOrder(kgrid, medium, source, sensor, varargin)
%KSPACESECONDORDER  Fast time-domain simulation of wave propagation for homogeneous media.
%
% DESCRIPTION:
%       kspaceSecondOrder simulates the time-domain propagation of linear
%       compressional waves through a one, two, or three dimensional
%       homogeneous acoustic medium given four input structures: kgrid,
%       medium, source, and sensor. The computation is based on an exact
%       second-order k-space model for media with power law absorption. At
%       each time-step (defined by kgrid.t_array), the pressure at the
%       positions defined by sensor.mask are recorded and stored. If
%       kgrid.t_array is set to 'auto', this array is automatically
%       generated using makeTime. To prevent wave wrapping, the
%       computational domain can be automatically expanded by a factor of
%       two by setting the optional input 'ExpandGrid' to true.
%
%       An initial pressure distribution can be specified by assigning a
%       matrix (the same size as the computational grid) of arbitrary
%       numeric values to source.p0. An initial pressure gradient can
%       similarly be specified using source.dp0dt. The pressure is returned
%       as an array of time series at the sensor locations defined by
%       sensor.mask. This is specified as a binary matrix (i.e., a matrix
%       of 1's and 0's the same size as the computational grid)
%       representing the grid points within the computational grid that
%       will collect the data. The sensor_data is returned using MATLAB's
%       standard column-wise linear matrix index ordering with the recorded
%       data indexed as sensor_data(sensor_position, time). The final
%       pressure field over the complete computational grid can also be
%       obtained by setting sensor.record to {'p', 'p_final'}. In this
%       case, the output sensor_data is returned as a structure with the
%       outputs appended as the structure fields sensor_data.p and
%       sensor_data.p_final.
%
%       Compared to the first-order simulation functions
%       kspaceFirstOrder1D, kspaceFirstOrder2D, and kspaceFirstOrder3D,
%       kspaceSecondOrder is restricted to homogeneous media and has less
%       functionality. However, it is also more computationally efficient
%       and allows an initial pressure gradient to be specified.
%
% USAGE:
%       sensor_data = kspaceSecondOrder(kgrid, medium, source, sensor)
%       sensor_data = kspaceSecondOrder(kgrid, medium, source, sensor, ...)
%
% INPUTS:
%       kgrid               - k-Wave grid structure returned by makeGrid
%                             containing Cartesian and k-space grid fields
%       kgrid.t_array       - evenly spaced array of time values [s] (set
%                             to 'auto' by makeGrid)
%
%       medium.sound_speed  - homogeneous sound speed within the acoustic
%                             medium [m/s]
%       medium.alpha_power  - power law absorption exponent
%       medium.alpha_coeff  - power law absorption coefficient
%                             [dB/(MHz^y cm)]
%       medium.alpha_mode   - optional input to force either the absorption
%                             or dispersion terms in the equation of state
%                             to be excluded; valid inputs are
%                             'no_absorption' or 'no_dispersion'
%
%       source.p0           - initial pressure within the acoustic medium
%       source.dp0dt        - initial pressure gradient within the acoustic
%                             medium
%
%       sensor.mask         - binary grid specifying where the pressure is
%                             recorded at each time-step
%       sensor.record       - cell array of the acoustic parameters to
%                             record in the form sensor.record = {'p'};
%                             valid inputs are:
%                               'p'       (acoustic pressure)
%                               'p_final' (final pressure field)
%
% OPTIONAL INPUTS:
%       Optional 'string', value pairs that may be used to modify the
%       default computational settings.
%
%       'ExpandGrid'    - Boolean controlling whether the grid size is
%                         expanded on two sides to delay the time before
%                         wave wrapping occurs (default = false).
%       'MeshPlot'      - Boolean controlling whether mesh is used in place
%                         of imagesc to plot the pressure field (default =
%                         false).
%       'PlotFrames'    - Boolean controlling whether the pressure field
%                         for each time step is plotted in a new window
%                         (default = false).
%       'PlotFreq'      - The number of iterations which must pass before
%                         the simulation plot is updated (default = 10).
%       'PlotScale'     - [min, max] values used to control the scaling for
%                         imagesc (visualisation) (default = [-1, 1].
%       'PlotSim'       - Boolean controlling whether the simulation
%                         iterations are progressively plotted (default =
%                         true).
%       'Smooth'        - Boolean controlling whether source.p0 is smoothed
%                         using smooth before computation (default = true).
%
% OUTPUTS:
% If sensor.record is not defined by the user:
%       sensor_data         - time varying pressure recorded at the sensor
%                             positions given by sensor.mask
%
% If sensor.record is defined by the user:
%       sensor_data.p       - time varying pressure recorded at the sensor
%                             positions given by sensor.mask (returned if
%                             'p' is set)
%       sensor_data.p_final - final pressure field over the complete domain
%                             (returned if 'p_final' is set)
%
% ABOUT:
%       author      - Bradley Treeby and Ben Cox
%       date        - 21st August 2008
%       last update - 25th August 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox
%
% See also kspaceFirstOrder1D, kspaceFirstOrder2D, kspaceFirstOrder3D,
% makeGrid, makeTime, smooth

% 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/>.

% start the timer
tic;

% =========================================================================
% DEFINE LITERALS
% =========================================================================

% minimum number of input variables
NUM_REQ_INPUT_VARIABLES = 4;

% optional input defaults
EXPAND_GRID_DEF = false;
MESH_PLOT_DEF = false;
PLOT_FRAMES_DEF = false;
PLOT_FREQ_DEF = 10;
PLOT_SCALE_DEF = [-1 1];
PLOT_SIM_DEF = true;
SMOOTH_P0_DEF = true;

% create colormap for visualisations
COLOR_MAP = getColorMap;

% =========================================================================
% CHECK INPUT STRUCTURES
% =========================================================================

% calculate t_array using makeTime if it is not given
if strcmp(kgrid.t_array, 'auto')
    kgrid.t_array = makeTime(kgrid, medium.sound_speed);
end

% check medium fields
checkFieldNames(medium, {'sound_speed', 'density', 'alpha_coeff', 'alpha_power', 'alpha_mode'});
enforceFields(medium, {'sound_speed'});
if isfield(medium, 'density')
    disp('WARNING: medium.density is not a valid input parameter');
end

% check source fields
checkFieldNames(source, {'p0', 'dp0dt'});

% check the absorption mode input is valid
if isfield(medium, 'alpha_mode')
    if ~ischar(medium.alpha_mode) || (~strcmp(medium.alpha_mode, 'no_absorption') && ~strcmp(medium.alpha_mode, 'no_dispersion'))
        error('medium.alpha_mode must be set to ''no_absorption'' or ''no_dispersion''');
    end
end

% extract which source fields have been given
source_case = 0;
if isfield(source, 'p0')
    source_case = source_case + 1;
end
if isfield(source, 'dp0dt')
    source_case = source_case + 2;
end

% check sensor fields
checkFieldNames(sensor, {'mask', 'record'});
enforceFields(sensor, {'mask'});

% check the sensor mask is binary
if sum(sensor.mask(:)) ~= numel(sensor.mask) - sum(sensor.mask(:) == 0)
    error('sensor.mask must be a binary grid (numeric values must be 0 or 1)');
end

% set default record options
record.p = true;
record.p_final = false;

% check for sensor.record and set usage flags - if no flags are given, the
% time history of the acoustic pressure is recorded by default
if isfield(sensor, 'record')

    % list of allowable record flags
    record.flags = {'p', 'p_final'};

    % check the contents of the cell array are valid inputs
    for record_index = 1:length(sensor.record)
        if ~ismember(sensor.record(record_index), record.flags);
            error(['''' sensor.record{record_index} ''' is not a valid input for sensor.record']);
        end
    end

    % set the usage flags depending on the cell array
    if ismember('p', sensor.record)
        record.p = true;
    else
        % set record.p to false if a user input for sensor.record is given
        % and 'p' is not set
        record.p = false;
    end
    if ismember('p_final', sensor.record)
        record.p_final = true;
    end

end

% =========================================================================
% CHECK OPTIONAL INPUTS
% =========================================================================

% shorten commonly used field names
t_array = kgrid.t_array;
c = medium.sound_speed;
if isfield(medium, 'alpha_coeff')
    a0 = medium.alpha_coeff;
    y = medium.alpha_power;
else
    a0 = 0;
end

% assign default input parameters
expand_grid = EXPAND_GRID_DEF;
mesh_plot = MESH_PLOT_DEF;
plot_frames = PLOT_FRAMES_DEF;
plot_freq = PLOT_FREQ_DEF;
plot_scale = PLOT_SCALE_DEF;
plot_sim = PLOT_SIM_DEF;
smooth_p0 = SMOOTH_P0_DEF;

% replace defaults with user defined values if provided and check inputs
if nargin < NUM_REQ_INPUT_VARIABLES
    error('Not enough input parameters');
elseif rem(nargin - NUM_REQ_INPUT_VARIABLES, 2) %**
    error('Optional input parameters must be given as param, value pairs');
elseif ~isempty(varargin)
    for input_index = 1:2:length(varargin)
        switch varargin{input_index}
            case 'ExpandGrid'
                expand_grid = varargin{input_index + 1};
                if ~islogical(expand_grid)
                    error('Optional input ExpandGrid must be Boolean');
                end
            case 'MeshPlot'
                mesh_plot = varargin{input_index + 1};
                if ~islogical(mesh_plot)
                    error('Optional input MeshPlot must be Boolean');
                end
            case 'PlotFrames'
                plot_frames = varargin{input_index + 1};
                if ~islogical(plot_frames)
                    error('Optional input PlotFrames must be Boolean');
                end
            case 'PlotFreq'
                plot_freq = varargin{input_index + 1};
                if ~(numel(plot_freq) == 1 && isnumeric(plot_freq))
                    error('Optional input PlotFreq must be a single numerical value');
                end
            case 'PlotScale'
                plot_scale = varargin{input_index + 1};
                if ~(numel(plot_scale) == 2 && isnumeric(plot_scale))
                    error('Optional input PlotScale must be a 2 element numerical array');
                end
            case 'PlotSim'
                plot_sim = varargin{input_index + 1};
                if ~islogical(plot_sim)
                    error('Optional input PlotSim must be Boolean');
                end
            case 'Smooth'
                smooth_p0 = varargin{input_index + 1};
                if ~islogical(smooth_p0)
                    error('Optional input Smooth must be Boolean');
                end
            otherwise
                error('Unknown optional input');
        end
    end
end

% cleanup unused variables
clear *_DEF NUM_REQ_INPUT_VARIABLES time;

% update command line status
disp('Running k-space simulation...');

% =========================================================================
% UPDATE COMMAND LINE STATUS
% =========================================================================

% update command line status
disp(['  time steps: ' num2str(length(t_array))]);
switch numDim(kgrid.k)
    case 1
        disp(['  input grid size: ' num2str(kgrid.Nx) ' grid points (' scaleSI(kgrid.x_size) 'm)']);
    case 2
        [x_sc, scale, prefix] = scaleSI(min(kgrid.x_size, kgrid.y_size)); %#ok<*ASGLU>
        disp(['  input grid size: ' num2str(kgrid.Nx) ' by ' num2str(kgrid.Ny) ' grid points (' num2str(kgrid.x_size*scale) ' by ' num2str(kgrid.y_size*scale) prefix 'm)']);
    case 3
        [x_sc, scale, prefix] = scaleSI(min([kgrid.x_size, kgrid.y_size, kgrid.z_size])); %#ok<ASGLU>
        disp(['  input grid size: ' num2str(kgrid.Nx) ' by ' num2str(kgrid.Ny) ' by ' num2str(kgrid.Nz) ' grid points (' num2str(kgrid.x_size*scale) ' by ' num2str(kgrid.y_size*scale) ' by ' num2str(kgrid.z_size*scale) prefix 'm)']);
end
disp(['  maximum supported frequency: ' scaleSI( kgrid.k_max * min(c(:)) / (2*pi) ) 'Hz']);

% =========================================================================
% CALCULATE FFT OF SOURCE FUNCTIONS AND EXPAND GRID
% =========================================================================

% smooth p0 distribution if required restoring the maximum magnitude
if smooth_p0
    if source_case ~= 2
        disp('  smoothing source.p0 distribution...');
        source.p0 = smooth(kgrid, source.p0, true);
    end
    if source_case > 1
        disp('  smoothing source.dp0dt distribution...');
        source.dp0dt = smooth(kgrid, source.dp0dt, true);
    end
end

if ~expand_grid
    % extract wavenumber matrix from kgrid
    k = kgrid.k;

    % extract the location of the sensor points
    sensor_index = (sensor.mask == 1);

    % compute FFT of source functions
    if source_case ~= 2
        p0_k = fftn(source.p0);
    end
    if source_case > 1
        dp0dt_k = fftn(source.dp0dt);
    end
else
    switch numDim(kgrid.k)
        case 1

            % create a larger kgrid to prevent wave wrapping
            kgrid_APE = makeGrid(kgrid.Nx*2, kgrid.dx);

            % extract wavenumber matrix from kgrid
            k = kgrid_APE.k;

            % expand the sensor mask
            sensor_mask = [sensor.mask, zeros(size(sensor.mask))];

            % expand the source functions
            if source_case ~= 2
                p0 = zeros(size(k));
                p0(1:kgrid.Nx) = source.p0;
            end
            if source_case > 1
                dp0dt = zeros(size(k));
                dp0dt(1:kgrid.Nx) = source.dp0dt;
            end

        case 2

            % create a larger kgrid to prevent wave wrapping
            kgrid_APE = makeGrid(kgrid.Nx*2, kgrid.dx, kgrid.Ny*2, kgrid.dy);

            % extract wavenumber matrix from kgrid
            k = kgrid_APE.k;

            % expand the sensor mask
            sensor_mask = zeros(size(k));
            sensor_mask(1:kgrid.Nx, 1:kgrid.Ny) = sensor.mask;

            % expand the source functions
            if source_case ~= 2
                p0 = zeros(size(k));
                p0(1:kgrid.Nx, 1:kgrid.Ny) = source.p0;
            end
            if source_case > 1
                dp0dt = zeros(size(k));
                dp0dt(1:kgrid.Nx, 1:kgrid.Ny) = source.dp0dt;
            end

        case 3

            % create a larger kgrid to prevent wave wrapping
            kgrid_APE = makeGrid(kgrid.Nx*2, kgrid.dx, kgrid.Ny*2, kgrid.dy, kgrid.Nz*2, kgrid.dz);

            % extract wavenumber matrix from kgrid
            k = kgrid_APE.k;

            % extract the location of the sensor points within the enlarged grid
            sensor_mask = zeros(size(k));
            sensor_mask(1:kgrid.Nx, 1:kgrid.Ny, 1:kgrid.Nz) = sensor.mask;

            % expand the source functions
            if source_case ~= 2
                p0 = zeros(size(k));
                p0(1:kgrid.Nx, 1:kgrid.Ny, 1:kgrid.Nz) = source.p0;
            end
            if source_case > 1
                dp0dt = zeros(size(k));
                dp0dt(1:kgrid.Nx, 1:kgrid.Ny, 1:kgrid.Nz) = source.dp0dt;
            end

    end

    % extract the location of the sensor points within the enlarged grid
    sensor_index = (sensor_mask == 1);

    % compute FFT of source functions
    if source_case ~= 2
        p0_k = fftn(p0);
    end
    if source_case > 1
        dp0dt_k = fftn(dp0dt);
    end

    % delete unused variables
    clear kgrid_APE sensor_mask p0 dp0dt source;
end

% =========================================================================
% DEFINE TIME PROPAGATORS
% =========================================================================

% shift the wavenumbers
k = ifftshift(k);

% find the index of the zero wave numbers
if source_case > 1
    k_0_index = find(k == 0);
end

% define time propagation function
if a0 ~= 0

    % convert attenuation to nepers
    a0 = db2neper(a0, y);

    % define coefficients
    if y == 2 || (isfield(medium, 'alpha_mode') && strcmp('no_dispersion', medium.alpha_mode))

        % compute Upsilon
        Upsilon = sqrt(1 - a0^2*c^(2*y)*k.^(2*y-2));

        % reset first value to avoid possible INF when k = 0
        Upsilon(1) = 1;

        % precalculate coefficients to speed up computation
        ckU = c*k.*Upsilon;
        coeff_1 = a0*c^(y + 1)*k.^(y);
        coeff_2 = 0;

    elseif (isfield(medium, 'alpha_mode') && strcmp('no_absorption', medium.alpha_mode))

        % compute Upsilon
        Upsilon = sqrt(1 - 2*a0*c^y*k.^(y-1)*tan(pi*y/2));

        % reset first value to avoid possible INF when k = 0
        Upsilon(1) = 1;

        % precalculate coefficients to speed up computation
        ckU = c*k.*Upsilon;
        coeff_1 = 0;
        coeff_2 = a0*c^y*k.^(y-1) ./ Upsilon;

        % reset first value to avoid possible INF when k = 0
        coeff_2(1) = 0;

    else

        % compute Upsilon
        Upsilon = sqrt(1 - a0^2*c^(2*y)*k.^(2*y-2) - 2*a0*c^y*k.^(y-1)*tan(pi*y/2));

        % reset first value to avoid possible INF when k = 0
        Upsilon(1) = 1;

        % precalculate coefficients to speed up computation
        ckU = c*k.*Upsilon;
        coeff_1 = a0*c^(y + 1)*k.^(y);
        coeff_2 = a0*c^y*k.^(y-1) ./ Upsilon;

        % reset first value to avoid possible INF when k = 0
        coeff_2(1) = 0;

    end

else
    % precalculate coefficients to speed up computation
    ck = c*k;
end

% =========================================================================
% PREPARE VISUALISATIONS AND STORAGE VARIABLES
% =========================================================================

% preallocate storage variables
if record.p
    sensor_data.p = zeros(sum(sensor.mask(:)), length(t_array));
end

% pre-compute suitable axes scaling factor
if plot_sim
    switch numDim(kgrid.k)
        case 1
            [x_sc, scale, prefix] = scaleSI(max(kgrid.x_vec));
        case 2
            [x_sc, scale, prefix] = scaleSI(max([kgrid.x_vec; kgrid.y_vec]));
        case 3
            [x_sc, scale, prefix] = scaleSI(max([kgrid.x_vec; kgrid.y_vec; kgrid.z_vec]));
    end
end

% initialise the figures used for animation if 'PlotSim' is set to 'true'
if plot_sim
    % create empty figure
    img = figure;

    % create waitbar and shift position so it doesn't overlap the figure window
    pbar = waitbar(0, 'Computing Pressure Field', 'Visible', 'off');
    posn_pbar = get(pbar, 'OuterPosition');
    posn_img = get(img, 'OuterPosition');
    posn_pbar(2) = max(min(posn_pbar(2) - posn_pbar(4), posn_img(2) - posn_pbar(4) - 10), 0);
    set(pbar, 'OuterPosition', posn_pbar, 'Visible', 'on');
end

% update command line status
disp(['  precomputation completed in ' scaleTime(toc)]);
disp('  starting time loop...');

% =========================================================================
% LOOP THROUGH TIME STEPS
% =========================================================================

% loop through each value of t
for t_index = 1:length(t_array)

    % extract the time point
    t = t_array(t_index);

    % compute pressure field
    switch (source_case + 3*(a0 ~= 0))
        case 1
            TP_p0 = cos(ck*t);
            p = ifftn(  p0_k.*TP_p0  );
        case 2
            TP_dp0dt = sin(ck*t) ./ (ck);
            TP_dp0dt(k_0_index) = t; %#ok<*FNDSB>
            p = ifftn(  dp0dt_k.*TP_dp0dt  );
        case 3
            TP_p0 = cos(ck*t);
            TP_dp0dt = sin(ck*t) ./ (ck);
            TP_dp0dt(k_0_index) = t;
            p = ifftn(  p0_k.*TP_p0 + dp0dt_k.*TP_dp0dt  );
        case 4
            TP_p0 = exp(-coeff_1*t) .* ( cos(ckU*t) - coeff_2 .* sin(ckU*t) );
            p = ifftn(  p0_k.*TP_p0  );
        case 5
            TP_dp0dt = sin(ckU*t) .* exp(-coeff_1*t) ./ ckU;
            TP_dp0dt(k_0_index) = t.* exp(-coeff_1(k_0_index)*t);
            p = ifftn(  dp0dt_k.*TP_dp0dt  );
        case 6
            TP_p0 = exp(-coeff_1*t) .* ( cos(ckU*t) - coeff_2 .* sin(ckU*t) );
            TP_dp0dt = sin(ckU*t) .* exp(-coeff_1*t) ./ ckU;
            TP_dp0dt(k_0_index) = t.* exp(-coeff_1(k_0_index)*t);
            p = ifftn(  p0_k.*TP_p0 + dp0dt_k.*TP_dp0dt  );
    end

    % extract required data
    if record.p
        sensor_data.p(:, t_index) = real(p(sensor_index));
    end

    % plot data if required
    if plot_frames || ( plot_sim && rem(t_index, plot_freq) == 0 )

        % update progress bar
        waitbar(t_index/length(t_array), pbar);
        drawnow;

        % update plot
        if plot_frames
            figure;
        end

        p = real(p);

        switch numDim(kgrid.k)
            case 1
                % extract the required pressure field from the enlarged grid
                if expand_grid
                    p = p(1:kgrid.Nx);
                end
                plot(kgrid.x*scale, p);
                xlabel(['x-position [' prefix 'm]']);
                set(gca, 'YLim', plot_scale);

            case 2
                % extract the required pressure field from the enlarged grid
                if expand_grid
                    p = p(1:kgrid.Nx, 1:kgrid.Ny);
                end

                if mesh_plot
                    mesh(kgrid.x_vec*scale, kgrid.y_vec*scale, p, 'EdgeColor', 'Black');
                    axis image;
                    set(gca, 'ZLim', plot_scale);
                    axis off;
                else
                    % add sensor mask onto plot
                    p(sensor.mask == 1) = plot_scale(2);

                    imagesc(kgrid.y_vec*scale, kgrid.x_vec*scale, p, plot_scale);
                    colormap(COLOR_MAP);
                    axis image;
                    ylabel(['x-position [' prefix 'm]']);
                    xlabel(['y-position [' prefix 'm]']);
                end

            case 3
                % extract the required pressure field from the enlarged grid
                if expand_grid
                    p = p(1:kgrid.Nx, 1:kgrid.Ny, 1:kgrid.Nz);
                end
                planeplot(kgrid, p, '', plot_scale, scale, prefix, COLOR_MAP);
        end
    end
end

% =========================================================================
% CLEAN UP
% =========================================================================

% assign output data
if ~isfield(sensor, 'record')
    sensor_data = sensor_data.p;
elseif record.p_final
    switch numDim(kgrid.k)
        case 1
            sensor_data.p_final = p(1:kgrid.Nx);
        case 2
            sensor_data.p_final = p(1:kgrid.Nx, 1:kgrid.Ny);
        case 3
            sensor_data.p_final = p(1:kgrid.Nx, 1:kgrid.Ny, 1:kgrid.Nz);
    end
end

% clean up used figures
if plot_sim
    close(img);
    close(pbar);
end

% update command line status
disp(['  computation completed in ' scaleTime(toc)]);

function planeplot(kgrid, data, data_title, plot_scale, scale, prefix, color_map)
% Subfunction to produce a plot of a three-dimensional matrix through the
% three central planes

subplot(2, 2, 1), imagesc(kgrid.y_vec*scale, kgrid.x_vec*scale, squeeze(data(:, :, kgrid.Nz/2)), plot_scale);
title([data_title 'x-y plane']);
axis image;
subplot(2, 2, 2), imagesc(kgrid.z_vec*scale, kgrid.x_vec*scale, squeeze(data(:, kgrid.Ny/2, :)), plot_scale);
title('x-z plane');
axis image;
xlabel(['(All axes in ' prefix 'm)']);
subplot(2, 2, 3), imagesc(kgrid.z_vec*scale, kgrid.y_vec*scale, squeeze(data(kgrid.Nx/2, :, :)), plot_scale);
title('y-z plane');
axis image;
colormap(color_map);
drawnow;


function results = test_tc( dataname )

load( ['..\..\data\', dataname] );
addpath( '..\utils' );

% data.W = tenzeros( size(data.X) );
% data.W(data.Omega) = data.b;
data.W = tenzeros( size(data.X) );
N = length( size(data.W) );
data.Lamb = cell(1,N);
for i = 1:N
    data.Lamb{i} = tenzeros( size(data.X) );
end
data.Lambda = tenzeros( size(data.X) );

params.mu0 = 1e1;
params.sigma = 1e-3;
params.lambda = 1;
params.max_iter = 100;
params.opt_tol = 1e-3;
params.R = 50;

while params.lambda < 100
    results = TC_ADAL_Nuc( data, params );
%     results = TC_MSA( data, params );
    data.W = results.X;
    params.lambda = params.lambda * 5;
end

end


%  px = ellipseinteriorpixels(im,ell)
function px = ellipseinteriorpixels(im,ell)

[nr,nc,ncolors] = size(im);

% get a bounding box around the ellipse
bb = [ell.y+ell.a*[-1,1],ell.x+ell.a*[-1,1]];
bb([1,3]) = floor(bb([1,3]));
bb([2,4]) = ceil(bb([2,4]));
bb = max(bb,1);
bb(1:2) = min(bb(1:2),nr);
bb(3:4) = min(bb(1:2),nc);

% crop out this region
imbb = im(bb(1):bb(2),bb(3):bb(4),:);
bw = ellipsepixels([ell.x,ell.y,ell.a*2,ell.b*2,ell.theta],bb);
idx = find(bw);
n = length(idx);
px = zeros(n,ncolors);
for color = 1:ncolors,
  px(:,color) = imbb(idx+(color-1)*nr*nc);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   example 4 DOF planar robot.
%
%   Author: Arturo Gil. Universidad Miguel Hern�ndez de Elche.
%   email: arturo.gil@umh.es date:   05/03/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

robot.name='Example 4DOF planar arm';

%kinematic data DH parameters
robot.DH.theta='[q(1) q(2) q(3) q(4)]';
robot.DH.d='[0  0  0  0]';
robot.DH.a='[0.4  0.3  0.2  0.1]';
robot.DH.alpha='[0  0  0  0]';

%number of degrees of freedom
robot.DOF = 4;

%Jacobian matrix
% if not defined, compute it with manipulator_jacobian
robot.J=[];
%the proper rows of J to compute manipulability
robot.selJ =[1,2,6];
robot.kind=['R' 'R' 'R' 'R'];

%Function name to compute inverse kinematic
robot.inversekinematic_fn = 'inverse_kinematics_4dofplanar(robot, T, q)';
robot.directkinematic_fn = 'directkinematic(robot, q)';

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-180) deg2rad(180); %Axis 1, minimum, maximum
                deg2rad(-180) deg2rad(180);
                deg2rad(-180) deg2rad(180)]; %Axis 2, minimum, maximum

            %minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-90) deg2rad(90); %Axis 1, minimum, maximum
                deg2rad(-90) deg2rad(90);
                deg2rad(-180) deg2rad(180)]; %Axis 2, minimum, maximum

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = []; %empty, not available

robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time
% end effectors maximum velocity
robot.linear_velmax = 0; %m/s, example, not available

%base reference system
robot.T0 = eye(4);

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);
robot.path = pwd;

%this is an ad hoc transformation between the piece and the robot's end
%effector
% robot.Tcoupling=[-1 0 0  0;
%      0 1 0  0;
%      0 0 -1 0;
%      0 0 0 1];
% Tc1 = [cos(-pi/2) 0 sin(-pi/2) 0;
%        0 1 0 0;
%        -sin(-pi/2) 0 cos(-pi/2) 0;
%        0  0  0  1       ];
% Tc2 = [1 0 0 0;
%        0 cos(pi) -sin(pi) 0;
%        0  sin(pi) cos(pi) 0;
%        0 0 0 1];
% robot.Tcoupling =Tc1*Tc2;
robot.Tcoupling = eye(4);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GRAPHICS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%read graphics files
robot.graphical.has_graphics=1;
robot.graphical.color = [255 20 40]./255;
%for transparency
robot.graphical.draw_transparent=0;
%draw DH systems
robot.graphical.draw_axes=1;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=1;
%adjust for a default view of the robot
robot.axis=[-3.5 3.5 -3.5 3.5 0 1]
robot = read_graphics(robot);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%DYNAMIC PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
robot.has_dynamics=1;

%consider friction in the computations
robot.dynamics.friction=0;

%link masses (kg)
robot.dynamics.masses=[1 1 1] ;

%COM of each link with respect to own reference system
robot.dynamics.r_com=[-0.5      0         0; %(rx, ry, rz) link 1
                      -0.5      0         0;
                      -0.5      0         0];%(rx, ry, rz) link 2

%link masses
m1 = robot.dynamics.masses(1);
m2 = robot.dynamics.masses(2);
m3 = robot.dynamics.masses(3);

a=eval(robot.DH.a);
L1 = a(1);
L2 = a(2);
L3 = a(3);

%Momentos de inercia de cada eslabon.
% Ixx	Iyy	Izz	Ixy	Iyz	Ixz, por cada fila
robot.dynamics.Inertia=[0   m1*L1^2/3   m1*L1^2/3    0	0	0;
                        0   m2*L2^2/3   m2*L2^2/3    0	0	0;
                        0   m3*L3^2/3   m3*L3^2/3    0	0	0];

%Actuator rotor inertia
robot.motors.Inertia=[0 0 0];
%Reduction ratio motor/joint speed
robot.motors.G=[1 1 1];
%consider friction
robot.friction=0;
%Viscous friction factor, motor referred
%robot.B = [1e-3  1e-3 1e-3];
robot.motors.Viscous = [10  10 10];
%Coulomb friction, motor referred
robot.motors.Coulomb = [0 0;
            0 0;
            0 0];

%SPECIAL PARAMETERS TO SOLVE THE INVERSE KINEMATICS
robot.parameters.step_time=0.1;
%Error in XYZ to stop inverse kinematics
robot.parameters.epsilonXYZ=0.01;
%Error in Quaternion to stop inverse kinematics.
robot.parameters.epsilonQ=0.01;
robot.parameters.stop_iterations=500;


function [data] = avgref(data, sel)

% AVGREF computes the average reference in each column
%   [data] = avgref(data)
%
% or it computes the re-referenced data relative to the
% average over the selected channels
%   [data] = avgref(data, sel)

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

% determine the dimension of the data
if length(size(data))==3
  % multiple epochs
  dim=3;
else
  % single epoch with multiple channels
  dim=2;
end

if nargin==1
  % default is to use all channels for average referencing
  if dim==3
    sel=1:size(data,2);
  else
    sel=1:size(data,1);
  end
end

if dim==3
  % the data contains multiple epochs
  for epoch=1:size(data,1)
    reference = mean(data(epoch,sel,:), 2);
    data(epoch,:,:) = data(epoch,:,:) - repmat(reference, [1 size(data,2) 1]);
  end
else
  % the data contains a single epoch
  reference = mean(data(sel,:), 1);
  data = data - repmat(reference, size(data,1), 1);
end


function lines = assign_lineclass(lines, vp)

THRES_THETA = 10;
if length(vp)>=1 && ~isempty(vp{1})
    lineclass1 = line_belongto_vp(lines, vp{1}, THRES_THETA);
else
    lineclass1 = zeros(1, length(lines));
end
if length(vp)>=2 && ~isempty(vp{2})
    lineclass2 = line_belongto_vp(lines, vp{2}, THRES_THETA);
else
    lineclass2 = zeros(1, length(lines));
end
if length(vp)>=3 && ~isempty(vp{3})
    lineclass3 = line_belongto_vp(lines, vp{3}, THRES_THETA);
else
    lineclass3 = zeros(1, length(lines));
end

for i = 1:length(lines)
	lines(i).lineclass1 = lineclass1(i);
	lines(i).lineclass2 = lineclass2(i);
	lines(i).lineclass3 = lineclass3(i);

	if lineclass1(i) + lineclass2(i) + lineclass3(i) == 1
		lines(i).lineclass = 1*lineclass1(i) + 2*lineclass2(i) + 3*lineclass3(i);
	else
		lines(i).lineclass = 0;
	end
end


  function x = flipdims(x, varargin)
%|function x = flipdims(x, varargin)
%|
%| Generalization of flipdim() that flips all dimensions.
%| This is useful for the adjoint of convolution operator.
%|
%| option
%|	'odd'	0|1	if 1, make each dimension odd sized after flipping
%|			by appending a zero.  default: 0
%|
%| Jeff Fessler

if nargin < 1, ir_usage, end

arg.odd = 0;
arg = vararg_pair(arg, varargin);

for ii = 1:ndims(x)

	if arg.odd && ~rem(size(x,ii), 2)
		sz = size(x);
		sz(ii) = 1;
		try
			tmp = zeros(sz, 'like', x);
		catch % for octave
			tmp = zeros(sz, class(x));
		end
		x = cat(ii, x, tmp); % append one zero along dim ii
	end

	x = flip(x, ii);
end


function [x,options]=simps(fun,x,ix,options,vlb,vub,varargin)
%SIMPS (Strategy Simplex) finds the constrained minimum.
%
%   Syntax:
%   [X1,OPTIONS]=SIMPS(FUN,X0,IX,OPTIONS,VLB,VUB,P1,P2...)
%   where IX,OPTIONS,VLB,VUB,P1,P2... are optional.
%
%   Compare to Matlab's functions:
%   [X1,OPTIONS]=FMINS(FUN,X0,OPTIONS,[],P1,P2...)
%   [X1,OPTIONS]=CONSTR(FUN,X0,OPTIONS,VLB,VUB,[],P1,P2...) - OPTIM
%   toolbox
%
%   'FUN' - function to be minimized (usually an M-file: FUN.M).
%   It must be of the form: [F,G]=FUN(X,P1,P2...), where F returns the
%   value, and G returns the vector of constraints.
%   Constraints are satisfied at the point X if the respective
%   components of G are non-positive. For the non-constrained
%   minimization, use G=[].
%   The point X can be a [real] vector, matrix, or several dimensional
%   array. Both F and G must be well defined for all VLB<=X<=VUB (see
%   below).
%   P1,P2... are optional problem-dependent parameters, bypassed by
%   the minimizer SIMPS.
%
%   X0 - initial guess,
%   X1 - point of minimum as obtained;
%   both of the same type and dimension as X in the function 'FUN'.
%
%   IX - integer vector, subset of 1:PROD(SIZE(X0)) (which is also its
%   default value). The minimization is performed only against X0(IX),
%   while the other components of X0 are supposed to be fixed.
%   Let us denote LX=LENGTH(IX).
%
%   OPTIONS - see FOPTIONS. Only the following components are used:
%   Input:
%   OPTIONS(1)  - Whether to display intermediate results (1),
%                 or not (0, default).
%   OPTIONS(2)  - Relative X-termination tolerance (default: 1e-4).
%   OPTIONS(3)  - Relative F-termination tolerance (default: 1e-4).
%   OPTIONS(14) - Maximum number of function evaluations per each
%                 internal LX-dim. and 2-dim. simplex call
%                 (default: 100*LX).
%   Output:
%   OPTIONS(8)  - Function value at the point X1 (minimum).
%   OPTIONS(10) - Total number of function evaluations.
%
%   VLB,VUB - lower and upper bounds for X.
%   Both must be vectors of length PROD(SIZE(X0)) - total number of
%   components of X0.
%   Supply -INF, resp. +INF, (default) for unrestricted coordinates.
%
%   Comments:
%
%   - SIMPS is an iterative method, where each iteration consists of
%   one LX (full dim.) Nelder-Mead simplex call (direct search),
%   followed by LX+1 two dim. Nelder-Mead simplex calls (working on 2
%   dim. subspaces). These LX+1 two-dim. simplex calls provide initial
%   vertices V for the next full dim. simplex call (next iteration).
%
%   - The number of outer iterations is limited to LX or until the
%   longest dimension of simplex V (see above) falls below the
%   X-tolerance (OPTIONS(2)).
%   The internal LX and 2 dim. simplex calls are limited by proposed
%   F-tolerance or max. number of function evaluations (OPTIONS(2) and
%   OPTIONS(14)).
%
%   - The inner LX and 2 dim. Nelder-Mead minimizers are both realized
%   through the calls to the internal AMOEBA function, based
%   essentially on the code of the Matlab's FMINS non-linear simplex
%   implementation.
%
%   - The constraints are basically implemented by penalizing the
%   function (see the internal WRAPPER function): unsatisfied
%   constraints (-sum(G(find(G<0))), multiplied by factor 1e6, are
%   being added to the function value F.
%
%   - The index vector IX provides a useful and convenient method for
%   numerical experiments with complicated unknown minima, allowing to
%   user to perform searches limited to smaller dimensional subspaces.
%   This extra feature does not require run-time editing on the side
%   of target function 'FUN', as would be the case if certain
%   variables were fixed by use of optional parameters P1,P2...
%
%   - In contrast to the Matlab's FMINS, CONSTR and other minimizers,
%   both X and F termination tolerances are considered relative, not
%   absolute.
%   Nevertheless, X, F and G should be ideally of the same order of
%   magnitude (1), at least locally around the point of minimum.
%
%   - There is of course no guarantee that the output X1 represents
%   the point of global minimum as required.
%   However, the method (strategy) provides an advantage over the
%   plain nonlinear simplex, and it has been proved to be specially
%   useful for target functions with plenty of narrow local minima -
%   standard traps for analitically based minimizers.
%   The method is not limited to continuos functions and does not
%   require derivatives.
%
%   Authors:
%   Zeljko Bajzer (bajzer@mayo.edu) and Ivo Penzar (penzar@mayo.edu)
%   Mayo Clinic and Foundation, Rochester, Minnesota, USA
%   June, 1998.

if nargin<2, error('simps requires two input arguments'); end
if nargin<3, ix=1:prod(size(x)); end
ix=unique(ix); lx=length(ix);
if nargin<4, options=[]; end
if nargin<5, vlb=-Inf+zeros(size(1,lx)); end
if nargin<6, vub=Inf+zeros(size(1,lx)); end

options=foptions(options);
prnt=options(1);
if ~options(14)
 options(14)=50*lx;
end
tolx=options(2);
tolf=options(3);
maxfcalls=options(14);

varargs={vlb,vub,fun,varargin{:}};

xx=x(ix)';
if all(abs(xx)<eps)
 xx=xx+eps;
end
d=0.5;
% Guess goes to the centre:
%v=repmat((1-d/(lx+1))*xx,1,lx+1)+[diag(d*xx) zeros(lx,1)];
% Guess goes to a corner:
v=repmat(xx,1,lx+1)+[diag(d*xx) zeros(lx,1)];
fcalls=0; dv=Inf;
if prnt
 fprintf('%7s %10s %12s\n','f-COUNT','FUNCTION','x-TOL');
end
i=0;
while (i<lx)&(dv>=tolx)
 [y,fmin,tmp4]=amoeba(v,tolf,maxfcalls,...
                      x,ix,varargs{:});
 y=y'; x(ix)=y;
 fcalls=fcalls+tmp4;
 if prnt
  fprintf('%7d %10.5f\n',fcalls,fmin);
 end
 if lx==1
  break
 end
 if lx==2
  v(:,1)=y;
 else
  for j=1:lx-1
   j1=[j j+1];
   [y(j1),fmin,tmp4]=amoeba([xx(j1) y(j1) [xx(j1(1)) y(j1(2))]'],...
                            tolf,maxfcalls,...
                            x,ix(j1),varargs{:});
   x(ix)=y; v(:,j)=y;
   fcalls=fcalls+tmp4;
  end
 end
 j1=[1 lx];
 [y(j1),fmin,tmp4]=amoeba([xx(j1) y(j1) [xx(j1(1)) y(j1(2))]'],...
                          tolf,maxfcalls,...
                          x,ix(j1),varargs{:});
 x(ix)=y; v(:,lx)=y;
 fcalls=fcalls+tmp4;
 if lx>3
  j1=[2 lx-1];
 end
 [y(j1),fmin,tmp4]=amoeba([xx(j1) y(j1) [y(j1(1)) xx(j1(2))]'],...
                          tolf,maxfcalls,...
                          x,ix(j1),varargs{:});
 x(ix)=y; v(:,lx+1)=y;
 fcalls=fcalls+tmp4;
 dv=max(tol(min(v,[],2),max(v,[],2)));
 if prnt
  fprintf('%7d %10.5f %12.6f\n',fcalls,fmin,dv);
 end
 i=i+1;
end
if prnt
 fprintf('\n');
end

options(8)=fmin;
options(10)=fcalls;

return

function [x,fmin,fcalls]=amoeba(v,tolf,maxfcalls,varargin)
% Internal Nelder-Mead minimizer, based essentially on the code
% of the Matlab's FMINS non-linear simplex implementation.
%
% - Input and output parameters are changed
%   (e.g., initial simplex v is provided from outside);
% - There is no x-tolerance termination checking;
% - There is no display of intermediate results.

rho=1; chi=2; psi=0.5; sigma=0.5;

n=length(v(:,1));
onesn=ones(1,n);
two2np1=2:n+1;
one2n=1:n;
zerosn=zeros(1,n);
x=zerosn;

fv=zeros(1,n+1);
for j=1:n+1
 x(:)=v(:,j);
 fv(j)=wrapper(x,varargin{:});
end
[fv,jj]=sort(fv);
v=v(:,jj);
fmin=fv(1); fcalls=n+1;

while (fcalls<maxfcalls)&...
      (tol(fmin,fv(n+1))>tolf)
 shrink=0;
 xbar=sum(v(:,one2n),2)/n;
 xr=(1+rho)*xbar-rho*v(:,n+1);
 x(:)=xr;
 fxr=wrapper(x,varargin{:});
 fcalls=fcalls+1;

 if fxr<fv(1)
  xe=(1+rho*chi)*xbar-rho*chi*v(:,n+1);
  x(:)=xe;
  fxe=wrapper(x,varargin{:});
  fcalls=fcalls+1;
  if fxe<fxr
   v(:,n+1)=xe;
   fv(n+1)=fxe;
  else
   v(:,n+1)=xr;
   fv(n+1)=fxr;
  end

 elseif fxr<fv(n)
  v(:,n+1)=xr;
  fv(n+1)=fxr;

 else
  if fxr<fv(n+1)
   xc=(1+psi*rho)*xbar-psi*rho*v(:,n+1);
   x(:)=xc;
   fxc=wrapper(x,varargin{:});
   fcalls=fcalls+1;
   if fxc<=fxr
    v(:,n+1)=xc;
    fv(n+1)=fxc;
   else
    shrink=1;
   end
  else
   xcc=(1-psi)*xbar+psi*v(:,n+1);
   x(:)=xcc;
   fxcc=wrapper(x,varargin{:});
   fcalls=fcalls+1;
   if fxcc<fv(n+1)
    v(:,n+1)=xcc;
    fv(n+1)=fxcc;
   else
    shrink=1;
   end
  end
  if shrink
   for j=two2np1
    v(:,j)=v(:,1)+sigma*(v(:,j)-v(:,1));
    x(:)=v(:,j);
    fxcc=wrapper(x,varargin{:});
   end
   fcalls=fcalls+n;
  end
 end

 [fv,jj]=sort(fv);
 v=v(:,jj);
 fmin=fv(1);
end
x(:)=v(:,1);

return

function r=tol(a,b)
% Internal helper function, calculates the relative distances
% between points a and b, componentwise.

c=(abs(b)+abs(a))/2;
j=find(c<eps); c(j)=ones(size(j));
r=abs(b-a)./c;

return

function fmin=wrapper(x,x0,ix,vlb,vub,funfcn,varargin)
% Internal wrapper function, provides a proper interface for
% AMOEBA to the user's target function.

x1=zeros(1,prod(size(x0)));
x1(:)=x0;
x1(ix)=x;
x0(:)=max(min(x1,vub),vlb);

funfcn=fcnchk(funfcn,1+length(varargin));
[fmin,fconstr]=feval(funfcn,x0,varargin{:});

fconstr=[vlb-x1,x1-vub,fconstr];
fmin=fmin+1e6*sum(fconstr(find(fconstr>0)));

return


function hogpic = jettify(hogpic)
%Turn a matrix which can be viewed via imagesc into an image using
%the same jet scheme, but now the result can be written to a file
NC = 200;
colorsheet = jet(NC);
dists = hogpic(:);
dists = dists - min(dists);
dists = dists / (max(dists)+eps);
dists = round(dists*(NC-1)+1);
colors = colorsheet(dists,:);
hogpic = reshape(colors,[size(hogpic,1) size(hogpic,2) 3]);


function [pvec, pstruct] = tapas_beta_obs_transp(r, ptrans)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

pvec    = NaN(1,length(ptrans));
pstruct = struct;

pvec(1)      = exp(ptrans(1));         % nupr
pstruct.nupr = pvec(1);

return;


%% CONTRAST_BATCH_LOCAL uses the BATCH command to run the CONTRAST code locally.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 April 2010
%
%  Author:
%
%    John Burkardt
%
  clear

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CONTRAST_BATCH_LOCAL\n' );
  fprintf ( 1, '  Run CONTRAST_SCRIPT locally.\n' );
%
%  Read a black and white image and filter it locally.
%
  x = imread ( 'surfsup.tif' );
  xf = nlfilter ( x, [3,3], @contrast_enhance );
  xf = uint8 ( xf );
%
%  Set the number of workers.
%
  n = 4;
%
%  BATCH sends the script for execution.
%
  job = batch ( 'contrast_script', ...
    'Configuration', 'local', ...
    'CaptureDiary', true, ...
    'FileDependencies', { 'contrast_fun', 'contrast_enhance', 'surfsup.tif' }, ...
    'matlabpool', n );
%
%  WAIT pauses the MATLAB session til the job completes.
%
  wait ( job );
%
%  DIARY displays any messages printed during execution.
%
  diary ( job );
%
%  LOAD makes the script's workspace available.
%
%  y = output #1.
%
  load ( job );
%
%  DESTROY cleans up data about the job we no longer need.
%
  destroy ( job );
%
%  Display the original, single filtered, and SPMD filtered versions.
%
  figure ( 1 );

  subplot ( 1, 3, 1 );
  imshow ( x );
  title ( 'Original image' );

  subplot ( 1, 3, 2 );
  imshow( xf );
  title( 'Filtered on Client' );

  subplot ( 1, 3, 3 );
  imshow ( y );
  title ( 'Filtered on 4 SPMD Workers' );

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


%
% The main script.
%
clear all;
addpath('bss_eval');

% sample rate
fs=16000;
% reverberation time
RT60_SET=[0.6, 0.3, 0.8];
% SIR (dB) of the test data
SIR_SET=[0];
% SER (dB) of the test data
SER_SET=[0, -10];
% no. of experiments
numrpts=20;
% for algorithm convergence
UTT_REPEAT=2;
% seed
rngseed=0;

for i=1:length(RT60_SET)
for j=1:length(SIR_SET)
for k=1:length(SER_SET)
    rt60=RT60_SET(i);
    sirdb=SIR_SET(j);
    serdb=SER_SET(k);
    savedir=['output/rt' num2str(rt60) '_sir' num2str(sirdb) '_ser' num2str(serdb)];
    if ~exist(savedir)
        mkdir(savedir);
    end

for rptcount=1:numrpts
    fprintf(1, 'rptcount = %d ...\n', rptcount);

    %% generate test data
    fprintf(1, 'generate test data\n');

    rngseed=(i-1)*1e4+(j-1)*1e3+(k-1)*1e2+rptcount;
    [testdata, target_interf_echo] = simulateDataImageRIR(sirdb, serdb, rt60, UTT_REPEAT, rngseed);
    siglen=size(testdata, 1)/UTT_REPEAT;
    audiowrite([savedir '/input_data_', num2str(rptcount), '.wav'], testdata(end-siglen+1:end, :), fs);
    audiowrite([savedir '/target_interf_echo', num2str(rptcount), '.wav'], target_interf_echo(end-siglen+1:end, :), fs);

    %% perform separation
    nummics=2;
    numrefs=1;

    fprintf(1, '1. WPE-NLMS-BSS\n');
    out=modeWPE_NLMS_BSS(nummics, numrefs, testdata);
    out(size(testdata, 1)+1:end, :)=[];
    audiowrite([savedir '/01_out_wpe_nlms_bss_', num2str(rptcount), '.wav'], out(end-siglen+1:end, :), fs);

    fprintf(1, '2. NLMS-WPE-BSS\n');
    out=modeNLMS_WPE_BSS(nummics, numrefs, testdata);
    out(size(testdata, 1)+1:end, :)=[];
    audiowrite([savedir '/02_out_nlms_wpe_bss_', num2str(rptcount), '.wav'], out(end-siglen+1:end, :), fs);

    fprintf(1, '3. DR-AEC-BSS\n');
    out=modeDR_AEC_BSS(nummics, numrefs, testdata);
    out(size(testdata, 1)+1:end, :)=[];
    audiowrite([savedir '/03_out_dr_aec_bss_', num2str(rptcount), '.wav'], out(end-siglen+1:end, :), fs);

    fprintf(1, '4. AEC-DR-BSS\n');
    out=modeAEC_DR_BSS(nummics, numrefs, testdata);
    out(size(testdata, 1)+1:end, :)=[];
    audiowrite([savedir '/04_out_aec_dr_bss_', num2str(rptcount), '.wav'], out(end-siglen+1:end, :), fs);

    fprintf(1, '5. DRAEC-BSS\n');
    out=modeDRAEC_BSS(nummics, numrefs, testdata);
    out(size(testdata, 1)+1:end, :)=[];
    audiowrite([savedir '/05_out_draec_bss_', num2str(rptcount), '.wav'], out(end-siglen+1:end, :), fs);

    fprintf(1, '6. Joint-SS\n');
    out=modeBSS(nummics, numrefs, testdata);
    out(size(testdata, 1)+1:end, :)=[];
    audiowrite([savedir '/06_out_joint_ss_', num2str(rptcount), '.wav'], out(end-siglen+1:end, :), fs);
end

Main_score;

end
end
end


function value = i4vec_any_nonzero ( n, a )

%*****************************************************************************80
%
%% I4VEC_ANY_NONZERO: ( any A nonzero ) for I4VEC's.
%
%  Discussion:
%
%    An I4VEC is a vector of I4's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 December 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries.
%
%    Input, integer A(N), the vector.
%
%    Output, logical VALUE is TRUE if any entry is nonzero.
%
  value = any ( a(1:n) ~= 0 );

  return
end


function K = sdlfmaXsdlfmKernCompute(sdlfmaKern1, sdlfmKern2, t1, t2, covIC)

% SDLFMAXSDLFMKERNCOMPUTE Cross kernel between a SDLFMA and a SDLFM kernels.
% FORMAT
% DESC computes cross kernel terms between the accel. and the position of
% two switching dynamical LFM kernels for the multiple output kernel.
% ARG sdlfmaKern1 : the kernel structure associated with the accel. of the
% first SDLFM kernel.
% ARG sdlfmKern2 : the kernel structure associated with the position of the
% second SDLFM kernel.
% ARG t : inputs for which kernel is to be computed.
% ARG covIC : covariance for the initial conditions
% RETURN K : block of values from kernel matrix.
%
% FORMAT
% DESC computes cross kernel terms between the accel. and the position of
% two SDLFM kernels for the multiple output kernel.
% ARG sdlfmaKern1 : the kernel structure associated with the accel. of
% the first SDLFM kernel.
% ARG sdlfmKern2 : the kernel structure associated with the position of
% the second SDLFM kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covIC : covariance for the initial conditions
% RETURN K : block of values from kernel matrix.
%
% SEEALSO : sdlfmaKernParamInit, sdlfmaKernCompute, sdlfmKernParamInit
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if nargin < 4
    t2 = t1;
end

K = sdlfmXsdlfmKernCompute(sdlfmaKern1, sdlfmKern2, t1, t2, covIC, 'AccelPos');


function cor_fn = samplingPanoBox(cor_id, corn, edg, edg2, im_h, im_w, options)

    corn = im2double(corn);
    edg = im2double(edg);

    score_fin = -Inf;
    score_fin_t = -Inf;
    search_grid = 5;
    line = cor_id(1:2:end,1)';
    line_can = repmat(line, size(line,2)*(2*search_grid+1),1);
    score_ln = interp2(corn,cor_id(:,1),cor_id(:,2));
    score_ln = log(score_ln);
    score_ln = [score_ln(1) + score_ln(2); score_ln(3) + score_ln(4);...
               score_ln(5) + score_ln(6); score_ln(7) + score_ln(8)];
    [~,score_id] = sort(score_ln);

    line_can(1:(2*search_grid+1),score_id(1)) = line(score_id(1))-search_grid:line(score_id(1))+search_grid;
    line_can((2*search_grid+1)+1:(2*search_grid+1)*2, score_id(2)) = line(score_id(2))-search_grid:line(score_id(2))+search_grid;
    line_can((2*search_grid+1)*2+1:(2*search_grid+1)*3, score_id(3)) = line(score_id(3))-search_grid:line(score_id(3))+search_grid;
    line_can((2*search_grid+1)*3+1:(2*search_grid+1)*4, score_id(4)) = line(score_id(4))-search_grid:line(score_id(4))+search_grid;
    edg2 = im2double(edg2);

    % no sampling
    %line_can = line;
    for line_n = 1:size(line_can,1)
        %disp(line_n)
        cor_ini = cor_id;
        cor_ini(1:2:end,1) = line_can(line_n,:);
        cor_ini(2:2:end,1) = line_can(line_n,:);
        [cor_ini, score_all] = sample_opt_pano_joint(cor_ini, im_w, im_h, corn, edg, edg2, options);
        if score_all > score_fin_t
            score_fin_t = score_all;
            cor_fn_t = cor_ini;
        end
        if mod(line_n,search_grid*2+1) == 0
            line_id = line_n/(search_grid*2+1);
            line_can(line_n+1:end,score_id(line_id)) = cor_fn_t(2*score_id(line_id),1);
            if score_fin_t > score_fin
                score_fin = score_fin_t;
                cor_fn = cor_fn_t;
            end
            score_fin_t = -Inf;
        end
    end
    % no sampling
    %if score_fin_t > score_fin
    %    score_fin = score_fin_t;
    %    cor_fn = cor_fn_t;
    %end
end


%
% [f, Covf, pseudoX, logtheta, Kpp, Kxp, Lp] =
%     gplearn(y, V, X, pseudoX, kX, logtheta, ...)
%
% V is inverse covariance (i.e. precision) matrix for observation noise -
% sparse format is recommended.
%
% In practice, inv(V) should be diagonal. However, inv(V) can be more
% complex sparse matrix as long as it has zeros where kX is non-zero. That
% is, observation noise can be correlated if the corresponding latent
% function values are independent a priori. This more general property is
% used in PCA model, where inv(V) and kX are block-diagonal (although rows
% and columns may be in mixed order!) but the blocks "overlap" only on the
% diagonal, that is, they both have non-zero elements at the same time only
% on the diagonal.
%
% y = inv(V)*c, that is, c = V*y
% N( inv(V)*c | 0, inv(V) + Kxp inv(Kpp) Kpx )

% $$$ function [f, Covf, pseudoX, logtheta, Kpp, Kxp, Lp] = gplearn(y, V, ...
% $$$                                                   X, pseudoX, kX, logtheta, ...
% $$$                                                   varargin)
function [f, Covf, Xp, logtheta, Kpp, Kxp, Lp] = gplearn(logtheta, covfunc, ...
                                                  X, y, V, Xp, varargin)

% function [] = gp_learn(covfunc, theta, y, V)

warning('Deprecated. Use gp_learn or gp_learn_pseudo');

if ~iscell(covfunc)
  covfunc = {covfunc};
end

opts = struct( ...
    'maxsearch', 10, ...
    'vy', [], ...
    'cholv', [], ...
    'checkgrad', false, ...
    'updatepseudo', true, ...
    'priorlogtheta', []);

[ opts, errmsg, wrnmsg ] = argschk( opts, varargin{:} );
if ~isempty(errmsg), error( errmsg ), end
if ~isempty(wrnmsg), warning( wrnmsg ), end

m = cols(Xp); % number of pseudoinputs
d = rows(X); % dimensionality of the input space

if isempty(opts.cholv)
  LV = chol(V, 'lower');
%  [LV,V] = safechol(V, 1e-16, 'lower');
else
  LV = opts.cholv;
end

if isempty(opts.vy)
  c = V*y;
else
  c = opts.vy;
end

if opts.maxsearch ~= 0
  if opts.updatepseudo && ~isempty(Xp)
    % Learn pseudo-inputs and hyperparameters
    x0 = [Xp(:); logtheta(:)];
    func = @(x) bound2cost(x, c, V, LV, X, covfunc, m, d, true);
    fiter = inf;
    while ~any(~isinf(fiter))
      [x, fiter] = minimize(x0, func, opts.maxsearch);
      x0 = x0 - 1; % adhoc trying to help ill conditioned matrices..
    end
    Xp = reshape(x(1:(m*d)), [d,m]);
    logtheta = x((m*d+1):end);
  else
    % Learn only hyperparameters
    x0 = logtheta(:);
    func = @(x) bound2cost(x, c, V, LV, X, covfunc, [], d, false, Xp);
    fiter = inf;
    while ~any(~isinf(fiter))
      [logtheta, fiter] = minimize(x0, func, opts.maxsearch);
      x0 = x0 - 1; % adhoc trying to help ill conditioned matrices..
    end
  end
  if opts.checkgrad
    mycheckgrad(func, x0, 1e-8)
  end
end

% THE FOLLOWING WOULD COME AS A SIDE PRODUCT FROM THE OPTIMIZATION BUT HOW
% TO GET THEM??

if isempty(Xp)
  % Full case (no pseudo-inputs)

  n = cols(X);
  In = diag(sparse(ones(n,1)));

  K = feval(covfunc{:}, logtheta, X, X);
% $$$   norm_K = normest(K);
% $$$   K = K + 1e-14*norm_K*In; % SET PROPERLY
%  K = K + 1e-6*In;

  % DEBUG: REGULARIZE
  K = regularize(K);
  Lp = chol(K, 'lower');
%  [Lp,K] = safechol(K, 1e-10, 'lower');
  %K = Lp*Lp';
  Kpp = K;
  Kxp = [];

  % We assume that V is DIAGONAL
  V = sparse(V);
  L = chol(inv(V) + K, 'lower');
%  L = safechol(inv(V) + K, 1e-16, 'lower');

  % Posterior mean and covariance
  if false
    % TODO: NOTE, THAT DIAGONAL COVARIANCE APPROXIMATION IS A BIT
    % ADHOC!!!!! CAUSES LOGLIKELIHOOD TO BE INCORRECT!!!
    f = K * c - K * solve_triu(L', solve_tril(L, K*c));
    Covf = spalloc(n,n,n);
    for i=1:n
      r = solve_tril(L,K(:,i));
      Covf(i,i) = K(i,i) - r'*r;
    end
  else
    invLK = full(solve_tril(L,K));
    Covf = K - invLK' * invLK;
    f = Covf * c;
  end

else
  % Pseudo input case

  Im = diag(sparse(ones(m,1)));

  % GP-prior covariances
  Kpp = feval(covfunc{:}, logtheta, Xp, Xp);
% $$$   norm_Kpp = normest(Kpp);
% $$$   Kpp = Kpp + 1e-14*norm_Kpp*Im; % SET PROPERLY
%  Kpp = Kpp + 1e-3*Im; % SET PROPERLY
  Kxp = feval(covfunc{:}, logtheta, X, Xp);

  % DEBUG: REGULARIZE
  %Kpp = regularize(Kpp);

  % Helpful variables
  reg = 1;
  while 1
    try
      Kpp = regularize(Kpp, reg);
      Lp = chol(Kpp, 'lower');
      break
    catch
      warning('Ill conditioned covariance matrix in learning phase!!');
      reg = reg * 10;
    end
  end
  %  [Lp, Kpp] = safechol(Kpp, 1e-10, 'lower');
  %Kpp = Lp*Lp';

  %szLp = size(Lp)
  %szKxp = size(Kxp)
  %szLV = size(LV)
  R = solve_tril(Lp, Kxp' * LV);
  S = Im + R*R';
  LS = chol(S, 'lower');
%  LS = safechol(S, 1e-16, 'lower');
  z = LV \ c;
  beta = solve_tril(LS, R*z);
  invLsLp = solve_tril(LS, Lp');

  % Posterior mean and covariance
  f = invLsLp' * beta;
  Covf = invLsLp' * invLsLp;

end

% $$$ if issparse(Lp)
% $$$   density_in_gplearn = nnz(Lp) / prod(size(Lp));
% $$$ end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [f, df] = bound2cost(x, c, V, LV, inputs, kX, m, d, updatepseudo, ...
                              pseudoinputs)

if updatepseudo
  pseudoinputs = reshape(x(1:(m*d)), [d,m]);
  logtheta = x((m*d+1):end);
  [bound, dlogtheta, dpseudoinputs] = pseudobound(c, V, LV, inputs, ...
                                                  pseudoinputs, logtheta, kX);
else
  logtheta = x(:);
  [bound, dlogtheta] = pseudobound(c, V, LV, inputs, pseudoinputs, logtheta, ...
                                   kX);
end

f = -bound;

if updatepseudo
  df = -[dpseudoinputs(:); dlogtheta(:)];
else
  df = -dlogtheta(:);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%
function [bound, dlogtheta, dpseudoinputs] = ...
    pseudobound(c, V, LV, inputs, pseudoinputs, logtheta, covfunc)

if ~isempty(pseudoinputs)
  %% PSEUDO INPUT CASE
  %disp('Pseudo case in gplearn > pseudobound');

  d = rows(pseudoinputs); % dimensionality of pseudo-inputs
  m = cols(pseudoinputs); % number of pseudo-inputs

  % GP-prior (co)variances and their gradients
  switch nargout
   case 1
    Kpp = feval(covfunc{:}, logtheta, pseudoinputs, pseudoinputs);
    Kxp = feval(covfunc{:}, logtheta, inputs, pseudoinputs);
    Kx = feval(covfunc{:}, logtheta, inputs, []);
   case 2
    [Kpp, dKpp_logtheta] = feval(covfunc{:}, logtheta, pseudoinputs, ...
                                   pseudoinputs);
    [Kxp, dKxp_logtheta] = feval(covfunc{:}, logtheta, inputs, pseudoinputs);
    [Kx, dKx_logtheta] = feval(covfunc{:}, logtheta, inputs, []);
   case 3
    [Kpp, dKpp_logtheta, dKpp_pseudo] = feval(covfunc{:}, logtheta, ...
                                                pseudoinputs, pseudoinputs);
    [Kxp, dKxp_logtheta, dKxp_pseudo] = feval(covfunc{:}, logtheta, ...
                                                inputs, pseudoinputs);
    [Kx, dKx_logtheta] = feval(covfunc{:}, logtheta, inputs, []);
   otherwise
    error('No defined action here')
  end

% $$$ if ~iscell(covfunc), covfunc = {covfunc}; end
% $$$ [K,tmp] = feval(covfunc{:}, logtheta, pseudoinputs, pseudoinputs);
% $$$ figure
% $$$ imagesc(K-Kpp)
% $$$ return

  Im = diag(sparse(ones(m,1)));
% $$$   norm_Kpp = normest(Kpp)
% $$$   Kpp = Kpp + 1e-14*norm_Kpp*Im; % SET PROPERLY
%  Kpp = Kpp + 1e-3*Im; % SET PROPERLY
% $$$   figure
% $$$   imagesc(Kpp);
% $$$   error('jou')

  % DEBUG: REGULARIZE
  %Kpp = regularize(Kpp);

  % Helpful variables
  Kpp = regularize(Kpp);
  try
    Lp = chol(Kpp, 'lower');
  catch
    warning('Covariance matrix ill conditioned');
    bound = -inf;
    dlogtheta = nan * zeros(length(logtheta),1);
    dpseudoinputs = nan * zeros(d,m);
    return
  end
%  [Lp,Kpp] = safechol(Kpp, 1e-10, 'lower');
  %Kpp = Lp*Lp';

  R = solve_tril(Lp, Kxp' * LV);
  S = Im + R*R';
  LS = chol(S, 'lower');
%  LS = safechol(S, 1e-16, 'lower');
  LA = Lp * LS;
  z = LV \ c;
  H = solve_tril(LS, R);
  T = solve_triu(LA', H);
  beta = H * z;
  b = T * z;
  invLpR = solve_triu(Lp', R);
  diagV = full(diag(V));
  dif = c - V*(Kxp*b);

  % Lower bound for loglikelihood
  bound = -logdettri(LS) - 0.5*z'*z + 0.5*beta'*beta ...
          - 0.5*Kx'*diagV ...
          + 0.5*traceprod(R,R,true);

  % General gradient coefficients
  Tpp = 0.5 * solve_triu(LA', solve_triu(LA', LS'*LS-eye(m))') ...
        - 0.5 * invLpR*invLpR';
  Txp = -LV * T' ...
        + LV*invLpR';

  % Gradient for hyperparameters
  if nargout >= 2
    n = length(logtheta);
    dlogtheta = zeros(n,1);
    for i=1:n
      dlogtheta(i) = traceprod(dKpp_logtheta(:,:,i), Tpp, true) ...
          - 0.5 * b' * dKpp_logtheta(:,:,i) * b ...
          + traceprod(dKxp_logtheta(:,:,i), Txp, true) ...
          + dif' * dKxp_logtheta(:,:,i) * b ...
          - 0.5 * dKx_logtheta(:,:,i)' * diagV;
    end
  end

  if nargout >= 3
    % Gradient for pseudo-inputs
    dpseudoinputs = zeros(d,m);
    Dpp = spalloc(m,m,2*m);
    for i=1:m
      for j=1:d
        Dpp(:) = 0;
        Dpp(i,:) = dKpp_pseudo(j,:,i);
        Dpp(:,i) = Dpp(:,i) + dKpp_pseudo(j,:,i)';

        dpseudoinputs(j,i) = traceprod(Dpp, Tpp, true) ...
            - 0.5 * b' * Dpp * b ...
            + dKxp_pseudo(j,:,i) * Txp(:,i) ...
            + dif' * dKxp_pseudo(j,:,i)' * b(i);

      end
    end
  end

else

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % FULL CASE (NO PSEUDO INPUTS)
% $$$   disp('Full case in gplearn > pseudobound');
  d = rows(inputs); % dimensionality of pseudo-inputs
  n = cols(inputs);
  %m = cols(pseudoinputs); % number of pseudo-inputs

  % GP-prior (co)variances and their gradients
  switch nargout
   case 1
    K = feval(covfunc{:}, logtheta, inputs, inputs);
   case 2
    [K, dK_logtheta] = feval(covfunc{:}, logtheta, inputs, inputs);
   otherwise
    error('No pseudo inputs given');
  end

  In = diag(sparse(ones(n,1)));
% $$$   norm_K = normest(K);
% $$$   K = K + 1e-14*norm_K*In; % SET PROPERLY
%  K = K + 1e-6*In;

  % DEBUG: REGULARIZE
  %K = regularize(K);

  % ASSUME THAT V is DIAGONAL
  V = sparse(V);
  Ky = inv(V) + K;
  try
    L = chol(Ky, 'lower');
  catch
    warning('Covariance matrix ill conditioned');
    bound = -inf;
    dlogtheta = nan * zeros(length(logtheta),1);
    return
  end
%  [L,Ky] = safechol(Ky, 1e-10, 'lower');
  % DEBUG:
  %Ky = L*L';
  y = sparse(V) \ c;

  z = solve_tril(L, y);

  % Lower bound for loglikelihood
  bound = -logdettri(L) - 0.5*z'*z;

% $$$   % General gradient coefficients
% $$$   Tpp = 0.5 * solve_triu(LA', solve_triu(LA', LS'*LS-eye(m))') ...
% $$$         - 0.5 * invLpR*invLpR';
% $$$   Txp = -LV * T' ...
% $$$         + LV*invLpR';

  % Gradient for hyperparameters
  if nargout >= 2
    b = solve_triu(L',z);
    n = length(logtheta);
    dlogtheta = zeros(n,1);
    if issparse(Ky)
      invKy = sinv(Ky);
    else
      invKy = inv(Ky);
    end
    for i=1:n
      dlogtheta(i) = 0.5 * b' * dK_logtheta(:,:,i) * b ...
          - 0.5 * traceprod( invKy, dK_logtheta(:,:,i), true );
% $$$       dlogtheta(i) = traceprod(dKpp_logtheta(:,:,i), Tpp, true) ...
% $$$           - 0.5 * b' * dKpp_logtheta(:,:,i) * b ...
% $$$           + traceprod(dKxp_logtheta(:,:,i), Txp, true) ...
% $$$           + dif' * dKxp_logtheta(:,:,i) * b ...
% $$$           - 0.5 * dKx_logtheta(:,:,i)' * diagV;
    end
  end

end


function report_sectors(XS,str)

ntickers = size(XS,1);

[Ctry,ia,ic] = unique(XS(:,3)); % ia (index of first appearance)
N_sectors = length(Ctry);
fprintf('\n');
fprintf('%d %s Sectors for %d stocks:\n',N_sectors,str,ntickers);
for i=1:N_sectors
  n_ic = sum(ic==i);
  fprintf(' %s (%d = %.1f%%)\n',Ctry{i},n_ic,100*n_ic/length(ic));
end
fprintf('\n');

%{
[Ctry,ia,ic] = unique(XS(:,4)); % ia (index of first appearance)
N_industries = length(Ctry);
fprintf('\n');
fprintf('%d %s Industries for %d stocks:\n',N_industries,str,ntickers);
for i=1:N_industries
  n_ic = sum(ic==i);
  fprintf(' %s (%d = %.1f%%)\n',Ctry{i},n_ic,100*n_ic/length(ic));
end
fprintf('\n');
%}

end


function [ri,ti] = traintestindex( s, l1, l2, l3, l4)
% Copyright (c) 2014-present University of Illinois at Urbana-Champaign
% All rights reserved.
%
% Developed by:     Po-Sen Huang, Paris Smaragdis
%                   Department of Electrical and Computer Engineering
%                   Department of Computer Science
%
% Create a training/testing data set based ont he leave-out info provided

ri = [];
ti = [];
for i = 1:s(1)
	for j = 1:s(2)
		for l = 1:s(3)
			for k = 1:s(4)
				if (ismember( i, l1) || isempty( l1)) && (ismember( j, l2) || isempty( l2)) ...
						&& (ismember( l, l3) || isempty( l3)) && (ismember( k, l4) || isempty( l4))
					ti(end+1) = sub2ind( s, i, j, l, k);
				elseif ~ismember( i, l1) && ~ismember( j, l2) && ~ismember( l, l3) && ~ismember( k, l4)
					ri(end+1) = sub2ind( s, i, j, l, k);
				end
			end
		end
	end
end


%% DEMO_febio_0055_clot_tube_slide_pressure.m
% Below is a demonstration for:
%
% * Building geometry for a spherical blob with tetrahedral elements
% which is being aspirated into a tube.
% This demo consists off:
% * Defining the boundary conditions
% * Coding the febio structure
% * Running the model
% * Importing and visualizing the displacement results

%% Keywords
%
% * febio_spec version 3.0
% * febio, FEBio
% * blood clot
% * contact, sliding, friction
% * rigid body constraints
% * hexahedral elements, hex8
% * quadrilaterl elements, quad4
% * shell elements
% * sphere
% * hyperelastic, Ogden
% * displacement logfile
% * stress logfile

%%

clear; close all; clc;

%% Plot settings
fontSize=15;
faceAlpha1=0.8;
faceAlpha2=0.3;
markerSize=40;
lineWidth=3;
cMap=[1 0.5 0.4; 0.9 0.3 0.27; 0.8 0.2 0.18; 0.7 0.1 0.09; 0.6 0 0; 0.5 0 0; 0.4 0 0;];
[cMap]=resampleColormap(cMap,250);

%% Control parameters

% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'data','temp');

% Defining file names
febioFebFileNamePart='tempModel';
febioFebFileName=fullfile(savePath,[febioFebFileNamePart,'.feb']); %FEB file name
febioLogFileName=fullfile(savePath,[febioFebFileNamePart,'.txt']); %FEBio log file name
febioLogFileName_disp=[febioFebFileNamePart,'_disp_out.txt']; %Log file name for exporting displacement
febioLogFileName_strainEnergy=[febioFebFileNamePart,'_energy_out.txt']; %Log file name for exporting strain energy density

% Sphere parameters
sphereRadius=3;%
numElementsMantel=6;

% Ground plate parameters
tubeRadius=sphereRadius.*[1 0.1];
tubeAngle=3*(pi/180);
tubeLength=abs(diff(tubeRadius))/tan(tubeAngle);

% Material parameter set
c1=1e-4; %Shear-modulus-like parameter MPa
m1=2; %Material parameter setting degree of non-linearity
k_factor=10; %Bulk modulus factor
k=c1*k_factor; %Bulk modulus
d=1e-9; %Density

% FEA control settings
max_refs=25; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=15; %Optimum number of iterations
max_retries=25; %Maximum number of retires
symmetric_stiffness=0;
min_residual=1e-20;

timeTotal=1; %Analysis time
numTimeSteps=100; %Number of time steps desired
step_size=timeTotal/numTimeSteps;
dtmin=(timeTotal/numTimeSteps)/100; %Minimum time step size
dtmax=(timeTotal/numTimeSteps)*10; %Maximum time step size
analysisType='DYNAMIC';

%Contact parameters
contactPenalty=10;
laugon=0;
minaug=1;
maxaug=10;
fric_coeff=0.1;

%Specifying load
pressureValue=7.5e-5;

%% Creating model geometry and mesh
%

%Control settings
cPar.sphereRadius=sphereRadius;
cPar.coreRadius=cPar.sphereRadius/2;
cPar.numElementsMantel=numElementsMantel;
cPar.numElementsCore=round(numElementsMantel*1.5);
cPar.outputStructType=2;
cPar.makeHollow=0;
cPar.cParSmooth.n=25;

%Creating sphere
[meshOutput]=hexMeshSphere(cPar);

% Access model element and patch data
Fb_blob=meshOutput.facesBoundary;
Cb_blob=meshOutput.boundaryMarker;
V_blob=meshOutput.nodes;
E_blob=meshOutput.elements;

%%
% Visualize blob mesh

hFig=cFigure;
subplot(1,2,1); hold on;
gpatch(Fb_blob,V_blob,Cb_blob,'k',0.8);
patchNormPlot(Fb_blob,V_blob);
axisGeom(gca,fontSize);
colormap(gjet); icolorbar;
camlight headlight;

hs=subplot(1,2,2); hold on;
title('Cut view of solid mesh','FontSize',fontSize);
optionStruct.hFig=[hFig hs];
gpatch(Fb_blob,V_blob,'kw','none',0.25);
meshView(meshOutput,optionStruct);
axisGeom(gca,fontSize);
drawnow;

%% Creating tube model
%

pointSpacingBlob=mean(patchEdgeLengths(Fb_blob,V_blob));
pointSpacingTube=pointSpacingBlob;

rEnd=sphereRadius+(sphereRadius.*((sphereRadius-tubeRadius(2))/tubeLength));
V_curve_tube=[sphereRadius rEnd 0; -tubeLength tubeRadius(2) 0;];

nResample=ceil(max(pathLength(V_curve_tube))./pointSpacingTube);
V_curve_tube=evenlySampleCurve(V_curve_tube,nResample,'pchip',0);

cPar.closeLoopOpt=1;
cPar.numSteps=[]; %If empty the number of steps is derived from point spacing of input curve
cPar.w=[1 0 0];
[F_tube,V_tube]=polyRevolve(V_curve_tube,cPar);
[F_tube,V_tube]=mergeVertices(F_tube,V_tube);
center_of_mass_tube=mean(V_tube,1);

%% Join model node sets

V=[V_blob; V_tube; ];
F_tube=F_tube+size(V_blob,1);

%%
% Visualizing model

cFigure; hold on;
gtitle('Model components',fontSize);
hl(1)=gpatch(Fb_blob,V,'rw','k',0.8);
hl(2)=gpatch(F_tube,V,'kw','k',0.5);
legend(hl,{'Blob','Tube'}); clear hl;
axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Get contact surfaces
%

F_contact_blob=Fb_blob;

%%
% Visualize contact surfaces

cFigure; hold on;
title('Tube blob contact pair','fontsize',fontSize);
hl(1)=gpatch(F_tube,V,'rw','k',0.8);
patchNormPlot(F_tube,V);
hl(2)=gpatch(F_contact_blob,V,'kw','k',0.5);
patchNormPlot(F_contact_blob,V);
legend(hl,{'Master','Slave'}); clear hl;
axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Get pressure surface

N=patchNormal(F_contact_blob,V);
x=[1 0 0];
D=dot(N,x(ones(size(N,1),1),:),2);

logicFace=D>-1e-6; %Logic for current face set
F_pressure=F_contact_blob(logicFace,:); %The current face set

%%
% Visualizing boundary conditions. Markers plotted on the semi-transparent
% model denote the nodes in the various boundary condition lists.

cFigure; hold on;
title('Pressure surface','fontsize',fontSize);

gpatch(F_pressure,V,'kw','k',0.5);
patchNormPlot(F_pressure,V);

% legend(hl,{'Master','Slave'}); clear hl;
axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Defining the FEBio input structure
% See also |febioStructTemplate| and |febioStruct2xml| and the FEBio user
% manual.

%Get a template with default settings
[febio_spec]=febioStructTemplate;

%febio_spec version
febio_spec.ATTR.version='3.0';

%Module section
febio_spec.Module.ATTR.type='solid';

%Control section
febio_spec.Control.analysis=analysisType;
febio_spec.Control.time_steps=numTimeSteps;
febio_spec.Control.step_size=timeTotal/numTimeSteps;
febio_spec.Control.solver.max_refs=max_refs;
febio_spec.Control.solver.max_ups=max_ups;
febio_spec.Control.solver.symmetric_stiffness=symmetric_stiffness;
febio_spec.Control.time_stepper.dtmin=dtmin;
febio_spec.Control.time_stepper.dtmax=dtmax;
febio_spec.Control.time_stepper.max_retries=max_retries;
febio_spec.Control.time_stepper.opt_iter=opt_iter;

%Material section
materialName1='Material1';
febio_spec.Material.material{1}.ATTR.name=materialName1;
febio_spec.Material.material{1}.ATTR.type='Ogden unconstrained';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.c1=c1;
febio_spec.Material.material{1}.m1=m1;
febio_spec.Material.material{1}.c2=c1;
febio_spec.Material.material{1}.m2=-m1;
febio_spec.Material.material{1}.cp=k;
febio_spec.Material.material{1}.density=d;

materialName2='Material2';
febio_spec.Material.material{2}.ATTR.name=materialName2;
febio_spec.Material.material{2}.ATTR.type='rigid body';
febio_spec.Material.material{2}.ATTR.id=2;
febio_spec.Material.material{2}.density=1;
febio_spec.Material.material{2}.center_of_mass=center_of_mass_tube;

%Mesh section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='nodeSet_all'; %The node set name
febio_spec.Mesh.Nodes{1}.node.ATTR.id=(1:size(V,1))'; %The node id's
febio_spec.Mesh.Nodes{1}.node.VAL=V; %The nodel coordinates

% -> Elements
partName1='Part1';
febio_spec.Mesh.Elements{1}.ATTR.name=partName1; %Name of this part
febio_spec.Mesh.Elements{1}.ATTR.type='hex8'; %Element type
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(E_blob,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=E_blob; %The element matrix

partName2='Part2';
febio_spec.Mesh.Elements{2}.ATTR.name=partName2; %Name of this part
febio_spec.Mesh.Elements{2}.ATTR.type='quad4'; %Element type
febio_spec.Mesh.Elements{2}.elem.ATTR.id=size(E_blob,1)+(1:1:size(F_tube,1))'; %Element id's
febio_spec.Mesh.Elements{2}.elem.VAL=F_tube; %The element matrix

% -> Surfaces
surfaceName1='Surface1_contact';
febio_spec.Mesh.Surface{1}.ATTR.name=surfaceName1;
febio_spec.Mesh.Surface{1}.quad4.ATTR.id=(1:1:size(F_tube,1))';
febio_spec.Mesh.Surface{1}.quad4.VAL=F_tube;

surfaceName2='Surface2_contact';
febio_spec.Mesh.Surface{2}.ATTR.name=surfaceName2;
febio_spec.Mesh.Surface{2}.quad4.ATTR.id=(1:1:size(F_contact_blob,1))';
febio_spec.Mesh.Surface{2}.quad4.VAL=F_contact_blob;

surfaceName3='Surface3_pressure';
febio_spec.Mesh.Surface{3}.ATTR.name=surfaceName3;
febio_spec.Mesh.Surface{3}.quad4.ATTR.id=(1:1:size(F_pressure,1))';
febio_spec.Mesh.Surface{3}.quad4.VAL=F_pressure;

% -> Surface pairs
contactPairName='Contact1';
febio_spec.Mesh.SurfacePair{1}.ATTR.name=contactPairName;
febio_spec.Mesh.SurfacePair{1}.primary=surfaceName2;
febio_spec.Mesh.SurfacePair{1}.secondary=surfaceName1;

%MeshDomains section
febio_spec.MeshDomains.SolidDomain.ATTR.name=partName1;
febio_spec.MeshDomains.SolidDomain.ATTR.mat=materialName1;

febio_spec.MeshDomains.ShellDomain.ATTR.name=partName2;
febio_spec.MeshDomains.ShellDomain.ATTR.mat=materialName2;

%Loads
febio_spec.Loads.surface_load{1}.ATTR.type='pressure';
febio_spec.Loads.surface_load{1}.ATTR.surface=surfaceName3;
febio_spec.Loads.surface_load{1}.pressure.ATTR.lc=1;
febio_spec.Loads.surface_load{1}.pressure.VAL=pressureValue;
febio_spec.Loads.surface_load{1}.symmetric_stiffness=1;

%Rigid section
% -> Prescribed rigid body boundary conditions
febio_spec.Rigid.rigid_constraint{1}.ATTR.name='RigidFix_1';
febio_spec.Rigid.rigid_constraint{1}.ATTR.type='fix';
febio_spec.Rigid.rigid_constraint{1}.rb=2;
febio_spec.Rigid.rigid_constraint{1}.dofs='Rx,Ry,Rz,Ru,Rv,Rw';

%Contact section
febio_spec.Contact.contact{1}.ATTR.type='sliding-elastic';
febio_spec.Contact.contact{1}.ATTR.surface_pair=contactPairName;
febio_spec.Contact.contact{1}.two_pass=0;
febio_spec.Contact.contact{1}.laugon=laugon;
febio_spec.Contact.contact{1}.tolerance=0.2;
febio_spec.Contact.contact{1}.gaptol=0;
febio_spec.Contact.contact{1}.minaug=minaug;
febio_spec.Contact.contact{1}.maxaug=maxaug;
febio_spec.Contact.contact{1}.search_tol=0.01;
febio_spec.Contact.contact{1}.search_radius=0.01*sqrt(sum((max(V,[],1)-min(V,[],1)).^2,2));
febio_spec.Contact.contact{1}.symmetric_stiffness=0;
febio_spec.Contact.contact{1}.auto_penalty=1;
febio_spec.Contact.contact{1}.penalty=contactPenalty;
febio_spec.Contact.contact{1}.fric_coeff=fric_coeff;

%LoadData section
% -> load_controller
febio_spec.LoadData.load_controller{1}.ATTR.id=1;
febio_spec.LoadData.load_controller{1}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{1}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{1}.points.point.VAL=[0 0; 1 1];

%Output section
% -> log file
febio_spec.Output.logfile.ATTR.file=febioLogFileName;
febio_spec.Output.logfile.node_data{1}.ATTR.file=febioLogFileName_disp;
febio_spec.Output.logfile.node_data{1}.ATTR.data='ux;uy;uz';
febio_spec.Output.logfile.node_data{1}.ATTR.delim=',';

febio_spec.Output.logfile.element_data{1}.ATTR.file=febioLogFileName_strainEnergy;
febio_spec.Output.logfile.element_data{1}.ATTR.data='sed';
febio_spec.Output.logfile.element_data{1}.ATTR.delim=',';
febio_spec.Output.logfile.element_data{1}.VAL=1:size(E_blob,1);

%% Quick viewing of the FEBio input file structure
% The |febView| function can be used to view the xml structure in a MATLAB
% figure window.

%%
% |febView(febio_spec); %Viewing the febio file|

%% Exporting the FEBio input file
% Exporting the febio_spec structure to an FEBio input file is done using
% the |febioStruct2xml| function.

febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode

%% Running the FEBio analysis
% To run the analysis defined by the created FEBio input file the
% |runMonitorFEBio| function is used. The input for this function is a
% structure defining job settings e.g. the FEBio input file name. The
% optional output runFlag informs the user if the analysis was run
% succesfully.

febioAnalysis.run_filename=febioFebFileName; %The input file name
febioAnalysis.run_logname=febioLogFileName; %The name for the log file
febioAnalysis.disp_on=1; %Display information on the command window
febioAnalysis.runMode='external';%'internal';

[runFlag]=runMonitorFEBio(febioAnalysis);%START FEBio NOW!!!!!!!!

%% Import FEBio results

if 1%runFlag==1 %i.e. a succesful run

    % Importing nodal displacements from a log file
    [time_mat, N_disp_mat,~]=importFEBio_logfile(fullfile(savePath,febioLogFileName_disp)); %Nodal displacements
    time_mat=[0; time_mat(:)]; %Time

    N_disp_mat=N_disp_mat(:,2:end,:);
    sizImport=size(N_disp_mat);
    sizImport(3)=sizImport(3)+1;
    N_disp_mat_n=zeros(sizImport);
    N_disp_mat_n(:,:,2:end)=N_disp_mat;
    N_disp_mat=N_disp_mat_n;
    DN=N_disp_mat(:,:,end);
    DN_magnitude=sqrt(sum(DN.^2,2));
    V_def=V+DN;
    V_DEF=N_disp_mat+repmat(V,[1 1 size(N_disp_mat,3)]);
    X_DEF=V_DEF(:,1,:);
    Y_DEF=V_DEF(:,2,:);
    Z_DEF=V_DEF(:,3,:);
    %     [CF]=vertexToFaceMeasure(Fb_all,DN_magnitude);

    %%
    % Importing element strain energies from a log file
    [~,E_energy,~]=importFEBio_logfile(fullfile(savePath,febioLogFileName_strainEnergy)); %Element stresses

    %Remove nodal index column
    E_energy=E_energy(:,2:end,:);

    %Add initial state i.e. zero displacement
    sizImport=size(E_energy);
    sizImport(3)=sizImport(3)+1;
    E_energy_mat_n=zeros(sizImport);
    E_energy_mat_n(:,:,2:end)=E_energy;
    E_energy=E_energy_mat_n;

    %%

    indBlob=unique(Fb_blob(:));
    t=linspace(0,2*pi,250)';

    V_def_blob=V(indBlob,:)+N_disp_mat(indBlob,:,end);

    [~,indMin]=min(V_def_blob(:,1));
    [~,indMax]=max(V_def_blob(:,1));

    xEnd=V_def_blob(indMin,1);
    xStart=V_def_blob(indMax,1);
    rEnd=sphereRadius+(xEnd.*((sphereRadius-tubeRadius(2))/tubeLength));
    rStart=sphereRadius+(xStart.*((sphereRadius-tubeRadius(2))/tubeLength));

    xMid=mean([xStart xEnd]);%sum([rStart rEnd].*[xStart xEnd])./sum([rStart rEnd]);
    rMid=sphereRadius+(xMid.*((sphereRadius-tubeRadius(2))/tubeLength));

    V_plot_xEnd=[xEnd*ones(size(t)) rEnd*cos(t) rEnd*sin(t)];
    V_plot_xMid=[xMid*ones(size(t)) rMid*cos(t) rMid*sin(t)];
    V_plot_xStart=[xStart*ones(size(t)) rStart*cos(t) rStart*sin(t)];

   %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; hold on;
    ht=gtitle(['Radial stretch: ',num2str(rMid/sphereRadius)]);
    hp1=gpatch(Fb_blob,V_def,DN_magnitude,'none',1); %Add graphics object to animate

    hp2=plotV(V_plot_xEnd  ,'r-','LineWidth',3);
    hp3=plotV(V_plot_xMid  ,'r-','LineWidth',3);
    hp4=plotV(V_plot_xStart,'r-','LineWidth',3);

    gpatch(F_tube,V_def,'kw','none',0.25); %Add graphics object to animate
    axisGeom(gca,fontSize);
    colormap(cMap); colorbar;
    caxis([0 max(DN_magnitude(:))]); caxis manual;
    axis([min(X_DEF(:)) max(X_DEF(:)) min(Y_DEF(:)) max(Y_DEF(:)) min(Z_DEF(:)) max(Z_DEF(:))]);
    camlight headlight; lighting gouraud;
    view(0,0);
%     view(-30,30); zoom(1.5);
    axis off;
    drawnow;

    LMid=1;

    % Set up animation features
    animStruct.Time=time_mat; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments
        DN=N_disp_mat(:,:,qt); %Current displacement
        DN_magnitude=sqrt(sum(DN.^2,2));
        V_def=V+DN; %Current nodal coordinates

        V_def_blob=V_def(indBlob,:);
        [~,indMin]=min(V_def_blob(:,1));
        [~,indMax]=max(V_def_blob(:,1));

        xEnd=V_def_blob(indMin,1);
        xStart=V_def_blob(indMax,1);
        rEnd=sphereRadius+(xEnd.*((sphereRadius-tubeRadius(2))/tubeLength));
        rStart=sphereRadius+(xStart.*((sphereRadius-tubeRadius(2))/tubeLength));

        xMid=mean([xStart xEnd]);%sum([rStart rEnd].*[xStart xEnd])./sum([rStart rEnd]);
        rMid=sphereRadius+(xMid.*((sphereRadius-tubeRadius(2))/tubeLength));

        LMid=min(LMid,rMid/sphereRadius);

        V_plot_xEnd=[xEnd*ones(size(t)) rEnd*cos(t) rEnd*sin(t)];
        V_plot_xMid=[xMid*ones(size(t)) rMid*cos(t) rMid*sin(t)];
        V_plot_xStart=[xStart*ones(size(t)) rStart*cos(t) rStart*sin(t)];

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp1 hp1 hp2 hp2 hp2 hp3 hp3 hp3 hp4 hp4 hp4 ht]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','CData','XData','YData','ZData','XData','YData','ZData','XData','YData','ZData','String'}; %Properties of objects to animate
        animStruct.Set{qt}={V_def,DN_magnitude,...
            V_plot_xEnd(:,1),V_plot_xEnd(:,2),V_plot_xEnd(:,3),...
            V_plot_xMid(:,1),V_plot_xMid(:,2),V_plot_xMid(:,3),...
            V_plot_xStart(:,1),V_plot_xStart(:,2),V_plot_xStart(:,3),...
            ['Radial stretch: ',num2str(rMid/sphereRadius)]}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature

    drawnow;

end

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

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


clc;

S = [22  30];
bb = [1 1 15 15]';

grid = bb_scan(bb,S)
%  grid = grid(:,1:1000);
%  o = bb_overlap(grid,grid,2);


function [A, C, b, f] = HALS(Y, A, C, b, f, params)
%% Hierarchical alternating least square method for solving NMF problem
% Y = A*C + b*f

%input:
%   Y:      d1 X d2 X T, raw data. It will be reshaped to (d1*d2) X T in this
%   function
%   A:      (d1*d2) X K, initial value of spatial components
%   C:      K X T, initial value of temporal components
%   b:      (d1*d2) X nb, initial value of background spatial component
%   f:      nb X T, initial value of background temporal component
%   params: parameters used in this function.
%       bSiz:   blur size. A box kernel (bSiz X bSiz) will be convolved
%       with each neuron's initial spatial component, then all nonzero
%       pixels will be picked as pixels to be updated, and the rest will be
%       forced to be 0.
%       maxIter: maximum iteration of iterating HALS.

% Author: Pengcheng Zhou, Carnegie Mellon University, based on a python
% implementation from Johannes Friedrich, Columbia University, 2015.

%% parameters
if isfield(params, 'maxIter'), maxIter = params.maxIter; else maxIter=5; end
if isfield(params, 'search_method'); method=params.search_method; else method='ellipse'; end
if and(isfield(params, 'bSiz'), strcmpi(method, 'dilate'))
    params.se = strel('disk', params.bSiz);
end
Y = reshape(Y, size(A, 1), []);
% search locations
IND = determine_search_location(A, method, params);

%% update spatial and temporal components neuron by neurons

for miter=1:maxIter
    %% update neurons
    Yac = Y - b*f;
    ind_del = find(std(A,0,1)==0);
    A(:, ind_del) = [];
    C(ind_del, :) = [];
    IND(:, ind_del) = [];
    %   temporal
    C = HALS_temporal(Yac, A, C, 5);

    ind_del = find(std(C,0,2)==0);
    A(:, ind_del) = [];
    C(ind_del, :) = [];
    IND(:, ind_del) = [];
    %   spatial
    A = HALS_spatial(Yac, A, C, IND, 5);

    %% update background
    Ybg = Y-A*C;
    % temporal
    f = HALS_temporal(Ybg, b, f, 5);
    % spatial
    b = HALS_spatial(Ybg, b, f, [], 5);
end


function [SE_MR,SE_MR_asymptotic] = functionComputeSE_DL_MR_impairments(channelGainOverNoise,tau_c,M,K,L,p,rho,f,kappatUE,kapparBS,kappatBS,kapparUE)
%Compute DL SE for different transmit precoding schemes using Corollary
%6.6.
%
%INPUT:
%channelGainOverNoise = K x L x L matrix containing the average channel
%                       gains normalized by the noise variance of al
%                       channels (in dB). channelGainOverNoise(k,j,l) is
%                       the channel gain between between UE k in cell j
%                       and BS l
%tau_c                = Length of coherence block
%M                    = Number of antennas per BS
%K                    = Number of UEs per cell
%L                    = Number of BSs and cells
%p                    = Uplink transmit power per UE (same for everyone)
%rho                  = Downlink transmit power per UE (same for everyone)
%f                    = Pilot reuse factor
%kappatUE             = Hardware quality of the UEs' transmitters
%kapparBS             = Hardware quality of the BSs' receivers
%kappatBS             = Hardware quality of the BSs' transmitters
%kapparUE             = Hardware quality of the UEs' receivers
%
%OUTPUT:
%SE_MR            = K x L matrix where element (k,l) is the downlink SE of
%                   UE k in cell l achieved with MR precoding
%SE_MR_asymptotic = K x L matrix where element (k,l) is the asymptotic
%                   downlink SE of UE k in cell l achieved with MR precoding
%
%
%This Matlab function was developed to generate simulation results to:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Initiate the hardware qualities as perfect if these are not given as input
if nargin<9
    kappatUE = 1;
end

if nargin<10
    kapparBS = 1;
end

if nargin<11
    kappatBS = 1;
end

if nargin<12
    kapparUE = 1;
end

%Transform the (normalized) average channel gains from dB to linear scale
betaValues = 10.^(channelGainOverNoise/10);

%Compute length of pilot sequences
tau_p = f*K;

%Generate pilot pattern
if f == 1

    pilotPattern = ones(L,1);

elseif f == 2 %Only works in the running example with its 16 BSs

    pilotPattern = kron(ones(2,1),[1; 2; 1; 2; 2; 1; 2; 1]);

elseif f == 4 %Only works in the running example with its 16 BSs

    pilotPattern = kron(ones(2,1),[1; 2; 1; 2; 3; 4; 3; 4]);

elseif f == 16 %Only works in the running example with its 16 BSs

    pilotPattern = (1:L)';

end

%Prepare to store signal and interference terms for all UEs
signalTerm = zeros(K,L);
interferenceTerm = zeros(K,L);

%If the asymptotic SE should be computed
if nargout>1

    signalTerm_asymptotic = zeros(K,L);
    interferenceTerm_asymptotic = zeros(K,L);

end

%% Go through all cells
for j = 1:L

    %Compute the parameter in (6.50)
    G = (1 + kappatBS*(M-1))/(M*kappatBS*kapparUE);

    %Go through all UEs
    for k = 1:K

        %Extract cells that use same pilots as cell j
        groupMembers = find(pilotPattern==pilotPattern(j))';

        %Compute the parameter in (6.38)
        psi_jk = 1/( p*kappatUE*kapparBS*tau_p*sum(betaValues(k,groupMembers,j)) + p*(1-kappatUE*kapparBS)*sum(sum(betaValues(:,:,j))) + 1 );

        %Compute the numerator of (6.47)
        signalTerm(k,j) = rho*p*(betaValues(k,j,j))^2*tau_p*psi_jk*M;

        %Compute parameter in (6.49)
        Flijk = (1+p*betaValues(:,:,j)*psi_jk*( 1 - kappatUE*kapparBS + (1-kappatUE)*kappatBS*kapparBS*(M-1) ))/(kappatBS*kapparUE*kappatUE*kapparBS);

        %Compute the non-coherent interference that UE k in cell j causes
        %to all the UEs, which is a part of the first term in the
        %denominator of (6.47)
        interferenceTerm = interferenceTerm + rho*betaValues(:,:,j).*Flijk;

        %Compute the coherent interference that UE k in cell j causes to
        %all the pilot-sharing UEs, which is a part of the second term in
        %the denominator of (6.47)
        interferenceTerm(k,groupMembers) = interferenceTerm(k,groupMembers) + rho*p*(betaValues(k,groupMembers,j)).^2*tau_p*psi_jk*M*G;

        %Compute the last two terms in the denominator of (6.47)
        interferenceTerm(k,j) = interferenceTerm(k,j) - signalTerm(k,j) + 1/(kappatUE*kapparBS*kappatBS*kapparUE);

        %If the asymptotic SE in (6.51) should be computed
        if nargout>1

            %Compute the numerator of (6.51), multiplied with psi_jk
            signalTerm_asymptotic(k,j) = rho*(betaValues(k,j,j))^2*psi_jk;

            %Compute the part of the first term in the denominator of
            %(6.51) that UE k in cell j contributes to
            interferenceTerm_asymptotic = interferenceTerm_asymptotic + rho*(betaValues(:,:,j)).^2*psi_jk*(1-kappatUE)/(kappatUE*kapparUE*tau_p);

            %Compute the part of the second term in the denominator of
            %(6.51) that UE k in cell j contributes to
            interferenceTerm_asymptotic(k,groupMembers) = interferenceTerm_asymptotic(k,groupMembers) +  rho*(betaValues(k,groupMembers,j)).^2*psi_jk/kapparUE;

            %Subtract the numerator from the denominator, which is an
            %alternative way of obtaining the third term in the denominator
            interferenceTerm_asymptotic(k,j) = interferenceTerm_asymptotic(k,j) - rho*(betaValues(k,j,j))^2*psi_jk;

        end

    end

end

%Compute the prelog factor assuming only downlink transmission
prelogFactor = (tau_c-tau_p)/tau_c;

%Compute the final SE expression based on Corollary 6.6
SE_MR = prelogFactor * log2(1+signalTerm ./ real(interferenceTerm));

if nargout>1

    %Compute the final asymptotic SE expression based on Corollary 6.7
    SE_MR_asymptotic = prelogFactor * log2(1+signalTerm_asymptotic ./ real(interferenceTerm_asymptotic));

end