function [c, s, active_set] = oasisAR1(y, g, lam, smin, active_set)
%% Infer the most likely discretized spike train underlying an AR(1) fluorescence trace
% Solves the sparse non-negative deconvolution problem
%  min 1/2|c-y|^2 + lam |s|_1 subject to s_t = c_t-g c_{t-1} >=s_min or =0

%% inputs:
%   y:  T*1 vector, One dimensional array containing the fluorescence intensities
%withone entry per time-bin.
% OR %%
% len_active_set*4 matrix, active set

%   g:  scalar, Parameter of the AR(1) process that models the fluorescence ...
%impulse response.
%   lam:  scalar, sparsity penalty parameter lambda.
%   smin: scalar, optional, default 0
%miniumal non-zero activity within each bin (minimal 'spike size').
%   active_set: npool x 4 matrix, warm stared active sets

%% outputs
%   c: T*1 vector, the inferred denoised fluorescence signal at each time-bin.
%   s: T*1 vector, discetized deconvolved neural activity (spikes)
%   active_set: npool x 4 matrix, active sets

%% Authors: Pengcheng Zhou, Carnegie Mellon University, 2016
% ported from the Python implementation from Johannes Friedrich

%% References
% Friedrich J et.al., NIPS 2016, Fast Active Set Method for Online Spike Inference from Calcium Imaging

%% initialization
y = reshape(y, [], 1);
if isempty(y)
    T = sum(active_set(:,4));
else
T = length(y);
end
if ~exist('g', 'var') || isempty(g)
    g = estimate_time_constant(y);
elseif length(g)>1
    c = zeros(T,1);
    s = zeros(T,1);
    active_set = [];
    return;
end
if ~exist('lam', 'var') || isempty(lam);   lam = 0; end
if ~exist('smin', 'var') || isempty(smin);   smin = 0; end
if ~exist('active_set', 'var') || isempty(active_set)
    len_active_set = T;
    active_set = [y-lam*(1-g),ones(T,1),(1:T)',ones(T,1),(1:T)'-1, (1:T)'+1]; % each row is one pool: (vi, wi, t, l)
    active_set(end, :) = [y(end)-lam,1,T,1,T-1,nan] ;
    active_set(1,5) = nan;
else
    len_active_set = size(active_set,1);
    active_set(:,5) = [nan; (1:len_active_set-1)'];
    active_set(:,6) = [(2:len_active_set)';nan];
end
idx = true(len_active_set,1);

%% run OASIS
ii = 1;
ii_next = active_set(ii,6);
while ~isnan(ii_next)
    % find the active set
    while (~isnan(ii_next)) && (active_set(ii_next,1)/active_set(ii_next,2)...
            >=active_set(ii,1)/active_set(ii,2)*g^(active_set(ii,4))+smin)
        active_set(ii_next,5) = ii;
        ii = ii_next;
        ii_next = active_set(ii,6);
    end

    if isnan(ii_next); break; end

    %% merge pools
    active_set(ii,1) = active_set(ii,1) + active_set(ii_next,1)* (g^(active_set(ii,4)));
    active_set(ii,2) = active_set(ii,2) + active_set(ii_next,2)*(g^(2*active_set(ii,4)));
    active_set(ii,4) = active_set(ii,4) + active_set(ii_next,4);
    active_set(ii,6) = active_set(ii_next,6);
    idx(ii_next) = false;
    ii_next = active_set(ii,6);
    ii_prev = active_set(ii, 5);

    %% backtrack until violations fixed
    while (~isnan(ii_prev)) && (active_set(ii,1)/active_set(ii,2)<...
            max(0, active_set(ii_prev,1)/active_set(ii_prev,2)*g^(active_set(ii_prev,4)))+smin)
        ii_next = ii;
        ii = ii_prev;
        active_set(ii,1) = active_set(ii,1) + active_set(ii_next,1)* (g^(active_set(ii,4)));
        active_set(ii,2) = active_set(ii,2) + active_set(ii_next,2)*(g^(2*active_set(ii,4)));
        active_set(ii,4) = active_set(ii,4) + active_set(ii_next,4);
        active_set(ii,6) = active_set(ii_next,6);
        idx(ii_next) = false;

        ii_prev = active_set(ii, 5);
        ii_next = active_set(ii,6);
    end
end
active_set(~idx, :) = [];
len_active_set = size(active_set,1);

%% construct solution for all t
c = zeros(T, 1);
s = c;
for ii=1:len_active_set
    t0 = active_set(ii,3);
    tau = active_set(ii, 4);
    c(t0:(t0+tau-1)) = max(0,active_set(ii,1)/active_set(ii,2)) * (g.^(0:(tau-1)));
end

s(active_set(2:end,3)) = c(active_set(2:end,3)) - g*c(active_set(2:end,3)-1);


function P=mg_ns_prolong_step(nc,x,y,outbnd)
%mg_ns_prolong_step  GMG prolongation operator for step domain (Navier-Stokes)
%   P=mg_ns_prolong_step(nc,x,y,outbnd)
%   input
%          nc      grid parameter
%          x       x coordinate vector for coarse grid
%          y       y coordinate vector for coarse grid
%          outbnd  location of outflow boundary
%   output
%          P       prolongation operator
%
%   IFISS function: HCE; 5 May 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

nelf=2^nc;nelc=nelf/2;nelcx=outbnd*nelc;
nnf=(nelc+1)*((outbnd+1)*nelc+1)+nelc*(outbnd*nelc+1);
nnc=(nelc/2+1)*(((outbnd+1)/2)*nelc+1)+(nelc/2)*(outbnd*nelc/2+1);

% split coordinates
x1=x(1:nelc+1); x2=x(nelc+1:end);
y1=y(nelc+1:end); y2=y;

% block size
nnf1=nelc*(nelc+1);
nnc1=nelc/2*(nelc/2+1);

start_col=(nelc/2+1)*(nelc/2+1);

% Prolongation matrix has the form
% P = [P_1, 0, P_2]
%     [   0 ,  P_3]

% nonzero components of upper block, for upper left of step and coupling
[P_1,P_2]=mg_ellblock(nelc,nelc,x1,y1);
% P(1:nnf1,1:nnc1)=P_1;                % upper block
% P(1:nnf1,nnc-start_col+1:nnc)=P_2;   % coupling

% nonzero component of lower block
P_3=mg_prolong(nelcx,nelf,x2,y2);
% P(nnf1+1:end,nnc1+1:end)=P_3;

P = [[P_1,sparse(nnf1,nnc-start_col-nnc1),P_2];[sparse(nnf-nnf1,nnc1),P_3]];


function I = integral( f, varargin )
%INTEGRAL   Complete definite integral of DISKFUN.
%
%   I = INTEGRAL(F), returns the definite integral of a DISKFUN integrated
%   over its domain of definition.
%
%   I = INTEGRAL(F, g), returns the integral of F along the
%   curve defined by the complex-valued CHEBFUN g.
%
%   I = INTEGRAL(F, 'unitcircle') returns the integral of F along the
%   unit circle.
% See also DISKFUN/SUM2, DISKFUN/INTEGRAL2.

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

%check for empty
if isempty(f)
    I = 0;
    return
end

% Another way to do sum2(f)
if ( nargin == 1 )
    I = integral2( f );
else
    if ( ischar(varargin{1}) )
        if ( strcmpi(varargin{1},'unitcircle') )
            c = chebfun(@(t) exp(1i*t), [-pi, pi]);
        else
            error('CHEBFUN:DISKFUN:INTEGRAL:unrecognizedType',...
                'Unrecognized line integral type.  Did you mean "unitcircle"');
        end
    else
        c = varargin{1};
    end
    if ( ~isa( c, 'chebfun' ) )
        I = integral2( f, varargin{ : } );
    else                            % Line integral over a CHEBFUN
        % Make complex:
        c = c + realmin*1i;
        % Line integral:
        I = sum( feval(f, c ) .* abs( diff( c ) ) );
    end
end

end


function out = ij2xy(in, maxy)
% out = xy2ij(in, maxy)
%
% Converts shapes from "image" coordinates to cartesian coordinates.
% In cartesian coordinates, the origin is on the lower-left corner of
% the image and y moves vertically while x moves horizontally.
% Image coordinates follow a matrix notation where the origin is in
% the upper left corner and the first coordinate 'i' moves vertically
% (in the opposite direction of y), while the second 'j' coordinate
% moves horizontally in the same direction as x.
%
%                         PARAMETERS
%
% in A Nx2xS matrix containing S shapes and N landmarks,
%    in images coordinates: [i j]
% maxy Number of rows of the image matrix, or height of the displayed image,
%      in pixels.
%
%                          RETURNS
%
% out A Nx2xS matrix containing S shapes and N landmarks,
%     in cartesian coordinates: [x y]
%
% Author: Luca Vezzaro (elvezzaro@gmail.com)

	out(:,1,:) = in(:,2,:);
	out(:,2,:) = repmat(maxy, [size(in, 1) 1 size(in,3)]) - in(:,1,:);


function [U,Out] = RecPF(m,n,aTV,aL1,picks,B,PsiT,Psi,opts,varargin)
% [U,Out] = RecPF(m,n,aTV,aL1,picks,B,PsiT,Psi,opts,varargin)
%
% RecPF solves the TVL1-L2 model:
%
%   min aTV*TV(u) + aL1*|PsiT*U|_1 + 0.5|Fp*U - B|_2^2
%
% Inputs:
%
%  m, n     -- size of image
%  aTV, aL1 -- regularization parameters in the model
%  picks    -- sample positions in Fourier domain
%  B        -- measurment vector
%  PsiT     -- sparsifying basis, PsiT*U is the sparse representation of U
%  Psi      -- inverse of PsiT, Psi*x = U reconstructs the image
%  opts      --- contains parameters for algorithm
%              * opts.mit_inn: maxium inner iteration number for each beta {default 30}
%              * opts.mit_out: maxium outer iteration number {default 10}
%              * opts.tol_inn: inner iteration error tolerance {1.e-3}
%                (this option is effective only when "opts.stc = 2", see below)
%              * opts.tol_rel_inn: tolerance of relative change in an inner iteration {5.e-2}
%              * opts.tol_rel_out: tolerance of relative change in an outer iteration {1.e-2}
%                (the above two options are effective only when "opts.stc = 1", see below)
%              * opts.beta0:   initial penalty parameter {2^5}
%              * opts.beta_max: final penalty parameter  {2^15}
%              * opts.beta_rate: increase rate of beta
%              * opts.idisp: 0 or nonzero, display inner iteration info or not
%              * opts.recordf: 0 or 1, keep (or not) history of function values, TV and fidelity;
%              * opts.U0: starting poing {default "a least squares solution"}
%              * opts.wbarflag: 1 (on) or 0 (off), controls wait bar.
%              * opts.stc: 1 (stopping criterion based on Relative Change)
%                          or 2 (stopping criterion based on optimality conditions) {default 1}
%              * opts.TVtype: 1 or 2, corresponding to anisotropic/isotropic TV {default 2}
%  varargin{1}  -- a m x n matrix contains local weights of TV. Specifically, the local weigthed
%                  TV is discretized as:
%                     TV(u) = sum_i w_i ||D_i u||.
%                  If all w_i == 1, then it is just the isotropic discritization of normal TV.
%                  We require all w_i > 0.
%  varargin{2}  -- true image (used to compute relative errors when it is present)
%
% Outputs:
%     U   --- reconsctructed image
%     Out --- a structrue contains
%             * Out.iter: total iteration number
%             * Out.Inner: inner iteration numbers for each beta
%             * Out.ftrue: function values at each iteration
%             * Out.Fvalvscpu: function values decrease with respect to CPU time
%             * Out.TVhist: total variation changes with iteration
%             * Out.FIDhist: fidelity history
%              {the above four subfields are present only when opts.recordf = 1}
%             * Out.ITvsERR: relative error vs. iteration number
%             * Out.CPUvsERR: CPU time vs. relative error
%              {the above two subfields are present only when the original image is present}

%
% Yin Zhang, 11-10-2007
% CAAM, Rice University, Copyright (2007)
%
% Junfeng Yang, last modified on Feb. 2, 2009
%

global Ux Uy FU PsiTU
global Wx Wy
global Z PsiZ
global beta
global remain

if aTV == 0 && aL1 == 0; error('No regularization' ); end
if aTV <  0 || aL1 <  0; error('Regularization < 0'); end
if ~exist('opts','var'); opts = []; end

[mit_inn,mit_out,tol_inn,tol_rel_inn,tol_rel_out,beta0,beta_max, ...
    beta_rate,TVtype,idisp,recordf,U0,wbarflag,stc] = setopts(opts);

[Nomin1,Denom1,Denom2] = getC(picks,B,aTV,m,n);

remain = setdiff(1:m*n,picks);
if isempty(U0); U = zeros(m,n); else U = U0; clear U0; end

if ~isempty(varargin)
    Weights = varargin{1};
    [mm,nn] = size(Weights);
    if mm~=m || nn~= n
        Weights = ones(m,n);
    elseif find(Weights < 0,1)
        error('negative weights are not allowed!');
    end
else
    Weights = ones(m,n);
end
if length(varargin) == 2
    I = varargin{2};
    nrmI = norm(I(:));  rer = norm(U(:)-I(:))/nrmI;
    RER = zeros(500,2); RER(1,:) = [0,rer];
    CPUvsRER = zeros(500,2); CPUvsRER(1,:) = [0,rer];
end

if aL1 > 0; PsiTU = PsiT(U); end

if aTV > 0
    Ux = [diff(U,1,2), U(:,1) - U(:,n)];
    Uy = [diff(U,1,1); U(1,:) - U(m,:)];
end

if recordf
    fval_true = funcval(U,aTV,aL1,B,picks);
    FVAL = zeros(500,2);
    FVAL(1,:) = [0,fval_true];
    Fvalvscpu = zeros(500,2);
    Fvalvscpu(1,:) = [0,fval_true];
    TVhist = zeros(500,1);
    FIDhist = zeros(500,1);
end

% initialization
Inn = zeros(100,1);
beta = beta0;
totalIter = 0;
Outiter = 0;
stopc = 0;
t0 = cputime;

if wbarflag == 1
    wbar = waitbar(0, 'RecPF is running, please wait ...');
end

%% Main loop
while ~stopc
    Outiter = Outiter + 1;
    if stc == 1; Uo = U; end

    Denom = Denom1;
    if aTV > 0; Denom = Denom + (aTV*beta)*Denom2; end
    if aL1 > 0; Denom = Denom + aL1*beta; end

    stopci = 0; Inniter = 0;
    while ~stopci

        % ================================
        %  Begin Alternating Minimization
        % ----------------
        %   W-subprolem
        % ----------------
        if aTV > 0;
            switch TVtype
                case 1;   % anisotropic TV
                    Wx = sign(Ux).* max(abs(Ux)-Weights./beta,0);
                    Wy = sign(Uy).* max(abs(Uy)-Weights./beta,0);
                case 2;   % isotropic TV
                    V = sqrt(Ux.^2 + Uy.^2);
                    S = max(V - Weights./beta, 0);
                    S = S ./ max(V,eps);
                    Wx = S.*Ux; Wy = S.*Uy;
                    clear V S
                otherwise; error('TVtype must be 1 or 2');
            end
        end

        % ----------------
        %   Z-subprolem
        % ----------------
        if aL1 > 0;
            Z = sign(PsiTU).*max(abs(PsiTU)-1/beta,0);
            PsiZ = Psi(Z);
        end

        % ----------------
        %   U-subprolem
        % ----------------
        if stc == 1; Ui = U; end
        rhs = 0;
        if aTV > 0
            rhs = [Wx(:,end) - Wx(:, 1), -diff(Wx,1,2)];
            rhs = rhs + [Wy(end,:) - Wy(1, :); -diff(Wy,1,1)];
            rhs = (aTV*beta)*rhs;
        end
        if aL1 > 0
            rhs = rhs + (aL1*beta)*PsiZ;
        end

        Nomin = Nomin1 + fft2(rhs);

        FU = Nomin./Denom;
        U = real(ifft2(FU));
        Inniter = Inniter + 1;
        totalIter = totalIter + 1;
        %
        %  End Alternating Minimization
        % ================================

        % -----------------------------------------
        % check if inner stopping criterion is met
        %
        if stc == 1
            if aTV > 0
                Ux = [diff(U,1,2), U(:,1) - U(:,n)];
                Uy = [diff(U,1,1); U(1,:) - U(m,:)];
            end
            if aL1 > 0; PsiTU = PsiT(U); end
            chg_inn = norm(Ui(:) - U(:))/norm(U(:));
            stopci = ((chg_inn < tol_rel_inn) || (Inniter >= mit_inn));
        elseif stc == 2
            [res_wn,res_wz,wzr,res_Zn,res_Zz,Zzr,res_u] = checkopt(U,aTV,aL1,PsiT,picks,B,0,Weights);
            res = [res_wn,res_wz,res_Zn,res_Zz,res_u];
            stopci = (max(res) < tol_inn || (Inniter >= mit_inn));
        else
            error('please specify a stopping criterion.');
        end

        if recordf && totalIter < 500
            [fval_true,tvu,fidu] = funcval(U,aTV,aL1,B,picks);
            TVhist(totalIter) = tvu;
            FIDhist(totalIter) = fidu;
            FVAL(totalIter+1,:) = [totalIter,fval_true];
            Fvalvscpu(totalIter+1,:) = [cputime - t0, fval_true];
        end
        if exist('I','var') && totalIter < 500
            rer = norm(U(:)-I(:))/nrmI;
            RER(totalIter+1,:) = [totalIter,rer];
            CPUvsRER(totalIter+1,:) = [cputime - t0, rer];
        end
        if idisp && stc == 2
            fprintf('Iter: %d, res_ wn: %4.1e, wz: %4.1e, Zn: %4.1e, Zz: %4.1e, u: %4.1e, wzr %2.0f%%, Zzr %2.0f%%\n', ...
                totalIter, res_wn,res_wz,res_Zn,res_Zz,res_u,100*wzr,100*Zzr);
        elseif idisp && stc == 1
            fprintf('Iter: %d, chg_inn %4.2f\n',totalIter,chg_inn);
        end

    end % inner
    Inn(Outiter) = Inniter;

    % ------------------------------------------
    % check if outer stopping criterion is met
    %
    beta = beta_rate*beta;
    if stc == 1
        chg_out = norm(U(:)-Uo(:))/norm(U(:));
        stopc = ((chg_out < tol_rel_out) || (Outiter >= mit_out));
    elseif stc == 2
        stopc = ((beta > beta_max) || (Outiter >= mit_out));
    else
        error('please specify a stopping criterion.');
    end
    if wbarflag == 1
        waitbar(log2(beta)/(log2(beta_max)+1), wbar)
    end

end % outer
Out.iter = totalIter;
Out.Inner = Inn(1:Outiter);
if wbarflag == 1
    close(wbar);
end
if recordf
    Out.ftrue = FVAL(1:totalIter+1,:);
    Out.Fvalvscpu = Fvalvscpu(1:totalIter+1,:);
    Out.TVhist = TVhist(1:totalIter);
    Out.FIDhist = FIDhist(1:totalIter);
end
if exist('I','var')
    Out.ITvsRER  = RER(1:totalIter+1,:);
    Out.CPUvsRER = CPUvsRER(1:totalIter+1,:);
end

%% ----------------- SUBFUNCTION ---------------------------
function [res_wn,res_wz,wzr,res_Zn,res_Zz,Zzr,res_u] = ...
    checkopt(U,aTV,aL1,PsiT,picks,f,flag,Weights)
%
% This function checks the optimality of
%                   (              beta                 )
% min   aTV * sum_i (||(W_i)||_2 + ---- ||D_iu - W_i||^2)
%                   (               2                   )
%
%                   (         beta             )
%     + aL1 *       ( |Z|_1 + ---- |Z-PsiT*u|^2 )
%                   (           2              )
%
%     + .5 |F_p*u - f|^2
%
% where W_i = (Wx_i;Wy_i);
%
% U     --- current point
% f     --- measurment vector
% flag  --- 0 (do not check res_u), otherwise check res_u;
%           Generally, res_u << 0, so setting flag = 0 is fine.
% Weights -- local weights of TV
%

% Junfeng Yang, Aug. 08, 2008

global Ux Uy FU PsiTU
global Wx Wy
global beta
global remain
global Z PsiZ

[m,n] = size(U);
mn = m*n;

% res_wn, res_wz, wzr
if aTV > 0
    Ux = [diff(U,1,2), U(:,1) - U(:,n)];
    Uy = [diff(U,1,1); U(1,:) - U(m,:)];

    Dx = Wx - Ux;
    Dy = Wy - Uy;

    V = sqrt(Wx.^2 + Wy.^2);
    Iz = find(V == 0);

    if isempty(Iz)
        In = 1:mn;
    else
        In = find(V ~= 0);
        V(Iz) = 1;
    end

    V = V*beta;

    RWx = Dx + Weights.*Wx./V;
    RWy = Dy + Weights.*Wy./V;
    S = sqrt(RWx.^2 + RWy.^2);

    wzr = length(Iz)/mn;
    if isempty(In)
        res_wn = 0;
    else
        res_wn = max(S(In));
    end

    if isempty(Iz);
        res_wz = -1;
    else
        res_wz = max(S(Iz) - Weights(Iz)/beta);
    end

else
    res_wn = 0;
    res_wz = 0;
    wzr = 0;
end

% res_Zn, res_Zz, Zzr
if aL1 > 0
    PsiTU = PsiT(U);
    absPsiTU = abs(PsiTU);

    ZIn = find(Z ~= 0);
    if isempty(ZIn)
        res_Zn = 0;
    else
        R3 = (1/beta)*sign(Z(ZIn)) + Z(ZIn) - PsiTU(ZIn);
        res_Zn = max(abs(R3));
    end

    ZIz = find(Z == 0);
    if isempty(ZIz)
        res_Zz = -1;
    else
        R4 = absPsiTU(ZIz) - 1/beta;
        res_Zz = max(R4);
    end

    Zzr = length(ZIz)/m/n;

else
    PsiTU = 0;
    res_Zn = 0;
    res_Zz = -1;
    Zzr = 0;
end

% res_u
res_u = 0;
if flag == 0
    return;
else
    % res_u
    FU(picks) = -f + FU(picks);
    FU(remain) = 0;
    Ru = ifft2(FU);
    Ru = real(Ru);

    if aTV > 0
        Dxx = [Dx(:,1) - Dx(:,n), diff(Dx,1,2)];
        Dyy = [Dy(1,:) - Dy(m,:); diff(Dy,1,1)];
        Ru = Ru + (beta*aTV)*(Dxx + Dyy);
    end

    if aL1 > 0
        Ru = Ru + (aL1*beta)*(U - PsiZ);
    end
    res_u = max(abs(Ru(:)));
end
%% ------------- SUBFUNCTION ---------------
function [Nomin1,Denom1,Denom2] = getC(picks,B,aTV,m,n)

% compute fixed quantities
Nomin1 = zeros(m,n);
Nomin1(picks) = B;
Denom1 = zeros(m,n);
Denom1(picks) = 1;
Denom2 = 0;
if aTV > 0
    Denom2 = abs(psf2otf([1,-1],[m,n])).^2 + abs(psf2otf([1;-1],[m,n])).^2;
end
%% ----------  SUBFUNCTION ---------------
function [mit_inn,mit_out,tol_inn,tol_rel_inn,tol_rel_out,beta0,beta_max, ...
    beta_rate,TVtype,idisp,recordf,U0,wbarflag,stc] = setopts(opts)

% define default option fields
mit_inn = 30;
mit_out = 10;
tol_inn  = 1.e-3;
tol_rel_inn = 5.e-3;
tol_rel_out = 1.e-2;
beta0 = 2^5;
beta_max = 2^15;
beta_rate = 2;
TVtype = 2;
idisp = 0;
recordf = 0;
U0 = [];
wbarflag = 0;
stc = 1;

% change to specified option fields if exist
if ~isempty(opts);
    if ~isa(opts,'struct'); error('L1pfi: opts not a struct'); end
    if isfield(opts,'mit_inn'); mit_inn = opts.mit_inn; end
    if isfield(opts,'mit_out'); mit_out = opts.mit_out; end
    if isfield(opts,'tol_inn'); tol_inn = opts.tol_inn; end
    if isfield(opts,'tol_rel_inn'); tol_rel_inn = opts.tol_rel_inn; end
    if isfield(opts,'tol_rel_out'); tol_rel_out = opts.tol_rel_out; end
    if isfield(opts,'beta0');   beta0 = opts.beta0; end
    if isfield(opts,'beta_max'); beta_max = opts.beta_max; end
    if isfield(opts,'beta_rate'); beta_rate = opts.beta_rate; end
    if isfield(opts,'TVtype'); TVtype = opts.TVtype; end
    if isfield(opts,'idisp'); idisp = opts.idisp; end
    if isfield(opts,'recordf'); recordf = opts.recordf; end
    if isfield(opts,'U0'); U0 = opts.U0; end
    if isfield(opts,'wbarflag'); wbarflag = opts.wbarflag; end
    if isfield(opts,'stc'); stc = opts.stc; end
end


function d = cohens_d_2sample(input_values,binary_outcome)
% d = cohens_d_2sample(input_values,binary_outcome)
%
% This is for the two-sample case
% By Phil Kragel
%
% input_values: Continuous input values to test
% binary_outcome: Logical vector of 1/0 values for group A/B assignment

n1 = sum(~isnan(input_values(binary_outcome)) & ~isinf(input_values(binary_outcome)));
n0 = sum(~isnan(input_values(~binary_outcome)) & ~isinf(input_values(~binary_outcome)));

meanpres = mean(input_values(binary_outcome));
meanabs = mean(input_values(~binary_outcome));

v1 = var(input_values(binary_outcome));
v0 = var(input_values(~binary_outcome));

pooledsd = sqrt((v1.*(n1-1) + v0.*(n0-1)) ./ (n1 + n0 - 2));

d = (meanpres - meanabs) ./ pooledsd;

end


function [sos,g] = driving_function_imp_nfchoa_ls(N,R,r,conf)
%DRIVING_FUNCTION_IMP_NFCHOA_LS second-order section representation for a
%line source in NFC-HOA
%
%   Usage: sos = driving_function_imp_nfchoa_ls(N,R,r,conf)
%
%   Input parameters:
%       N       - order of spherical Hankel function
%       R       - radius of secondary source array / m
%       r       - distance of line source from array center / m
%       conf    - configuration struct (see SFS_config)
%
%   Output parameters:
%       sos     - second-order section representation
%       g       - scalar gain factor
%
%   See also: sound_field_imp, sound_field_imp_nfchoa,
%       driving_function_imp_nfchoa

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% 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.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 4;
nargmax = 4;
narginchk(nargmin,nargmax);
isargpositivescalar(N,R,r);
isargstruct(conf);

%% ===== Configuration ==================================================
% Speed of sound
dimension = conf.dimension;
driving_functions = conf.driving_functions;

%% ===== Computation =====================================================
% Find spherical Hankel function zeros
z = sphbesselh_zeros(N);

% Get the delay and weighting factors
if strcmp('2D',dimension)

    % === 2-Dimensional ==================================================

    switch driving_functions
    case 'default'
        % --- SFS Toolbox ------------------------------------------------
        to_be_implemented;
    otherwise
        error(['%s: %s, this type of driving function is not implemented', ...
            'for a 2D line source.'],upper(mfilename),driving_functions);
    end

elseif strcmp('2.5D',dimension)

    % === 2.5-Dimensional ================================================

    switch driving_functions
    case 'default'
        % --- SFS Toolbox ------------------------------------------------
        to_be_implemented;
    otherwise
        error(['%s: %s, this type of driving function is not implemented', ...
            'for a 2.5D line source.'],upper(mfilename),driving_functions);
    end

elseif strcmp('3D',dimension)

    % === 3-Dimensional ==================================================

    switch driving_functions
    case 'default'
        % --- SFS Toolbox ------------------------------------------------
        to_be_implemented;
    otherwise
        error(['%s: %s, this type of driving function is not implemented', ...
            'for a 3D line source.'],upper(mfilename),driving_functions);
    end

else
    error('%s: the dimension %s is unknown.',upper(mfilename),dimension);
end


function [germs, germPaths] = centroidalVoronoi2d(germs, poly, varargin)
%CENTROIDALVORONOI2D Centroidal Voronoi tesselation within a polygon
%
%   PTS = centroidalVoronoi2d(NPTS, POLY)
%   Generate points in a polygon based on centroidal voronoi tesselation.
%   Centroidal germs can be computed by using the Llyod's algorithm:
%   1) initial germs are chosen at random within polygon
%   2) voronoi polygon of the germs is computed
%   3) the centroids of each domain are computed, and used as germs of the
%   next iteration
%
%   [PTS, PATHLIST] = centroidalVoronoi2d(NPTS, POLY)
%   Also returns the path of each germs at each iteration. The result
%   PATHLIST is a cell array with as many cells as the number of germs,
%   containing in each cell the successive positions of the germ.
%
%   PTS = centroidalVoronoi2d(.., PARAM, VALUE)
%   Specify one or several optional arguments. PARAM can be one of:
%   * 'nIter'   specifies the number of iterations of the algorithm
%       (default is 50)
%   * 'verbose' display iteration number. Default is false.
%
%   Example
%     poly = ellipseToPolygon([50 50 40 30 20], 200);
%     nGerms = 100;
%     germs = centroidalVoronoi2d(nGerms, poly);
%     figure; hold on;
%     drawPolygon(poly, 'k');
%     drawPoint(germs, 'bo');
%     axis equal; axis([0 100 10 90]);
%     % extract regions of the CVD
%     box = polygonBounds(poly);
%     [n, e] = boundedVoronoi2d(box, germs);
%     [n2, e2] = clipGraphPolygon(n, e, poly);
%     drawGraphEdges(n2, e2, 'b');
%
%   See also
%   graphs, boundedVoronoi2d, centroidalVoronoi2d_MC
%
%   Rewritten from programs found in
%   http://people.scs.fsu.edu/~burkardt/m_src/cvt/cvt.html
%
%   Reference:
%    Qiang Du, Vance Faber, and Max Gunzburger,
%    Centroidal Voronoi Tessellations: Applications and Algorithms,
%    SIAM Review, Volume 41, 1999, pages 637-676.
%

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

%% Parse input arguments

% Number of germs
if isscalar(germs)
    nGerms = germs;
    germs = [];
else
    nGerms = size(germs, 1);
end

% Number of iterations
nIter = 50;

verbose = false;

keepPaths = nargout > 1;

while length(varargin) > 1
    paramName = varargin{1};
    switch lower(paramName)
        case 'verbose'
            verbose = varargin{2};
        case 'niter'
            nIter = varargin{2};

        otherwise
            error(['Unknown parameter name: ' paramName]);
    end

    varargin(1:2) = [];
end

%% Initialisations

% bounding box of polygon
bbox = polygonBounds(poly);

% init germs if needed
if isempty(germs)
    germs = generatePointsInPoly(nGerms);
end
germIters = cell(nIter, 1);

%% Iteration of the Lloyd algorithm

for i = 1:nIter
     if verbose
        disp(sprintf('Iteration: %d/%d', i, nIter)); %#ok<DSPS>
    end

    if keepPaths
        germIters{i} = germs;
    end

    % Compute Clipped Voronoi diagram of germs
    if verbose
        disp('  compute Voronoi Diagram');
    end
    [n, e, f] = boundedVoronoi2d(bbox, germs);
    [n2, e2, f2] = clipMesh2dPolygon(n, e, f, poly); %#ok<ASGLU>

    % update the position of each germ
    if verbose
        disp('  compute centroids');
    end
    for iGerm = 1:nGerms
        polygon = n2(f2{iGerm}, :);
        germs(iGerm,:) = polygonCentroid(polygon);
    end

end

%% Evenutally compute germs trajectories

if nargout > 1
    % init
    germPaths = cell(nGerms, 1);
    path = zeros(nIter+1, 2);

    % Iteration on germs
    for i = 1:nGerms

        % create path corresponding to germ
        for j = 1:nIter
            pts = germIters{j};
            path(j,:) = pts(i,:);
        end
        path(nIter+1, :) = germs(i,:);

        germPaths{i} = path;
    end
end

function pts = generatePointsInPoly(nPts)
    % extreme coordinates
    xmin = bbox(1);  xmax = bbox(2);
    ymin = bbox(3);  ymax = bbox(4);

    % compute size of box
    dx = xmax - xmin;
    dy = ymax - ymin;

    % allocate memory for result
    pts = zeros(nPts, 2);

    % iterate until all points have been sampled within the polygon
    ind = (1:nPts)';
    while ~isempty(ind)
        NI = length(ind);
        x = rand(NI, 1) * dx + xmin;
        y = rand(NI, 1) * dy + ymin;
        pts(ind, :) = [x y];

        ind = ind(~polygonContains(poly, pts(ind, :)));
    end
end

end


function [W,H] = nmf(X,K,alg,maxiter,speak)
%
% NMF wrapper function
% function [W,H] = nmf(X,K,alg[,maxiter,speak])
%
% INPUT:
%           'X'     Inputmatrix
%           'K'     Number of components
%           'alg'   Algorithm to use:
%                   'mm'     multiplicative updates using euclidean
%                            distance. Lee, D..D., and Seung, H.S., (2001)
%                   'cjlin'  alternative non-negative least squares using
%                            projected gradients, author: Chih-Jen Lin,
%                            National Taiwan University.
%                   'prob'   probabilistic NFM interpretating X as samples
%                            from a multinomial, author: Lars Kai Hansen,
%                            Technical University of Denmark
%                   'als'    Alternating Least Squares. Set negative
%                            elements to zero.
%                   'alsobs' Alternating Least Squares. Set negative elements
%                            to zero and adjusts the other elements acording
%                            to Optimal Brain Surgeon.
%           'maxiter'   Maximum number of iterations, default = 1000.
%           'speak'     Print information to screen unless speak = 0,
%                       default = 0
%
% OUTPUT:
% W       : N x K matrix
% H       : K x M matrix
%
% Kasper Winther Joergensen
% Informatics and Mathematical Modelling
% Technical University of Denmark
% kwj@imm.dtu.dk
% 2006/12/15

switch(nargin)
    case {0,1,2}
        error('Missing parameter. Type "help nmf" for usage.');
        return
    case 3
        maxiter = 1000;
        speak = 0;
    case 4
        speak = 0;
    case 5
        % empty
    otherwise
        error('Too many parameters. Type "help nmf" for usage.');
        return
end

% find dimensionallity of X
[D,N] = size(X);

% switch algorithm
switch lower(alg)
    case 'mm'
        if speak, disp('Using mm algorithm'),end
        [W,H]=nmf_mm(X,K,maxiter,speak);
    case 'prob'
        if speak, disp('Using prob algorithm'),end
        [W,H]=nmf_prob(X,K,maxiter,speak);
    case 'cjlin'
        if speak, disp('Using cjlin algorithm'),end
        [W,H]=nmf_cjlin(X,rand(D,K),rand(K,N),0.000001,10000,maxiter);
    case 'als'
        if speak, disp('Using als algorithm'),end
        [W,H]=nmf_als(X,K,maxiter,speak);
    case 'alsobs'
        if speak, disp('Using alsobs algorithm'),end
        [W,H]=nmf_alsobs(X,K,maxiter,speak);
    otherwise
        error('Unknown method. Type "help nmf" for usage.');
        return
end

[W,H,nrgy] = order_comp(W,H);


%PLOTSOM Plot the Self-Organizing Map in 2D
%
%    PRPLOTSOM(W)
%
% Plot the Self-Organizing Map W, trained by som.m. This is only
% possible if the map is 2D.
%
% SEE ALSO (PRTools Guide)
% SOM

% Copyright: D.M.J. Tax, davidt@ph.tn.tudelft.nl
% Faculty of Applied Physics, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

% Maybe I should introduce the possibility to set the linewidth and
% markersize...
%
% Changes:
% DR1 - Dick de Ridder, 02-03-2006:
%       If the mapped is trained on 3D data, plot it like this.

function h = prplotsom(W)

if ~ismapping(W)
	error('SOM mapping expected');
end
if ( ~strcmp(getmapping_file(W),'som') & ...
	 ~strcmp(getmapping_file(W),'som_dd') )
    error('I expect a SOM mapping!');
end
if size(W.data.neurons,2)~=2
	error('The SOM can only be plotted in 2D');
end

% Get the data:
W = +W;  w=W.neurons; k=W.k;
% Plot the bloody thing:
hold on;

% DR: Handle maps trained on 3D data.

if (size(w,2) == 3)
	% The 'horizontal' lines:
	for i=0:k(2)-1
			h=plot3(w(i*k(1)+(1:k(1)),1),w(i*k(1)+(1:k(1)),2),w(i*k(1)+(1:k(1)),3),'o-');
			set(h,'linewidth',2,'markersize',8);
	end
	I = reshape(1:k(1)*k(2),k(1),k(2))';
	% The 'vertical' lines:
	for i=0:k(1)-1
			h=plot3(w(I(i*k(2)+(1:k(2))),1),w(I(i*k(2)+(1:k(2))),2),w(I(i*k(2)+(1:k(2))),3),'o-');
			set(h,'linewidth',2,'markersize',8);
	end
	view(3);
else
	% The 'horizontal' lines:
	for i=0:k(2)-1
			h=plot(w(i*k(1)+(1:k(1)),1),w(i*k(1)+(1:k(1)),2),'o-');
			set(h,'linewidth',2,'markersize',8);
	end
	I = reshape(1:k(1)*k(2),k(1),k(2))';
	% The 'vertical' lines:
	for i=0:k(1)-1
			h=plot(w(I(i*k(2)+(1:k(2))),1),w(I(i*k(2)+(1:k(2))),2),'o-');
			set(h,'linewidth',2,'markersize',8);
	end
end;

% Return the handle only when it is required
if (nargout==0)
	clear h;
end

return


function y = tmat_mxp2 ( a, n, x )

%*****************************************************************************80
%
%% TMAT_MXP2 multiplies a geometric transformation matrix times N points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Foley, van Dam, Feiner, Hughes,
%    Computer Graphics, Principles and Practice,
%    Addison Wesley, Second Edition, 1990.
%
%  Parameters:
%
%    Input, real A(4,4), the geometric transformation matrix.
%
%    Input, integer N, the number of points to be multiplied.
%
%    Input, real X(3,N), the points to be multiplied.
%
%    Output, real Y(3,N), the transformed points.  Each product is
%    accumulated in a temporary vector, and then assigned to the
%    result.  Therefore, it is legal for X and Y to share memory.
%
  for k = 1 : n
    y(1:3,k) = a(1:3,4) + a(1:3,1:3) * x(1:3,k);
  end

  return
end


function [model, B, elapse] = LSH_learn(A, maxbits)
%   This is a function of LSH (Locality Sensitive Hashing) learning.
%
%	Usage:
%	[model, B,elapse] = LSH_learn(A, maxbits)
%
%	      A: Rows of vectors of data points. Each row is sample point
%   maxbits: Code length
%
%     model: Used for encoding a test sample point.
%	      B: The binary code of the input data A. Each row is sample point
%    elapse: The coding time (training time).
%
%
%
%   Reference:
%
%   Moses S. Charikar: Similarity estimation techniques from rounding
%   algorithms. Proceedings of the thiry-fourth annual ACM symposium on
%   Theory of computing, 2002.
%
%
%   version 2.0 --Nov/2016
%   version 1.0 --Jan/2010
%
%   Written by  Yue Lin (linyue29@gmail.com)
%               Deng Cai (dengcai AT gmail DOT com)
%

tmp_T = tic;

[~,Nfeatures] = size(A);
k = maxbits;

U = normrnd(0, 1, Nfeatures, k);
Z = A * U;

B = (Z > 0);
model.U = U;

elapse = toc(tmp_T);
end


%% Misorientation Distribution Function
% Explains how to compute and analyze misorientation distribution
% functions.
%
%% TODO
% Please help to redo the section
%
%%
% When speaking about the misorientation distribution function (MDF) one
% has to differentiate to cases
%
% # the boundary (correlated) misorientation distribution function
% # the uncorelated misorientation distribution function
%
% While the first one considers only misorientations at grain boundaries
% the second one considers misorietation between arbitrary crystal
% orientations. To illustrate the difference lets consider the following
% EBSD data set

% Lets import some EBSD data and reconstruct the grains.

mtexdata forsterite
grains = calcGrains(ebsd)

%% The boundary misorientation distribution function
%
% In order to compute the boundary misorientation distribution function for
% the phase transition from Forsterite to Enstatite we first extract the
% misorientations along all Forsterite to Enstatite boundary segements

mori_boundary = grains.boundary('Fo','En').misorientation

%%
% and second compute the corresponding density function using the command
% <orientation.calcDensity.html calcDensity>

mdf_boundary = calcDensity(mori_boundary,'halfwidth',5*degree)

%%

adf_boundary = mdf_boundary.calcAxisDistribution

plot(adf_boundary)

%%

%%
% The misorientation distribution function can be processed as any other
% ODF. E.g. we can compute the prefered misorientation via

[v,mori] = max(mdf_boundary)

%%
% or plot the pole figure corresponding to the crystal axis (1,0,0)

plotPDF(mdf_boundary,Miller(1,0,0,ebsd('Fo').CS))

%% The uncorrelated misorientation distribution function
%
% Alternatively the uncorrelated misorientation distribution function can be
% computed by providing the option *uncorrelated*

mori = calcMisorientation(ebsd('En'),ebsd('Fo'))
mdf_uncor = calcDensity(mori)

%%
% Obviously it is different from the boundary misorientation distribution
% function.

plotPDF(mdf_uncor,Miller(1,0,0,ebsd('Fo').CS))

%% Computing the uncorrelated misorientation function from two ODFs
%
% Let given two odfs

odf_fo = calcDensity(ebsd('fo').orientations,'halfwidth',10*degree)
odf_en = calcDensity(ebsd('en').orientations,'halfwidth',10*degree)

%%
% Then the uncorrelated misorientation function between these two ODFs can
% be computed by

mdf = calcMDF(odf_en,odf_fo)

%%
% This misorientation distribution function should be similar to the
% uncorrelated misorientation function computed directly from the ebsd data

plotPDF(mdf,Miller(1,0,0,ebsd('Fo').CS))

%% Analyzing misorientation functions
%
%

%% SUB: Angle distribution
%
% Let us first compare the actual angle distribution of the boundary
% misorientations with the theoretical angle distribution of the
% uncorrelated MDF.

close all
plotAngleDistribution(grains.boundary('fo','en').misorientation)

hold on

plotAngleDistribution(mdf)

hold off

%%
% For computing the exact values see the commands
% <SO3Fun.calcAngleDistribution.html calcAngleDistribution(mdf)> and
% <orientation.calcAngleDistribution.html calcAngleDistribution(ori)>.

%% SUB: Axis distribution
%
% The same we can do with the axis distribution. First the actual angle distribution of the boundary
% misorientations

plotAxisDistribution(grains.boundary('fo','en').misorientation,'smooth')

%%
% Now the theoretical axis distribution of the
% uncorrelated MDF.

plotAxisDistribution(mdf)

%%
% For computing the exact values see the commands
% <SO3Fun.calcAxisDistribution.html calcAxisDistribution(mdf)> and
% <orientation.calcAxisDistribution.html calcAxisDistribution(grains)>.

aD = calcDensity(axis(grains.boundary('fo','en').misorientation))


function h = complex(f, g)
%COMPLEX   Construct complex CHEBFUN3 from real and imaginary parts.
%   H = COMPLEX(F, G) returns the complex CHEBFUN3 F + i G, where F and G
%   are real valued CHEBFUN3 objects with the same domain.
%
% See also CHEBFUN3/IMAG, CHEBFUN3/CONJ, CHEBFUN3/ABS and CHEBFUN3/REAL.

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

if ( ~isreal(f) || ~isreal(g) )
    error('CHEBFUN:CHEBFUN3:complex:notReal1', ...
        'Inputs must be real.');
end
h = f + 1i*g;

end


% 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 [value] = BinarySABR_3(f, k, t, a, b, r, n)
% the value of a digital option within a SABR model
    epsilon = 1e-004;
    kp = k + epsilon;
    km = k - epsilon;
    sigmap = svol_2(a,b,r,n,f,kp,t);
    sigmam = svol_2(a,b,r,n,f,km,t);
    d1p = 1./(sigmap .* sqrt(t)).*(log(f./kp)+0.5*sigmap.^2*t);
    d2p = 1./(sigmap .* sqrt(t)).*(log(f./kp)-0.5*sigmap.^2*t);
    d1m = 1./(sigmam .* sqrt(t)).*(log(f./km)+0.5*sigmam.^2*t);
    d2m = 1./(sigmam .* sqrt(t)).*(log(f./km)-0.5*sigmam.^2*t);
    pp = f * normcdf(d1p) - kp .* normcdf(d2p);
    pm = f * normcdf(d1m) - km.* normcdf(d2m);
    value = (pm-pp) / (2*epsilon);
end


function  peri = perimeter(grains,varargin)
% calculates the perimeter of a grain with or without inclusions
%
% Syntax
%
%   grains.perimeter
%   perimter(grains)
%   perimter(grains,'withInclusions')
%
% Input
%  grains - @grain2d
%
% Output
%  peri - perimeter (in measurement units)
%
% See also
% grain2d/equivalentPerimeter

if check_option(varargin,'withInclusion')

  bnd = grains.boundary;
  grainId = bnd.grainId;
  segLength = bnd.segLength;

  isGrain1 = grainId(:,1)>0;
  isGrain2 = grainId(:,2)>0;
  peri = accumarray(grainId(isGrain1,1),segLength(isGrain1),[max(grainId(:)),1]) ...
    + accumarray(grainId(isGrain2,2),segLength(isGrain2),[max(grainId(:)),1]);

  peri = peri(grains.id);

else

  poly = grains.poly;

  % remove inclusions
  incl = grains.inclusionId;
  for i = find(incl>0).'
    poly{i} = poly{i}(1:end-incl(i));
  end

  V = grains.V;

  peri =  cellfun(@(ind) sum(sqrt(sum(diff(V(ind,:)).^2,2))),poly);

end


% Reeds Shepp path planner sample code
%
% based on python code from Python Robotics by Atsushi Sakai(@Atsushi_twi)
%
% Peter 3/18
%
% Finds the shortest path between 2 configurations:
% - robot can move forward or backward
% - the robot turns at zero or maximum curvature
% - there are discontinuities in velocity and steering commands (cusps)
% to see what it does run
%
% >> ReedsShepp.test
%
% References::
% - Reeds, J. A.; Shepp, L. A.
%   Optimal paths for a car that goes both forwards and backwards.
%   Pacific J. Math. 145 (1990), no. 2, 367--393.
%   https://projecteuclid.org/euclid.pjm/1102645450

% each path is described by a 3-letter word.
% the algorithm finds a bunch of possible paths, then chooses the shortest
% one.  Each word is represented by a structure with fields:
% - word      a 3-letter sequence drawn from the letters LRLS
% - L         total path length
% - lengths   a 3-vector of lengths, signed to indicate the direction of
%             curvature
% - traj      a cell array of 3xN matrices giving the path for each segment
% - dir       the direction of travel: +1 or -1
%
% TODO: display all the solutions in one figure, as subplots

classdef ReedsShepp < handle
    properties
        best  % the best path
        words
        maxc
    end

    methods
        function obj = ReedsShepp(q0, qf, maxcurv, dl)

            obj.maxc = maxcurv;

            % return the word describing the shortest path
            obj.words = generate_path(q0, qf, maxcurv);

            if isempty(obj.words)
                error('no path');
            end

            % find shortest path
            [~,k] = min( [obj.words.L] );

            obj.best = obj.words(k);

            % add the trajectory
            obj.best = generate_trajectories(obj.best, maxcurv, dl, q0);
        end

        function p = path(obj)
            p = [obj.best.traj{:}]';
        end

        function show(obj)
            for w=obj.words
                fprintf('%s (%g): [%g %g %g]\n', w.word, w.L, w.lengths);
            end
        end

        function plot(obj, varargin)

            opt.circles = [];
            opt.join = [];

            opt = tb_optparse(opt, varargin);

            if ~ishold
                clf
            end
            hold on
            word = obj.best;

            for i=1:3

                if word.dir(i) > 0
                    color = 'b';
                else
                    color = 'r';
                end
                if i == 1
                    x = word.traj{i}(1,:);
                    y = word.traj{i}(2,:);
                else
                    % ensure we join up the lines in the plot
                    x = [x(end) word.traj{i}(1,:)];
                    y = [y(end) word.traj{i}(2,:)];
                end

                if ~isempty(opt.join) && i<3
                    plot(x(end), y(end), opt.join{:});
                end
                if ~isempty(opt.circles)
                    T = SE2(word.traj{i}(:,1));
                    R = 1/obj.maxc;
                    c = T*[0; word.dir(i)*R];

                    plot_circle(c, R, opt.circles)
                    plot_point(c, 'k+')
                end

                plot(x, y, color, 'LineWidth', 2);
            end
            grid on; xlabel('X'); ylabel('Y')
            hold off
            axis equal
            title('Reeds-Shepp path');
        end

        function s = char(obj)
            s = '';
            s = strvcat(s, sprintf('Reeds-Shepp path:  %s, length %f', obj.best.word, obj.best.L));
            s = strvcat(s, sprintf(' segment lengths:  %f %f %f', obj.best.lengths));
        end

        function display(obj)
            disp( char(obj) );
        end
    end

    methods(Static)
        function test()
            maxcurv = 1;
            dl = 0.05;
            q0 = [0 0 pi/4]'; qf = [0 0 pi]';
            p = ReedsShepp(q0, qf, maxcurv, dl)

            p.plot('circles', 'k--', 'join', {'Marker', 'o', 'MarkerFaceColor', 'k'});
        end
    end
end % class ReedsShepp

function out = generate_trajectories(word, maxc, d, q0)

    % initialize the configuration
    p0 = q0;

    % output struct is same as input struct, but we will add:
    %  - a cell array of trajectories
    %  - a vector of directions -1 or +1
    out = word;

    for i=1:3
        m = word.word(i);
        l = word.lengths(i);

        x = [0:d:abs(l) abs(l)];

        p = pathseg(x, sign(l), m, maxc, p0);

        % add new fields to the struct

        if i == 1
            out.traj{i} = p;
        else
            % for subsequent segments skip the first point, same as last
            % point of previous segment
            out.traj{i} = p(:,2:end);
        end
        out.dir(i) = sign(l);

        % initial state for next segment is last state of this segment
        p0 = p(:,end);
    end
end

function q = pathseg(l, dir, m, maxc, p0)
    q0 = p0(:);
    switch m
        case 'S'
            f = @(t,q) dir*[cos(q(3)), sin(q(3)), 0]';
        case {'L', 'R'}
            f = @(t,q) dir*[cos(q(3)), sin(q(3)), dir*maxc]';
    end
    [t,q] = ode45(f, l, q0);
    q = q';  % points are column vectors
end

function words = generate_path(q0, q1, maxc)
    % return a list of all possible words
    q0 = q0(:); q1 = q1(:);
    dq = q1 - q0;
    dth = dq(3);

    xy = rot2(q0(3))' * dq(1:2) * maxc;
    x = xy(1); y = xy(2);

    words = [];
    words = SCS(x, y, dth, words);
    words = CSC(x, y, dth, words);
    words = CCC(x, y, dth, words);

    % account for non-unit curvature
    for i=1:numel(words)
        words(i).lengths = words(i).lengths / maxc;
        words(i).L = words(i).L / maxc;
    end

end

%%
function owords = SCS(x, y, phi, words)

    words = SLS([ x  y  phi], 1, 'SLS', words);
    words = SLS([ x -y -phi], 1, 'SRS', words);

    owords = words;
end

function owords = CCC(x, y, phi, words)

    words = LRL([ x  y  phi],  1, 'LRL', words);
    words = LRL([-x  y -phi], -1, 'LRL', words);
    words = LRL([ x -y -phi],  1, 'RLR', words);
    words = LRL([-x -y  phi], -1, 'RLR', words);

    % backwards
    xb = x * cos(phi) + y * sin(phi);
    yb = x * sin(phi) - y * cos(phi);

    flip = [0 1 0; 1 0 0; 0 0 1];  % flip u and v

    words = LRL([ xb  yb  phi],  flip, 'LRL', words);
    words = LRL([-xb  yb -phi], -flip, 'LRL', words);
    words = LRL([ xb -yb -phi],  flip, 'RLR', words);
    words = LRL([-xb -yb  phi], -flip, 'RLR', words);

    owords = words;
end

function owords = CSC(x, y, phi, words)

    words = LSL([ x  y  phi],  1, 'LSL', words);
    words = LSL([-x  y -phi], -1, 'LSL', words);
    words = LSL([ x -y -phi],  1, 'RSR', words);
    words = LSL([-x -y  phi], -1, 'RSR', words);
    words = LSR([ x  y  phi],  1, 'LSR', words);
    words = LSR([-x  y -phi], -1, 'LSR', words);
    words = LSR([ x -y -phi],  1, 'RSL', words);
    words = LSR([-x -y  phi], -1, 'RSL', words);

    owords = words;
end

% requires LSL, LSR, SLS, LRL

%%

function owords = SLS(q, sign, word, words)
    x = q(1); y = q(2); phi = mod(q(3), 2*pi);

    if y > 0.0 && phi > 0.0 && phi < pi * 0.99
        xd = - y / tan(phi) + x;
        t = xd - tan(phi / 2.0);
        u = phi;
        v = norm( [(x - xd) y]) - tan(phi / 2.0);
        owords = addpath(words, sign*[t, u, v], word);
    elseif y < 0.0 && phi > 0.0 && phi < pi * 0.99
        xd = - y / tan(phi) + x;
        t = xd - tan(phi / 2.0);
        u = phi;
        v = -norm([(x - xd) y]) - tan(phi / 2.0);
        owords = addpath(words, sign*[t, u, v], word);
    else
        owords = words;
    end
end

function owords = LSL(q, sign, word, words)
    x = q(1); y = q(2); phi = mod(q(3), 2*pi);

    [t,u] = cart2pol(x - sin(phi), y - 1.0 + cos(phi));
    if t >= 0.0
        v = angdiff(phi - t);
        if v >= 0.0
            owords = addpath(words, sign*[t, u, v], word);
            return
        end
    end

    owords = words;
end

function owords = LRL(q, sign, word, words)
    x = q(1); y = q(2); phi = mod(q(3), 2*pi);

    [t1,u1] = cart2pol(x - sin(phi), y - 1.0 + cos(phi));

    if u1 <= 4.0
        u = -2.0 * asin(0.25 * u1);
        t = angdiff(t1 + 0.5 * u + pi);
        v = angdiff(phi - t + u);

        if t >= 0.0 && u <= 0.0
            owords = addpath(words, [t, u, v]*sign, word);
            return
        end
    end

    owords = words;
end

function owords = LSR(q, sign, word, words)
    x = q(1); y = q(2); phi = mod(q(3), 2*pi);

    [t1,u1] = cart2pol(x + sin(phi), y - 1.0 - cos(phi));
    u1 = u1^2;
    if u1 >= 4.0
        u = sqrt(u1 - 4.0);
        theta = atan2(2.0, u);
        t = angdiff(t1 + theta);
        v = angdiff(t - phi);

        if t >= 0.0 && v >= 0.0
            owords = addpath(words, sign*[t, u, v], word);
            return
        end
    end

    owords = words;
end

%%
function owords = addpath(words, lengths, ctypes)

    % create a struct to represent this segment
    word.word = ctypes;
    word.lengths = lengths;

    % check same path exist
    for p = words
        if strcmp(p.word, word.word)
            if sum(p.lengths) - sum(word.lengths) <= 0.01
                owords = words;
                return %not insert path
            end
        end
    end

    word.L = sum(abs(lengths));

    % long enough to add?
    if word.L >= 0.01
        owords = [words word];
    end
end


% DEMCMU35GPLVMVARGPLVMSIMPLE Run variational GPLVM with dynamics on a single
% sequence of the CMU35 data.

% VARGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

% Define constants
experimentNo = 1000;
latentDim = 9; % this is Q, the number of latent dimensions
indPoints = 100; % number of inducing points
% dynamicKern = {'matern32', 'bias', 'white'};
dynamicKern = {'rbf', 'bias', 'white'}; % kernel k_t for the GP for x(t)
initX ='ppca'; % initialize latent space with ppca

% load data
dataSetName = 'cmu35gplvm';
[Ytmp, lbls, Y, lblstest] = lvmLoadData(dataSetName);

% Set training and test sets
indicesTraining = [1:40 61:size(Y,1)];
indicesTest = setdiff(1:size(Y,1),indicesTraining);
Ytr = Y(indicesTraining,:);
Ytest = Y(indicesTest,:);

% Corresponding timestamps (artificial and equally spaced for this demo)
t = linspace(0, 2*pi, size(Y, 1)+1)';
t = t(1:end-1, 1);
timeStampsTraining = t(indicesTraining);
timeStampsTest = t(indicesTest);

Y = Ytr; clear('Ytr','Ytmp');

% Set up model
options = vargplvmOptions('dtcvar');
options.kern = {'rbfard2', 'bias', 'white'}; % Kernel k_x for the GP for f(x)
options.numActive = indPoints;
options.optimiser = 'scg';

d = size(Y, 2);
fprintf(1,'# Creating the model...\n');
model = vargplvmCreate(latentDim, d, Y, options);
model = vargplvmParamInit(model, model.m, model.X);
model.beta=1/(0.01*var(model.m(:)));
model.vardist.covars = 0.5*ones(size(model.vardist.covars)) + 0.001*randn(size(model.vardist.covars));

%-------- Add dynamics to the model -----
optionsDyn.type = 'vargpTime';
optionsDyn.t=timeStampsTraining;
optionsDyn.inverseWidth=30;
optionsDyn.initX = initX;

% Dynamic kernel:
kern = kernCreate(t, dynamicKern);
% The following is related to the expected number of
% zero-crossings.(larger inv.width numerator, rougher func)
if ~strcmp(kern.comp{1}.type,'ou')
    kern.comp{1}.inverseWidth = optionsDyn.inverseWidth./(((max(t)-min(t))).^2);
    kern.comp{1}.variance = 1;
end
optionsDyn.kern = kern;

% Fill in with default values whatever is not already set
optionsDyn = vargplvmOptionsDyn(optionsDyn);
model = vargplvmAddDynamics(model, 'vargpTime', optionsDyn, optionsDyn.t, 0, 0,optionsDyn.seq);

fprintf(1,'# Further calibration of the initial parameters...\n');
model = vargplvmInitDynamics(model,optionsDyn);
model.vardist.parallel=1;
% do not learn beta for few iterations for intitilization
model.learnBeta = 0;
display = 1;
fprintf(1,'# Intitiliazing the model (fixed beta) %d iterations...\n',100);
model = vargplvmOptimise(model, display, 100);
disp('# Saving model after optimising beta...')
modelWriteResult(model, dataSetName, experimentNo);

% Optimise the model.
model.learnBeta = 1;
iters = 1000; % Default: 1000
fprintf(1,'# Optimising the model for %d iterations...\n',iters);
model = vargplvmOptimise(model, display, iters);
% Save the results.
fprintf(1,'# Saving model after doing %d iterations\n',iters)
modelWriteResult(model, dataSetName, experimentNo);

% See the final lengthscales (see how some dimensions are switched-off).
 bar(model.kern.comp{1}.inputScales)

%% ----------------- PREDICTIONS -----------------------------
fprintf('# Only times prediction...\n');
% Prediction using the only information in the test time points
[Testmeans2 Testcovars2] = vargplvmPredictPoint(model.dynamics, timeStampsTest);
Varmu2 = vargplvmPosteriorMeanVar(model, Testmeans2, Testcovars2);

errorPredictions = mean(abs(Varmu2(:) - Ytest(:)));
fprintf(1,'# Predictions Error:%d\n', errorPredictions);

plot(Varmu2(1,:),'r'),hold on, plot(Ytest(1,:),'g')

skel = acclaimReadSkel('35.asf');
[tmpchan, skel] = acclaimLoadChannels('35_01.amc', skel);
channels{1} = demCmu35VargplvmLoadChannels(Ytest,skel);
channels{2} = demCmu35VargplvmLoadChannels(Varmu2,skel);
skelPlayData2(skel, channels,1/15,{'Ytest','Varmu2'});


function Xform = inplane2VolXform(rot,trans,scaleFac)
%
% Xform = inplane2VolXform(rot,trans,scaleFac)
%
% Returns 4x4 homogeneous tranform that tranforms from inplane to
% volume.
%
% djh/gmb, '97
%
% Modification:
% - Flip first 2 rows and cols so that it deals with
% (y,x,z) coords instead of (x,y,z).  DJH, 7/98.

A=diag(scaleFac(2,:))*rot*diag(1./scaleFac(1,:));
b = (scaleFac(2,:).*trans)';

Xform = zeros(4,4);
Xform(1:3,1:3)=A;
Xform(1:3,4)=b;
Xform(4,4)=1;

Xform([1 2],:) = Xform([2 1],:);
Xform(:,[1 2]) = Xform(:,[2 1]);


%| denoise_threshold_test.m
%| Test denoising based on l_1 type penalty, cf thresholding
%| min_x 1/2 |y - x|^2 + \beta pot(x)
%|
%| Copyright 2005-4-22, Jeff Fessler, University of Michigan

yi = linspace(-10,10,101)';
mask = true(size(yi));
A = Gdiag(ones(size(mask))); % identity "matrix"

if 1
	f.l2b_q = 1;
%	f.l2b_n = 5;
	f.l2b_n = 1;
	f.cut = 5; % cutoff point
%	f.delta = 0.2;
	f.delta = f.cut / (1 + 2^f.l2b_n); % for broken parabola
	f.tik = (1 + 2^f.l2b_n) * f.delta;
	f.niter = 40;
%	f.type = 'cauchy';
	f.type = 'broken';

	Rq = Reg1(mask, 'type_denom', 'matlab', ...
		'offsets', 0, ... % trick for identity
		'beta', 2^f.l2b_q);
%		'pot_arg', {'quad'}, 'beta', 2^f.l2b_q);

	xq = pwls_sps_os(0*yi(:), yi(:), [], A, Rq, ...
			2, [-inf inf], [], [], 1);
end

if 1
	Rc = Reg1(mask, 'type_denom', 'matlab', ...
		'offsets', 0, ... % trick for identity
		'pot_arg', {f.type, f.delta}, 'beta', 2^f.l2b_n);

	xc = pwls_sps_os(0*yi(:), yi(:), [], A, Rc, ...
		f.niter, [-inf inf], [], [], 1);

	Rh = Reg1(mask, 'type_denom', 'matlab', ...
		'offsets', 0, ... % trick for identity
		'pot_arg', {'hyper3', f.delta}, 'beta', 2^f.l2b_n);

	xh = pwls_sps_os(yi(:), yi(:), [], A, Rh, ...
		f.niter, [-inf inf], [], [], 1);
end

if im
	clf, plot(yi, yi, ':', ...
		yi, xh(:,end), '-', ...
		yi, xc(:,end), '-.', ...
		yi, xq(:,end), '--')
%	axis equal
	axis square
	tik = [min(yi), -f.tik 0 f.tik max(yi)];
%%%%%%%%	xtick(tik), ytick(tik)
	grid
	ir_legend({'I', 'hyper', f.type, 'quad'})
end


function [p] = integrateSubIntervals(x, cdf)

[xLB, xUB] = subIntervals(x);

cdfUB = cdf(xUB);
cdfLB = cdf(xLB);

p = (cdfUB - cdfLB) ./ (xUB - xLB);

end


function indx = r8row_sort_heap_index_a ( m, n, a )

%*****************************************************************************80
%
%% R8ROW_SORT_HEAP_INDEX_A does an indexed heap ascending sort of an R8ROW.
%
%  Discussion:
%
%    An R8ROW is an M by N array of R8's, regarded as an array of M rows,
%    each of length N.
%
%    The sorting is not actually carried out.  Rather an index array is
%    created which defines the sorting.  This array may be used to sort
%    or index the array, or to sort or index related arrays keyed on the
%    original array.
%
%    A(I1,*) < A(I1,*) if the first nonzero entry of A(I1,*)-A(I2,*)
%    is negative.
%
%    Once the index array is computed, the sorting can be carried out
%    "implicitly:
%
%      A(INDX(1:M),1:N) is sorted.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 May 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in each column of A.
%
%    Input, integer N, the number of columns in A.
%
%    Input, real A(M,N), the array.
%
%    Output, integer INDX(M), the sort index.  The I-th element of the sorted
%    array is row INDX(I).
%
  indx(1:m) = 1 : m;

  if ( m <= 1 )
    return
  end

  l = floor ( m / 2 ) + 1;
  ir = m;

  while ( 1 )

    if ( 1 < l )

      l = l - 1;
      indxt = indx(l);
      row(1:n) = a(indxt,1:n);

    else

      indxt = indx(ir);
      row(1:n) = a(indxt,1:n);
      indx(ir) = indx(1);
      ir = ir - 1;

      if ( ir == 1 )
        indx(1) = indxt;
        break
      end

    end

    i = l;
    j = l + l;

    while ( j <= ir )

      if ( j < ir )

        if ( r8row_compare ( m, n, a, indx(j), indx(j+1) ) < 0 )
          j = j + 1;
        end

      end

      if ( r8vec_compare ( n, row, a(indx(j),1:n) ) < 0 )
        indx(i) = indx(j);
        i = j;
        j = j + j;
      else
        j = ir + 1;
      end

    end

    indx(i) = indxt;

  end

  return
end


function [ fm ] = gsp_modulate( G,f,k )
%GSP_MODULATE Generalized modulation of the signal f to the frequency k
%   Usage: fm = gsp_modulate( G,f,k );
%
%   Input parameters
%       G   : Graph
%       f   : Signal (column)
%       k   : Indices of frequencies (int)
%   Output parameters
%       fm  : Modulated signal
%
%   This function modulate the column vector *f* onto the node i. If f is a
%   matrix, the modulation will be applicated to each column.
%

% Author: Nathanael Perraudin
% Date  : 09.12.2013

nt = size(f,2);

fm = sqrt(G.N)*repmat(f,1,nt).*repmat(G.U(:,k+1),1,nt);

end


function determ = smoke_determinant ( n )

%*****************************************************************************80
%
%% SMOKE_DETERMINANT returns the determinant of the SMOKE matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real DETERM, the determinant.
%
  if ( mod ( n, 2 ) == 0 )
    determ =   2.0;
  else
    determ = - 2.0;
  end

  return
end


function yp = p25_fun ( neqn, t, y )

%*****************************************************************************80
%
%% P25_FUN evaluates the function for problem P25.
%
%  Discussion:
%
%    2 equations
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Wayne Enright, John Pryce,
%    Algorithm 648,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 1, pages 28-34.
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the derivative
%    function.
%
%    Output, real YP(NEQN), the value of the derivative function.
%
  yp = zeros ( neqn, 1 );

  yp(1) = y(2);
  yp(2) = sqrt ( 1.0 + y(2).^2 ) / ( 25.0 - t );

  return
end


function order = order_table ( rule )

%*****************************************************************************80
%
%% ORDER_TABLE returns the order of a Lebedev rule.
%
%  Modified:
%
%    13 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Vyacheslav Lebedev, Dmitri Laikov,
%    A quadrature formula for the sphere of the 131st
%    algebraic order of accuracy,
%    Russian Academy of Sciences Doklady Mathematics,
%    Volume 59, Number 3, 1999, pages 477-481.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule, between 1 and 65.
%
%    Output, integer ORDER, the order of the rule.
%
  rule_max = 65;

  table = [ ...
       6,   14,   26,   38,   50,   74,   86,  110,  146,  170, ...
     194,  230,  266,  302,  350,  386,  434,  482,  530,  590, ...
     650,  698,  770,  830,  890,  974, 1046, 1118, 1202, 1274, ...
    1358, 1454, 1538, 1622, 1730, 1814, 1910, 2030, 2126, 2222, ...
    2354, 2450, 2558, 2702, 2810, 2930, 3074, 3182, 3314, 3470, ...
    3590, 3722, 3890, 4010, 4154, 4334, 4466, 4610, 4802, 4934, ...
    5090, 5294, 5438, 5606, 5810 ]';

  if ( rule < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'ORDER_TABLE - Fatal error!\n' );
    fprintf ( 1, '  RULE < 1.\n' );
    error ( 'ORDER_TABLE - Fatal error!' );
  elseif ( rule_max < rule )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'ORDER_TABLE - Fatal error!\n' );
    fprintf ( 1, '  RULE_MAX < RULE.\n' );
    error ( 'ORDER_TABLE - Fatal error!' );
  end

  order = table(rule);

  return
end


function test_nearest

% MEM 3gb
% WALLTIME 00:10:00
% DEPENDENCY nearest

% Use as
%   [indx] = nearest(array, val, insideflag, toleranceflag)

% these are some normal cases
assert(nearest([1 2 3], 1)==1)
assert(nearest([1 2 3], 2)==2)
assert(nearest([1 2 3], 3)==3)
assert(nearest([1 2 3], -inf)==1)
assert(nearest([1 2 3],  inf)==3)

% unsorted arrays should be supported
assert(nearest([1 3 2], -inf)==1)
assert(nearest([3 1 2], -inf)==2)
assert(nearest([3 1 2], 1)==2)

% outside the range
try
  nearest([1 2 3], 0, true);
  error('this should have returned an error');
end
try
  nearest([1 2 3], 4, true);
  error('this should have returned an error');
end

% just inside the tolerance range
assert(nearest([1 2 3], 0.5, true, true)==1);
assert(nearest([1 2 3], 3.5, true, true)==3);

% just outside the tolerance range
try
  nearest([1 2 3], 0.499, true, true);
  error('this should have returned an error');
end
try
  nearest([1 2 3], 3.501, true, true);
  error('this should have returned an error');
end

% new functionality of 'val' being a [minval maxval] input pair

assert(all(nearest(.1:.1:1.0,[.1 .3])==[1 3]))
assert(all(nearest(.1:.1:1.0,[.11 .3])==[2 3]))
assert(all(nearest(.1:.1:1.0,[.1 .29])==[1 2]))
assert(all(nearest(.1:.1:1.0,[0 .3])==[1 3]))
assert(all(nearest(.1:.1:1.0,[.11 .29])==[2 2]))
assert(all(nearest(.1:.1:1.0,[-inf 1])==[1 10]))
assert(all(nearest(.1:.1:1.0,[.79 inf])==[8 10]))
assert(all(nearest(.1:.1:1.0,[.8 10])==[8 10]))
assert(all(nearest(.1:.1:1.0,[-2 8])==[1 10]))
assert(all(nearest(.1:.1:1.0,[.79 .99])==[8 9]))
assert(all(nearest(.001:.001:.01,[.002 .003])==[2 3]))
assert(all(nearest(.001:.001:.1,[0 1])==[1 100]))

try
  nearest(.1:.1:1.0,[3 8]);
catch me
  if ~strcmp(me.message,'The limits you selected are outside the range available in the data')
    error('wrong error message in nearest')
  end
end

% create a large array and test

x = 0:492:(492*(25*10^7));
y = 8424306*492;
indx = nearest(x,y);
indx2 = find(x<=y, 1, 'last');

% temporal comparison

tic
for i=1:10
  indx = nearest(x,y);
end
t1 = toc;

tic
for i=1:10
  x = sort(x);
  indx2 = find(x<=y, 1, 'last');
  indx3 = find(x>=y, 1, 'first');
  if abs(x(indx2)-y) < abs(x(indx3)-y)
    indx4 = indx2;
  else
    indx4 = indx3;
  end
end
t2 = toc;

fprintf('Time needed for nearest function: %.2s per 100 calls\n', t1/100);
fprintf('Time needed for find function: %.2s per 100 calls\n', t2/100);

if indx~=indx2,
  error('nearest does not output the correct value');
  fprintf('nearest is off by %d samples', indx2-indx);
end
% indx2 = 8424307


function [ a, b ] = p01_ab ( m )

%*****************************************************************************80
%
%% P01_AB returns bounds for problem 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Harald Niederreiter, Kevin McCurley,
%    Optimization of functions by quasi-random search methods,
%    Computing,
%    Volume 22, Number 2, 1979, pages 119-123.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Output, real A(M,1), B(M,1), lower and upper bounds.
%
  a(1:m,1) = 0.0;
  b(1:m,1) = 1.0;

  return
end


function [C, sigma] = dataset3Params(X, y, Xval, yval)
%DATASET3PARAMS returns your choice of C and sigma for Part 3 of the exercise
%where you select the optimal (C, sigma) learning parameters to use for SVM
%with RBF kernel
%   [C, sigma] = DATASET3PARAMS(X, y, Xval, yval) returns your choice of C and
%   sigma. You should complete this function to return the optimal C and
%   sigma based on a cross-validation set.
%

% You need to return the following variables correctly.
C = 1;
sigma = 0.3;

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return the optimal C and sigma
%               learning parameters found using the cross validation set.
%               You can use svmPredict to predict the labels on the cross
%               validation set. For example,
%                   predictions = svmPredict(model, Xval);
%               will return the predictions on the cross validation set.
%
%  Note: You can compute the prediction error using
%        mean(double(predictions ~= yval))
%

% C_vec = [0.01,0.03,0.1,0.3,1,3,10,30];
% sigma_vec = [0.01,0.03,0.1,0.3,1,3,10,30];
% error = zeros(8,8);
%
% for i=1:8
%     for j=1:8
%         model= svmTrain(X, y, C_vec(i), @(x1, x2) gaussianKernel(x1, x2, sigma_vec(j)));
%         predictions = svmPredict(model, Xval);
%         error(i,j) = mean(double(predictions ~= yval));
%     end
% end
%
% temp= min(min(error));
%
% [row, colume ] = find(error == temp)

C = 1;
sigma = 0.1;

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

end


function dt = EulerDT2D(Q, gamma)

% function dt = EulerDT2D(Q, gamma)
% purpose: compute the time step dt for the compressible Euler equations

Globals2D;

rho = Q(:,:,1); rhou = Q(:,:,2); rhov = Q(:,:,3); Ener = Q(:,:,4);
rho = rho(vmapM); rhou = rhou(vmapM); rhov = rhov(vmapM); Ener = Ener(vmapM);

u = rhou./rho; v = rhov./rho;
p = (gamma-1.0)*(Ener - rho.*(u.^2+v.^2)/2); c = sqrt(abs(gamma*p./rho));

dt = 1/max( ((N+1)^2)*.5*Fscale(:).*(sqrt ( u(:).^2 + v(:).^2 ) + c(:)));

rhoprange = [min(min(rho)), max(max(rho)), min(min(p)), max(max(p))]
return


function im_out = linear_stretch(im_in)
% The top 0.00001 pixels of the image are clipped to 1, and the
% image is linearly stretched.
img1 = max(im_in, [],3);   % maximum between RGB channels
img1 = img1(:);
img1 = sort(img1, 'descend');
idx = round(0.00001*length(img1));
scale = img1(idx);
im_out = im_in./scale;
im_out = min(im_out,1);


function [R,T,Xc,best_solution,opt]=efficient_pnp_gauss(x3d_h,x2d_h,A)

% EFFICIENT_PNP_GAUSS Main Function to solve the PnP problem
%       as described in:
%
%       Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua.
%       Accurate Non-Iterative O(n) Solution to the PnP Problem.
%       In Proceedings of ICCV, 2007.
%
%       Note: In this version of the software we perform a final
%       optimization using Gauss-Newton,which is not described in the
%       paper.
%
%       x3d_h: homogeneous coordinates of the points in world reference
%       x2d_h: homogeneous position of the points in the image plane
%       A: intrincic camera parameters
%       R: Rotation of the camera system wrt world reference
%       T: Translation of the camera system wrt world reference
%       Xc: Position of the points in the camera reference
%       best solution: dimension of the kernel for the best solution
%                     (before applying Gauss Newton).
%       opt: some parameters of the optimization process
%
% Copyright (C) <2007>  <Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the version 3 of the GNU General Public License
% as published by the Free Software Foundation.
%
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
% General Public License for more details.
% You should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.
%
% Francesc Moreno-Noguer, CVLab-EPFL, October 2007.
% fmorenoguer@gmail.com, http://cvlab.epfl.ch/~fmoreno/

Xw=x3d_h(:,1:3);
U=x2d_h(:,1:2);

THRESHOLD_REPROJECTION_ERROR=20;%error in degrees of the basis formed by the control points.
%If we have a larger error, we will compute the solution using a larger
%number of vectors in the kernel

%define control points in a world coordinate system (centered on the 3d
%points centroid)
Cw=define_control_points();

%compute alphas (linear combination of the control points to represent the 3d
%points)
Alph=compute_alphas(Xw,Cw);

%Compute M
M=compute_M_ver2(U,Alph,A);

%Compute kernel M
Km=kernel_noise(M,4); %in matlab we have directly the funcion km=null(M);

%1.-Solve assuming dim(ker(M))=1. X=[Km_end];------------------------------
dim_kerM=1;
X1=Km(:,end);
[Cc,Xc,sc]=compute_norm_sign_scaling_factor(X1,Cw,Alph,Xw);

[R,T]=getrotT(Xw,Xc);  %solve exterior orientation
err(1)=reprojection_error_usingRT(Xw,U,R,T,A);

sol(1).Xc=Xc;
sol(1).Cc=Cc;
sol(1).R=R;
sol(1).T=T;
sol(1).error=err(1);
sol(1).betas=[1];
sol(1).sc=sc;
sol(1).Kernel=X1;

%2.-Solve assuming dim(ker(M))=2------------------------------------------
Km1=Km(:,end-1);
Km2=Km(:,end);

%control points distance constraint
D=compute_constraint_distance_2param_6eq_3unk(Km1,Km2);
dsq=define_distances_btw_control_points();
betas_=inv(D'*D)*D'*dsq;
beta1=sqrt(abs(betas_(1)));
beta2=sqrt(abs(betas_(3)))*sign(betas_(2))*sign(betas_(1));
X2=beta1*Km1+beta2*Km2;

[Cc,Xc,sc]=compute_norm_sign_scaling_factor(X2,Cw,Alph,Xw);

[R,T]=getrotT(Xw,Xc);  %solve exterior orientation
err(2)=reprojection_error_usingRT(Xw,U,R,T,A);

sol(2).Xc=Xc;
sol(2).Cc=Cc;
sol(2).R=R;
sol(2).T=T;
sol(2).error=err(2);
sol(2).betas=[beta1,beta2];
sol(2).sc=sc;
sol(2).Kernel=[Km1,Km2];

%3.-Solve assuming dim(ker(M))=3------------------------------------------
if min(err)>THRESHOLD_REPROJECTION_ERROR %just compute if we do not have good solution in the previus cases

    Km1=Km(:,end-2);
    Km2=Km(:,end-1);
    Km3=Km(:,end);

    %control points distance constraint
    D=compute_constraint_distance_3param_6eq_6unk(Km1,Km2,Km3);
    dsq=define_distances_btw_control_points();
    betas_=inv(D)*dsq;
    beta1=sqrt(abs(betas_(1)));
    beta2=sqrt(abs(betas_(4)))*sign(betas_(2))*sign(betas_(1));
    beta3=sqrt(abs(betas_(6)))*sign(betas_(3))*sign(betas_(1));

    X3=beta1*Km1+beta2*Km2+beta3*Km3;

    [Cc,Xc,sc]=compute_norm_sign_scaling_factor(X3,Cw,Alph,Xw);

    [R,T]=getrotT(Xw,Xc);  %solve exterior orientation
    err(3)=reprojection_error_usingRT(Xw,U,R,T,A);

    sol(3).Xc=Xc;
    sol(3).Cc=Cc;
    sol(3).R=R;
    sol(3).T=T;
    sol(3).error=err(3);
    sol(3).betas=[beta1,beta2,beta3];
    sol(3).sc=sc;
    sol(3).Kernel=[Km1,Km2,Km3];

end

%4.-Solve assuming dim(ker(M))=4------------------------------------------
if min(err)>THRESHOLD_REPROJECTION_ERROR %just compute if we do not have good solution in the previus cases
    Km1=Km(:,end-3);
    Km2=Km(:,end-2);
    Km3=Km(:,end-1);
    Km4=Km(:,end);

    D=compute_constraint_distance_orthog_4param_9eq_10unk(Km1,Km2,Km3,Km4);
    dsq=define_distances_btw_control_points();
    lastcolumn=[-dsq',0,0,0]';
    D_=[D,lastcolumn];
    Kd=null(D_);

    P=compute_permutation_constraint4(Kd);
    lambdas_=kernel_noise(P,1);
    lambda(1)=sqrt(abs(lambdas_(1)));
    lambda(2)=sqrt(abs(lambdas_(6)))*sign(lambdas_(2))*sign(lambdas_(1));
    lambda(3)=sqrt(abs(lambdas_(10)))*sign(lambdas_(3))*sign(lambdas_(1));
    lambda(4)=sqrt(abs(lambdas_(13)))*sign(lambdas_(4))*sign(lambdas_(1));
    lambda(5)=sqrt(abs(lambdas_(15)))*sign(lambdas_(5))*sign(lambdas_(1));

    betass_=lambda(1)*Kd(:,1)+lambda(2)*Kd(:,2)+lambda(3)*Kd(:,3)+lambda(4)*Kd(:,4)+lambda(5)*Kd(:,5);
    beta1=sqrt(abs(betass_(1)));
    beta2=sqrt(abs(betass_(5)))*sign(betass_(2));
    beta3=sqrt(abs(betass_(8)))*sign(betass_(3));
    beta4=sqrt(abs(betass_(10)))*sign(betass_(4));
    X4=beta1*Km1+beta2*Km2+beta3*Km3+beta4*Km4;

    [Cc,Xc,sc]=compute_norm_sign_scaling_factor(X4,Cw,Alph,Xw);

    [R,T]=getrotT(Xw,Xc);  %solve exterior orientation
    err(4)=reprojection_error_usingRT(Xw,U,R,T,A);

    sol(4).Xc=Xc;
    sol(4).Cc=Cc;
    sol(4).R=R;
    sol(4).T=T;
    sol(4).error=err(4);
    sol(4).betas=[beta1,beta2,beta3,beta4];
    sol(4).sc=sc;
    sol(4).Kernel=[Km1,Km2,Km3,Km4];
end

%5.-Gauss Newton Optimization------------------------------------------------------
[min_err,best_solution]=min(err);
Xc=sol(best_solution).Xc;
R=sol(best_solution).R;
T=sol(best_solution).T;
Betas=sol(best_solution).betas;
sc=sol(best_solution).sc;
Kernel=sol(best_solution).Kernel;

if best_solution==1
    Betas=[0,0,0,Betas];
elseif best_solution==2
    Betas=[0,0,Betas];
elseif best_solution==3
    Betas=[0,Betas];
end

Km1=Km(:,end-3);
Km2=Km(:,end-2);
Km3=Km(:,end-1);
Km4=Km(:,end);
Kernel=[Km1,Km2,Km3,Km4];

%refine the solution iterating over the betas
Beta0=Betas/sc;
[Xc_opt,R_opt,T_opt,err_opt,iter]=optimize_betas_gauss_newton(Kernel,Cw,Beta0,Alph,Xw,U,A);

%Just update R,T,Xc if Gauss Newton improves results (which is almost
%always)
if err_opt<min_err
    R=R_opt;
    T=T_opt;
    Xc=Xc_opt;
end

opt.Beta0=Beta0;
opt.Kernel=Kernel;
opt.iter=iter;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [R, T]=getrotT(wpts,cpts)

% This routine solves the exterior orientation problem for a point cloud
%  given in both camera and world coordinates.

% wpts = 3D points in arbitrary reference frame
% cpts = 3D points in camera reference frame

n=size(wpts,1);
M=zeros(3);

ccent=mean(cpts);
wcent=mean(wpts);

for i=1:3
  cpts(:,i)=cpts(:,i)-ccent(i)*ones(n,1);
  wpts(:,i)=wpts(:,i)-wcent(i)*ones(n,1);
end
for i=1:n
   M=M+cpts(i,:)'*wpts(i,:);
end
[U S V]=svd(M);
R=U*V';
if det(R)<0
  R=-R;
end
T=ccent'-R*wcent';
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [err,Urep]=reprojection_error_usingRT(Xw,U,R,T,A)

%clear all; close all; load reprojection_error_usingRT;
n=size(Xw,1);

P=A*[R,T];
Xw_h=[Xw,ones(n,1)];
Urep_=(P*Xw_h')';

%project reference points into the image plane
Urep=zeros(n,2);
Urep(:,1)=Urep_(:,1)./Urep_(:,3);
Urep(:,2)=Urep_(:,2)./Urep_(:,3);

%reprojection error
err_=sqrt((U(:,1)-Urep(:,1)).^2+(U(:,2)-Urep(:,2)).^2);
err=sum(err_)/n;


%sim_evolve.m
%Jamie Near, 2014.
%
% USAGE:
% d_out = sim_evolve(d_in,H,t)
%
% DESCRIPTION:
% This function simulates free evolution of the spin system under the
% effects of chemical shift and scalar coupling.
%
% INPUTS:
% d_in      = input density matrix structure.
% H         = Hamiltonian operator structure.
% t         = duration of evolution (s)
%
% OUTPUTS:
% d_out     = output density matrix following free evolution.

function d_out = sim_evolve(d_in,H,t)

for n=1:length(H) %JN - loop through the different parts of the spin-system
    p=expm(1i*H(n).HAB*t);
    d_out{n} = p' * d_in{n} * p;  %Faster than doing expm twice.  Suggested by Martin Froeling.
end

end


function varargout = norm_nuclear(varargin)
% NORM_NUCLEAR Returns sum of singular values
%
% s = SUMK(X,k)
%
% For a vector X, NORM_NUCLEAR returns the sum of singular values
%
% For a matrix X, NORM_NUCLEAR returns the sum of absolute values.
%
% See also SUMABSK

switch class(varargin{1})

    case 'double' % What is the numerical value of this argument (needed for displays etc)
        varargout{1} = sum(svd(varargin{1}));

    case 'sdpvar'
        varargout{1} = yalmip('define',mfilename,varargin{:});

    case 'char' % YALMIP sends 'model' when it wants the epigraph or hypograph
        if isequal(varargin{1},'graph')
            t = varargin{2};
            X = varargin{3};
            [n,m] = size(X);
            if is(X,'real')
                U = sdpvar(m);
                V = sdpvar(n);
            else
                U = sdpvar(m,m,'hermitian','complex');
                V = sdpvar(n,n,'hermitian','complex');
            end
            F = [trace(U)+trace(V) <= 2*t, [U X';X V]>=0];
            varargout{1} = F;
            varargout{2} = struct('convexity','convex','monotonicity','none','definiteness','positive','model','graph');
            varargout{3} = X;
        else
            varargout{1} = [];
            varargout{2} = [];
            varargout{3} = [];
        end
    otherwise
end


classdef cubochoricPlot < axisAnglePlot

  methods

    function oP = cubochoricPlot(varargin)
      % create a 3d plot of rotations in cubochoric coordinates

      oP = oP@axisAnglePlot(varargin{:});

     end

     function [x,y,z] = project(oP,ori,varargin)

      if ~check_option(varargin,'noBoundaryCheck')
        switch oP.fRMode
          case 'project2FundamentalRegion'
            ori = project2FundamentalRegion(ori);
          case 'restrict2FundamentalRegion'
            ori(~oP.oR.checkInside) = NaN;
        end
      end

      [x,y,z] = double(cubochoric(ori));

    end

    function ori = iproject(oP,x,y,z,varargin)
      ori = orientation.id;
    end

    function ori = makeGrid(oP,varargin)

      [ori,S2G,omega] = makeGrid@axisAnglePlot(oP,varargin{:});

      [oP.plotGrid.x, oP.plotGrid.y, oP.plotGrid.z] = ...
        double( S2G .* (3./4 * (omega - sin(omega))).^(1/3));

    end
  end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   demo script for mesh generation from binarized volumetric image
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% preparation
% user must add the path of iso2mesh to matlab path list
% addpath('../');

% user need to add the full path to .../iso2mesh/bin directory
% to windows/Linux/Unix PATH environment variable

%% load the sample data
load rat_head.mat

% volimage is a volumetric image such as an X-ray or MRI image
% A,b are registration matrix and vector, respectively
%% perform mesh generation

%% use the alternative 'simplify' method: first create voxel-based
% surface mesh, and then resample it to desired density.
% this method does not guarantee to be free of self-intersecting
% element, as 'cgalsurf' promises.

[node,elem,face]=vol2mesh(volimage>0.05,1:size(volimage,1),1:size(volimage,2),...
                          1:size(volimage,3),0.1,2,1,'simplify');

%% visualize the resulting mesh

plotmesh(node,face);
axis equal;


function SIIval = SII(E,N,Mtype)

% Implements  the ANSI S3.5-1997 standard:
% "Methods for calculation of the Speech Intelligibility Index".
%
%   INPUT:
%
%     'E' Speech Spectrum Level (Section 3.6 in the standard)
%         Level needs to be in dB SPL
%     'N' Noise Spectrum Level (Section 3.15 in the standard)
%         Level needs to be in dB SPL
%     'M' Speech material (needed to specify band-importance function)
%         A scalar having a value of either 1, 2, 3, 4, 5, 6. The Band-importance functions associated with each scalar are
%		                1:	various nonsense syllable tests where most English phonemes occur equally often (as specified in Table B.2)
%		                2:	CID-22 (as specified in Table B.2)
%		                3:	NU6 (as specified in Table B.2)
%		                3:	Diagnostic Rhyme test (as specified in Table B.2)
%		                5:	short passages of easy reading material (as specified in Table B.2)
%		                6:	SPIN (as specified in Table B.2)
%
%    OUTPUT:
%       Returns the SII value (0 to 1)
%
%     Note that only the one-third-octave band procedure is implemented.
%     Dimensions of 'E' and 'N' are 18x1, containing the speech and noise levels
%     at 1/3-octave frequencies (see line 78 for 1/3-octave center frequencies)
%
% EXAMPLE
% sp=[40 45 50 24 56 60 55 55 52 48 50 51 55 67 76 67 56 31];
% ns=[30 50 60 20 60 50 70 45 80 40 60 20 60 22 55 50 67 40];
% M= 5;
% sv = SII (sp,ns, M);
%
% (c)2012 Philipos C. Loizou

if length(E)~=18 | length(N)~=18
    error('The target and masker spectra vectors have incorrect dimension - needs to be 18.');
end
if Mtype>6 | Mtype<1
  error('Band-importance function type takes values between 1 and 6');
end

%================== DEFINE INPUT VARIABLES ==============================

G=zeros(1,18);  % insertion gains - needed if used in the context of amplification devices (hearing aids)
T=G;            % threshold levels in dB HL (for normal-hearing listeners, T=[0 0 ...0] )
VocalEffort = 'normal';  % or "raised", "loud" and "shout"

% Standard speech spectrum level for different vocal efforts (Table 3)
%
SpV=[32.41	33.81	35.29	30.77;
	34.48	33.92	37.76	36.65;
	34.75	38.98	41.55	42.5;
	33.98	38.57	43.78	46.51;
	34.59	39.11	43.3	47.4;
	34.27	40.15	44.85	49.24;
	32.06	38.78	45.55	51.21;
	28.3	36.37	44.05	51.44;
	25.01	33.86	42.16	51.31;
	23		31.89	40.53	49.63;
	20.15	28.58	37.7	47.65;
	17.32	25.32	34.39	44.32;
	13.18	22.35	30.98	40.8;
	11.55	20.15	28.21	38.13;
	9.33	16.78	25.41	34.41;
	5.31	11.47	18.35	28.24;
	2.59	7.67	13.87	23.45;
	1.13	5.07	11.39	20.72];

switch lower(VocalEffort)
	case 'normal', EV = SpV(:,1)';
	case 'raised', EV = SpV(:,2)';
	case 'loud',   EV = SpV(:,3)';
	case 'shout',  EV = SpV(:,4)';
	otherwise, error('Unknown level of vocal effort')
end;

% Define band center frequencies for 1/3rd octave procedure (Table 3)
f = [160 200 250 315 400 500 630 800 1000 1250 1600 2000, ...
     2500 3150 4000 5000 6300 8000];

% Define Internal Noise Spectrum Level (Table 3)
X = [0.6 -1.7 -3.9 -6.1 -8.2 -9.7 -10.8 -11.9 -12.5 -13.5 -15.4 -17.7, ...
	-21.2 -24.2 -25.9 -23.6 -15.8 -7.1];

% ----------------- start processing ----------------------
%
% Equivalent Speech Spectrum Level (5.1.3, Eq. 17)
E = E + G;

% Self-Speech Masking Spectrum (Sec 4.3.2.1, Eq. 5)
V = E - 24;

% 4.3.2.2
B = max(V,N+G);

% Calculate Equivalent Masking Spectrum Level (Sec 4.3.2.5, Eq. 9)
%
C = 0.6.*(B+10*log10(f)-6.353) - 80;  % slope parameter Ci (4.3.2.3 Eq. 7)
Z(1) = B(1);

for i = 2:18
	Z(i) = 10*log10(10.^(0.1*N(i)) + ...
	sum(10.^(0.1*(B(1:(i-1))+3.32.*C(1:(i-1)).*log10(0.89*f(i)./f(1:(i-1)))))));
end;

% Equivalent Internal Noise Spectrum Level (Sec 4.4 Eq. 10)
X = X + T;

% Compute disturbance Spectrum Level (4.5)
D = max(Z,X);

% Level Distortion Factor (Sec 4.6, Eq. 11)
%
L = 1 - (E - EV - 10)./160;
L = max(0,min(L,1));

% 4.7.1 Eq. 12
K = (E-D+15)/30;
K=max(0,min(K,1));

% Band Audibility Function (7.7.2 Eq. 13)
%
A = L.*K;

% Speech Intelligibility Index (4.8 Eq. 14)
%
SIIval = sum(BandImportance(Mtype).*A);

return;

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

function BIF = BandImportance(type)
%
% Band importance functions, taken from Table B.2:
% type  =
%		1:	Nonsense syllable tests where most English
%			phonemes occur equally often
%		2:	CID-22
%		3:	NU6 (monosyllables)
%		4:	Diagnostic Rhyme test (DRT)
%		5:	short passages of easy reading material
%		6:	SPIN (monosyllables)

if (nargin ~= 1) | (type>6) | (type<1)
	error('Incorrect argument to BandImportance');
end;

IFu   = [	0		0.0365	0.0168	0		0.0114	0
			0		0.0279	0.013	0.024	0.0153	0.0255
            0.0153  0.0405	0.0211	0.033	0.0179	0.0256
			0.0284	0.0500	0.0344	0.039	0.0558	0.036
			0.0363	0.0530	0.0517	0.0571	0.0898	0.0362
			0.0422	0.0518	0.0737	0.0691	0.0944	0.0514
			0.0509	0.0514	0.0658	0.0781	0.0709	0.0616
			0.0584	0.0575	0.0644	0.0751	0.066	0.077
			0.0667	0.0717	0.0664	0.0781	0.0628	0.0718
			0.0774	0.0873	0.0802	0.0811	0.0672	0.0718
			0.0893	0.0902	0.0987	0.0961	0.0747	0.1075
			0.1104	0.0938	0.1171	0.0901	0.0755	0.0921
			0.112	0.0928	0.0932	0.0781	0.082	0.1026
			0.0981	0.0678	0.0783	0.0691	0.0808	0.0922
			0.0867	0.0498	0.0562	0.048	0.0483	0.0719
			0.0728	0.0312	0.0337	0.033	0.0453	0.0461
			0.0551	0.0215	0.0177	0.027	0.0274	0.0306
			0		0.0253	0.0176	0.024	0.0145	0];

BIF = IFu(:,type)';


function x=ibm2num(b)
% ibm2num : convert IBM 32 bit floating point format to doubles
%    x=num2ibm(b)
% b is a matrix of uint32
% x is a corresponding matrix of doubles
%
%
% See also num2ibm

%
%    This program is free software; you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation; either version 2 of the License, or
%    (at your option) any later version.
%
%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program; if not, write to the Free Software
%    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
%
%
% (C) Brian Farrelly, 22 October 2001
%  mailto:Brian.Farrelly@nho.hydro.com          Norsk Hydro Research Centre
%  phone +47 55 99 68 74                 (((                  Postboks 7190
%  fax   +47 55 99 69 70                2oooS                 N-5020 Bergen
%  home  +47 55 13 78 49                HYDRO                        Norway
%

x=repmat(NaN,size(b));

sign=bitget(b,32);                            % get sign from first bit
sign=double(sign);

% format hex
exp=bitand(b,uint32(hex2dec('7f000000')));    % get exponent from first byte, last 7 bits
exp=bitshift(exp,-24);
%format long

exp=double(exp)- 64;                          % remove bias from exponent

%format hex
frac=bitand(b,uint32(hex2dec('00ffffff')));   % get mantissa from last 3 bytes
%format long
frac=double(frac);
frac=frac/2^24;

x=(1-2*sign).*16.^exp .* frac;

err = frac==0 & (exp~=-64 | sign~=0);         % bias removal is incorrect for zero
if any(err)
   % TMH 19/06/2003
   disp(['WARNING : ',mfilename,' Invalid zero input --> Sure data are IBM FLOAT formatted ?'])
   return;
   %warning('Invalid zero input in ibm2num for the following:')
   % format hex; disp(b(err)); format
end

err = frac~=0 & (frac<1/16 | frac>=1);
if any(err)
   % TMH 19/06/2003
   disp(['WARNING : ',mfilename,' Invalid mantissa input --> Sure data are IBM FLOAT formatted ?'])
   return;
   % warning('Invalid mantissa input in ibm2num for the following:')
   % format hex; disp(b(err)); format
end


function result = cartesianProduct(sets)
    c = cell(1, numel(sets));
    [c{:}] = ndgrid( sets{:} );
    result = cell2mat( cellfun(@(v)v(:), c, 'UniformOutput',false) );
end


function [x, fx, exitFlag] = bisection(f,lb,ub,target,options)%#codegen
    % BISECTION Fast and robust root-finding method that handles n-dim arrays.
    %
    %   [x,fVal,ExitFlag] = BISECTION(f,LB,UB,target,options) finds x +/- TolX
    %   (LB < x < UB) such that f(x) = target +/- TolFun.
    %
    %   x = BISECTION(f,LB,UB) finds the root(s) of function f on the interval
    %   [LB, UB], i.e. finds x such that f(x) = 0 where LB <= x <= UB. f will
    %   never be evaluated outside of the interval specified by LB and UB. f
    %   should have only one root and f(UB) and f(LB) must bound it. Elements
    %   of x are NaN for instances where a solution could not be found.
    %
    %   x = BISECTION(f,LB,UB,target) finds x such that f(x) = target.
    %
    %   x = BISECTION(f,LB,UB,target,TolX) will terminate the search when the
    %   search interval is smaller than TolX (TolX must be positive).
    %
    %   x = BISECTION(f,LB,UB,target,options) solves with the default
    %   parameters replaced by values in the structure OPTIONS, an argument
    %   created with the OPTIMSET function. Used options are TolX and TolFun.
    %   Note that OPTIMSET will not allow arrays for tolerances, so set the
    %   fields of the options structure manually for non-scalar TolX or TolFun.
    %
    %   [x,fVal] = BISECTION(f,...) returns the value of f evaluated at x.
    %
    %   [x,fVal,ExitFlag] = BISECTION(...) returns an ExitFlag that describes
    %   the exit condition of BISECTION. Possible values of elements of
    %   ExitFlag and the corresponding exit conditions are
    %
    %       1   Search interval smaller than TolX.
    %       2   Function value within TolFun of target.
    %       3   Search interval smaller than TolX AND function value within
    %           TolFun of target.
    %      -1   No solution found.
    %
    %   Any or all of f(scalar), f(array), LB, UB, target, TolX, or TolFun may
    %   be scalar or n-dim arrays. All non-scalar arrays must be the same size.
    %   All outputs will be this size.
    %
    %   Default values are target = 0, TolX = 1e-6, and TolFun = 0.
    %
    %   There is no iteration limit. This is because BISECTION (with a TolX
    %   that won't introduce numerical issues) is guaranteed to converge if f
    %   is a continuous function on the interval [UB, LB] and f(x)-target
    %   changes sign on the interval.
    %
    %   The bisection method is very robust root-finding method. The absolute
    %   error is halved at each step so the method converges linearly. However,
    %   Brent's method (such as implemented in FZERO) can converge
    %   superlinearly and is as robust. FZERO also has more features and input
    %   checking, so use BISECTION in cases where either the optimization
    %   toolbox is unavailable or if FZERO would have to be implemented in a
    %   loop to solve multiple cases, in which case BISECTION will be much
    %   faster because of vectorization.
    %
    %   Define LB, UB, target, TolX, and TolFun for each specific application
    %   using great care for the following reasons:
    %     - There is no iteration limit, so given an unsolvable task, such as
    %       TolX = TolFun = 0, BISECTION remains in an unending loop.
    %     - Spacing between very large floating point numbers is likely to be
    %       greater than TolX.
    %     - There is no initial check to make sure that f(x) - target changes
    %       sign between LB and UB.
    %     - Very large or very small numbers can introduce numerical issues.
    %
    %   Example 1: Find cube root of array 'target' without using NTHROOT and
    %   compare speed to using FZERO.
    %       options = optimset('TolX', 1e-9);
    %       target = [(-100:.1:100)' (-1000:1:1000)'];
    %
    %       tic;
    %       xfz = zeros(size(target));
    %       for ii = 1:numel(target)
    %           xfz(ii) = fzero(@(x) x.^3-target(ii), [-20 20], options);
    %       end
    %       fzero_time = toc
    %
    %       tic;
    %       xbis = bisection(@(x) x.^3, -20, 20, target, options);
    %       bisection_time = toc
    %
    %       fprintf('FZERO took %0.0f times longer than BISECTION.\n',...
    %                   fzero_time/bisection_time)
    %
    %   Example 2: Find roots by varying the function coefficients.
    %       [A, B] = meshgrid(linspace(1,2,6), linspace(4,12,10));
    %       f = @(x) A.*x.^0.2 + B.*x.^0.87 - 15;
    %       xstar = bisection(f,0,5);
    %
    %   See also FZERO, FMINBND, OPTIMSET, FUNCTION_HANDLE.
    %
    %   [x,fVal,ExitFlag] = BISECTION(f,LB,UB,target,options)

    %   Copyright 2010-2013 Sky Sartorius
    %   Author  - Sky Sartorius
    %   Contact - www.mathworks.com/matlabcentral/fileexchange/authors/101715

    % --- Process inputs. ---
    % Set default values
    tolX    = 1e-6;
    tolFun  = 0;
    if nargin == 5
        if isstruct(options)
            if isfield(options,'TolX') && ~isempty(options.TolX)
                tolX = options.TolX;
            end
            if isfield(options,'TolFun') && ~isempty(options.TolFun)
                tolFun = options.TolFun;
            end
        else
            tolX = options;
        end
    end
    if nargin<4 || isempty(target); target=0; end

    ub_in = ub; lb_in = lb;
    f = @(x) f(x) - target;

    % --- Flip UB and LB if necessary. ---
    isFlipped = lb>ub;
    if any(isFlipped(:))
        ub(isFlipped) = lb_in(isFlipped);
        lb(isFlipped) = ub_in(isFlipped);
        ub_in = ub; lb_in = lb;
    end

    % --- Make sure everything is the same size for a non-scalar problem. ---
    if isscalar(lb) && isscalar(ub)
        % Test if f returns multiple outputs for scalar input.
        if ~isscalar(target)
            ub = ub + zeros(size(target));
        else
            jnk = f(ub);
            if ~isscalar(jnk)
                ub = ub + zeros(size(jnk));
            end
        end
    end

    % Check if lb and/or ub need to be made into arrays.
    if isscalar(lb) && ~isscalar(ub)
        lb = lb + zeros(size(ub));
    elseif ~isscalar(lb) && isscalar(ub)
        ub = ub + zeros(size(lb));
    end

    x = zeros(size(lb));
    [x, fx, outsideTolFun, outsideTolX, stillNotDone] = testconvergence(f, ub, lb, tolFun, tolX);
    % --- Iterate ---
    iterations = 0;
    while any(stillNotDone(:))
        bigger  = fx.*f(ub) > 0;
        ub(bigger)= x(bigger);
        lb(~bigger)= x(~bigger);

        [x, fx, outsideTolFun, outsideTolX, stillNotDone] = testconvergence(f, ub, lb, tolFun, tolX);
        iterations = iterations + 1;

        if(iterations > 10000)
            %         error('Too many iterations in bisection!');
            break;
        end
    end

    % --- Check that f(x+tolX) and f(x-tolX) have opposite sign. ---
    fu = f(min(x+tolX,ub_in));
    fl = f(max(x-tolX,lb_in));
    unboundedRoot = (fu.*fl) > 0;

    % Throw out unbounded results if not meeting TolFun convergence criteria.

    x(unboundedRoot & outsideTolFun) = NaN;

    % --- Catch NaN elements of UB, LB, target, or other funky stuff. ---
    x(isnan(fx)) = NaN;

    % --- Characterize results. ---
    fx = fx + target;
    if nargout > 2
        exitFlag                                    = +~outsideTolX;
        exitFlag(~outsideTolFun)                    =  2;
        exitFlag(~outsideTolFun & ~outsideTolX)     =  3;
        exitFlag(isnan(x))                          = -1;
    end
end

function [x, fx, outsideTolFun, outsideTolX, stillNotDone] = testconvergence(f, ub, lb, tolFun, tolX)
    x=(ub+lb)/2;
    fx=f(x);
    outsideTolFun =  abs(fx)  > tolFun;
    outsideTolX =   (ub - lb) > tolX;
    stillNotDone = outsideTolX & outsideTolFun;
end

% V2: July     2010
% V3: December 2012
% don't remember when
%   typo line 39; added fn handle to see also; made array in example 2
%   smaller; changed wording in example 1
% 2013-08-23
%  -changed scalar*ones(...) calls to scalar+zeros(...) calls based on
%   http://undocumentedmatlab.com/blog/allocation-performance-take-2/
%  -rearranged help block and formatted a tiny bit