function [cpulse, verbose] = tapas_physio_get_oxy_pulses_filtered(c, t, ...
            dt120, verbose)
% Determines peaks of pulse oximeter data after Gaussian and high pass
% filtering, thresholding and assuming maximum heart rate (minimum peak
% distance)
%
%   [cpulse, verbose] = tapas_physio_get_oxy_pulses_filtered(c, t, ...
%            dt120, verbose);
%
% IN
%   c               [nSamples, 1] raw pulse oximeter samples
%   t               [nSamples, 1] time vector corresponding to samples (in seconds)
%   dt120           number of samples corresponding to a heart rate of 120 beats
%                   per minutes, i.e. number of samples acquiredi 0.5 seconds
%   verbose         Substructure of PhysIO, holding verbose.level and
%                   verbose.fig_handles with plotted figure handles
%                   debugging plots for thresholding are only provided, if verbose.level >=2
%
% OUT
%   cpulse          time points (seconds) of detected cardiac pulses
%   (heartbeat events, e.g. R-peaks)
%   verbose         Substructure of PhysIO, augmentedy by the additional
%                   figure handles created during this function
%
% EXAMPLE
%   tapas_physio_get_oxy_pulses_filtered
%
%   See also

% Author: Lars Kasper
% Created: 2014-08-03
% Copyright (C) 2014 TNU, Institute for Biomedical Engineering, University of Zurich and ETH Zurich.
%
% This file is part of the TAPAS PhysIO 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/>.

dt = t(2) - t(1);
c = c-mean(c); c = c./max(c); % normalize time series

% smooth noisy pulse oximetry data to detect peaks
w = tapas_physio_gausswin(2*floor(dt120/2)+1, 1);  % Odd number of samples
sc = tapas_physio_conv(c, w, 'symmetric');
sc = sc-mean(sc); sc = sc./max(sc); % normalize time series

% Highpass filter to remove drifts
cutoff = 1/dt; %1 seconds/per sampling units
forder = 2;
[b,a] = butter(forder,2/cutoff, 'high');
sc =filter(b,a, sc);
sc = sc./max(sc);

[tmp, cpulse] = tapas_physio_findpeaks(sc, 'minpeakheight',...
    thresh_cardiac.min, 'minpeakdistance', dt120);

if verbose.level >=2 % visualise influence of smoothing on peak detection
    verbose.fig_handles(end+1) = tapas_physio_get_default_fig_params();
    set(gcf, 'Name', 'Preproc: PPU-OXY: Tresholding Maxima for Heart Beat Detection');
    [tmp, cpulse2] = tapas_physio_findpeaks(c,'minpeakheight',thresh_cardiac.min,'minpeakdistance', dt120);
    plot(t, c, 'k');
    hold all;
    plot(t, sc, 'r', 'LineWidth', 2);
    hold all
    hold all;stem(t(cpulse2),c(cpulse2), 'k--');
    hold all;stem(t(cpulse),sc(cpulse), 'm', 'LineWidth', 2);
    plot(t, repmat(thresh_cardiac.min, length(t),1),'g-');
    legend('Raw PPU time series', 'Smoothed PPU Time Series', ...
        'Detected Heartbeats in Raw Time Series', ...
        'Detected Heartbeats in Smoothed Time Series', ...
        'Threshold (Min) for Heartbeat Detection');
end

cpulse = t(cpulse);


function [y] = spm_gx_hdm(x,u,P,M)
% Simulated BOLD response to input.
% FORMAT [y] = spm_gx_hdm(x,u,P,M)
% y    - BOLD response (%)
% x    - state vector     (see spm_fx_fmri)
% P    - Parameter vector (see spm_fx_fmri)
%__________________________________________________________________________
%
% This function implements the BOLD signal model described in:
%
% Stephan KE, Weiskopf N, Drysdale PM, Robinson PA, Friston KJ (2007)
% Comparing hemodynamic models with DCM. NeuroImage 38: 387-401.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston & Klaas Enno Stephan
% $Id: spm_gx_hdm.m 6856 2016-08-10 17:55:05Z karl $

% biophysical constants for 1.5 T:
%==========================================================================

% hemodynamic parameters
%--------------------------------------------------------------------------
%   H(1) - signal decay                                   d(ds/dt)/ds)
%   H(2) - autoregulation                                 d(ds/dt)/df)
%   H(3) - transit time                                   (t0)
%   H(4) - exponent for Fout(v)                           (alpha)
%   H(5) - resting oxygen extraction                      (E0)
%   H(6) - ratio of intra- to extra-vascular components   (epsilon)
%--------------------------------------------------------------------------
if isstruct(P)
    H     = [0.64 0.32 2.00 0.32 0.4];
    for i = 1:numel(P.decay)
        H(6)   = P.epsilon;
        y(i,1) = spm_gx_hdm(x(i,:),u(i),H,M);
    end
    return
end

% echo time (seconds)
%--------------------------------------------------------------------------
try
    TE = M(1).TE;
catch
    TE = 0.04;
end

% resting venous volume
%--------------------------------------------------------------------------
V0    = 100*0.08;

% slope r0 of intravascular relaxation rate R_iv as a function of oxygen
% saturation Y:  R_iv = r0*[(1-Y)-(1-Y0)]
%--------------------------------------------------------------------------
r0    = 25;

% frequency offset at the outer surface of magnetized vessels
%--------------------------------------------------------------------------
nu0   = 40.3;

% region-specific resting oxygen extraction fractions
%--------------------------------------------------------------------------
E0    = P(5);

% region-specific ratios of intra- to extravascular components of
% the gradient echo signal (prior mean = 1, log-normally distributed
% scaling factor)
%--------------------------------------------------------------------------
epsi  = exp(P(6));

% coefficients in BOLD signal model
%--------------------------------------------------------------------------
k1    = 4.3.*nu0.*E0.*TE;
k2    = epsi.*r0.*E0.*TE;
k3    = 1 - epsi;

% exponentiation of hemodynamic state variables
%--------------------------------------------------------------------------
x     = exp(x);

% BOLD signal
%--------------------------------------------------------------------------
v     = x(3);
q     = x(4);
y     = V0*(k1.*(1 - q) + k2.*(1 - (q./v)) + k3.*(1 - v));


function varargout = gallerysphere(name)
%CHEB.GALLERYSPHERE   Spherefun example functions.
%   F = CHEB.GALLERYSPHERE(NAME) returns a spherefun corresponding to NAME.
%   See the listing below for available names.
%
%   For example,  plot(cheb.gallerysphere('football')) plots the classic
%   icosahedral pattern of the Addias Telestar football (or soccer ball for
%   Americans).  For details of how each function is constructed try
%   'type +cheb/gallerysphere' or 'edit cheb.gallerysphere'.
%
%   [F,FA] = CHEB.GALLERYSPHERE(NAME) also returns the anonymous function
%   FA used to define the function. Some gallery functions are generated by
%   operations beyond the usual Spherefun constructor, so FA in those cases
%   is equal to F.
%
%   CHEB.GALLERYSPHERE with no input argument returns a function chosen at
%   random from the gallery.
%
%   CHEB.GALLERYSPHERE with no output argument creates a plot of the selected
%   function.
%
%   Available names:
%
%   deathstar   A function resembling the surface of the Death Star.
%   gaussian    Gaussian function on the sphere centered at Gauss's birth place.
%   geomag      Radial component of the International Geomagnetic Reference
%               field from the IGRF-12 model for 2015.
%   football    Icosahedral pattern found on a traditional soccer ball.
%   jet         A zonal jet stream over the mid-latitudes of the northern hemisphere.
%   moire       Moire pattern from waves generated at two point sources.
%   neamtu      Function created by Mike Neamtu for testing various spline
%               interpolation methods on the sphere (see Alfeld, Neamtu,
%               Schumaker, J. Comput. Appl. Math. 1996).
%   peaks       Peaks like function on the sphere taken from the geopeaks
%               function in the MATLAB mapping toolbox.
%   randn       Random linear combination of all real spherical harmonics
%               of exact degree 40.  The coefficients are generated from a
%               i.i.d. Gaussian (normal) distribution with std=1.
%   reprodkern  Reproducing kernel for spherical harmonics of degree 10
%               centered at (x,y,z) = (-1/sqrt(3),-1/sqrt(3),1/sqrt(3)).
%   soccerball  Same as football, but for the American users.
%   stripes     Alternating striped pattern.
%   vortices    Two antipodal vortices taken from  Nair, Cote, and
%               Staniforth (1999).
%   :)          A function to make you happy
%
%   Gallery functions are subject to change in future releases of Chebfun.
%
% See also CHEB.GALLERY, CHEB.GALLERYTRIG, CHEB.GALLERY2, CHEB.GALLERY3, CHEB.GALLERYDISK.

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

% If the user did not supply an input, return a function chosen at random
% from the gallery.
if ( nargin == 0 )
    names = {'football','jet','soccerball','deathstar', 'vortices'...
        'gaussian','reprodkern','geomag','peaks','neamtu','randn',...
        'moire','stripes',':)'};
    name = names{randi(length(names))};
end

type=1;                  % Default plotting type
addEarthPlot = 0;        % Flag on whether or not to plot the earth (1=yes)
viewAngle = [-37.5 30];  % Default viewing angle
clrmap = parula(64);     % Default colormap
clraxis = [];            % Color axis for the colormap; empty means use default

% The main switch statement.
switch lower(name)
    % The classic football (or soccerball) pattern
    case {'football','soccerball'}
        f = spherefun.sphharm(6,0) + sqrt(14/11)*spherefun.sphharm(6,5);
        fa = f;
        cntrlvl = -[0.25 0.25];
        type = 3;

    % A function resembling the surface of the Death Star
    case 'deathstar'
        fa = @(x,y,z) -(exp(-30*((y+sqrt(3)/2).^2 + x.^2 + (z-1/2).^2)) + exp(-100*z.^2));
        f = spherefun(fa);
        type = 1;
        clrmap = flipud(jet);
        viewAngle = [-35 8];

    % Two diametrically oposed vortices taken from
    % R. D. Nair, J. Cote, A. Staniforth, Cascade interpolation for
    % semi-Lagrangian advection over the sphere, Quart. J. Roy. Meteor.
    % Soc. 125 (1999) 1445-1468
    case 'vortices'
        rho = @(th) 3*(sin(th)); w = @(th) (3*sqrt(2)/2*sech(rho(th)).^2.*tanh(rho(th)))./(rho(th)+eps);
        fa = @(lam,th) -tanh(rho(th)/5.*sin(lam-20*w(th)));
        f = spherefun(fa);
        cntrlvl = [0 0];
        type = 3;
        clrmap = jet;

    % A simple Gaussian
    case 'gaussian'
        % Coordinates of Braunschweig, where Gauss was born;
        coords = [10.516667 52.266667]/180*pi;
        [xc,yc,zc] = sph2cart(coords(1),coords(2),1);
        fa = @(x,y,z,xc,yc,zc) exp(-20*((x-xc).^2 + (y-yc).^2 + (z-zc).^2));
        f = spherefun(@(x,y,z) fa(x,y,z,xc,yc,zc));
        addEarthPlot = 1;
        viewAngle = [80 15];

    % The reproducing kernel for all spherical harmonics of degree 20
    case 'reprodkern'
        [lam0,th0] = cart2sph(-1/sqrt(3),-1/sqrt(3),1/sqrt(3));
        f = spherefun(@(lam,th) sphRPK(lam,th,lam0,pi/2-th0,20));
        fa = f;
        viewAngle = [-50 5];

    % The IGRF-12 geomagnetic field
    case 'geomag'
        % Spherical harmonic coefficients for the IGRF-12 2015 Geomagnetic
        % Model; see http://www.ngdc.noaa.gov/IAGA/vmod/igrf.html
        c = [-29442.0, -1501.0, 4797.1, -2445.1, 3012.9, -2845.6, 1676.7,...
            -641.9, 1350.7, -2352.3, -115.3, 1225.6, 244.9, 582.0, -538.4,...
            907.6, 813.7, 283.3, 120.4, -188.7, -334.9, 180.9, 70.4, -329.5,...
            -232.6, 360.1, 47.3, 192.4, 197.0, -140.9, -119.3, -157.5, 16.0,...
            4.1, 100.2, 70.0, 67.7, -20.8, 72.7, 33.2, -129.9, 58.9, -28.9,...
            -66.7, 13.2, 7.3, -70.9, 62.6, 81.6, -76.1, -54.1, -6.8, -19.5,...
            51.8, 5.7, 15.0, 24.4, 9.4, 3.4, -2.8, -27.4, 6.8, -2.2, 24.2, 8.8,...
            10.1, -16.9, -18.3, -3.2, 13.3, -20.6, -14.6, 13.4, 16.2, 11.7,...
            5.7, -15.9, -9.1, -2.0, 2.1, 5.4, 8.8, -21.6, 3.1, 10.8, -3.3, 11.8,...
            0.7, -6.8, -13.3, -6.9, -0.1, 7.8, 8.7, 1.0, -9.1, -4.0, -10.5, 8.4,...
            -1.9, -6.3, 3.2, 0.1, -0.4, 0.5, 4.6, -0.5, 4.4, 1.8, -7.9, -0.7,...
            -0.6, 2.1, -4.2, 2.4, -2.8, -1.8, -1.2, -3.6, -8.7, 3.1, -1.5, -0.1,...
            -2.3, 2.0, 2.0, -0.7, -0.8, -1.1, 0.6, 0.8, -0.7, -0.2, 0.2, -2.2,...
            1.7, -1.4, -0.2, -2.5, 0.4, -2.0, 3.5, -2.4, -1.9, -0.2, -1.1, 0.4,...
            0.4, 1.2, 1.9, -0.8, -2.2, 0.9, 0.3, 0.1, 0.7, 0.5, -0.1, -0.3, 0.3,...
            -0.4, 0.2, 0.2, -0.9, -0.9, -0.1, 0.0, 0.7, 0.0, -0.9, -0.9, 0.4,...
            0.4, 0.5, 1.6, -0.5, -0.5, 1.0, -1.2, -0.2, -0.1, 0.8, 0.4, -0.1,...
            -0.1, 0.3, 0.4, 0.1, 0.5, 0.5, -0.3, -0.4, -0.4, -0.3, -0.8];
        % Compute the vertical component of the magnetic field
        f = 0*spherefun.sphharm(0,0);
        k = 1;
        for l=1:13
            f = f + -((l+1))*sqrt(4*pi/(2*l+1))*c(k)*spherefun.sphharm(l,0);
            k = k + 1;
            for m=1:l
                f = f - (-1)^m*((l+1))*sqrt(4*pi/(2*l+1))*...
                        (c(k)*spherefun.sphharm(l,m) + ...
                        c(k+1)*spherefun.sphharm(l,-m));
                k = k + 2;
            end
        end
        fa = f;
        type = 3;
        addEarthPlot = 1;
        cntrlvl = -60000:5000:60000;

    % A "peaks-like" function for the sphere
    case 'peaks'
        fa = @(x,y,z) 8*(1-x).^2.*exp(-4*(x - 0.059).^2 - 2*(y + 0.337).^2 - 2*(z + 0.940).^2) - ...
            30*(z/10 - x.^3 - y.^5) .* exp(-3*(x - 0.250).^2 - 2*(y - 0.433).^2 - 3*(z - 0.866).^2) + ...
            (20*y - 8*z.^3) .* exp(-2*(x + 0.696).^2 - 3*(y + 0.123).^2 - 2*(z - 0.707).^2) + ...
            (7*y - 10*x + 10*z.^3) .* exp(-3*(x - 0.296).^2 - 3*(y + 0.814).^2 - 3*(z + 0.5).^2);
        f = spherefun(fa);

    % A function developed by Mike Neamtu of Vanderbilt University to test
    % approximation schemes on the sphere.
    case 'neamtu'
        fa = @(x,y,z) 1 + x.^8 + exp(2*y.^3) + exp(2*z.^2) + 10*x.*y.*z;
        f = spherefun(fa);

    % A Guassian random function on the sphere suggested by Dmitry Belyaev
    % at Oxford.
    case 'randn'
        % Compute a random spherefun
        f = randnfunsphere(0.079,'monochromatic');
        fa = f;
        type = 4;
        clrmap = gray(2);
        % Set the color axis so that the transition from white to black is
        % exactly at the value zero.
        minmax = minandmax2est(f);
        clraxis = [-1 1]*max(abs(minmax));

    % An interesting Moire pattern generated by two sources on the sphere
    case 'moire'
        % Centers of the beacons
        boise = [-116.237651 43.613739]*pi/180;
        oxford = [-1.257778 51.751944]*pi/180;
        % ithaca = [-76.5 42.443333]*pi/180;
        % stellenbosh = [18.86 -33.92]*pi/180;
        [xb,yb,zb] = sph2cart(boise(1),boise(2),1);
        [xo,yo,zo] = sph2cart(oxford(1),oxford(2),1);
        % Pick the number of oscillations and make each of the "waves"
        % vanish at the anti-podal points from their centers.
        omega = besselroots(0,30); omega = omega(end)/2;
        % Use a combination of the J0 bessel functions centered at Boise
        % and Oxford to generate the Moire pattern.
        fa = @(x,y,z,omega) 2 + besselj(0,omega*sqrt((x-xb).^2+(y-yb).^2+(z-zb).^2)) + ...
            2 + besselj(0,omega*sqrt((x-xo).^2+(y-yo).^2+(z-zo).^2));
        f = spherefun(@(x,y,z) fa(x,y,z,omega));
        type = 1;
        addEarthPlot = 1;
        viewAngle = [32 8];

    % A zipper-like stripe pattern for the sphere
    case 'stripes'
        fa = @(x,y,z) (1 + cos(10*pi*x)).*exp(-exp(-20*z)) + (1 - cos(10*pi*x)).*exp(-exp(20*z));
        f = spherefun(fa);
        type = 3;
        cntrlvl = [1 1];
        viewAngle = [-36 8];
        clrmap = hot;

    % A zonal jet stream in the mid-latitudes of the northern hemisphere.
    case 'jet'
        fa = spherefun(@(lam,th) sin(th-(pi/4+0.01*cos(12*lam))).*exp(-300*(1-cos(th-(pi/4+0.01*cos(12*lam))))));
        f = spherefun(fa);
        type = 1;
        clrmap = jet;
        viewAngle = [32 8];
        addEarthPlot = 1;

    % A smiley face emoticon
    case ':)'
        fa = @(x,y,z) exp(-20*(4*(x+cos(pi/6)*sin(pi/4)).^2 + 4*(y+sin(pi/6)*sin(pi/4)).^2 + (z-cos(pi/4)).^2)) + ...
                      exp(-20*(4*(x+cos(-pi/6)*sin(pi/4)).^2 + 4*(y+sin(-pi/6)*sin(pi/4)).^2 + (z-cos(pi/4)).^2)) + ...
                   9*exp(-100*((x + 1).^2 - x.*z + (z - 1/2).^2 - 1/10).^2);
        f = spherefun(fa);
        viewAngle = [-75 20];

    otherwise
        error('CHEB:GALLERYSPHERE:unknown:unknownFunction', ...
            'Unknown function.')
end

% Only return something if there is an output argument.
if ( nargout > 0 )
    varargout = {f, fa};
    return;
end

ptitle = [name ', rank = ' num2str(length(f))];
% Determine the type of plot to make
if type==1
    % Plot the function with a grid
    surf(f,'grid','k-')
    axis off, title(ptitle)
elseif type==2
    contour(f)
    axis off, title(ptitle)
elseif type==3
    plot(f), hold on
    contour(f,cntrlvl,'k-'), hold off
    axis off, title(ptitle)
else
    plot(f)
    axis off, title(ptitle)
end

view(viewAngle);
colormap(clrmap);

% Set the color axis if required.
if ( ~isempty(clraxis) )
    caxis(clraxis);
end

if ( addEarthPlot )
    hold on, spherefun.plotEarth('w-'), hold off
end

end

function f = sphRPK(lam,th,lam0,th0,deg)
%SPHRPK  Reproducing kernel for the spherical harmonics of a given degree.
%   F = SPHRPK(LAM,TH,LAM0,TH0,DEG) is the reproducing kernel centered at
%   (LAM0,TH0) for the space of spherical harmonics of degree DEG.

t = cos(lam).*sin(th).*cos(lam0).*sin(th0) + sin(lam).*sin(th).*sin(lam0).*sin(th0) + cos(th).*cos(th0);
pl = legpoly(0:deg,[-1,1]);
[m,n] = size(t);
t = t(:);
c = ones(length(t),1)*((2*(0:deg)+1)/4/pi);
f = reshape(sum(c.*pl(t),2),m,n);

end


function res = difonzo(Y,x,z,ta,sc,type,f)
% PURPOSE: Multivariate temporal disaggregation with transversal constraint
% ----------------------------------------------------------------------------------
% SYNTAX: res = difonzo(Y,x,z,ta,sc,type,f);
% ----------------------------------------------------------------------------------
% OUTPUT: res: a structure
%         res.meth  = 'Multivariate Di Fonzo';
%         res.N     = Number of low frequency data
%         res.n     = Number of high frequency data
%         res.pred  = Number of extrapolations
%         res.ta    = Type of disaggregation
%         res.sc     = Frequency conversion
%         res.type  = Model for high frequency innovations
%         res.beta  = Model parameters
%         res.y     = High frequency estimate
%         res.d_y   = High frequency estimate: std. deviation
%         res.z     = High frequency constraint
%         res.et    = Elapsed time
% ----------------------------------------------------------------------------------
% INPUT: Y: NxM  ---> M series of low frequency data with N observations
%        x: nxm  ---> m series of high frequency data with n observations, m>=M see (*)
%        z: nzx1 ---> high frequency transversal constraint with nz obs.
%        ta: type of disaggregation
%            ta=1 ---> sum (flow)
%            ta=2 ---> average (index)
%            ta=3 ---> last element (stock) ---> interpolation
%            ta=4 ---> first element (stock) ---> interpolation
%        sc: number of high frequency data points for each low frequency data points
%            sc= 4 ---> annual to quarterly
%            sc=12 ---> annual to monthly
%            sc= 3 ---> quarterly to monthly
%        type: model for the high frequency innvations
%            type=0 ---> multivariate white noise
%            type=1 ---> multivariate random walk
% (*) Optional:
%        f: 1xM ---> Set the number of high frequency indicators linked to
%                    each low frequency variable. If f is explicitly included,
%                    the high frequency indicators should be placed in
%                    consecutive columns
% ----------------------------------------------------------------------------------
% NOTE: Extrapolation is automatically performed when n>sN.
%       If n=nz>sN restricted extrapolation is applied.
%       Finally, if n>nz>sN extrapolation is perfomed in constrained
%       form in the first nz-sN observatons and in free form in
%       the last n-nz observations.
% ----------------------------------------------------------------------------------
% LIBRARY: aggreg, dif, vec, desvec
% ----------------------------------------------------------------------------------
% SEE ALSO: denton, rossi, mtd_print, mtd_plot
% ----------------------------------------------------------------------------------
% REFERENCE: Di Fonzo, T.(1990)"The estimation of M disaggregate time
% series when contemporaneous and temporal aggregates are known", Review
% of Economics and Statistics, vol. 72, n. 1, p. 178-182.

% written by:
%  Enrique M. Quilis
%  Macroeconomic Research Department
%  Ministry of Economy and Competitiveness
%  <enrique.quilis@mineco.es>

% Version 3.0 [August 2006]

% ----------------------------------------------------------------------------------

t0 = clock;

%--------------------------------------------------------
%       Preliminary checking

[N,M] = size(Y);
[n,m] = size(x);
[nz,mz] = size(z);

if ((M > m) | (n < sc*N) | (mz ~= 1) | (nz > n) | (nz < sc*N))
   error (' *** INCORRECT DIMENSIONS *** ');
else
   % Number of extrapolations
   h1 = n - nz;
   h2 = n - sc*N;
   clear nz mz;
end

%--------------------------------------------------------
%       Checking of "ta"

if (ta < 1) | (ta > 4)
    error (' *** INCORRECT TA OPTION *** ');
end

%--------------------------------------------------------
%       Checking of "sc"

if (sc ~= 3) & (sc ~= 4) & (sc ~= 12)
    error (' *** INCORRECT FREQUENCY CONVERSION (sc) *** ');
end

%--------------------------------------------------------
%       Checking of "type"

if (type < 0) | (type > 1)
    error (' *** INCORRECT TYPE OPTION *** ');
end

%--------------------------------------------------------
%       Checking (and definition) of vector f

if (nargin == 6)
    f = ones(1,M);
end

if ( (f < 1) | (sum(f) ~= m) | (length(f) ~= M))
   error (' *** IMPROPER ASSIGNMENT OF INDICATORS IN f VECTOR *** ');
end

%--------------------------------------------------------
%  **** CONSTRAINT MATRICES ***
%--------------------------------------------------------
% Required:
%              H1 ---> transversal
%              H2 ---> longitudinal
%
%---------------------------------------------------------------
%       Generate H1: (n-h1) x nM

H1 = kron(ones(1,M),[eye(n-h1) zeros(n-h1,h1)]);

%---------------------------------------------------------------
%       Generate H2: NM x nM.
%
% Generation of aggregation matrix C

C = aggreg(ta,N,sc);
C = [C zeros(N,h2)];

H2 = kron(eye(M),C);

%---------------------------------------------------------------
%       Generate H: (n-h1+NM) x nM.
%
%       H = [ H1
%             H2 ]

H = [H1
   H2];

%--------------------------------------------------------
%  **** PREPARING DATA MATRICES ***
%--------------------------------------------------------
% Required:
%               x_diag
%               Y_big,  Y_e
%               X_diag, X_e

%--------------------------------------------------------
%       Generate x_diag: nM x M+m
%
% It is a diagonal matrix formed by the high frequency
% indicators, including a vector of ones for the intercept
%
%       x_diag = [ x1 0  0  ... 0
%                  0  x2 0  ... 0
%                  0  0  x3 ... 0
%                  ..............
%                  0  0  0  ... xM ]
%
% It is made by means of a recursion.

ac(1)=f(1);                             % Initialization of the recursion
x_diag = [ones(n,1) x(:,1:ac(1))];

j=2;
while (j <= M)
   xaux = [ones(n,1) x(:,ac(j-1)+1:ac(j-1)+f(j))];
   [a2,b2] = size(xaux);
   [a1,b1] = size(x_diag);
   x_diag = [ x_diag         zeros(a1,b2)
              zeros(a2,b1)   xaux ];
   ac(j) = ac(j-1) + f(j);
   j = j + 1;
end
clear xaux;

%--------------------------------------------------------
%       Generate X_diag: NM x M+m
%
% Low frequency analog of x_diag. It is the result of
% applying the temporal aggregator H2 to x_diag.

X_diag = H2 * x_diag;

%--------------------------------------------------------
%       Generate X_e: (n-h1+NM) x M+m
%
%
% It is the result of applying the complete aggregator H
% (temporal as well as transversal).
% Lower part of X_e is X_diag.

X_e = H * x_diag;

%--------------------------------------------------------
%       Generate Y_big: NM x 1
%
% It is column vector containing all the observations on the
% low frequency series according to: Y_big = [Y1 Y2 ... YM]'
% Formally: Y_big = vec(Y)

Y_big = vec(Y);

%--------------------------------------------------------
%       Generate Y_e: (n-h1+NM) x 1
%
% It is column vector containing the transversal constraint
% and all the observations on the low frequency series
% according to: Y_e = [ z Y1 Y2 ... YM]' = [z Y_big]'

Y_e = [ z
      Y_big];

%--------------------------------------------------------
%  **** PRELIMINARY ESTIMATION OF SIGMA ***
%--------------------------------------------------------
% The method of di Fonzo requires the previous estimation of VCV
% matrix SIGMA for the (implied) low frequency model. This
% preliminary estimation is performed by means of estimating, equation
% by equation, the model. Formally, this is equivalent to estimate an
% unrelated SURE model. Computationally, this is also the applied procedure.

BETA = (X_diag' * X_diag) \ (X_diag' * Y_big); % OLS estimator
U_big = Y_big - X_diag * BETA;                 % Residuals in vec format

% Residuals (columnwise) U: NxM

U = desvec(U_big,M);

% Preliminary estimation of SIGMA

SIGMA = cov(U,1);

%--------------------------------------------------------
%  **** APPLYING DI FONZO PROCEDURE ***
%--------------------------------------------------------

%--------------------------------------------------------
%       High frequency VCV matrix v: nM x nM

switch type
case 0 % White noise
   v = kron(SIGMA,eye(n));
case 1 % Random walk, with U(0)=0
   D = dif(1,n);
   DDi = inv(D'*D);
   v = kron(SIGMA,DDi);
end;

%--------------------------------------------------------
%       Low frequency VCV matrix V: (n-h1+NM) x (n-h1+NM)
%       and its generalized inverse

V = H * v * H';
Vi = pinv(V);      % Moore-Penrose generalized inverse

%--------------------------------------------------------
%       Generation of distribution filter L: nM x (n-h1+NM)

L = v * H' * Vi;

%--------------------------------------------------------
%	     GLS estimation of beta in a SURE context

beta = (X_e' * Vi * X_e) \ (X_e' * Vi * Y_e);

U_e = Y_e - X_e * beta;

%--------------------------------------------------------
%       Estimation of high frequency series

y_big = x_diag * beta + L * U_e;

% Series y columnwise y: nxM

y = desvec(y_big,M);

%--------------------------------------------------------
%       VCV matrix of estimations y: nM x nM

sigma_y = (eye(n*M) - L*H)*v + ...
   (x_diag - L*X_e)*inv(X_e'*Vi*X_e)*(x_diag - L*X_e)';

% Vector format of std. dev.

d_y_big = sqrt(diag(sigma_y));

% Std. dev. series in column format dt_y: n x M

d_y = desvec(d_y_big,M);

% -----------------------------------------------------------------------
% Loading the structure
% -----------------------------------------------------------------------
% Basic parameters

res.meth = 'Multivariate Di Fonzo';
res.N = N;
res.n = n;
res.pred = h2;
res.ta= ta;
res.sc = sc;
res.type = type;

% -----------------------------------------------------------------------
% Parameters

res.beta=beta;

% -----------------------------------------------------------------------
% Series

res.y   = y;
res.d_y = d_y;
res.z   = z;

% -----------------------------------------------------------------------
% Elapsed time

res.et        = etime(clock,t0);


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = psabr_4_2(a, b, r, n, f, k, t,m, mu, nu, l, u)
% sabr prices using an admissible region kl <= k <= ku where sabr is used
% for 0 <= k <= kl    we use a put pricing function
%           f(x) = x^mu exp(a + bx + cx^2)
% for ku < k < +infty we use a call pricing function
%           f(x) = x^(-nu) exp(a + bx^(-1) + cx(-2)
% a big range (small kl and big ku) guarantee that the prices are in line
% with the observed calls and puts
%
% this is the preferred version of the density for computations
%

eps = 1e-004;                              % .1 bp

s = @(x) psabr(a, b, r, n, f, x, t);
index = find(s(k)>0,1,'first');

if (isempty(index) || index ==1)
   kl = l *f;
else
    kl = max(l * f,k(index)+(f-k(index))/m);% lower strike level
end
s1 = s(kl-eps);                          % for calc of derivatives
s2 = s(kl);                              % for calc of derivatives
s3 = s(kl+eps);                          % for calc of derivatives

V1 = log(s2);                            % log density

U2 = (s3-s1)/(2*eps);                    % derivative of density
V2 = U2/s2;                              % derivative of log density

U3 = (s3-2*s2+s1)/eps^2;                 % second derivative of density
V3 = U3/s2 - V2^2;                       % second derivative of log density

% fix mu and solve
%       V1 = mu log kl + a + b kl + c kl^2
%       V2 = mu / kl + b + 2c kl
%       V3 = - mu / kl^2 + 2c

%mu = 1.5;                               % controls the left tails
cl = .5*(V3+mu/kl^2);
bl = V2-mu/kl - 2*cl*kl;
al = V1 - mu * log(kl) - bl*kl - cl*kl^2;

% upper strike level (the bigger the strike level the better fit the call
% prices to Hagan formula!)
ku = u * f;
s = @(x) psabr(a, b, r, n, f, x, t);        % sabr price (1) for calls
s1 = s(ku-eps);                             % for calc of derivatives
s2 = s(ku);                                 % for calc of derivatives
s3 = s(ku+eps);                             % for calc of derivatives

V1 = log(s2);                               % log call price

U2 = (s3-s1)/(2*eps);                       % derivative of  density
V2 = U2/s2;                                 % derivative of log density

U3 = (s3-2*s2+s1)/eps^2;                    % second derivative of density
V3 = U3/s2 - (U2/s2)^2;                     % second derivative of density

% fix nu and solve
%       V1 = -nu log ku + a + b/ku + c/ku^2
%       V2 = -nu / ku - b / ku^2 - 2c/ku^3
%       V3 = nu / ku^2 + 2 b / ku^3 - 6 c / ku^4

cu = (-1.5*nu / ku + .5*V3 * ku - V2)*ku^3/5;
bu = -ku^2*(V2 + nu/ku +2*cu/ku^3);
au = V1 + nu * log(ku) - bu / ku - cu / ku^2;

    yl = real(k(k<kl).^mu .* exp(al + bl .* k(k<kl) + cl * k(k<kl).^2));
    ym = real(psabr(a,b,r,n,f,k((kl<=k)&(k<=ku)),t));
    yu = real(k(k>ku).^(-nu) .* exp(au+bu./k(k>ku)+cu./k(k>ku).^2));

y = [yl ym yu];                            % output
end


function [mg] = slug2mg(slug)
% Convert units of mass from slugs to milligrams.
% Chad A. Greene 2012
mg = slug*14.5939*1000000;


% Get vertex coordinates for the patch type 'P'
% [C, N, S, W, E] = HealpixGetPatchVertexCoordsP(n, i, j, INFO)
%
% Parameters
% n : grid resolution
% i : ring index
% j : intra-ring index
% INFO : intermediate information (output of HealpixSelectPatchClass())
% C : intra-patch coordinates
% N : coordinates for north vertex
% S : coordinates for south vertex
% W : coordinates for west vertex
% E : coordinates for east vertex

function [C, N, S, W, E] = HealpixGetPatchVertexCoordsP(n, i, j, INFO)

% gradient of the border
grad = INFO.polar_part;

decimal_i = INFO.decimal_i_n;
int_i = INFO.int_i_n;

int_j = fix(j - grad * decimal_i);
decimal_j = j - int_j;
if int_j == 0
    int_j = 4 * int_i;
end

if decimal_j > (grad + 1) * decimal_i
    north_i = int_i - 1;
    south_i = int_i + 1;
    north_int_j = mod(int_j - grad - 1, 4 * north_i) + 1;
    south_int_j = mod(int_j + (grad + 1) - 1, 4 * south_i) + 1;
    east_int_j = mod(int_j + 1 - 1, 4 * int_i) + 1;

    N = [north_i, north_int_j];
    S = [south_i, south_int_j];
    W = [int_i  , int_j];
    E = [int_i  , east_int_j];

    offset_j = (grad + 0.5) * decimal_i;
    C = [decimal_i, decimal_j - offset_j - 0.5];
else
    curnt_i = int_i + 1;
    south_i = int_i + 2;
    if south_i > n
        south_i = n;
    end
    west_int_j = mod(int_j + grad - 1, 4 * curnt_i) + 1;
    east_int_j = mod(int_j + grad + 1 - 1, 4 * curnt_i) + 1;
    south_int_j = mod(west_int_j + (grad + 1) - 1, 4 * south_i) + 1;

    N = [int_i    , int_j];
    S = [int_i + 2, south_int_j];
    W = [int_i + 1, west_int_j];
    E = [int_i + 1, east_int_j];

    offset_j = (grad + 0.5) * (1 - decimal_i);
    C = [decimal_i - 1, decimal_j + offset_j - (grad + 0.5)];
end

if INFO.is_south_pole
    TMP = N;
    N = S;
    S = TMP;
    N(1) = 4 * n - N(1);
    S(1) = 4 * n - S(1);
    W(1) = 4 * n - W(1);
    E(1) = 4 * n - E(1);
    C(1) = -C(1);
end


function [w,run] = train_mis(x,w,lambda)
% Modified iterative scaling
% x is premultiplied by y

% Written by Thomas P Minka

if nargin < 3
  lambda = 0;
end
if lambda > 0
  error('must have lambda = 0')
end
[d,n] = size(x);
flops(0);
step = 1/max(sum(abs(x),1));

i1 = (x > 0);
x1 = abs(x).*i1;
x2 = abs(x).*(1-i1);
flops(flops + 3*d*n);
if nargout > 1
  run.w = [];
  run.flops = [];
  run.e = [];
end
for iter = 1:10000
  old_w = w;
  % s1 = 1-sigma
  s1 = 1./(1+exp(w'*x));
  delta = (x1*s1')./(x2*s1');
  w = w + step*0.5*log(delta);
  if iter == 1
    % same for every iteration
    % use spmul because x1,x2 have structural zeros
    fl = flops_mul(w',x)+n*(flops_exp+2) + ...
	flops_spmul(x1,s1')+flops_spmul(x2,s1')+d + ...
	d*(flops_exp+2);
  end
  flops(flops + fl);

  if nargout > 1 & rem(iter,100) == 1
    run.w(:,end+1) = w;
    run.flops(end+1) = flops;
    run.e(end+1) = logProb(x,w) -0.5*lambda*w'*w;
  end
  if rem(iter,1000) == 0
    fprintf('MIS iter %d\n', iter)
  end

  if max(abs(w - old_w)) < 1e-6
    break
  end
end
if iter == 10000
  warning('not enough iters')
end
if nargout > 1
  figure(2)
  plot(run.e)
end


function error=matlab2tsai_error(X,camera,uv_d,xy_matlab_N)

% JMM Montiel. Sept 2005
%
% Error between the Tsia undistortion model and the
%   Swaminathan model
% Input
%   k1      -Swaminathan distortion model
%   camera  -Camera parameters. camera.k1 is irrelevant
%   uv_d    -Distorted point coordinates
%   uv_u    -Undistorted coordinates according to Tsai model

k1=X(1);
k2=X(2);
f=X(3);
cx = X(4);
cy = X(5);

camera.f = f;
camera.Cx = cx;
camera.Cy = cy;
camera.k1 = k1;
camera.k2 = k2;

xy_tsai_N = undistort_and_normalize_fm( uv_d, camera );
if(1)
    cla;
    plot(xy_matlab_N(1,:),xy_matlab_N(2,:),'+r');
    hold on
    plot(xy_tsai_N(1,:),xy_tsai_N(2,:),'+g');
    xlabel('red, matlab.    green Tsai with 1 radial distorion parameter')
end
error=[xy_tsai_N(1,:)-xy_matlab_N(1,:);xy_tsai_N(2,:)-xy_matlab_N(2,:)];


function [Y,W,SetupStruc] = Process_ICA_Sawada(s,Transfer,SetupStruc)
K = SetupStruc.ICA_Sawada.K;
hop = SetupStruc.ICA_Sawada.hop;
win = hanning(K,'periodic');
win = win/sqrt(sum(win(1:hop:K).^2));
SetupStruc.ICA_Sawada.win = win;  % Preserve 'win' in 'SetupStruc'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = size(s,2);
for i = 1:N
    X(:,:,i) = fft(enframe(s(:,i),win,hop)');
end
frame_N = size(X,2);
K_m = K/2+1;
Num = size(Transfer,3);
Y = zeros((frame_N-1)*hop+K,Num);
Y_f = zeros(size(X,1),size(X,2),Num);
Y_P = zeros(frame_N,Num,K_m);
%%%%%%%%%%%%%%%%%%%%%%%%%% Obtain processing matrix 'W'
theta = 10^-4;
W = zeros(Num,N,K_m);
A = zeros(1001,K/2)-1; %%%% Show the decrease of non-linear correlation, ICA max iterations 1000
for i = 2:K_m
    X_f = permute(X(i,:,:),[3 2 1]);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%% PCA and ICA processing
    [E,D] = PCA(X_f,1,Num);
    V = sqrt(D)\E';
    X_f = V*X_f;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     Steer = permute(Transfer(i,:,:),[2 3 1]);
%     Ori = V*Steer;
%     if rcond(Ori)<theta
%         Ori = Ori+eye(Num)*min(diag(Ori));
%     end
%     [Y_,W_ICA,A] = FDICA(X_f,inv(Ori),A,i);  %%% 'A', 'i' record the decrease for observation
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    [Y_,W_ICA,A] = FDICA(X_f,eye(Num),A,i);  %%% 'A', 'i' record the decrease for observation
    W(:,:,i) = W_ICA*V;
    Y_P(:,:,i) = Y_.';
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Process the ambiguity of permutation and amplitude
P = Permu_Sawada(W,Y_P,SetupStruc,'all');  %%%% Options: 'DOA','cor', 'all'
for i = 2:K_m
    W(:,:,i) = P(:,:,i)*W(:,:,i);
    Y_ = permute(Y_P(:,:,i),[2 1 3]);
    Y_ = P(:,:,i)*Y_;
    Y_f(i,:,:) = Y_.';
    if(i~=K_m)
        Y_f(K+2-i,:,:) = Y_';
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Recover signals
if(K/hop==2)
    win = ones(K,1);
end
for i = 1:Num
    Y(:,i) = overlapadd(real(ifft(Y_f(:,:,i)))',win,hop);
end
return;


function recall = Recall(SEG, GT)
    % SEG, GT are the binary segmentation and ground truth areas, respectively.
    % recall
    recall = double(sum(uint8(SEG(:) & GT(:)))) / double(sum(uint8(GT(:))));
end


function r8vec_index_delete_dupes_test ( )

%*****************************************************************************80
%
%% R8VEC_INDEX_DELETE_DUPES_TEST tests R8VEC_INDEX_DELETE_DUPES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  n_max = 25;
  n = 0;
  x = [];
  indx = [];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8VEC_INDEX_DELETE_DUPES_TEST\n' );
  fprintf ( 1, '  R8VEC_INDEX_DELETE_DUPES deletes duplicates.\n' );
  fprintf ( 1, '\n' );

  xval = 8.0;
  [ n, x, indx ] = r8vec_index_insert ( n, x, indx, xval );

  xval = 7.0;
  [ n, x, indx ] = r8vec_index_insert ( n, x, indx, xval );

  seed = 123456789;

  for i = 1 : 20
    [ xval, seed ] = r8_uniform_ab ( 0.0, 20.0, seed );
    xval = round ( xval );
    fprintf ( 1, '  %f\n', xval );
    [ n, x, indx ] = r8vec_index_insert ( n, x, indx, xval );
  end

  xval = 7.0;
  [ n, x, indx ] = r8vec_index_insert ( n, x, indx, xval );

  xval = 8.0;
  [ n, x, indx ] = r8vec_index_insert ( n, x, indx, xval );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Indexed list of entries:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  I  INDX(I)  X(I)  X(INDX(I))\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %3d  %3d  %9f  %9f\n', i, indx(i), x(i), x(indx(i)) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Call R8VEC_INDEX_DELETE_DUPES to delete duplicates:\n' );

  [ n, x, indx ] = r8vec_index_delete_dupes ( n, x, indx );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Indexed list of unique entries:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  I  INDX(I)  X(I)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %3d  %3d  %9f\n', i, indx(i), x(i) );
  end

  return
end


function plot_spreads(treedata,fig,lw,rel)

% Plots the spreads as a polar plot with different height layers presented
% with different colors. Inputs "fig" and "lw" define the figure number and
% the line width. Input Rel = 1 specifies relative spreads, i.e. the
% maximum spread is one, otherwise use the actual values.

if nargin == 2
    lw = 1;
    rel = 1;
elseif nargin == 3
    rel = 1;
end

spreads = treedata.spreads;
figure(fig)
n = size(spreads,1);
col = zeros(n,3);
col(:,1) = (0:1/n:(n-1)/n)';
col(:,3) = (1:-1/n:1/n)';
d = max(max(spreads));
D = [spreads(1,end) spreads(1,:)];
if rel
    polarplot(D/d,'-','Color',col(1,:),'Linewidth',lw)
else
    polarplot(D,'-','Color',col(1,:),'Linewidth',lw)
end
hold on
for i = 1:n
    D = [spreads(i,end) spreads(i,:)];
    if rel
        polarplot(D/d,'-','Color',col(i,:),'Linewidth',lw)
    else
        polarplot(D,'-','Color',col(i,:),'Linewidth',lw)
    end
end
hold off
if rel
    rlim([0 1])
else
    rlim([0 d])
end


clear all; close all; clc

%% folder setup
isSaveFig = 1;
isPlotFig = 1;

outputDir = GetOutputDataDir;

ClusterIDs = [2,1]; % init; can overrride
prct_const = 2; % init; can overrride

%% init

hfig = figure;
InitializeAppData(hfig);
ResetDisplayParams(hfig);

setappdata(hfig,'isMotorseed',1);

%%
IM_scatter = cell(2,18); % L and R

IM_maps = cell(5,18);
% IM_Xonly = cell(n_reg,18);
% IM_Yonly = cell(n_reg,18);
% IM_XY = cell(n_reg,18);
% IM_X = cell(n_reg,18);
% IM_Y = cell(n_reg,18);

caseID = 1;
switch caseID
    case 1
        load(fullfile(outputDir,'4D_SM_betas.mat'));
        M_reg_name = {'2x2motormaps'};
        range_fish = GetFishRange;% init; can overrride
    case 2
        load(fullfile(outputDir,'4D_SM_stimrangePT_betas.mat'));
        M_reg_name = {'2x2motormaps_PT'};
        range_fish = 6:18;
    case 3
        load(fullfile(outputDir,'4D_SM_stimrangeOMR_betas.mat'));
        M_reg_name = {'2x2motormaps_OMR'};
        range_fish = 8:18;
    case 4
        load(fullfile(outputDir,'4D_SM_stimrangelooming_betas.mat'));
        M_reg_name = {'2x2motormaps_looming'};
        range_fish = [9:15,17:18];
    case 5
        load(fullfile(outputDir,'4D_SM_stimrangeDF_betas.mat'));
        M_reg_name = {'2x2motormaps_DF'};
        range_fish = [12:15,17:18];
end
%% run fish
for i_fish = range_fish

    cIX_all = LoadSingleFishDefault(i_fish,hfig,ClusterIDs,[],0);

    %% main loop for left/right motor
    M_pass = cell(5,2);

    for i_lr = 1:2
        %% scatter plot with 4D components
        % loaded betas for this fish for the combo data (including both stim)
        betas = Betas{i_lr,i_fish};
        % set up plot dimensions
        X = betas(:,1);%b3;%b1;
        Y = betas(:,2);
        %                 Y = sqrt(b2.^2+b3.^2);%b2;
        Xname = 'motor res.';
        Yname = 'motor avr.';

        numcell = length(X);

        A = X;
        topN = round(prct_const/100*numcell); % top x% cutoff
        [~,IX] = sort(A,'descend');
        thresA = A(IX(topN));

        B = Y;
        topN = round(prct_const/100*numcell);
        [~,IX] = sort(B,'descend');
        thresB = B(IX(topN));

        IX_passX = find(A>=thresA);
        IX_passY = find(B>=thresB);
        IX_passXonly = setdiff(IX_passX,IX_passY);
        IX_passYonly = setdiff(IX_passY,IX_passX);%find(B>=thresB);%
        IX_passXY = intersect(IX_passX,IX_passY);
        IX_pass = union(IX_passX,IX_passY);
        IX_fail = intersect(find(A<thresA),find(B<thresB));%find(A<thresA);

        % get min/max
        x0 = min(X(IX_pass));
        x1 = max(X(IX_pass));
        y0 = min(Y(IX_pass));
        y1 = max(Y(IX_pass));

        gIX_in = (1:length(X))';

        M_pass{1,i_lr} = IX_passX;
        M_pass{2,i_lr} = IX_passY;
        M_pass{3,i_lr} = IX_passXonly;
        M_pass{4,i_lr} = IX_passYonly;
        M_pass{5,i_lr} = IX_passXY;
        %         PassX_2{i_lr} = IX_passX;
        %         PassY_2{i_lr} = IX_passY;
        %         PassXonly_2{i_lr} = IX_passXonly;
        %         PassYonly_2{i_lr} = IX_passYonly;
        %         PassXY_2{i_lr} = IX_passXY;

        %% scatter plot
        clrX = [0.3,0.8,0];
        clrY = [0.9,0.2,0.9];
        clrXY = [0,0.3,0.9];
        clr_fail = [0.5,0.5,0.5];

        h = figure('Position',[500,100,300,250]); hold on
        scatter(X,Y,1,clr_fail,'filled')

        scatter(X(IX_passXonly),Y(IX_passXonly),1,clrX);%,'filled');
        scatter(X(IX_passYonly),Y(IX_passYonly),1,clrY);%,'filled');
        scatter(X(IX_passXY),Y(IX_passXY),1,clrXY);%[1,0.5,0.5]);%,'filled');

        plot([x0,x1],[thresB,thresB],'k--');
        plot([thresA,thresA],[y0,y1],'k--');

        xlabel(Xname);ylabel(Yname);
        axis equal
        %         set(gca,'XTick',0:0.5:1,'YTick',-0.5:0.5:1);
        axis tight
        %% save bubble plot
        IM_scatter{i_lr,i_fish} = print('-RGBImage');
        close(h)

    end

    %% component anat map
    setappdata(hfig,'clrmap_name','hsv_old');
    for i_map = 1:5
        cIX12 = M_pass(i_map,:);
        cIX = vertcat(cIX12{1},cIX12{2});
        gIX = vertcat(ones(size(cIX12{1})),2*ones(size(cIX12{2})));
        I = LoadCurrentFishForAnatPlot(hfig,cIX,gIX);
        [h,~,im] = DrawCellsOnAnat(I);
        close(h);
        IM_maps{i_map,i_fish} = im;

        %export code - added by Joe and Misha
        cIX_abs = I.absIX(I.cIX); %is referencing through absIX correct?
        M_xyz = I.CellXYZ(cIX_abs,:);
        % Determine size from anat sizes, apparently yxz
        siz_ = [size(I.anat_yx,1), size(I.anat_yx,2), size(I.anat_zx,1)];
        func_map=zeros(siz_(1),siz_(2),siz_(3));
        anat_map=zeros(siz_(1),siz_(2),siz_(3));
        for i=1:size(M_xyz,1),func_map(M_xyz(i,1),M_xyz(i,2),M_xyz(i,3))=1;end
        for i=1:size(I.CellXYZ,1),anat_map(I.CellXYZ(i,1),I.CellXYZ(i,2),I.CellXYZ(i,3))=1;end
        eval(['save export_fig3_fish' num2str(i_fish) '_case' num2str(caseID) '_mapnum' num2str(i_map) ' func_map anat_map'])
        %mapnums are above threshold for  X, Y, onlyX, onlyY, X and Y

    end

end

%% save as tiff stack
M_lr = {'-L','-R'};

M_comp_names = {'passX','passY','passXonly','passYonly','passXY'};
n_reg = 1;
for i_set = 1:n_reg
    range_im = range_fish;%M_fishrange_im{i_set};

    for i_lr = 1:2
        tiffdir = fullfile(outputDir,[M_reg_name{i_set},M_lr{i_lr},'_scatter_allfish.tiff']);
        IM = IM_scatter(i_lr,range_im);
        SaveImToTiffStack(IM,tiffdir);
    end

    for i_map = 1:5
        tiffdir = fullfile(outputDir,[M_reg_name{i_set},'_',M_comp_names{i_map},'_anat_allfish.tiff']);
        IM = IM_maps(i_map,range_im);
        SaveImToTiffStack(IM,tiffdir);
    end
end

%% Average Plot
% M_k_scale = {1,1.5,1};
% M_k_contrast = {1.2,1.5,1.2};

for i_set = 1:n_reg
    range_im = range_fish;%M_fishrange_im{i_set};%[1:3,5:7];%[1:3,5:18];

    for i_map = 1:5
        %%
        IM = IM_maps(i_map,range_im);%IM_2(i_lr,range_im);

        % adjust params for visualization
        k_scale = 0.5;%1/1.5;%M_k_scale{i_set};
        k_contrast = 1;%M_k_contrast{i_set};

        [h_anat,im_avr] = AverageAnatPlot(IM,k_contrast,k_scale);

        tiffdir = fullfile(outputDir,[M_reg_name{i_set},'_',M_comp_names{i_map},'_anat_avr.tiff']);
        imwrite(im_avr, tiffdir, 'compression','none','writemode','overwrite');
    end
end


classdef MOCMA < ALGORITHM
% <multi> <real/integer>
% Multi-objective covariance matrix adaptation evolution strategy

%------------------------------- Reference --------------------------------
% C. Igel, N. Hansen, and S. Roth, Covariance matrix adaptation for multi-
% objective optimization, Evolutionary computation, 2007, 15(1): 1-28.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Generate the initial individuals in CMA-ES
            Population = Problem.Initialization();
            ptarget    = 1/5.5;
            a          = struct('x',num2cell(Population.decs,2)','psucc',ptarget,'sigma',0.5,'pc',0,'C',eye(Problem.D),'Individual',num2cell(Population));

            %% Optimization
            while Algorithm.NotTerminated([a.Individual])
                % Generate new individuals
                for k = 1 : Problem.N
                    a1(k)            = a(k);
                    a1(k).x          = mvnrnd(a(k).x,a(k).sigma^2*a(k).C,1);
                    a1(k).Individual = Problem.Evaluation(a1(k).x);
                end

                % Update the fitness of each individual
                Q           = [a,a1];
                Population  = [Q.Individual];
                % Penalized fitness for handling box constraints
                PopObj      = Population.objs + repmat(1e-6*sum((cat(1,Q.x)-Population.decs).^2,2),1,Problem.M);
                % Calculate the fitness of each individual
                FrontNo     = NDSort(PopObj,inf);
                CrowdDis    = CrowdingDistance(PopObj,FrontNo);
                [~,rank]    = sortrows([FrontNo;-CrowdDis]');
                [~,fitness] = sort(rank);

                % Update the CMA models
                for k = 1 : Problem.N
                    a(k)  = updateStepSize(a(k),fitness(Problem.N+k)<fitness(k),ptarget);
                    a1(k) = updateStepSize(a1(k),fitness(Problem.N+k)<fitness(k),ptarget);
                    a1(k) = updateCovariance(a1(k),(a1(k).x-a(k).x)/a(k).sigma);
                end

                % Individuals for next generation
                Q = [a,a1];
                a = Q(rank(1:Problem.N));
            end
        end
    end
end


% Create sample waypoint data for trajectory generation
% NOTE: Modify this script with your own rigid body tree and
%       trajectory reference points
%       (We recommend saving a copy of this file)
%
% Copyright 2019 The MathWorks, Inc.

%% Common parameters
% Rigid Body Tree information
load gen3
load gen3positions
eeName = 'Gripper';
numJoints = numel(gen3.homeConfiguration);
ikInitGuess = gen3.homeConfiguration;

% Maximum number of waypoints (for Simulink)
maxWaypoints = 20;

% Positions (X Y Z)
waypoints = toolPositionHome' + ...
            [0 0 0.2 ; -0.1 0.2 0.4 ; -0.2 0 0.1 ; -0.1 -0.2 0.4 ; 0 0 0.2]';

% Euler Angles (Z Y X) relative to the home orientation
orientations = [0     0    0;
                pi/8  0    0;
                0    pi/2  0;
               -pi/8  0    0;
                0     0    0]';

% Array of waypoint times
waypointTimes = 0:4:16;

% Trajectory sample time
ts = 0.2;
trajTimes = 0:ts:waypointTimes(end);

%% Additional parameters

% Boundary conditions (for polynomial trajectories)
% Velocity (cubic and quintic)
waypointVels = 0.1 *[ 0  1  0;
                     -1  0  0;
                      0 -1  0;
                      1  0  0;
                      0  1  0]';

% Acceleration (quintic only)
waypointAccels = zeros(size(waypointVels));

% Acceleration times (trapezoidal only)
waypointAccelTimes = diff(waypointTimes)/4;


classdef SubPixel_Conv < dagnn.ElementWise
%%% Sub-pixel convolution layer %%%
%
% Performs sub-pixel convolution or pixel shuffle as specified in [1].
% From an input of size [H, W, C, N] stored in inputs{1}, this layer
% produces the output of size [scale*H, scale*W, C/(scale*scale), N] in outputs{1}.
% *Back-propagation (backward function) implemented*
%
% [1] Wenzhe Shi et al. "Real-Time Single Image and Video Super-Resolution
% Using an Efficient Sub-Pixel Convolutional Neural Network", CVPR, 2016.

  properties
    scale = 2;
  end

  methods
    function outputs = forward(obj, inputs, params)
        input = inputs{1};
        output=gpuArray(zeros(size(input, 1)*obj.scale, size(input, 2)*obj.scale, size(input, 3)/obj.scale/obj.scale, size(input, 4)));
        for channel = 1:size(input, 3)
            ch = floor((channel-1)/obj.scale/obj.scale)+1;
            c = mod(channel,obj.scale*obj.scale);
            if c == 0, c = obj.scale*obj.scale; end
            q = floor((c-1)/obj.scale)+1;
            r = mod(c, obj.scale);
            if r == 0, r = obj.scale; end
            output(q:obj.scale:end, r:obj.scale:end, ch, :) = input(:, :, channel, :);
        end
        outputs{1} = gpuArray(single(output));
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
        output = derOutputs{1};
        input = gpuArray(zeros(size(output, 1)/obj.scale, size(output, 2)/obj.scale, size(output, 3)*obj.scale*obj.scale, size(output, 4)));
        for channel = 1:size(input, 3)
            ch = floor((channel-1)/obj.scale/obj.scale)+1;
            c = mod(channel,obj.scale*obj.scale);
            if c == 0, c = obj.scale*obj.scale; end
            q = floor((c-1)/obj.scale)+1;
            r = mod(c, obj.scale);
            if r == 0, r = obj.scale; end
            input(:, :, channel, :) = output(q:obj.scale:end, r:obj.scale:end, ch, :);
        end
      derInputs{1} = gpuArray(single(input));
      derParams = {} ;
    end

    function obj = SubPixel_Conv(varargin)
      obj.load(varargin) ;
      obj.scale = obj.scale;
    end
  end
end


% Evaluates hamming kNN classifier using 1 upto K nearest neighbors for prediction

function [acc acc2] = eval_hammknn(data, W, K, nonlinearity)

Ntraining = data.Ntraining;
Xtraining = data.Xtraining;
Ltraining = double(data.Ltraining);

Ntest = data.Ntest;
Xtest = data.Xtest;
Ltest = double(data.Ltest);

if (~iscell(W))
  B1 = W * Xtraining;
  B2 = W * Xtest;
else
  resp1 = compute_NN_output(W, Xtraining, nonlinearity);
  B1 = resp1{end};
  resp2 = compute_NN_output(W, Xtest, nonlinearity);
  B2 = resp2{end};
end

B1 = logical(single(B1 > 0));
B1 = compactbit(B1);

B2 = logical(single(B2 > 0));
B2 = compactbit(B2);

[nw1 n1] = size(B1);
[nw2 n2] = size(B2);
nb = nw1*8;

if (nw1 ~= nw2)
  error('nw1 ~= nw2\n');
end

if ~exist('K', 'var')
  K = 3;
end

% Assuming there are only 10 labels
% TODO: change if you have more labels
nlabels = 10;

[ret ret2] = hammknn_mex(nlabels, B1, uint32(Ltraining), B2, uint32(Ltest), n1, nb, K);

acc = double(ret) / Ntest;
acc2 = double(ret2) / Ntraining;


function [yd3] = l2yd3(l)
% Convert volume from liters to cubic yards.
% Chad Greene 2012
yd3 = l*0.0013079506193;


function [ml] = likelihood(hyp, inf, mean, cov, lik, input, target)
% Calculates the negative log marginal likelihood.
%
%% Syntax
%  ml = likelihood(hyp, inf, mean, cov, lik, input, target)
%
%% Description
% Function for calculating the negative log marginal likelihood of given
% hyperparameters, covariance function and data. If more sets of
% hyperparameters are given it outputs a vector of calculated negative log
% marginal likelihoods.
%
% Based on the work of C.E.Rasmussen.
%
% Input:
% * hyp    ... the hyperparameter struct(s), row vector of structs
% * inf    ... the function specifying the inference method
% * mean   ... the prior mean function
% * cov    ... the prior covariance 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
%
% Output:
% * ml     ... the negative log marginal likelihood(s), vector
%
% See Also:
% gpx, minimize, covFunction, gp_initial
%
% Examples:
% gp_initial.m
%%

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

ml=zeros(size(hyp)); % allocate

for i = 1 : size(hyp,2)
  try

    mlt = gp(hyp(i), inf, mean, cov, lik, input, target);
  catch
    mlt = +Inf;
  end

  % stability
  if (~isfinite(mlt) || isnan(mlt) || ~isreal(mlt))
    mlt = +Inf;
  end
  ml(i) = mlt;
end


function bow2 = mydatesow_aux (epoch2)
    % beginning of day:
    epoch2_vec = mydatevec(epoch2);
    bod2_vec = epoch2_vec;  bod2_vec(:,4:6) = 0;
    bod2 = mydatenum(bod2_vec);

    % beginning of week:
    %weekday(datenum(bod2_vec))-1  % DEBUG
    bow2 = bod2 - (weekday(datenum(bod2_vec))-1)*24*3600;
end

%!test
%! % mydatesow_aux()
%! test('mydatesow')
%! test('mydatesowi')


function [Y, X] = svmlread(fname)
% SVMLREAD - Read a data file generated by SVM light
%
%   Y = SVMLREAD(FNAME)
%   FNAME gives the name of an output file generated by SVM light. It
%   may contain predicted labels, coefficients alpha, or an input
%   (example) file with class values and features. From this file the
%   data in the first column (class labels or alphas) is extrated and
%   returned in Y.
%   [Y, X] = SVMLREAD(FNAME), where FNAME is the name of an input file
%   with class values and features, returns both the vector of class
%   labels Y and the matrix of examples X. Each line of X corresponds to
%   a line in the file.
%   Attention: this may take a while...
%
%   See also SVML, SVM_LEARN, SVM_CLASSIFY, SVMLOPT, SVMLWRITE
%

%
% Copyright (c) by Anton Schwaighofer (2001)
% $Revision: 1.6 $ $Date: 2002/02/19 12:26:07 $
% mailto:anton.schwaighofer@gmx.net
%
% This program is released unter the GNU General Public License.
%

error(nargchk(1, 1, nargin));

X = [];
Y = [];

f = fopen(fname, 'rt');
if (f<0),
  error(sprintf('Unable to open file %s', fname));
end

i = 0;
% fprintf('Scanning ');
while ~feof(f),
  s = fgetl(f);
  [Yi, count, errmsg, nextind] = sscanf(s, '%f', 1);
  % read the class label resp. anything else that is in the first column
  if (count==1),
    i = i+1;
    Y(i,1) = Yi;
    [Xi, count] = sscanf(s(nextind:end), ' %i:%f');
    % scan for the feature:value pairs
    if (rem(count,2)==0) & (count~=0),
      % if they really come in pairs, then accept
      ind = 2:2:count;
      if isempty(X),
        maxCol = max(Xi(ind-1));
        approxSparsity = (count/2)/maxCol;
        % a rough estimate of the sparsity, based on the first line of
        % data
        if approxSparsity>0.5,
          approxSparsity = 1;
          X = zeros(maxCol, 1000);
        else
          X = spalloc(maxCol, 1000, round(1000*maxCol*approxSparsity));
          % allocate for 1000 data points (lines) beforehand
          % We store everything *columnwise* and transpose afterwards,
          % this greatly improves performance
        end
      end
      X(Xi(ind-1),i) = Xi(ind);
%       if (rem(i,100)==0),
%         fprintf(' %i', i);
%       end
    end
  end
end
% fprintf(' done.\n');
if ~isempty(X),
  X = X(:,1:i)';
  sparsity = length(find(X))/prod(size(X));
  if sparsity<0.5,
    X = sparse(X);
    % remove any surplus lines & convert to sparse a second time for
    % optimal memory usage
  end
end

fclose(f);


function [U,S,output] = lmlra_nls(T,U0,S0,options)
%LMLRA_NLS LMLRA by nonlinear least squares.
%   [U,S,output] = lmlra_nls(T,U0,S0) computes the factor matrices U{1},
%   ..., U{N} and core tensor S belonging to a low multilinear rank
%   approximation of the N-th order tensor T by minimizing
%   0.5*frob(T-lmlragen(U,S))^2. Each term U{r} is a cell array of N factor
%   matrices U{r}{n}, followed by a core tensor U{r}{N+1}. The algorithm is
%   initialized with the factor matrices U0{n} and core tensor S0. The
%   structure output returns additional information:
%
%      output.Name  - The name of the selected algorithm.
%      output.<...> - The output of the selected algorithm.
%
%   lmlra_nls(T,U0,S0,options) may be used to set the following options:
%
%      options.Algorithm =   - The desired optimization method.
%      [@nls_gncgs| ...
%       {@nls_gndl}|@nls_lm]
%      options.M =           - The preconditioner to use when
%      [{'block-Jacobi'}|...   options.LargeScale is true.
%       false]
%      options.<...>         - Parameters passed to the selected method,
%                              e.g., options.TolFun, options.TolX and
%                              options.PlaneSearchOptions. See also help
%                              [options.Algorithm].
%
%   See also lmlra_minf.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] L. Sorber, M. Van Barel, L. De Lathauwer, "Structured data fusion,"
%       ESAT-SISTA Internal Report 13-177, KU Leuven, 2013.
%   [2] L. Sorber, M. Van Barel, L. De Lathauwer, "Unconstrained
%       optimization of real functions in complex variables," SIAM J. Opt.,
%       Vol. 22, No. 3, 2012, pp. 879-898.

% Forward problem to btd_nls as a one-term BTD.
if nargin < 4, options = struct; end
[U,output] = btd_nls(T,{[U0(:).',S0]},options);
S = U{1}{end};
U = U{1}(1:end-1);


function [ a ] = e_qgreedy(Q,s,epsilon )
%E_QGREEDY
%
% input -------------------------------------------------------------------
%
%       o Q       : (num_states x num_actions), Q-value table
%
%       o s       : (1 x 1), current state index
%
%       o epsilon : (1 x 1), exploration noise [0,1]
%

actions = size(Q,2);
if (rand()>epsilon)
    [~,a] = max(Q(s,:));
else
    a = randi(actions);
end

end


%ISCONTOURCONVEX  Tests a contour convexity
%
%     status = cv.isContourConvex(contour)
%
% ## Input
% * __contour__ Input vector of 2D points, stored in numeric array
%   (Nx2/Nx1x2/1xNx2) or cell array of 2-element vectors (`{[x,y], ...}`).
%
% ## Output
% * __status__ Output logical value.
%
% The function tests whether the input contour is convex or not. The contour
% must be simple, that is, without self-intersections. Otherwise, the function
% output is undefined.
%
% See also: cv.convexityDefects
%


function result = clara(x,kclus,vtype,stdize,metric,nsamp,sampsize)

%CLARA is the 'Clustering Large Applications' clustering algorithm.
% It returns a list representing a clustering of the data
% into kclus clusters following the clara algorithm which is
% designed for large data sets.
%
%The algorithm is fully described in:
%   Kaufman, L. and Rousseeuw, P.J. (1990),
%   "Finding groups in data: An introduction to cluster analysis",
%   Wiley-Interscience: New York (Series in Applied Probability and
%   Statistics), ISBN 0-471-87876-6.
%
% Required input arguments:
%       x : Data matrix (rows = observations, columns = variables)
%   kclus : The number of desired clusters
%   vtype : Variable type vector (length equals number of variables)
%           Possible values are 1  Asymmetric binary variable (0/1)
%                               2  Nominal variable (includes symmetric binary)
%                               3  Ordinal variable
%                               4  Interval variable
%
% Optional input arguments:
%     stdize : standardise the variables given by the x-matrix
%              Possible values are 0 : no standardisation (default)
%                                  1 : standardisation by the mean
%                                  2 : standardisation by the median
%     metric : Metric to be used
%              Possible values are 'eucli' Euclidian (all interval variables, default)
%                                  'manha' Manhattan
%                                  'mixed' Mixed (not all interval variables, default)
%      nsamp : Number of samples to be drawn from the data set
%   sampsize : Number of observations in each sample (should be higher
%              than the number of clusters and lower than the number of
%              observations)
%
% I/O:
%   result=clara(x,kclus,vtype,'eucli',5,40+2*kclus)
%
% Example (subtracted from the referenced book)
%   load obj200.mat
%   result=clara(obj200,3,[4 4]);
%
% The output of CLARA is a structure containing:
%   result.dysobs     : dissimilarities for each observation with the medoids
%   result.metric     : metric used
%   result.number     : number of observations
%   result.idmed      : Id of medoid observations
%   result.ncluv      : A vector with length equal to the number of observations,
%                       giving for each observation the number of the cluster to
%                       which it belongs
%   result.obj        : Objective function for the best subsample
%   result.clusinf    : Matrix, each row gives numerical information for
%                       one cluster. These are the cardinality of the cluster
%                       (number of observations), the maximal and average
%                       dissimilarity between the observations in the cluster
%                       and the cluster's medoid, the diameter of the cluster
%                       (maximal dissimilarity between two observations of the
%                       cluster), and the separation of the cluster (minimal
%                       dissimilarity between an observation of the cluster
%                       and an observation of another cluster).
%   result.sylinf     : Matrix based on the best subsample, with for each
%                       observation i of this subsample the cluster to
%                       which i belongs, as well as the neighbor cluster of i
%                       (the cluster, not containing i, for which the average
%                       dissimilarity between its observations and i is minimal),
%                       and the silhouette width of i.
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Written by Guy Brys (May 2006)

%Checking and filling out the inputs
if (nargin<3)
    error('Three input arguments required')
elseif (nargin<4)
    stdize = 0;
    if (sum(vtype)~=4*size(x,2))
        metric = 'mixed';
    else
        metric = 'eucli';
    end
    nsamp=5;
    sampsize=40+2*kclus;
elseif (nargin<5)
    if (sum(vtype)~=4*size(x,2))
        metric = 'mixed';
    else
        metric = 'eucli';
    end
    nsamp=5;
    sampsize=40+2*kclus;
elseif (nargin<6)
    nsamp=5;
    sampsize=40+2*kclus;
elseif (nargin<7)
    sampsize=40+2*kclus;
end

%Standardization
if (stdize==1)
    x = ((x - repmat(mean(x),size(x,1),1))./(repmat(std(x),size(x,1),1)));
elseif (stdize==2)
    x = ((x - repmat(median(x),size(x,1),1))./(repmat(mad(x),size(x,1),1)));
end

%Actual calculations
obj = Inf;
for i=1:nsamp
    sampindex = randperm(size(x,1));
    restemp = pam(x(sampindex(1:sampsize),:),kclus,vtype,metric);
    if (restemp.obj(1)<obj)
        obj = restemp.obj(1);
        idmed = sampindex(restemp.idmed);
    end
end

%Calculating some extra dissimilarities for output
for i=1:size(x,1)
    for j=1:kclus
        distemp = daisy(x([i idmed(j)],:),vtype,metric);
        disv(i,j) = distemp.disv(1);
    end
    [zz,clu(i)] = min(disv(i,:));
end
mindisv=[];
for j=1:kclus
    clusinf(j,1) = length(find(clu==j));
    clusinf(j,2) = max(disv(find(clu==j),j));
    clusinf(j,3) = mean(disv(find(clu==j),j));
end
for i=1:kclus
    for j=1:kclus
        distemp = daisy(x([idmed(i) idmed(j)],:),vtype,metric);
        mindisv = [mindisv distemp.disv(1)];
    end
end
clusinf(:,4) = clusinf(:,2)/min(mindisv(mindisv~=0));

%Putting things together
result = struct('dysobs',disv,'metric',metric,'number',size(x,1),...
    'idmed',idmed,'ncluv',clu,'obj',obj,'clusinf',clusinf,...
    'sylinf',restemp.sylinf,'x',x);


function varargout = clipPoints3d(points, shape, varargin)
%CLIPPOINTS3D Clip a set of points by a box or other 3d shapes.
%
%   CLIP = clipPoints3d(POINTS, BOX);
%   Returns the set of points which are located inside of the box BOX.
%
%   [CLIP, IND] = clipPoints3d(POINTS, BOX);
%   Also returns the indices of clipped points.
%
%   ... = clipPoints3d(..., 'shape', 'sphere') Specify the shape.
%   Default is 'box'. But it is also possible to use 'sphere' or 'plane'.
%
%   ... = clipPoints3d(..., 'inside', false) returns the set of
%   points outside the shape instead of inside.
%
%   See also
%   points3d, boxes3d, spheres
%

% ------
% Author: David Legland, oqilipo
% E-mail: david.legland@inra.fr
% Created: 2008-10-13, using Matlab 7.4.0.287 (R2007a)
% Copyright 2008-2022 INRA - BIA Nantes - MIAJ Jouy-en-Josas

parser = inputParser;
validStrings = {'box', 'sphere', 'plane'};
addParameter(parser, 'shape', 'box', @(x) any(validatestring(x, validStrings)));
addParameter(parser, 'inside', true, @islogical);
parse(parser,varargin{:});

switch parser.Results.shape
    case 'box'
        LI = clipPointsByBox(points, shape);
    case 'plane'
        LI = clipPointsByPlane(points, shape);
    case 'sphere'
        LI = clipPointsBySphere(points, shape);
end

if parser.Results.inside
    % keep points inside the shape
    ind = find(LI);
else
    % keep points outside the shape
    ind = find(~LI);
end
points = points(ind, :);

% process output arguments
varargout{1} = points;
if nargout == 2
    varargout{2} = ind;
end

    function LI = clipPointsByBox(points, box)
        % get bounding box limits
        xmin = box(1);
        xmax = box(2);
        ymin = box(3);
        ymax = box(4);
        zmin = box(5);
        zmax = box(6);

        % compute indices of points inside visible area
        xOk = points(:,1) >= xmin & points(:,1) <= xmax;
        yOk = points(:,2) >= ymin & points(:,2) <= ymax;
        zOk = points(:,3) >= zmin & points(:,3) <= zmax;

        LI = xOk & yOk & zOk;
    end

    function LI = clipPointsByPlane(points, plane)
        % points inside and on the surface of the sphere
        LI = isBelowPlane(points, plane);
    end

    function LI = clipPointsBySphere(points, sphere)
        % points inside and on the surface of the sphere
        LI = distancePoints3d(points, sphere(1:3)) <= sphere(4);
    end

end


function errorbar_width(h, x, interval)
% Work with errorbar.m: Adjust the width of errorbar
%
% :Usage:
% ::
%
%    errorbar_width(h, x, interval)
%
% :Inputs:
%
%   **h:**
%        errorbar graphic handle
%
%   **x:**
%        vector x, which is used in errorbar
%
%   **interval:**
%        e.g., [-.1 .1] or [0 0]
%
% :Examples: you can see this output in
% http://wagerlab.colorado.edu/wiki/doku.php/help/core/figure_gallery
% ::
%
%    x = 1:5; % x values
%    y = [32 40 55 84 130]; % mean
%    e = [6 6 6 6 6]; % standard error of the mean
%
%    create_figure(y_axis);
%    set(gcf, 'Position', [1   512   268   194]);
%    col = [0.3333    0.6588    1.0000];
%    markercol = col-.2;
%
%    h = errorbar(x, y, e, 'o', 'color', 'k', 'linewidth', 1.5, 'markersize', 7, 'markerfacecolor', col);
%    hold on;
%    sepplot(x, y, .75, 'color', col, 'linewidth', 2);
%    errorbar_width(h, x, [0 0]); % here
%
%    set(gca, 'xlim', [.5 5.5], 'linewidth', 1.5);
%
%    try
%       pagesetup(gcf);
%       saveas(gcf, 'example.pdf');
%    catch
%       pagesetup(gcf);
%       saveas(gcf, 'example.pdf');
%    end
%
% ..
%    Copyright (C) 2014  Wani Woo
% ..

xdata = [];
for i = x
    xdata = [xdata repmat(i,1,2) NaN (repmat(i,1,2) + interval) NaN (repmat(i,1,2) + interval) NaN];
end

hh = get(h, 'children'); set(hh(2), 'XData', xdata);

end


function a = r8bb_set ( n1, n2, ml, mu, a, i, j, value )

%*****************************************************************************80
%
%% R8BB_SET sets an entry of a R8BB matrix.
%
%  Discussion:
%
%    The R8BB storage format is for a border banded matrix.  Such a
%    matrix has the logical form:
%
%      A1 | A2
%      ---+---
%      A3 | A4
%
%    with A1 a (usually large) N1 by N1 banded matrix, while A2, A3 and A4
%    are dense rectangular matrices of orders N1 by N2, N2 by N1, and N2 by N2,
%    respectively.
%
%    A should be defined as a vector.  The user must then store
%    the entries of the four blocks of the matrix into the vector A.
%    Each block is stored by columns.
%
%    A1, the banded portion of the matrix, is stored in
%    the first (2*ML+MU+1)*N1 entries of A, using standard LINPACK
%    general band format.  The reason for the factor of 2 in front of
%    ML is to allocate space that may be required if pivoting occurs.
%
%    The following formulas should be used to determine how to store
%    the entry corresponding to row I and column J in the original matrix:
%
%    Entries of A1:
%
%      1 <= I <= N1, 1 <= J <= N1, (J-I) <= MU and (I-J) <= ML.
%
%      Store the I, J entry into location
%      (I-J+ML+MU+1)+(J-1)*(2*ML+MU+1).
%
%    Entries of A2:
%
%      1 <= I <= N1, N1+1 <= J <= N1+N2.
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+(J-N1-1)*N1+I.
%
%    Entries of A3:
%
%      N1+1 <= I <= N1+N2, 1 <= J <= n1.
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%
%    Entries of A4:
%
%      N1+1 <= I <= N1+N2, N1+1 <= J <= N1+N2
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%      (same formula used for A3).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 March 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N1, N2, the order of the banded and dense blocks.
%    N1 and N2 must be nonnegative, and at least one must be positive.
%
%    Input, integer ML, MU, the lower and upper bandwidths.
%    ML and MU must be nonnegative, and no greater than N1-1.
%
%    Input, real A((2*ML+MU+1)*N1+2*N1*N2+N2*N2), the R8BB matrix.
%
%    Input, integer I, J, the row and column of the entry to be set.
%
%    Input, real VALUE, the value to be assigned to the (I,J) entry.
%
%    Output, real A((2*ML+MU+1)*N1+2*N1*N2+N2*N2), the updated R8BB matrix.
%
  if ( i <= 0 | n1+n2 < i )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8BB_SET - Fatal error!\n' );
    fprintf ( 1, 'R8BB_SET - Illegal value of row index I = %d\n', i );
    error ( 'R8BB_SET - Fatal error!' );
  end

  if ( j <= 0 | n1+n2 < j )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8BB_SET - Fatal error!\n' );
    fprintf ( 1, 'R8BB_SET - Illegal value of column index J = %d\n', j );
    error ( 'R8BB_SET - Fatal error!' );
  end
%
%  The A1 block of the matrix.
%
%  Check for out of band problems.
%
%  Normally, we would check the condition MU < (J-I), but the storage
%  format requires extra entries be set aside in case of pivoting, which
%  means that the condition becomes MU+ML < (J-I).
%
  if ( i <= n1 & j <= n1 )
    if ( mu+ml < (j-i) | ml < (i-j) )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8BB_SET - Warning!\n' );
      fprintf ( 1, 'R8BB_SET - Unable to set entry A(%d,%d).\n', i, j );
      error ( 'R8BB_SET - Warning!' );
    else
      ij = (i-j+ml+mu+1)+(j-1)*(2*ml+mu+1);
    end
%
%  The A2 block of the matrix.
%
  elseif ( i <= n1 & n1 < j )
    ij = (2*ml+mu+1)*n1+(j-n1-1)*n1+i;
%
%  The A3 and A4 blocks of the matrix.
%
  elseif ( n1 < i )
    ij = (2*ml+mu+1)*n1+n2*n1+(j-1)*n2+(i-n1);
  end

  a(ij) = value;

  return
end


function coord_w = minc_voxel2world(coord_v,mat,opt);
% Convert coordinates in the voxel space into coordinates in the world
% space.
%
% SYNTAX:
% COORD_W = MINC_VOXEL2WORLD(COORD_V,MAT,OPT)
%
% _________________________________________________________________________
% INPUTS:
%
% COORD_V
%       (matrix N*3) each row is a vector of 3D coordinates in voxel space.
%
% MAT
%       (matrix 4*4) an affine transformation from voxel to world
%       coordinates. See the help of NIAK_READ_VOL for more infos. It is
%       generally the HDR.INFO.MAT field of the header of a volume file.
%
% OPT
%       (structure, optional) with the following fields :
%
%       FLAG_ZERO
%           (boolean, default false) if FLAG_ZERO is true, voxel
%           coordinates start from 1 (default behaviour in matlab),
%           otherwise they start from 0 (default behaviour in C/C++ or
%           MINC).
%
% _________________________________________________________________________
% OUTPUTS:
%
% COORD_W
%       (matrix N*3) each row is a vector of 3D coordinates in world space.
%
% _________________________________________________________________________
% SEE ALSO:
% MINC_READ, MINC_WRITE, MINC_VOXEL2WORLD, MINC_WORLD2VOXEL
%
% _________________________________________________________________________
% COMMENTS:
%
% Copyright (c) Pierre Bellec, Centre de recherche de l'institut de
% gériatrie de Montréal, Département d'informatique et de recherche
% opérationnelle, Université de Montréal, 2013.
% See licensing information in the code.
% Keywords : affine transformation, coordinates

% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the Software is
% furnished to do so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
% THE SOFTWARE.
if nargin < 3
    flag_zero = false;
else
    if isfield(opt,'flag_zero')
        flag_zero = opt.flag_zero;
    else
        flag_zero = false;
    end
end
if flag_zero
    coord_w = [coord_v ones([size(coord_v,1) 1])]*(mat');
else
    coord_w = [coord_v-1 ones([size(coord_v,1) 1])]*(mat');
end
coord_w = coord_w(:,1:3);


function [U,Sigma,V,numiter,out]  = SVT(n,Omega,b,tau,delta,maxiter,tol,EPS)
% [U,Sigma,V,numiter,output]  = SVT(n,Omega,b,tau,delta,maxiter,tol,EPS)
%
% Finds the minimum of   tau ||X||_* + .5 || X ||_F^2
%
% subject to P_Omega(X) = P_Omega(M)
%
% using linear Bregman iterations
%
% Usage:  [U,S,V,numiter]  = SVT(n,Omega,b,delta,maxiter,tol)
%
% Inputs:
%
%   n - size of the matrix X assumed n(1) by n(2). If n is a single integer, it
% is understood that n(1) = n(2).
%
%   Omega - set of observed entries.  Should be linearly indexed.
%
%   b - data vector of the form M(Omega)
%
%   tau - parameter defining the objective functional
%
%   delta - step size.  Choose delta less than 2 to be safe but
%       conservative; choose delta closer to n(1)*n(2)/length(Omega)
%       to be riskier (i.e. algorithm may diverge)
%
%   maxiter - maximum number of iterations
%
%   tol - stopping criteria (default: 1e-4)
%
%   EPS - noise constraint.  This relaxes the constraints, so that they
%       are now of the form | X(i,j) - M(i,j) | <= EPS,
%       for all indices (i,j) in omega.  Default: 0
%
% Outputs: matrix X stored in SVD format X = U*diag(S)*V'
%
%   U - n1xr left singular vectors
%
%   S - rx1 singular values
%
%   V - n2xr right singular vectors
%
%   numiter - number of iterations to achieve convergence
%
%   output - a structure with data from each iteration.  Includes:
%       output.nuclearNorm  - nuclear norm of current iterate
%       output.rank         - rank of current iterate
%       output.time         - time taken for one iteraration
%       output.residual     - the relative residual, norm(x-b)/norm(b)
% Description:
% Reference:
%
%    Cai, Candes and Shen
%    A singular value thresholding algorithm for matrix completion
%    Submitted for publication, October 2008.
%
%    See also more general code as part of the TFOCS package,
%    available at tfocs.stanford.edu as of November 2010.
%
% Written by: Emmanuel Candes
% Email: emmanuel@acm.caltech.edu
% Created: October 2008
% Efficient mex-file and PROPACK version: Stephen Becker, Nov 2008
% Modified: Stephen Becker, March 2009
% Modified: Stephen Becker, May 2009  works with complex numbers
% Modified: Farshad Harirchi and Stephen Becker, April 2011, fixing a bug.

global VERBOSE
if isempty(VERBOSE)
    % -- feel free to change these 'verbosity' parameters
    % VERBOSE = false;
    VERBOSE = 1;    % a little bit of output
    % VERBOSE = 2;    % even more output
end

time1 = cputime;
if nargin < 8 || isempty(EPS)
    EPS = false;
end
if nargin < 7 || isempty(tol)
    tol = 1e-4;
end
if nargin < 6 || isempty(maxiter)
    maxiter = 500;
end

if length(n) == 1,
    n1 = n(1); n2 = n1;
elseif length(n) == 2,
    n1 = n(1); n2 = n(2);
end
if n1*n2 < 100*100, SMALLSCALE = true; else SMALLSCALE = false; end

m = length(Omega); [temp,indx] = sort(Omega);
% simpler: sort b also
incre = 5;
normb = norm(b);

[i, j] = ind2sub([n1,n2], Omega);
USE_SLOW_UPDATE     = false;
if EPS
    % with inequality constraints, should take delta = delta/sqrt(2) at
    % least, or delta = delta/2
    delta = delta/sqrt(2);
%     delta = delta/2; tau = 2*tau;
%     y1 = max(b-EPS,0); y2 = max(-b-EPS,0); % doesn't work well
    y1 = max(b,0); y2 = max(-b,0);
    Y = sparse(i,j,y1-y2,n1,n2,m);
    normProjM = normest(Y,1e-2);
    k0 = ceil(tau/(delta*normProjM));
    y1 = k0*delta*y1;
    y2 = k0*delta*y2;
    try
        updateSparse(Y,y1-y2,indx);
    catch
        l = lasterror;
        if strcmpi( l.identifier, 'MATLAB:UndefinedFunction')
            % mex file not installed, so do this instead:
            [indx_i,indx_j,s] = find(Y);
            Y = updateSparse_slow(Y,y1-y2,indx,indx_i,indx_j);
            USE_SLOW_UPDATE     = true;
        else
            % some other error (unexpected)
            rethrow(lasterror)
        end
    end
else
    Y = sparse(i,j,b,n1,n2,m);
    normProjM = normest(Y,1e-2);
    k0 = ceil(tau/(delta*normProjM));
    normb = norm(b);
    y = k0*delta*b; % kicking by k0 steps
    try
        updateSparse(Y,y,indx);
    catch
        l = lasterror;
        if strcmpi( l.identifier, 'MATLAB:UndefinedFunction')
            % mex file not installed, so do this instead:
            [indx_i,indx_j,s] = find(Y);
            Y = updateSparse_slow(Y,y,indx,indx_i,indx_j);
            USE_SLOW_UPDATE     = true;
        else
            % some other error (unexpected)
            rethrow(lasterror)
        end
    end

end
r = 0;

out.residual = zeros(maxiter,1);
out.rank= zeros(maxiter,1);
out.time = zeros(maxiter,1);
out.nuclearNorm = zeros(maxiter,1);

% What the best way to multiply a sparse matrix?
[forwardType, transposeType] = findBestMultiply(Y,.2);

if VERBOSE==1, fprintf('\nIteration:   '); end
for k = 1:maxiter,
    if VERBOSE==1, fprintf('\b\b\b\b%4d',k);  end
    s = r + 1;

    rInc = 4;  % make this larger for more accuracy
    %if tol < 1e-4  && relRes < 1e-1
    %rInc = rInc + max( round(log10( abs(1e-1/relRes) )), 5 );
    %end
    s = min( [r + rInc, n1, n2] );

    if SMALLSCALE
        [U,Sigma,V] = svd(full(Y),'econ');
    else
        % Make routines for multiplying by a sparse matrix
        Yt = Y';
        switch forwardType
            case 1, Yforward = @(x) Y*x;
            case 2, Yforward = @(x) Yt'*x;
            case 3, Yforward = @(x) smvp(Y,x);
        end
        switch transposeType
            case 1, Ytranspose = @(x) Yt*x;
            case 2, Ytranspose = @(x) Y'*x;
            case 3, Ytranspose = @(x) smvp(Yt,x);
        end
        OK = 0;
        while ~OK
            opts = [];
            if ~isreal(b), opts.eta = 1e-16; end
            [U,Sigma,V] = lansvd(Yforward,Ytranspose,n1,n2,s,'L',opts);
            %[U,Sigma,V] = lansvd(Y,s,'L');
            OK = (Sigma(s,s) <= tau) || ( s == min(n1,n2) );
            s = min(s + incre, min(n1,n2));
        end
    end

    sigma = diag(Sigma); r = sum(sigma > tau);
    U = U(:,1:r); V = V(:,1:r); sigma = sigma(1:r) - tau; Sigma = diag(sigma);

    x = XonOmega(U*diag(sigma),V,Omega);
    eTime = cputime - time1;
    if VERBOSE == 2
        fprintf('iteration %4d, rank is %2d, rel. residual is %.1e\n',k,r,norm(x-b)/normb);
    end
    relRes = norm(x-b)/normb;
    out.residual(k) = relRes;
    out.time(k) = eTime;
    out.rank(k) = r;
    out.nuclearNorm(k) = sum(sigma);

    time1 = cputime;

    if (relRes < tol)
        break
    end
    if EPS && norm(x-b,'inf') < 2*EPS
        break
    end
    if (norm(x-b)/normb > 1e5)
        disp('Divergence!');
        break
    end

    if EPS
        y1 = max( y1 + delta*( -(x-b) - EPS), 0 );
        y2 = max( y2 + delta*(  (x-b) - EPS), 0 );
        if USE_SLOW_UPDATE
            % mex file not installed, so do this instead:
            Y = updateSparse_slow(Y,y1-y2,indx,indx_i,indx_j);
        else
            updateSparse(Y,y1-y2,indx);
        end
    else
        y = y + delta*(b-x);
        if USE_SLOW_UPDATE
            % mex file not installed, so do this instead:
            Y = updateSparse_slow(Y,y,indx,indx_i,indx_j);
        else
            updateSparse(Y,y,indx);
        end
    end
end

if VERBOSE==1, fprintf('\n'); end
numiter = k;
out.residual = out.residual(1:k,:);
out.time = out.time(1:k,:);
out.rank= out.rank(1:k,:);
out.nuclearNorm= out.nuclearNorm(1:k,:);


function varargout = drawCuboid(cuboid, varargin)
%DRAWCUBOID Draw a 3D cuboid, eventually rotated.
%
%   drawCuboid(CUBOID)
%   Displays a 3D cuboid on current axis. CUBOID is given by:
%   [XC YC ZC L W D YAW PITCH ROLL],
%   where (XC, YC, ZC) is the cuboid center, L, W and H are the lengths of
%   the cuboid main axes, and YAW PITCH ROLL are Euler angles representing
%   the cuboid orientation, in degrees.
%
%   If cuboid is axis-aligned, it can be specified using only center and
%   side lengths:
%   CUBOID = [XC YC ZC L W H]
%
%   Example
%   % Draw a basic rotated cuboid
%     figure; hold on;
%     drawCuboid([10 20 30   90 40 10   10 20 30], 'FaceColor', 'g');
%     axis equal;
%     view(3);
%
%     % Draw three "borromean" cuboids
%     figure; hold on;
%     drawCuboid([10 20 30 90 50 10], 'FaceColor', 'r');
%     drawCuboid([10 20 30 50 10 90], 'FaceColor', 'g');
%     drawCuboid([10 20 30 10 90 50], 'FaceColor', 'b');
%     view(3); axis equal;
%     set(gcf, 'renderer', 'opengl')
%
%   See also
%   meshes3d, polyhedra, createCube, drawEllipsoid, drawCube
%

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

phi   = 0;
theta = 0;
psi   = 0;

%% Parses the input
if nargin == 0
    % no input: assumes cuboid with default shape
    xc = 0;	yc = 0; zc = 0;
    a = 5; b = 4; c = 3;

else
    % one argument: parses elements
    xc  = cuboid(:,1);
    yc  = cuboid(:,2);
    zc  = cuboid(:,3);
    a   = cuboid(:,4);
    b   = cuboid(:,5);
    c   = cuboid(:,6);
    if size(cuboid, 2) >= 9
        k   = pi / 180;
        phi   = cuboid(:,7) * k;
        theta = cuboid(:,8) * k;
        psi   = cuboid(:,9) * k;
    end
end

%% Compute cuboid coordinates

% create unit centered cube
[v, f] = createCube;
v = bsxfun(@minus, v, mean(v, 1));

% convert unit basis to ellipsoid basis
sca     = createScaling3d(a, b, c);
rotZ    = createRotationOz(phi);
rotY    = createRotationOy(theta);
rotX    = createRotationOx(psi);
tra     = createTranslation3d([xc yc zc]);

% concatenate transforms
trans   = tra * rotZ * rotY * rotX * sca;

% transform mesh vertices
[x, y, z] = transformPoint3d(v, trans);

%% Process output
if nargout == 0
    % no output: draw the cuboid
    drawMesh([x y z], f, varargin{:});

elseif nargout == 1
    % one output: draw the cuboid and return handle
    varargout{1} = drawMesh([x y z], f, varargin{:});

elseif nargout == 3
    % 3 outputs: return computed coordinates
    varargout{1} = x;
    varargout{2} = y;
    varargout{3} = z;
end


%########################################################################
%
%	- PPGI Toolbox -
%   A MATLAB toolbox for Photoplethysmography Imaging (PPGI)
%
% Author   : Christian S. Pilz
% Company  : The Nature of Space of Time
% Date     : 07.05.2019
%
% Contact  : cpi@partofthestars.com
% Web Page : www.partofthestars.com
%
% Version  : beta0.1
%
%########################################################################
%
%	test_spherical_mean.m:
%
% Description:
%
%   test of the spherical mean feature extraction on given sample rgb data
%
%
clear all;
close all;

load('./../media/data/example_data.mat');

if ~exist('skin_pixels')
    disp('error: no skin pixels available. execute test_skin.m first!');
    return;
end

spm=spherical_mean();

for f=1:size(skin_pixels,2)
    f
    [signal(f,:) ssr]=spm.get(skin_pixels{f});
end

fs=25;
low_frequency=0.5;
high_frequency=2.5;
bpf=bandpass_filter(fs,low_frequency,high_frequency);
signal_filtered=bpf.get(signal);

[pearson, rmse, snr, snr_var, bpm] = ground_truth_stats.get(ppg,signal_filtered(:,1),fs);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Jellyfish Example Courtesy of Alexander P. Hoover, PhD
%
% Converted from IBAMR: 1/16/2018 by NAB.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Make_Jelly_Geometry()

close all;
clear all;
L = 8;                              % height of computational domain (m) for keeping desired resolution
Lh = 10;                            % actual height of computational domain (m) (MATCHES INPUT2D)
Lw = 3;                             % width of computational domain (m) (MATCHES INPUT2D)
N = 768;                            % number of Cartesian grid meshwidths at the finest level of the AMR grid
dx = L/N;                           % Cartesian mesh width (m)
ds = dx/2;

a=.5;                               % bell radius (semi-minor axis, horizontal axis, note width=2a)
b=.75;                              % bell semi-major axis
d=-0.25;
factor_a=.8;

F=1e5; %5e0

theta=zeros(1000,1);
theta_lim=asin(d/b);
theta_test=pi/2;

x_points=zeros(1000,1);
z_points=zeros(1000,1);
id_points=zeros(1000,1);
offset = 0;

kappa_spring = 1e7; %1e5               % spring constant (Newton)
kappa_beam = 2.5e5; %1e5    %5e3              % beam stiffness constant (Newton m^2)
%kappa_beam_flexible = kappa_beam/5;   % beam stiffness constant (Newton m^2)
kappa_target = kappa_spring;           % target point penalty spring constant (Newton)

c=0;
while(theta_test<(pi-theta_lim))
    c=c+1;
    theta(c)=theta_test;

    x_points(c)=a*cos(theta(c));
    z_points(c)=b*sin(theta(c));
    id_points(c)=c-1;

    theta_test=ds/((a*sin(theta(c)))^(2)+(b*cos(theta(c)))^(2))^(.5)+theta(c);

end

c_stiff=c;

npts=2*c-1;
npts_wing=floor(npts/2);
npts_musc=floor(npts_wing/4);

for j=(c+1):(npts)
    x_points(j)=-1*x_points(j-c+1);
    z_points(j)=z_points(j-c+1);
    id_points(j)=j-1;
end

mesh_name = 'jelly';
xShift = 1.5;
yShift = 2;

x_points=x_points(1:npts)+xShift;
z_points=z_points(1:npts)+yShift;
it_points=id_points(1:npts);

plot(x_points(:),z_points(:),'*'); hold on;
axis([0 8 0 8])

% Lag Pts to Mess up Flow At Edge
xBlock = ds:4*ds:Lw-ds;
yBlock = (Lh-5*ds)*ones(1,length(xBlock))+ds;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Print .vertex information
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

vertex_fid = fopen([mesh_name num2str(N) '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', npts + npts_musc*2 + length(xBlock));
    lag_ct = 0;

    %
    % bell
    %
    for j=1:npts
        fprintf(vertex_fid, '%1.16e %1.16e\n', x_points(j), z_points(j));
        lag_ct = lag_ct + 1;
    end

    %
    % muscles
    %
    for s = 1:npts_musc
        fprintf(vertex_fid, '%1.16e %1.16e\n', x_points(npts_wing+1-npts_musc+s), z_points(npts_wing+1-npts_musc+s));
        plot(x_points(npts_wing+1-npts_musc+s),z_points(npts_wing+1-npts_musc+s),'r*'); hold on;
        lag_ct = lag_ct + 1;
    end
    for s = 1:npts_musc
        fprintf(vertex_fid, '%1.16e %1.16e\n', x_points(npts-npts_musc+s), z_points(npts-npts_musc+s));
        plot(x_points(npts-npts_musc+s),z_points(npts-npts_musc+s),'r*'); hold on;
        lag_ct = lag_ct + 1;
    end

    for ii=1:length(xBlock)
        fprintf(vertex_fid, '%1.16e %1.16e\n', xBlock(ii), yBlock(ii));
    end

fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Print .spring information
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

spring_fid = fopen([mesh_name num2str(N) '.spring'], 'w');

    npts_spring_type1=npts-1;

    fprintf(spring_fid, '%d\n', npts-1 + npts_musc);

    fprintf('\nNumber of springs before muscles: %d \n\n',npts-1)

    factor = 1;%ds^2/ds;

    %
    % bell
    %
    for s = 1:c-1
        resting=sqrt((x_points(s)-x_points(s+1))^(2)+(z_points(s)-z_points(s+1))^(2));
        fprintf(spring_fid, '%d %d %1.16e %1.16e %d\n', id_points(s)+1, id_points(s+1)+1, kappa_spring*ds/(ds^2)*factor, resting, 1);
    end
    for s = c+1:npts-1
        resting=sqrt((x_points(s)-x_points(s+1))^(2)+(z_points(s)-z_points(s+1))^(2));
        fprintf(spring_fid, '%d %d %1.16e %1.16e %d\n', id_points(s)+1, id_points(s+1)+1, kappa_spring*ds/(ds^2)*factor, resting, 1);
    end
    resting=sqrt((x_points(1)-x_points(c+1))^(2)+(z_points(1)-z_points(c+1))^(2));
    fprintf(spring_fid, '%d %d %1.16e %1.16e %d\n', id_points(1)+1, id_points(c+1)+1, kappa_spring*ds/(ds^2)*factor, resting, 1);

    %
    % muscles
    %
    for s = 1:npts_musc
        fprintf(spring_fid, '%d %d %1.16e %1.16e %d\n',npts+s-1+1, npts+s+npts_musc-1+1, F, 0, 1);
    end

    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Print .nonInv_beam information
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

beam_fid = fopen([mesh_name num2str(N) '.nonInv_beam'], 'w');

    fprintf(beam_fid, '%d\n', npts-2);

    factor=1;% = (ds^4)/ds;

    for s = 2:c-1
        C1 = x_points(s-1)+x_points(s+1)-2*x_points(s);
        C2 = z_points(s-1)+z_points(s+1)-2*z_points(s);
        fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', id_points(s-1)+1, id_points(s)+1, id_points(s+1)+1, kappa_beam*ds/(ds^4)*factor, C1, C2);
    end
    for s = c+2:npts-1
        C1 = x_points(s-1)+x_points(s+1)-2*x_points(s);
        C2 = z_points(s-1)+z_points(s+1)-2*z_points(s);
        fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', id_points(s-1)+1, id_points(s)+1, id_points(s+1)+1, kappa_beam*ds/(ds^4)*factor, C1, C2);
    end

    C1 = x_points(c+2)+x_points(1)-2*x_points(c+1);
    C2 = z_points(c+2)+z_points(1)-2*z_points(c+1);
    fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', id_points(c+2)+1, id_points(c+1)+1, id_points(1)+1, kappa_beam*ds/(ds^4)*factor, C1, C2);

    C1 = x_points(c+1)+x_points(2)-2*x_points(1);
    C2 = z_points(c+1)+z_points(2)-2*z_points(1);
    fprintf(beam_fid, '%d %d %d %1.16e %1.16e %1.16e\n', id_points(c+1)+1, id_points(1)+1, id_points(2)+1, kappa_beam*ds/(ds^4)*factor, C1, C2);

    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PRINT TARGET POINTS!!!
%
% print target points (flow blocker along edge)
k_Target = 2.5e6;
nBefore = lag_ct; % Counts pts in jellyfish for bookkeeping for .target file
struct_name = ['jelly' num2str(N)];
print_Lagrangian_Target_Pts(xBlock,k_Target,struct_name,nBefore)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints TARGET points to a file called 'struct_name'.target
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name,nBefore)

    N = length(xLag);
    Nstart = nBefore+1;
    Nend = nBefore+N;

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = Nstart:Nend
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    fclose(target_fid);


% Conduct a binary search
%
% binSearch: Conducts a binary search and continues until the score is
% close enough to the target. An upper limit should be set on the number of
% iterations
%
%     [SearchPoint, Step, ReLoop] = iosr.auditory.binSearch(Score, ...
%       TargetScore, ...
%       CloseEnough, ...
%       NextSearchPointa, ...
%       Step, ...
%       LoopCount, ...
%       MaxLoops);
% end
%
% inputs:
% - Score: the test value
% - Target: the 'finished' test value
% - CloseEnough: the distance from the Target at which it is acceptable to
% discontinue the binary search
% - SearchPoint: the next work input value to try
% - Step: the distance which the SearchPoint can move by
% - LoopCount: a counter for the number of iterations completed so far
% - MaxLoops: The upper limit on the number of iterations allowed
%
% outputs:
% - SearchPoint: the next work input value you should try
% - Step: the distance which can be stepped on the NEXT iteration. You
%   should store this value.
% - ReLoop: If this flag is set to 0, the upper level while loop will be
%   terminated
%
%
% example: you wish to use an audibility model to find the level at which
% you can be 50% confident of detecting the signal (-+ 1%). You choose to
% start with an input level of 20dB and take no more than 10 steps of
% 40,20,10 dB etc.
%
% You would implement this in the following way:
%
% TargetPercent = 0.5;
% CloseEnough = 0.01;
% Signal Level = 40;
% Step = 40;
% MaxLoops = 10;
% LoopCount = 0;
%
% while ReLoop = 1
%
%   LoopCount = LoopCount + 1;
%
%   Percent = RunAudibilityModel(SignalLevel)
%
%   [SignalLevel Step ReLoop] = iosr.auditory.binSearch(Percent, ...
%       TargetPercent, ...
%       CloseEnough, ...
%       SignalLevel, ...
%       Step, ...
%       LoopCount, ...
%       MaxLoops);
%
% end
%

%   Copyright 2016 University of Surrey.

function [SearchPoint, Step, ReLoop] = binSearch(Score, Target, CloseEnough, SearchPoint, Step, LoopCount, MaxLoops)

% input tests
% test input types
assert(isnumeric(Score) ...
    &isnumeric(Target) ...
    &isnumeric(CloseEnough) ...
    &isnumeric(SearchPoint) ...
    &isnumeric(Step) ...
    &isnumeric(LoopCount) ...
    &isnumeric(MaxLoops),'input arguments must be numeric!');

% Define next search point
if Score > Target
    SearchPoint = SearchPoint - Step;
else
    SearchPoint = SearchPoint + Step;
end

% half the distance of the next step
Step = Step / 2;

% if we got close enough to our taget, or it was the last iteration
if (abs((Score-Target))<CloseEnough)||(LoopCount>=MaxLoops)
    ReLoop = 0;
else
    ReLoop = 1;
end

end