function [Ain, Cin, b_in, f_in, center, res] = greedyROI(Y, K, params, ROI_list)
% component initialization using a greedy algorithm to identify neurons in 2d or 3d calcium imaging movies
%
% Usage:     [Ain, Cin, bin, fin, center, res] = greedyROI2d(data, K, params)
%
% Input:
% Y          d1 x d2 x (d3 x) T movie, raw data, each column is a vectorized movie
% K          number of neurons to extract (if K is a vector then the algorithm is run multiple times with different parameters)
% params     tuning parameter for fine-tuning the shape (optional)
%            params.nIter: number of iterations for shape tuning (default 5)
%            params.gSig: variance of Gaussian kernel to use (default 5) If params.gSig is a cell, then the algorithm is run with multiple times with different parameters
%            params.gSiz: size of kernel (default 2*gSiz+1)
%            params.nb: rank of background component (default 1)
%            params.save_memory: flag for processing data in chunks to save memory (default 0)
%            params.windowSiz: size of spatial window when computing the median (default 32 x 32)
%            params.chunkSiz: number of timesteps to be processed simultaneously if on save_memory mode (default: 100)
%            params.med_app: number of timesteps to be interleaved for fast (approximate) median calculation (default: 1, no approximation)
%            params.rolling_sum: flag for using rolling sum to detect new components (default: True)
%            params.rolling_length: length of rolling window (default: 100)
% ROI_list   Kn x 2 (or 3) matrix with user specified centroids. If this are present, then the algorithm only finds components around these centroids

%Output:
% Ain        (d) x K matrix, location of each neuron
% Cin        T x K matrix, calcium activity of each neuron
% center     K x 2 (or 3) matrix, inferred center of each neuron
% bin        (d) X nb matrix, initialization of spatial background
% fin        nb X T matrix, initalization of temporal background
% res        d1 x d2 x (d3 x) T movie, residual
%
% Author: Yuanjun Gao with modifications from Eftychios A. Pnevmatikakis and Weijian Yang

use_sum = false;
if nargin < 4 || isempty(ROI_list)
    user_ROIs = 0;
else
    user_ROIs = 1;
    K = size(ROI_list,1);
end

dimY = ndims(Y) - 1;  % dimensionality of imaged data (2d or 3d)
sizY = size(Y);
T = sizY(end);        % # of timesteps
dx = sizY(1:dimY);    % # of voxels in each axis
d = prod(dx);         % total # of voxels

if ~exist('K', 'var'),  %find K neurons
    K = 30;
    warning(['number of neurons are not specified, set to be the default value', num2str(K)]);
end

if ~exist('params', 'var') params = []; end

if ~isfield(params, 'gSig') || isempty(params.gSig);
    if dimY == 2; params.gSig = [5, 5]; else params.gSig = [5,5,5]; end
elseif length(params.gSig) == 1, params.gSig = params.gSig + zeros(1,dimY);
    if dimY == 3; params.gSig(3) = params.gSig(3)/2; end
end

if ~isfield(params, 'gSiz') || isempty(params.gSiz);
    if ~iscell(params.gSig)
        params.gSiz = ceil(2*params.gSig + 1);
    else
        for j = 1:length(params.gSig)
            params.gSiz{j,1} = 2*params.gSig{j}+1; %cellfun(@times,params.gSig{j},num2cell(ones(size(params.gSig{j}))*2));
        end
    end
elseif length(params.gSiz) == 1, params.gSiz = params.gSiz + zeros(1,dimY);
    if dimY == 3; params.gSiz(3) = ceil(params.gSiz(3)/2); end
end

if isfield(params,'ssub');
    if ~iscell(params.gSig); params.gSig(1:2) = params.gSig(1:2)/params.ssub; params.gSiz(1:2) = ceil(params.gSiz(1:2)/params.ssub);
    else
        for j = 1:length(params.gSig)
            params.gSig{j,1} = params.gSig{j}/params.ssub; %cellfun(@times,params.gSig{j},num2cell(ones(size(params.gSig{j}))/params.ssub));
            params.gSiz{j,1} = params.gSiz{j}/params.ssub; %cellfun(@times,params.gSiz{j},num2cell(ones(size(params.gSiz{j}))/params.ssub));
        end
    end
end

if ~isfield(params,'nb'), nb = 1; else nb = params.nb; end

if ~isfield(params, 'nIter'), nIter = 5;
else nIter = params.nIter; end

if ~isfield(params, 'save_memory'), save_memory = 0;
else save_memory = params.save_memory; end

if ~isfield(params, 'chunkSiz'), chunkSiz = 100; else chunkSiz = params.chunkSiz; end
if ~isfield(params, 'windowSiz'), windowSiz = 32; else windowSiz = params.windowSiz; end
if ~isfield(params, 'med_app'), med_app = 1; else med_app = params.med_app; end

if ~isfield(params,'rem_prct') || isempty(params.rem_prct); params.rem_prct = 20; end

Tint = 1:med_app:T;
% if save_memory
%     med = zeros(M,N);
%     for ii = 1:ceil(M/windowSiz)
%         intx = (ii-1)*windowSiz+1:min(ii*windowSiz,M);
%         for jj = 1:ceil(N/windowSiz)
%             inty = (jj-1)*windowSiz+1:min(jj*windowSiz,N);
%             med(intx,inty) = median(data(intx,inty,Tint),3);
%         end
%     end
% else
%if dimY == 2; med = median(Y(:,:,Tint), 3); else med = median(Y(:,:,:,Tint), 4); end
if dimY == 2; med = prctile(Y(:,:,Tint),params.rem_prct, 3); else med = prctile(Y(:,:,:,Tint),params.rem_prct,4); end

% end

Y = bsxfun(@minus, Y, med);
if iscell(params.gSig); params.gSig = cell2mat(params.gSig); params.gSiz = cell2mat(params.gSiz); end
if length(K) > 1  % order size of components to be found in descending order
    [~,ord] = sort(sum(params.gSig,2),'descend');
    K = K(ord);
    params.gSig = params.gSig(ord,:);
    params.gSiz = params.gSiz(ord,:);
end

if ~iscell(params.gSiz)
    Ain = spalloc(d,sum(K),K(:)'*ceil(params.gSiz(:,1).^2)); %zeros(M*N,sum(K));
else
    Ain = sparse(d,sum(K));
end
Cin = zeros(sum(K),T);
center = zeros(sum(K),dimY);

for r = 1:length(K)
    gSig = params.gSig(r,:);
    gSiz = params.gSiz(r,:);

    gHalf = floor(gSiz / 2); %half size of the kernel, used to calculate margin
    gSiz = 2 * gHalf + 1; %actual size

    Atemp = spalloc(d,K(r),K(r)*ceil(prod(gSiz))); %zeros(M, N, K(r));
    %basis = spalloc(M*N,K,K*prod(gSiz));
    trace = zeros(T, K(r));
    centers = zeros(K(r), dimY);

    %scan the whole image (only need to do this at the first iteration)
    rho = imblur(Y, gSig, gSiz, dimY, save_memory, chunkSiz); %covariance of data and basis

    if ~params.rolling_sum
        v = sum(rho.^2, dimY+1); %variance explained
    else
        % running maximum
        avg_fil = ones(1,params.rolling_length)/params.rolling_length;
        rho_s = filter(avg_fil,1,rho.^2,[],dimY+1);
        v = max(rho_s,[],dimY+1);
    end

    for k = 1:K(r),
        if user_ROIs
            iHat = ROI_list(k,:);
        else
            [~, ind] = max(v(:));
            %[iHat, jHat] = ind2sub([M, N], ind);
            iHat = zeros(1,dimY);
            if dimY == 2; [iHat(1),iHat(2)] = ind2sub(dx,ind); else [iHat(1),iHat(2),iHat(3)] = ind2sub(dx,ind); end
        end
        centers(k,:) = round(iHat);

        %iSig = [max(iHat - gHalf(1), 1), min(iHat + gHalf(1), M)]; iSigLen = iSig(2) - iSig(1) + 1;
        %jSig = [max(jHat - gHalf(2), 1), min(jHat + gHalf(2), N)]; jSigLen = jSig(2) - jSig(1) + 1;
        iSig = [max(iHat - gHalf,1)', min(iHat + gHalf,dx)']; iSigLen = iSig(:,2) - iSig(:,1) + 1;

        %fine tune the shape
        if dimY == 2
            dataTemp = Y(iSig(1,1):iSig(1,2), iSig(2,1):iSig(2,2), :);
            traceTemp = squeeze(rho(iHat(1), iHat(2), :));
        else
            dataTemp = Y(iSig(1,1):iSig(1,2), iSig(2,1):iSig(2,2), iSig(3,1):iSig(3,2), :);
            traceTemp = squeeze(rho(iHat(1), iHat(2), iHat(3), :));
        end
        [coef, score] = finetune(dataTemp, traceTemp, nIter);

        dataSig = bsxfun(@times, coef, reshape(score, [ones(1,dimY),T]));
        %[xSig,ySig] = meshgrid(iSig(1):iSig(2),jSig(1):jSig(2));
        basis = zeros(dx);
        if dimY == 2;  basis(iSig(1,1):iSig(1,2), iSig(2,1):iSig(2,2)) = coef; else  basis(iSig(1,1):iSig(1,2), iSig(2,1):iSig(2,2), iSig(3,1):iSig(3,2)) = coef; end
        Atemp(:,k) = basis(:);
        %basis(sub2ind([M,N],xSig(:),ySig(:)),k) = coef(:);

        trace(:, k) = score';

        if dimY == 2;
            Y(iSig(1,1):iSig(1,2), iSig(2,1):iSig(2,2), :) = Y(iSig(1,1):iSig(1,2), iSig(2,1):iSig(2,2), :) - dataSig; %update residual
        else
            Y(iSig(1,1):iSig(1,2), iSig(2,1):iSig(2,2), iSig(3,1):iSig(3,2), :) = Y(iSig(1,1):iSig(1,2), iSig(2,1):iSig(2,2),  iSig(3,1):iSig(3,2),:) - dataSig;
        end

        if mod(k,10) == 0; fprintf('found %i out of %i neurons..\n', k,K(r)); end

        %get next basis;
        if k < K(r)
            %iMod = [max(iHat - 2 * gHalf(1), 1), min(iHat + 2 * gHalf(1), M)]; iModLen = iMod(2) - iMod(1) + 1;%patches to modify
            %jMod = [max(jHat - 2 * gHalf(2), 1), min(jHat + 2 * gHalf(2), N)]; jModLen = jMod(2) - jMod(1) + 1;
            iMod = [max(iHat - 2*gHalf,1)', min(iHat + 2*gHalf,dx)']; iModLen = iMod(:,2) - iMod(:,1) + 1;
            %iLag = iSig - iMod(1) + 1; %relative location of iSig in the small patch
            %jLag = jSig - jMod(1) + 1;
            iLag = iSig - repmat(iMod(:,1),1,2) + 1;
            %dataTemp = zeros(iModLen, jModLen);
            dataTemp = zeros(iModLen(:)');
            %dataTemp(iLag(1):iLag(2), jLag(1):jLag(2)) = reshape(coef, [iSigLen, jSigLen]);
            if dimY == 2
                dataTemp(iLag(1,1):iLag(1,2), iLag(2,1):iLag(2,2)) = reshape(coef, iSigLen(:)');
            else
                dataTemp(iLag(1,1):iLag(1,2), iLag(2,1):iLag(2,2), iLag(3,1):iLag(3,2)) = reshape(coef, iSigLen(:)');
            end
            dataTemp = imblur(dataTemp, gSig, gSiz, dimY, 0, chunkSiz);
            rhoTemp = bsxfun(@times, dataTemp, reshape(score, [ones(1,dimY),T]));
            if dimY == 2
                rhoTemp = rho(iMod(1,1):iMod(1,2), iMod(2,1):iMod(2,2), :)  - rhoTemp;
                rho(iMod(1,1):iMod(1,2), iMod(2,1):iMod(2,2), :)  = rhoTemp;
                if ~params.rolling_sum
                    v(iMod(1,1):iMod(1,2), iMod(2,1):iMod(2,2)) = sum(rhoTemp.^2, 3);
                else
                    rho_filt = filter(avg_fil,1,rhoTemp.^2,[],3);
                    v(iMod(1,1):iMod(1,2), iMod(2,1):iMod(2,2)) = max(rho_filt,[],3);
                end
            else
                rhoTemp = rho(iMod(1,1):iMod(1,2), iMod(2,1):iMod(2,2), iMod(3,1):iMod(3,2), :)  - rhoTemp;
                rho(iMod(1,1):iMod(1,2), iMod(2,1):iMod(2,2), iMod(3,1):iMod(3,2), :)  = rhoTemp;
                if ~params.rolling_sum
                    v(iMod(1,1):iMod(1,2), iMod(2,1):iMod(2,2), iMod(3,1):iMod(3,2)) = sum(rhoTemp.^2, 4);
                else
                    rho_filt = filter(avg_fil,1,rhoTemp.^2,[],4);
                    v(iMod(1,1):iMod(1,2), iMod(2,1):iMod(2,2), iMod(3,1):iMod(3,2)) = max(rho_filt,[],4);
                end
            end
        end
    end
    Ain(:,sum(K(1:r-1))+1:sum(K(1:r))) = Atemp; %sparse(reshape(basis,d,K(r)));
    Cin(sum(K(1:r-1))+1:sum(K(1:r)),:) = trace';
    center(sum(K(1:r-1))+1:sum(K(1:r)),:) = centers;
end
res = reshape(Y,d,T) + repmat(med(:),1,T);

%% clear data matrix from local memory (avoid out-of-memory? see greedyROI_corr.m)
clear Y

%[b_in,f_in] = nnmf(max(res,0),nb);
%[b_in,f_in] = nnsvd(max(res,0),nb);
f_in = [mean(res);rand(nb-1,T)];

for nmfiter = 1:100
    b_in = max((res*f_in')/(f_in*f_in'),0);
    f_in = max((b_in'*b_in)\(b_in'*res),0);
end

function [ain,cin] = finetune(data,cin,nIter)

    %rank-1 semi-NMF to fine tune the inferred components
    %
    %Input:
    %data   d1 x d2 x (d3 x) T matrix, small patch containing one neuron
    %trace  initial value for trace
    %nIter  number of coordinate descent steps
    %
    %Output:
    %basis  d1 x d2 (x d3) matrix, result of the fine-tuned neuron shape
    %trace  1 x T matrix, result of the neuron

    if ~exist('nIter', 'var'), nIter = 1; end
    data = reshape(data,prod(iSigLen),T);
    for iter = 1:nIter
        %nc = norm(cin)^2;
        ain = max(data*cin,0)/norm(cin);
        an = norm(ain);
        if an > 0
            ain = ain/an;
        else
            fprintf('found degenerate component!\n')

            %break
        end
        cin = data'*ain;
    end
    ain = reshape(ain,iSigLen(:)');
    cin = cin(:)';
end

function data = imblur(data, sig, siz, nDimBlur, save_memory, chunkSiz)
%Gaussian blur for high dimensional data
%Input:
%data       original data
%sig        std of gaussian kernel
%siz        size of kernel
%nDimBlur   number of dims to blur (default: ndims(data) - 1)
%
%Output:
%data       result after the Gaussian blur

if ~exist('nDimBlur', 'var'), nDimBlur = ndims(data) - 1;
else nDimBlur = min(nDimBlur, ndims(data)); end

if length(sig) == 1, sig = sig * ones(1,nDimBlur); end
assert(nDimBlur == length(sig));

if length(siz) == 1, siz = siz * ones(1,nDimBlur); end
assert(nDimBlur == length(siz));

for i = 1:nDimBlur,
    if sig(i) > 0,
        x = -floor(siz(i) / 2):floor(siz(i) / 2);
        H = exp(-(x.^2/ (2 * sig(i)^2)));
        H = H' / norm(H(:));
        if nDimBlur > 1,
            indH = 1:nDimBlur; indH(i) = 1; indH(1) = i;
            H = permute(H, indH);
        end
        if save_memory
            L = size(data,ndims(data));
            for ci = 1:ceil(L/chunkSiz)
                int = (ci-1)*chunkSiz+1:min(ci*chunkSiz,L);
                data(:,:,int) = imfilter(data(:,:,int), H, 'same', 0);
            end
        else
            data = imfilter(data, H, 'same', 0);
        end
    end
end
end

end


function obj = t3_hand_eye_func_new(in1,in2,in3)
coefs_tq1_1 = in2(1);
coefs_tq1_2 = in2(4);
coefs_tq1_3 = in2(7);
coefs_tq1_4 = in2(10);
coefs_tq1_5 = in2(13);
coefs_tq1_6 = in2(16);
coefs_tq1_7 = in2(19);
coefs_tq1_8 = in2(22);
coefs_tq1_9 = in2(25);
coefs_tq2_1 = in2(2);
coefs_tq2_2 = in2(5);
coefs_tq2_3 = in2(8);
coefs_tq2_4 = in2(11);
coefs_tq2_5 = in2(14);
coefs_tq2_6 = in2(17);
coefs_tq2_7 = in2(20);
coefs_tq2_8 = in2(23);
coefs_tq2_9 = in2(26);
coefs_tq3_1 = in2(3);
coefs_tq3_2 = in2(6);
coefs_tq3_3 = in2(9);
coefs_tq3_4 = in2(12);
coefs_tq3_5 = in2(15);
coefs_tq3_6 = in2(18);
coefs_tq3_7 = in2(21);
coefs_tq3_8 = in2(24);
coefs_tq3_9 = in2(27);
coefs_tq1_10 = in2(28);
coefs_tq1_11 = in2(31);
coefs_tq2_10 = in2(29);
coefs_tq2_11 = in2(32);
coefs_tq3_10 = in2(30);
coefs_tq3_11 = in2(33);
pinvG3_1 = in1(3);
pinvG3_2 = in1(6);
pinvG3_3 = in1(9);
q0 = in3(1,:);
q1 = in3(2,:);
q2 = in3(3,:);
q3 = in3(4,:);
obj = q0.^2.*(coefs_tq1_1.*pinvG3_1+coefs_tq2_1.*pinvG3_2+coefs_tq3_1.*pinvG3_3)+q1.^2.*(coefs_tq1_5.*pinvG3_1+coefs_tq2_5.*pinvG3_2+coefs_tq3_5.*pinvG3_3)+q2.^2.*(coefs_tq1_8.*pinvG3_1+coefs_tq2_8.*pinvG3_2+coefs_tq3_8.*pinvG3_3)+q3.^2.*(coefs_tq1_10.*pinvG3_1+coefs_tq2_10.*pinvG3_2+coefs_tq3_10.*pinvG3_3)+coefs_tq1_11.*pinvG3_1+coefs_tq2_11.*pinvG3_2+coefs_tq3_11.*pinvG3_3+q0.*q1.*(coefs_tq1_2.*pinvG3_1+coefs_tq2_2.*pinvG3_2+coefs_tq3_2.*pinvG3_3)+q0.*q2.*(coefs_tq1_3.*pinvG3_1+coefs_tq2_3.*pinvG3_2+coefs_tq3_3.*pinvG3_3)+q0.*q3.*(coefs_tq1_4.*pinvG3_1+coefs_tq2_4.*pinvG3_2+coefs_tq3_4.*pinvG3_3)+q1.*q2.*(coefs_tq1_6.*pinvG3_1+coefs_tq2_6.*pinvG3_2+coefs_tq3_6.*pinvG3_3)+q1.*q3.*(coefs_tq1_7.*pinvG3_1+coefs_tq2_7.*pinvG3_2+coefs_tq3_7.*pinvG3_3)+q2.*q3.*(coefs_tq1_9.*pinvG3_1+coefs_tq2_9.*pinvG3_2+coefs_tq3_9.*pinvG3_3);


function [positionSLR,marksSLR,ind,messages]=SLR2(marks,messages)
% post processing rule for delineation
%
% [positionSLR,marksSLR,ind,messages]=SLR2(marks,messages)
%
% INPUT:
% marks: structure with a matrix of annotations in samples; in each field
%        each matrix of annotations has one line per beat and 10 colunms
%        corresponding respectively to P onset, P peak, P end, QRS onset, QRS
%        main peak, QRS end, T onset, T peak, T prima, T end
% messages.setup.SLR.freq: sampling frequency of the recording.
% messages.setup.SLR.QRSlimit: admited diference between marks of the same beat in ms (optional)
%           by default QRSlimit= 250 ms.
% messages.setup.SLR.k: number of neighbours to consider in SLR rule (optional)
%    by default k=3
% messages.setup.SLR.delta: neighbourhood to consider in SLR rule in ms (optional)
%    by default delta = [4 nan 4 12 nan 10 12 nan nan 12] ms for 10 marks
%    or  delta = [4 nan nan 4 nan 12  nan nan nan nan 10 12 nan nan 12] ms for 15 marks
% messages.setup.wavedet.refrper: minimum time between two admisible beats, by default 257ms
% messages.setup.SLR.tolerance = time in ms to define each individual QRS wave, by default 10ms
%
% OUTPUT:
%positionSLR: struture with multilead based on marks obtained by SLR rule
% marksSLR: matrix multilead based on marks obtained by SLR rule
% ind: indexes of the initial column vectors for each set, corresponding to the sincronized beats
%      each line of ind corresponds to a beat and each column to a set
%      if a beat is absent of a set
% messages.errors: errors during the processing
% messages.errors_desc:errors description
% messages.status: sucess of the procesing (1) ou fail (0)
% messages.status: parameters considered
% Last update: 27JAN2012
positionSLR=[];
marksSLR=[];
ind=[];
if nargin<2
    messages.errors= 'Fatal error in SLR.';
    messages.errors_desc= 'Inputs marks and messages.setup.SLR.freq required';
    messages.status=0;
    return
else
    if size(marks{1},2)~=10 && size(marks{1},2)~=15
        messages.errors= 'Fatal error in SLR.';
        messages.errors_desc= 'Input marks should include 10 or 15 marks.';
        messages.status=0;
        return
    end
    if ~isfield(messages,'setup') || ((~isfield(messages.setup,'SLR') ||...
            (~isfield(messages.setup.SLR,'freq') && ~isfield(messages.setup.SLR,'fa')))  &&...
            (~isfield(messages.setup.wavedet,'wavedet') || ( isfield(messages.setup.wavedet,'wavedet') &&...
            ~isfield(messages.setup.wavedet,'freq')))) %
        messages.errors= 'Fatal error in SLR.';
        messages.errors_desc= 'Inputs messages.setup.SLR.freq required';
        messages.status=0;
        return
    else
        try
            fa=messages.setup.SLR.freq;
        catch me
            me.message = 'Not exist messages.setup.SLR.freq';
            try
                fa=messages.setup.SLR.fa;
            catch me
                me.message = 'Not exist messages.setup.SLR.fa';
                fa= messages.setup.wavedet.freq;
            end
        end
    end
end
messages.status=1;
if ~isfield(messages,'errors')
    messages.errors=[];
end
if ~isfield(messages,'errors_desc')
    messages.errors_desc=[];
end
if ~isfield(messages,'warnings')
    messages.warnings=[];
end
if ~isfield(messages,'status')
    messages.status=1;
end
if ~isfield(messages.setup.SLR,'k')
    messages.setup.SLR.k=min(3,size(marks{1},1)-1);
end
if ~isfield(messages.setup.SLR,'QRSlimit')
    messages.setup.SLR.QRSlimit=250;
end
if ~isfield(messages.setup.SLR,'delta')
    if size(marks{1},2)==10
        messages.setup.SLR.delta = [4 nan 4 12 nan 10 12 nan nan 12];
    elseif size(marks{1},2)==15
        messages.setup.SLR.delta = [4 nan nan 4 nan 12  nan nan nan nan 10 12 nan nan 12];
    end
end

QRSlimit= messages.setup.SLR.QRSlimit*fa/1000;
delta= messages.setup.SLR.delta*fa/1000;
k= messages.setup.SLR.k;
if isempty(k) || ~isnumeric(k) || isnan(k)
    k=min(3,size(marks{1},1)-1);
    messages.setup.SLR.k=k;
end
if isempty(QRSlimit) || ~isnumeric(QRSlimit) || isnan(QRSlimit)
    messages.setup.SLR.QRSlimit=250;
    QRSlimit=messages.setup.SLR.QRSlimit*fa/1000;
end
if isempty(delta) | size(delta)~=size(marks{1},2) %#ok<OR2>
    if size(marks{1},2)==10
        messages.setup.SLR.delta = [4 nan 4 12 nan 10 12 nan nan 12];
    elseif size(marks{1},2)==15
        messages.setup.SLR.delta = [4 nan nan 4 nan 12  nan nan nan nan 10 12 nan nan 12];
    end
    delta= messages.setup.SLR.delta*fa/1000;
end

if ~isfield(messages.setup.SLR,'refrper')
    if isfield(messages.setup,'wavedet') && isfield(messages.setup.wavedet,'refrper')
        messages.setup.SLR.refrper=messages.setup.wavedet.refrper*fa/1000;
    else
        messages.setup.SLR.refrper=275*fa/1000;
    end
end
if ~isfield(messages.setup.SLR,'tolerance')
    messages.setup.SLR.tolerance=10*fa/1000;
end

if isstruct(marks)
    leads = fieldnames(marks);
    messages.setup.SLR.nleads=length(leads);
else
   messages.setup.SLR.nleads = size(marks,2);
end

if length(delta)==10
    messages.setup.SLR.marks_desc={'(','P',')','(','QRS',')','(','T','T''',')'};
    peaks=[2 5 8 9];
    onsets=[1 4 6];
    ends=[3 6 10];
    refmark=5;
elseif length(delta)==15
    messages.setup.SLR.marks_desc={'(','P','P''',')','QRS_first_peak','(','Q','R','S','R''',')','(','T','T''',')'};
    peaks=[2 3 8 13 14];
    onsets=[1 6 12];
    ends=[4 11 15];
    refmark=[8 9 7 10];%RUTE % porque la onda R puede ni existir en una derivacion especifica!
end
nmarks=length(delta);
beats=0;
refmark_n=refmark;

for g=1:messages.setup.SLR.nleads
    if isstruct(marks)
        f = getfield(marks, char(leads(g)));  %#ok<GFLD>
    else
        f = marks{g};
    end
    if length(f)> size(beats,1)
        beats((size(beats,1)+1):length(f),1:g)=zeros(length((size(beats,1)+1):length(f)),g);
    end
    for n=1:size(f,1)
        refmark(n)=refmark_n(find(~isnan(f(n,refmark_n)),1,'first')); %#ok<AGROW>
        beats(n,g)=f(n,refmark(n));  %#ok<AGROW>
    end
end

beats(beats==0)=NaN;
[ind,messages1]=sincronizabeats(beats,QRSlimit);
if messages1.status == 0
    return;
end
ind(sum(~isnan(ind),2)<k+1,:) = [];

beats=NaN*ones(size(ind));
for g=1:size(ind,2)
    if isstruct(marks)
        f = getfield(marks, char(leads(g)));  %#ok<GFLD>
    else
        f = marks{g};
    end
    beats(~isnan(ind(:,g)),g)=f(ind(~isnan(ind(:,g)),g),5);
end
ind(find(diff(nanmedian(beats,2)) < messages.setup.SLR.refrper)+1,:) = [];

nbeats=size(ind,1);
marksSLR=NaN.*ones(nbeats,nmarks);
marksSLR_lead=NaN.*ones(nbeats,nmarks);
for mark=peaks
    beats=NaN*ones(size(ind));
    for g=1:size(ind,2)
        if isstruct(marks)
            f = getfield(marks, char(leads(g)));  %#ok<GFLD>
        else
            f = marks{g};
        end
        beats(~isnan(ind(:,g)),g)=f(ind(~isnan(ind(:,g)),g),mark);
    end
    marksSLR(:,mark)=nanmedian(beats,2);
end

if  nmarks==15;
    mark=7:10;
    QRS{1}=NaN*ones(size(ind));
    QRS{2}=QRS{1};
    QRS{3}=QRS{1};
    QRS{4}=QRS{1};
    for g=1:size(ind,2)
        if isstruct(marks)
            f = getfield(marks, char(leads(g)));  %#ok<GFLD>
        else
            f = marks{g};
        end
        QRS{1}(~isnan(ind(:,g)),g)=f(ind(~isnan(ind(:,g)),g),mark(1));  %#ok<AGROW>
        QRS{2}(~isnan(ind(:,g)),g)=f(ind(~isnan(ind(:,g)),g),mark(2)); %#ok<AGROW>
        QRS{3}(~isnan(ind(:,g)),g)=f(ind(~isnan(ind(:,g)),g),mark(3)); %#ok<AGROW>
        QRS{4}(~isnan(ind(:,g)),g)=f(ind(~isnan(ind(:,g)),g),mark(4)); %#ok<AGROW>
    end
    for beat=1:nbeats
        AA=[QRS{1}(beat,:);QRS{2}(beat,:);QRS{3}(beat,:);QRS{4}(beat,:)];
        %look for small R waves that should go with Q waves
        %RS that is an inverted QR or RSR' that is an inverted QRS
        ii=find(AA(2,:)-nanmedian(AA(2,:))<-messages.setup.SLR.tolerance);
        QRS{1}(beat,ii)=QRS{2}(beat,ii); %#ok<AGROW>
        QRS{2}(beat,ii)=QRS{3}(beat,ii); %#ok<AGROW>
        QRS{3}(beat,ii)=QRS{4}(beat,ii); %#ok<AGROW>
        QRS{4}(beat,ii)=NaN; %#ok<AGROW>
        AA=[QRS{1}(beat,:);QRS{2}(beat,:);QRS{3}(beat,:);QRS{4}(beat,:)];
        %look for Q waves that should go with R waves
        %QS complexes or QR that is an inverted RS
        ii=find(AA(1,:)-nanmedian(AA(1,:))>messages.setup.SLR.tolerance);
        QRS{4}(beat,ii)=QRS{3}(beat,ii); %#ok<AGROW>
        QRS{3}(beat,ii)=QRS{2}(beat,ii); %#ok<AGROW>
        QRS{2}(beat,ii)=QRS{1}(beat,ii); %#ok<AGROW>
        QRS{1}(beat,ii)=NaN; %#ok<AGROW>
        AA=[QRS{1}(beat,:);QRS{2}(beat,:);QRS{3}(beat,:);QRS{4}(beat,:)];
        %look for R' waves that should go with S waves
        ii=find(AA(4,:)-nanmedian(AA(4,:))<-messages.setup.SLR.tolerance);
        QRS{1}(beat,ii)=QRS{2}(beat,ii); %#ok<AGROW>
        QRS{2}(beat,ii)=QRS{3}(beat,ii); %#ok<AGROW>
        QRS{3}(beat,ii)=QRS{4}(beat,ii); %#ok<AGROW>
        QRS{4}(beat,ii)=NaN;  %#ok<AGROW>
        AA=[QRS{1}(beat,:);QRS{2}(beat,:);QRS{3}(beat,:);QRS{4}(beat,:)];
        marksSLR(beat,mark)=nanmedian(AA,2)';
    end
end
if size(marksSLR,2) == 15
    positionSLR.P = marksSLR(:,2);
    positionSLR.Pprima = marksSLR(:,3);
    positionSLR.qrs = marksSLR(:,8);
    positionSLR.QRSon = marksSLR(:,6);
    positionSLR.Q = marksSLR(:,7);
    positionSLR.R = marksSLR(:,8);
    positionSLR.S = marksSLR(:,9);
    positionSLR.Rprima = marksSLR(:,10);
    positionSLR.T = marksSLR(:,13);
    positionSLR.Tprima = marksSLR(:,14);
else
    positionSLR.P = marksSLR(:,2);
    positionSLR.qrs = marksSLR(:,5);
    positionSLR.T = marksSLR(:,8);
    positionSLR.Tprima = marksSLR(:,9);
end
for mark=onsets, %onsets
    beats=NaN.*ones(nbeats,nmarks);
    for g=1:size(ind,2)
        if isstruct(marks)
            f = getfield(marks, char(leads(g))); %#ok<GFLD>
        else
            f = marks{g};
        end
        beats(~isnan(ind(:,g)),g)=f(ind(~isnan(ind(:,g)),g),mark);
    end
    aux= sum(~isnan(beats),2);
    for beat=1:nbeats
        if aux(beat)>k, % if the mark was found in more than k of the leads
            kk = beats(beat,:);
            kk(isnan(kk))=[];
            [kk kki] =sort(kk); % sorted the mark found in the leads in beat
            while (length(kk)>k && kk(k+1)>(kk(1)+delta(mark))), %eliminate the first mark until kk satisfy the rule or length(kk)<k
                kk(1)=[];
                kki(1)=[];
            end
            %if length(kk)<k there is not a multilead mark
            if length(kk)==k
                marksSLR(beat,mark)=nan;
            else
                marksSLR(beat,mark)=kk(1);
                marksSLR_lead(beat,mark)=kki(1);
            end
        else
            marksSLR(beat,mark)=nan;
        end
    end
end
positionSLR.Pon = marksSLR(:,onsets(1));
positionSLR.QRSon = marksSLR(:,onsets(2));
positionSLR.Ton = marksSLR(:,onsets(3));
positionSLR.Pon_lead = marksSLR_lead(:,onsets(1));
positionSLR.QRSon_lead = marksSLR_lead(:,onsets(2));
positionSLR.Ton_lead = marksSLR_lead(:,onsets(3));

for mark=ends, % ends
    beats=NaN.*ones(nbeats,nmarks);
    for g=1:size(ind,2)
        if isstruct(marks)
            f = getfield(marks, char(leads(g))); %#ok<GFLD>
        else
            f = marks{g};
        end
        beats(~isnan(ind(:,g)),g)=f(ind(~isnan(ind(:,g)),g),mark);
    end
    aux= sum(~isnan(beats),2);
    for beat=1:nbeats
        if aux(beat)>k, %
            kk = beats(beat,:);
            kk(isnan(kk))=[];
            [kk kki] =sort(kk,'descend');
           % kk =flipud(sort(kk)')';
            while (length(kk)>k && kk(k+1)<(kk(1)-delta(mark))),
                kk(1)=[];
                kki(1)=[];
            end
            if length(kk)==k
                marksSLR(beat,mark)=nan;
            else
                marksSLR(beat,mark)=kk(1);
                marksSLR_lead(beat,mark)=kki(1);
            end
        else
            marksSLR(beat,mark)=nan;
        end
    end
end
positionSLR.Poff = marksSLR(:,ends(1));
positionSLR.QRSoff = marksSLR(:,ends(2));
positionSLR.Toff = marksSLR(:,ends(3));
positionSLR.Poff_lead = marksSLR_lead(:,ends(1));
positionSLR.QRSoff_lead = marksSLR_lead(:,ends(2));
positionSLR.Toff_lead = marksSLR_lead(:,ends(3));


function obj = Make_f11(obj,D_filename,file_suffixes,bathyfile)
% obj = Make_f11(obj,D_filename,file_suffixes)
% Input a msh class object get the values of the density over the depth
% based on D_filename, and computes the depth-averaged value which
% populates the f11 struct in the msh class object.
%
%  Author:      William Pringle
%  Created:     March 19 2018
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if isempty(obj.b)
    error('No bathymetry data in grid to calculate the depth-averaged value')
end
rho0 = 1000;
%g = 9.807;

% %% Do some projection for obj.p
% proj = 'Mercator';
% R = 6378206.4; %[m]
% m_proj(proj,'lon',[ min(obj.p(:,1)) max(obj.p(:,1)) ],...
%             'lat',[ min(obj.p(:,2)) max(obj.p(:,2))])
% [xx,yy] = m_ll2xy(obj.p(:,1),obj.p(:,2));
% xx = R*xx; yy= R*yy;
%
% %% Get the element areas and slopes, connectivity table
% A = polyarea(xx(obj.t(:,1:3))',yy(obj.t(:,1:3))')';
% % Get x differences
% a = [ xx(obj.t(:,3)) - xx(obj.t(:,2))
%       xx(obj.t(:,1)) - xx(obj.t(:,3))
%       xx(obj.t(:,2)) - xx(obj.t(:,1)) ] ;
% a = reshape(a,[],3);
%
% % Get y differences
% b = [ yy(obj.t(:,2)) - yy(obj.t(:,3))
%       yy(obj.t(:,3)) - yy(obj.t(:,1))
%       yy(obj.t(:,1)) - yy(obj.t(:,2)) ];
% b = reshape(b,[],3);
%
% % Get the vertex to element table
% vtoe = VertToEle(obj.t);
% vtoe(vtoe == 0) = length(obj.t) + 1;
% A(end+1) = 0;
% An = sum(A(vtoe))';

% nearest neighbour extrapolation of T and S (0 for no extrapolation)?
FillNaN = 0;

[direc , title , ext ] = fileparts( D_filename);

if isempty(title)
    D_filename = dir(direc);
    D_filename(1:2) = [];
    idx = zeros(length(D_filename),1);
    for jj = 1:size(file_suffixes,1)
        for ii = 1:length(D_filename)
            if idx(ii) == 0
                idx(ii) = contains(D_filename(ii).name,file_suffixes(jj,:));
            end
        end
    end
    D_filename = D_filename(find(idx));
else
   name = D_filename; D_filename = [];
   D_filename.folder = direc;
   D_filename.name = [title ext];
end

Bx = NaN(length(obj.b),size(D_filename,1));
By = NaN(length(obj.b),size(D_filename,1));
tic
for ii = 1:size(D_filename,1)
    Dn = [D_filename(ii).folder '/' D_filename(ii).name];
    [~ , ~ , ext ] = fileparts( Dn);
    %% Read the grid and density data
    if strcmp(ext,'.nc')
        try
            title = ncreadatt(Dn,'/','title'); % Reading some
            lon = ncread(Dn,'lon');
            lat = ncread(Dn,'lat');
            z   = ncread(Dn,'depth');
            sigma_t = ncread(Dn,'I_an');
            time_all = 0;
        catch
            title = ncreadatt(Dn,'/','History'); % Reading some
            [time_all(ii),lon,lat,z,~,~,dpx,dpy] = Make_Gridded_rho(Dn,...
         {'time','depth','lon','lat','salinity','water_temp','surf_el'}); %,...
                                                                %bathyfile);
            disp(['Read time ' datestr(time_all(ii))])
            %if ii == 1
            %    sigma_t = zeros([size(rho) size(D_filename,1)]);
            %end
            %sigma_t(:,:,:,ii) = rho - 1000; clear rho
        end
    elseif strcmp(ext,'.mat')
        load(Dn);
        %sigma_t = rho - 1000; clear rho
    else
        error('does not understand file extension')
    end
%     % zeta on the finite-element mesh
%     [Lon,Lat] = ndgrid(lon,lat);
%     F = griddedInterpolant(Lon,Lat,zeta,'linear','none');
%     zeta = F(obj.p);
%     % Get rho on the finite-element mesh
%     [~,~,~,rho] = Gridded_to_Mesh_SeaBed_DepthAveraged(...
%             obj.p(:,1),obj.p(:,2),obj.b,z,rho,lon,lat,FillNaN);
%
%     % Compute bx, by for each element (loop over to reduce memory issues)
%     bxe = zeros(length(obj.t),length(z));
%     bye = zeros(length(obj.t),length(z));
%     for k = 1:length(z)
%         if k == 1
%             BCPe = zeta.*rho{k};
%         else
%             BCPe = rho{k} - rho0;
%         end
%         BCPe = BCPe(obj.t);
%         bxe(:,k) = 0.5 * sum(BCPe.*b,2);
%         bye(:,k) = 0.5 * sum(BCPe.*a,2);
%     end
%     bxe(end+1,:) = 0; bye(end+1,:) = 0;
%     bx = zeros(length(obj.b),length(z));
%     by = zeros(length(obj.b),length(z));
%     for k = length(z):-1:1
%         if k > 1
%             bxe(:,k) = trapz(z(1:k),bxe(:,1:k),2);
%             bye(:,k) = trapz(z(1:k),bye(:,1:k),2);
%         end
%         temp = sum(reshape(bxe(vtoe,k),size(vtoe,1),[]),'omitnan')';
%         bx(:,k) = temp./An;
%         by(:,k) = temp./An;
%     end

    % Interpolate the gradient at each depth
    dpx(isnan(dpy)) = NaN; dpy(isnan(dpx)) = NaN;
    [lonN,latN] = ndgrid(lon,lat);
    bx = NaN(length(obj.p),length(z));
    by = NaN(length(obj.p),length(z));
    for kk = 1:length(z)
        if kk == 1
            Fx = griddedInterpolant(lonN,latN,dpx(:,:,kk),'linear','none');
            Fy = griddedInterpolant(lonN,latN,dpy(:,:,kk),'linear','none');
        else
            Fx.Values = dpx(:,:,kk);
            Fy.Values = dpy(:,:,kk);
        end
        bb = obj.b >= z(kk);
        pt = obj.p(bb,:);
        bxt = Fx(obj.p(bb,:));
        byt = Fy(obj.p(bb,:));
        if ~isempty(find(isnan(bxt), 1))
            idn = find(isnan(bxt));
            idg = find(~isnan(bxt));
            idx = knnsearch(pt(idg,:),pt(idn,:));
            bxt(idn) = bxt(idg(idx));
        end
        if ~isempty(find(isnan(byt), 1))
            idn1 = find(isnan(byt));
            idg = find(~isnan(byt));
            if sum(idn - idn1) ~= 0
                idx = knnsearch(pt(idg,:),pt(idn1,:));
            end
            byt(idn1) = byt(idg(idx));
        end
        bx(bb,kk) = bxt;
        by(bb,kk) = byt;
    end
    % Now do the integration over the depth taking into account actual
    % depth on the computational mesh
    for kk = length(z):-1:1
        bb = isnan(Bx(:,ii)) & obj.b > z(kk) & ...
            ~isnan(bx(:,kk)) & ~isnan(by(:,kk));
        Bx(bb,ii) = trapz(z(1:kk),bx(bb,1:kk),2) + ...
                          bx(bb,kk).*(obj.b(bb) - z(kk));
        By(bb,ii) = trapz(z(1:kk),by(bb,1:kk),2) + ...
                          by(bb,kk).*(obj.b(bb) - z(kk));
    end
    % Get the depth averaged
    Bx(:,ii) = Bx(:,ii)./obj.b/rho0;
    By(:,ii) = By(:,ii)./obj.b/rho0;

%     figure;
%     fastscatter(obj.p(:,1),obj.p(:,2),9.807*hypot(Bx,By))
%     colormap(cmocean('speed'))
%     caxis([0 1e-4])
%     colorbar;
end
toc
disp(time_all)
DTIMINC = round(seconds(median(diff(time_all))));

%
% tic
% Bx = zeros(length(obj.b),size(sigma_t,4));
% By = zeros(length(obj.b),size(sigma_t,4));
% for t = 1:size(sigma_t,4)
%     disp(['Computing ' datestr(time_all(t))])
%     %% Calculate the depth-averaged density
%     [~,Sigma_tm,~,Sigma_3D] = Gridded_to_Mesh_SeaBed_DepthAveraged(...
%             obj.p(:,1),obj.p(:,2),obj.b,z,sigma_t(:,:,:,t),lon,lat,FillNaN);
%
%     %% Calculate the integrated barcolinic pressure's
%     Sigma_3D = reshape(cell2mat(Sigma_3D),[],length(Sigma_3D));
%     % Make sure derivative with "ground" returns NaN;
%     BCP = NaN(length(obj.b),length(z));
%     for k = 1:length(z)
%         bb = obj.b >= z(k);
%         BCP(bb,k) = trapz(z(1:k),Sigma_3D(bb,1:k),2);
%     end
%     BCP = BCP/rho0;
%
%     %% Calculate the bx/by's for each element
%     % Compute bx, by for each element (loop over to reduce memory issues)
%     bxe = zeros(length(obj.t),length(z));
%     bye = zeros(length(obj.t),length(z));
%     for k = 1:length(z)
%         BCPe = BCP(:,k);
%         BCPe = BCPe(obj.t);
%         bxe(:,k) = 0.5 * sum(BCPe.*b,2);
%         bye(:,k) = 0.5 * sum(BCPe.*a,2);
%     end
%     %% Sum the contributions of each element connected to a node
%     % Add in a zero ghost element and refer to the zero indices of vtoe to this
%     bxe(end+1,:) = 0; bye(end+1,:) = 0;
%
%     % Sum up all element contributions to each node and take the integral
%     bx = zeros(length(obj.b),length(z));
%     by = zeros(length(obj.b),length(z));
%     for k = 1:length(z)
%         bb = obj.b >= z(k);
%         temp = sum(reshape(bxe(vtoe,k),size(vtoe,1),[]),'omitnan')';
%         bx(bb,k) = temp(bb)./An(bb);
%         temp = sum(reshape(bye(vtoe,k),size(vtoe,1),[]),'omitnan')';
%         by(bb,k) = temp(bb)./An(bb);
%     end
%     Bx(:,t) = trapz(z,bx,2)./obj.b;
%     By(:,t) = trapz(z,by,2)./obj.b;
%     Bx(obj.b == 0,t) = NaN; By(obj.b == 0,t) = NaN;
% %     % On the structured grid
%     %% Calculate the integrated barcolinic pressure's
%     Sigma_3D = sigma_t(:,:,:,t);
%     % Make sure derivative with "ground" returns NaN;
%     BCP = NaN(size(Sigma_3D));
%     for k = 1:length(z)
%         BCP(:,:,k) = trapz(z(1:k),Sigma_3D(:,:,1:k),3);
%     end
%     BCP = BCP/rho0;
%
%     bx = zeros(size(Sigma_3D));
%     by = zeros(size(Sigma_3D));
%     for k = 1:length(z)
%         [bx(:,:,k),by(:,:,k)] = gradient(BCP(:,:,k),(lon(2)-lon(1))*111e3,(lat(2)-lat(1))*111e3);
%     end
%     bxv = reshape(bx,[],length(z));
%     byv = reshape(by,[],length(z));
%     Bx_struc = NaN(size(bxv,1),1);
%     By_struc = NaN(size(bxv,1),1);
%     for k = 1:length(z)
%         bb = ~isnan(bxv(:,k));
%         Bx_struc(bb) = trapz(z(1:k),bxv(bb,1:k),2)./z(k);
%         bb = ~isnan(byv(:,k));
%         By_struc(bb) = trapz(z(1:k),byv(bb,1:k),2)./z(k);
%     end
%     Bx_struc(Bx_struc == 0) = NaN;
%     By_struc(By_struc == 0) = NaN;
%     Bx_struc = reshape(Bx_struc,size(Sigma_3D,1),[])';
%     By_struc = reshape(By_struc,size(Sigma_3D,1),[])';
%     figure;
%     pcolor(hypot(Bx_struc,By_struc))
%     shading interp
%     colormap(cmocean('ice'))
%     caxis([0 2e-6])
%     colorbar;
%end
%toc
% Calculate the depth-averaged
% rhoxe = sum(Sigma_tm(obj.t).*b,2)/rho0;
% rhoye = sum(Sigma_tm(obj.t).*a,2)/rho0;
% rhoxe(end+1,:) = 0; rhoye(end+1,:) = 0;
% rhox = sum(rhoxe(vtoe))'./An;
% rhoy = sum(rhoye(vtoe))'./An;
% Bx2D = 0.5*obj.b.^2.*rhox;
% By2D = 0.5*obj.b.^2.*rhoy;

%% Make into f11 struct
%summary = ncreadatt(D_filename,'/','summary'); % global attributes
obj.f11.DataTitle = title;
%[~,name,~] = fileparts(D_filename);
%obj.f11.DataSubTitle = [summary ': ' name];
obj.f11.DTIMINC = DTIMINC;
obj.f11.TimeVec = time_all;
obj.f11.NumOfNodes = length(obj.b);
obj.f11.Val = cell(size(Bx,2),1); %zeros(3,length(obj.b),size(sigma_t,4));
for t = 1:size(Bx,2)
    idx = find(~isnan(Bx(:,t)) & ~isnan(By(:,t)));
    obj.f11.Val{t} = [idx'; Bx(idx,t)'; By(idx,t)']; % Sigma_tm';
end
%EOF
end


function [oz] = mi32oz(mi3)
% Convert volume from cubic miles to US liquid ounces.
% Chad Greene 2012
oz = mi3*140942994870000;


function [E,dE,f,g] = spm_DEM_eval(M,qu,qp)
% evaluates state equations and derivatives for DEM schemes
% FORMAT [E dE f g] = spm_DEM_eval(M,qu,qp)
%
% M  - model structure
% qu - conditional mode of states
%  qu.v{i} - casual states
%  qu.x(i) - hidden states
%  qu.y(i) - response
%  qu.u(i) - input
% qp - conditional density of parameters
%  qp.p{i} - parameter deviates for i-th level
%  qp.u(i) - basis set
%  qp.x(i) - expansion point ( = prior expectation)
%
% E  - generalised errors  (i.e.., y - g(x,v,P); x[1] - f(x,v,P))
%
% dE:
%  dE.du   - de[1:n]/du
%  dE.dy   - de[1:n]/dy[1:n]
%  dE.dc   - de[1:n]/dc[1:d]
%  dE.dp   - de[1:n]/dp
%  dE.dup  - d/dp[de[1:n]/du
%  dE.dpu  - d/du[de[1:n]/dp
%
% where u = x{1:d]; v[1:d]
%
% To accelerate computations one can specify the nature of the model using
% the field:
%
% M(1).E.linear = 0: full        - evaluates 1st and 2nd derivatives
% M(1).E.linear = 1: linear      - equations are linear in x and v
% M(1).E.linear = 2: bilinear    - equations are linear in x, v & x*v
% M(1).E.linear = 3: nonlinear   - equations are linear in x, v, x*v, & x*x
% M(1).E.linear = 4: full linear - evaluates 1st derivatives (for generalised
%                                  filtering, where parameters change)
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_DEM_eval.m 6270 2014-11-29 12:04:48Z karl $

% get dimensions
%==========================================================================
nl    = size(M,2);                       % number of levels
ne    = sum(spm_vec(M.l));               % number of e (errors)
nv    = sum(spm_vec(M.m));               % number of x (causal states)
nx    = sum(spm_vec(M.n));               % number of x (hidden states)
np    = sum(spm_vec(M.p));               % number of p (parameters)

% evaluate functions at each hierarchical level
%==========================================================================

% Get states {qu.v{1},qu.x{1}} in hierarchical form (v{i},x{i})
%--------------------------------------------------------------------------
v     = spm_unvec(qu.v{1},{M(1 + 1:end).v});
x     = spm_unvec(qu.x{1},{M(1:end - 1).x});
for i = 1:(nl - 1)
    p      = spm_unvec(spm_vec(M(i).pE) + qp.u{i}*qp.p{i},M(i).pE);
    f{i,1} = feval(M(i).f,x{i},v{i},p);
    g{i,1} = feval(M(i).g,x{i},v{i},p);
end

% Get Derivatives
%==========================================================================
persistent D
try
    method = M(1).E.linear;
catch
    method = 0;
end

switch method

    % get derivatives at each iteration of D-step - full evaluation
    %----------------------------------------------------------------------
    case{0}

        D     = spm_DEM_eval_diff(x,v,qp,M);

        % gradients w.r.t. states
        %------------------------------------------------------------------
        dedy  = D.dedy;
        dedc  = D.dedc;
        dfdy  = D.dfdy;
        dfdc  = D.dfdc;
        dgdx  = D.dgdx;
        dgdv  = D.dgdv;
        dfdv  = D.dfdv;
        dfdx  = D.dfdx;
        dgdxp = D.dgdxp;
        dfdxp = D.dfdxp;
        dgdvp = D.dgdvp;
        dfdvp = D.dfdvp;

        % gradients w.r.t. parameters
        %------------------------------------------------------------------
        dgdp  = D.dgdp;
        dfdp  = D.dfdp;

    % linear: assume equations are linear in x and v
    %----------------------------------------------------------------------
    case{1}

        % get derivatives and store expansion point (states)
        %------------------------------------------------------------------
        if isempty(D)

            D     = spm_DEM_eval_diff(x,v,qp,M);
            D.x   = x;
            D.v   = v;

            % gradients w.r.t. states
            %--------------------------------------------------------------
            dedy  = D.dedy;
            dedc  = D.dedc;
            dfdy  = D.dfdy;
            dfdc  = D.dfdc;
            dgdx  = D.dgdx;
            dgdv  = D.dgdv;
            dfdv  = D.dfdv;
            dfdx  = D.dfdx;

            % gradients w.r.t. parameters (state-dependent)
            %--------------------------------------------------------------
            dgdxp = D.dgdxp;
            dfdxp = D.dfdxp;
            dgdvp = D.dgdvp;
            dfdvp = D.dfdvp;

            % gradients w.r.t. parameters
            %--------------------------------------------------------------
            dgdp  = D.dgdp;
            dfdp  = D.dfdp;

        % linear expansion for derivatives w.r.t. parameters
        %------------------------------------------------------------------
        else

            % gradients w.r.t. states
            %--------------------------------------------------------------
            dedy  = D.dedy;
            dedc  = D.dedc;
            dfdy  = D.dfdy;
            dfdc  = D.dfdc;
            dgdx  = D.dgdx;
            dgdv  = D.dgdv;
            dfdv  = D.dfdv;
            dfdx  = D.dfdx;
            dgdxp = D.dgdxp;
            dfdxp = D.dfdxp;
            dgdvp = D.dgdvp;
            dfdvp = D.dfdvp;

            % gradients w.r.t. parameters
            %--------------------------------------------------------------
            dx    = spm_vec(qu.x{1}) - spm_vec(D.x);
            dv    = spm_vec(qu.v{1}) - spm_vec(D.v);
            dgdp  = D.dgdp;
            dfdp  = D.dfdp;
            for p = 1:np
                dgdp(:,p) = D.dgdp(:,p) + D.dgdxp{p}*dx + D.dgdvp{p}*dv;
                if nx
                    dfdp(:,p) = D.dfdp(:,p) + D.dfdxp{p}*dx + D.dfdvp{p}*dv;
                end
            end

        end

    % bilinear: assume equations are linear in x and v and x*v
    %----------------------------------------------------------------------
    case{2}

        % get derivatives and store expansion point (states)
        %------------------------------------------------------------------
        if isempty(D)

            % get high-order derivatives
            %--------------------------------------------------------------
            [Dv D] = spm_diff('spm_DEM_eval_diff',x,v,qp,M,2);

            for i = 1:nv, Dv{i} = spm_unvec(Dv{i},D); end
            D.x   = x;
            D.v   = v;
            D.Dv  = Dv;

            % gradients w.r.t. states
            %--------------------------------------------------------------
            dedy  = D.dedy;
            dedc  = D.dedc;
            dfdy  = D.dfdy;
            dfdc  = D.dfdc;
            dgdx  = D.dgdx;
            dgdv  = D.dgdv;
            dfdv  = D.dfdv;
            dfdx  = D.dfdx;
            dgdxp = D.dgdxp;
            dfdxp = D.dfdxp;
            dgdvp = D.dgdvp;
            dfdvp = D.dfdvp;

            % gradients w.r.t. parameters
            %--------------------------------------------------------------
            dgdp  = D.dgdp;
            dfdp  = D.dfdp;

        % linear expansion for derivatives w.r.t. parameters
        %------------------------------------------------------------------
        else

            % gradients w.r.t. causes and data
            %--------------------------------------------------------------
            dedy  = D.dedy;
            dedc  = D.dedc;
            dfdy  = D.dfdy;
            dfdc  = D.dfdc;

            % states (relative to expansion point)
            %--------------------------------------------------------------
            dv    = spm_vec(qu.v{1}) - spm_vec(D.v);

            % gradients w.r.t. states
            %--------------------------------------------------------------
            dgdx  = D.dgdx;
            dgdv  = D.dgdv;
            dfdx  = D.dfdx;
            dfdv  = D.dfdv;
            for i = 1:nv; dgdx = dgdx + D.Dv{i}.dgdx*dv(i); end
            for i = 1:nv; dgdv = dgdv + D.Dv{i}.dgdv*dv(i); end
            for i = 1:nv; dfdx = dfdx + D.Dv{i}.dfdx*dv(i); end
            for i = 1:nv; dfdv = dfdv + D.Dv{i}.dfdv*dv(i); end

            % second-order derivatives
            %--------------------------------------------------------------
            dgdxp = D.dgdxp;
            dgdvp = D.dgdvp;
            dfdxp = D.dfdxp;
            dfdvp = D.dfdvp;
            for p = 1:np
                for i = 1:nv; dgdxp{p} = dgdxp{p} + D.Dv{i}.dgdxp{p}*dv(i); end
                for i = 1:nv; dgdvp{p} = dgdvp{p} + D.Dv{i}.dgdvp{p}*dv(i); end
                for i = 1:nv; dfdxp{p} = dfdxp{p} + D.Dv{i}.dfdxp{p}*dv(i); end
                for i = 1:nv; dfdvp{p} = dfdvp{p} + D.Dv{i}.dfdvp{p}*dv(i); end
            end

            % gradients w.r.t. parameters
            %--------------------------------------------------------------
            dgdp  = D.dgdp;
            dfdp  = D.dfdp;
            for p = 1:np
                Dgdxp     = (D.dgdxp{p} + dgdxp{p})/2;
                Dgdvp     = (D.dgdvp{p} + dgdvp{p})/2;
                Dfdxp     = (D.dfdxp{p} + dfdxp{p})/2;
                Dfdvp     = (D.dfdvp{p} + dfdvp{p})/2;
                dgdp(:,p) = dgdp(:,p) + Dgdvp*dv;
                dfdp(:,p) = dfdp(:,p) + Dfdvp*dv;
            end

        end

    % nonlinear: assume equations are bilinear in x and v
    %----------------------------------------------------------------------
    case{3}

        % get derivatives and store expansion point (states)
        %------------------------------------------------------------------
        if isempty(D)

            % get high-order derivatives
            %--------------------------------------------------------------
            [Dx D] = spm_diff('spm_DEM_eval_diff',x,v,qp,M,1,'q');
            [Dv D] = spm_diff('spm_DEM_eval_diff',x,v,qp,M,2,'q');

            for i = 1:nx, Dx{i} = spm_unvec(Dx{i},D); end
            for i = 1:nv, Dv{i} = spm_unvec(Dv{i},D); end
            D.x   = x;
            D.v   = v;
            D.Dx  = Dx;
            D.Dv  = Dv;

            % gradients w.r.t. states
            %--------------------------------------------------------------
            dedy  = D.dedy;
            dedc  = D.dedc;
            dfdy  = D.dfdy;
            dfdc  = D.dfdc;
            dgdx  = D.dgdx;
            dgdv  = D.dgdv;
            dfdv  = D.dfdv;
            dfdx  = D.dfdx;
            dgdxp = D.dgdxp;
            dfdxp = D.dfdxp;
            dgdvp = D.dgdvp;
            dfdvp = D.dfdvp;

            % gradients w.r.t. parameters
            %--------------------------------------------------------------
            dgdp  = D.dgdp;
            dfdp  = D.dfdp;

        % linear expansion for derivatives w.r.t. parameters
        %------------------------------------------------------------------
        else

            % gradients w.r.t. causes and data
            %--------------------------------------------------------------
            dedy  = D.dedy;
            dedc  = D.dedc;
            dfdy  = D.dfdy;
            dfdc  = D.dfdc;

            % states (relative to expansion point)
            %--------------------------------------------------------------
            dx    = spm_vec(qu.x{1}) - spm_vec(D.x);
            dv    = spm_vec(qu.v{1}) - spm_vec(D.v);

            % gradients w.r.t. states
            %--------------------------------------------------------------
            dgdx  = D.dgdx;
            dgdv  = D.dgdv;
            dfdx  = D.dfdx;
            dfdv  = D.dfdv;
            for i = 1:nx; dgdx = dgdx + D.Dx{i}.dgdx*dx(i); end
            for i = 1:nv; dgdx = dgdx + D.Dv{i}.dgdx*dv(i); end
            for i = 1:nx; dgdv = dgdv + D.Dx{i}.dgdv*dx(i); end
            for i = 1:nv; dgdv = dgdv + D.Dv{i}.dgdv*dv(i); end
            for i = 1:nx; dfdx = dfdx + D.Dx{i}.dfdx*dx(i); end
            for i = 1:nv; dfdx = dfdx + D.Dv{i}.dfdx*dv(i); end
            for i = 1:nx; dfdv = dfdv + D.Dx{i}.dfdv*dx(i); end
            for i = 1:nv; dfdv = dfdv + D.Dv{i}.dfdv*dv(i); end

            % second-order derivatives
            %--------------------------------------------------------------
            dgdxp = D.dgdxp;
            dgdvp = D.dgdvp;
            dfdxp = D.dfdxp;
            dfdvp = D.dfdvp;
            for p = 1:np
                for i = 1:nx; dgdxp{p} = dgdxp{p} + D.Dx{i}.dgdxp{p}*dx(i); end
                for i = 1:nv; dgdxp{p} = dgdxp{p} + D.Dv{i}.dgdxp{p}*dv(i); end
                for i = 1:nx; dgdvp{p} = dgdvp{p} + D.Dx{i}.dgdvp{p}*dx(i); end
                for i = 1:nv; dgdvp{p} = dgdvp{p} + D.Dv{i}.dgdvp{p}*dv(i); end
                for i = 1:nx; dfdxp{p} = dfdxp{p} + D.Dx{i}.dfdxp{p}*dx(i); end
                for i = 1:nv; dfdxp{p} = dfdxp{p} + D.Dv{i}.dfdxp{p}*dv(i); end
                for i = 1:nx; dfdvp{p} = dfdvp{p} + D.Dx{i}.dfdvp{p}*dx(i); end
                for i = 1:nv; dfdvp{p} = dfdvp{p} + D.Dv{i}.dfdvp{p}*dv(i); end
            end

            % gradients w.r.t. parameters
            %--------------------------------------------------------------
            dgdp  = D.dgdp;
            dfdp  = D.dfdp;
            for p = 1:np
                Dgdxp     = (D.dgdxp{p} + dgdxp{p})/2;
                Dgdvp     = (D.dgdvp{p} + dgdvp{p})/2;
                Dfdxp     = (D.dfdxp{p} + dfdxp{p})/2;
                Dfdvp     = (D.dfdvp{p} + dfdvp{p})/2;
                dgdp(:,p) = dgdp(:,p) + Dgdxp*dx + Dgdvp*dv;
                dfdp(:,p) = dfdp(:,p) + Dfdxp*dx + Dfdvp*dv;
            end

        end

    % repeated evaluation of first order derivatives (for Laplace scheme)
    %----------------------------------------------------------------------
    case{4}

        % get derivatives and store expansion point (states)
        %------------------------------------------------------------------
        if isempty(D)

            D     = spm_DEM_eval_diff(x,v,qp,M);
            D.x   = x;
            D.v   = v;

            % gradients w.r.t. states
            %--------------------------------------------------------------
            dedy  = D.dedy;
            dedc  = D.dedc;
            dfdy  = D.dfdy;
            dfdc  = D.dfdc;
            dgdx  = D.dgdx;
            dgdv  = D.dgdv;
            dfdv  = D.dfdv;
            dfdx  = D.dfdx;

            % gradients w.r.t. parameters (state-dependent)
            %--------------------------------------------------------------
            dgdxp = D.dgdxp;
            dfdxp = D.dfdxp;
            dgdvp = D.dgdvp;
            dfdvp = D.dfdvp;

            % gradients w.r.t. parameters
            %--------------------------------------------------------------
            dgdp  = D.dgdp;
            dfdp  = D.dfdp;

        % re-evaluate first-order derivatives
        %------------------------------------------------------------------
        else

            % retain second-order gradients
            %--------------------------------------------------------------
            dgdxp = D.dgdxp;
            dfdxp = D.dfdxp;
            dgdvp = D.dgdvp;
            dfdvp = D.dfdvp;

            % re-evaluate first-order gradients
            %--------------------------------------------------------------
            D     = spm_DEM_eval_diff(x,v,qp,M,0);
            dedy  = D.dedy;
            dedc  = D.dedc;
            dfdy  = D.dfdy;
            dfdc  = D.dfdc;
            dgdx  = D.dgdx;
            dgdv  = D.dgdv;
            dfdv  = D.dfdv;
            dfdx  = D.dfdx;

            % replace second-order gradients
            %--------------------------------------------------------------
            D.dgdxp = dgdxp;
            D.dfdxp = dfdxp;
            D.dgdvp = dgdvp;
            D.dfdvp = dfdvp;

            % gradients w.r.t. parameters
            %--------------------------------------------------------------
            dx    = spm_vec(qu.x{1}) - spm_vec(x);
            dv    = spm_vec(qu.v{1}) - spm_vec(v);
            dgdp  = D.dgdp;
            dfdp  = D.dfdp;
            for p = 1:np
                dgdp(:,p) = D.dgdp(:,p) + D.dgdxp{p}*dx + D.dgdvp{p}*dv;
                if nx
                    dfdp(:,p) = D.dfdp(:,p) + D.dfdxp{p}*dx + D.dfdvp{p}*dv;
                end
            end

        end

    otherwise
        disp('Unknown method')

end

% order parameters (d = n = 1 for static models)
%--------------------------------------------------------------------------
d       = M(1).E.d + 1;                    % generalisation order of q(v)
n       = M(1).E.n + 1;                    % embedding order     (n >= d)

% Generalised prediction errors and derivatives
%==========================================================================
Ex      = cell(n,1);
Ev      = cell(n,1);
[Ex{:}] = deal(sparse(nx,1));
[Ev{:}] = deal(sparse(ne,1));

% prediction error (E) - causes
%--------------------------------------------------------------------------
for i = 1:n
    qu.y{i} = spm_vec(qu.y{i});
end
Ev{1} = [qu.y{1}; qu.v{1}] - [spm_vec(g); qu.u{1}];
for i = 2:n
    Ev{i} = dedy*qu.y{i} + dedc*qu.u{i} ...      % generalised response
          - dgdx*qu.x{i} - dgdv*qu.v{i};         % and prediction
end

% prediction error (E) - states
%--------------------------------------------------------------------------
try
    Ex{1} = qu.x{2} - spm_vec(f);
end
for i = 2:n - 1
    Ex{i} = qu.x{i + 1} ...                      % generalised motion
          - dfdx*qu.x{i} - dfdv*qu.v{i};         % and prediction
end

% error
%--------------------------------------------------------------------------
E     = spm_vec({Ev,Ex});

% Kronecker forms of derivatives for generalised motion
%==========================================================================
if nargout < 2, return, end

% dE.dp (parameters)
%--------------------------------------------------------------------------
dgdp  = {dgdp};
dfdp  = {dfdp};
for i = 2:n
    dgdp{i,1} = dgdp{1};
    dfdp{i,1} = dfdp{1};
    for p = 1:np
        dgdp{i,1}(:,p) = dgdxp{p}*qu.x{i} + dgdvp{p}*qu.v{i};
        dfdp{i,1}(:,p) = dfdxp{p}*qu.x{i} + dfdvp{p}*qu.v{i};
    end
end

% generalised temporal derivatives: dE.du (states)
%--------------------------------------------------------------------------
dedy  = kron(spm_speye(n,n),dedy);
dedc  = kron(spm_speye(n,d),dedc);
dfdy  = kron(spm_speye(n,n),dfdy);
dfdc  = kron(spm_speye(n,d),dfdc);
dgdx  = kron(spm_speye(n,n),dgdx);
dgdv  = kron(spm_speye(n,d),dgdv);
dfdv  = kron(spm_speye(n,d),dfdv);
dfdx  = kron(spm_speye(n,n),dfdx) - kron(spm_speye(n,n,1),speye(nx,nx));

% 1st error derivatives (states)
%--------------------------------------------------------------------------
dE.dy =  spm_cat({dedy; dfdy});
dE.dc =  spm_cat({dedc; dfdc});
dE.dp = -spm_cat({dgdp; dfdp});
dE.du = -spm_cat({dgdx, dgdv  ;
                  dfdx, dfdv});

% bilinear derivatives
%--------------------------------------------------------------------------
for i = 1:np
    dgdxp{i}  = kron(spm_speye(n,n),dgdxp{i});
    dfdxp{i}  = kron(spm_speye(n,n),dfdxp{i});
    dgdvp{i}  = kron(spm_speye(n,d),dgdvp{i});
    dfdvp{i}  = kron(spm_speye(n,d),dfdvp{i});
    dE.dup{i} = -spm_cat({dgdxp{i}, dgdvp{i};
                          dfdxp{i}, dfdvp{i}});
end
if np
    dE.dpu    =  spm_cell_swap(dE.dup);
else
    dE.dpu    =  {};
end


 classdef matRad_MinMaxDVH < DoseConstraints.matRad_DoseConstraint
    % matRad_MinMaxDVH Implements a MinMaxDVH constraint
    %   See matRad_DoseConstraint for interface description
    %
    % References
    %   -
    %
    % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %
    % Copyright 2020 the matRad development team.
    %
    % This file is part of the matRad project. It is subject to the license
    % terms in the LICENSE file found in the top-level directory of this
    % distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
    % of the matRad project, including this file, may be copied, modified,
    % propagated, or distributed except according to the terms contained in the
    % LICENSE file.
    %
    % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    properties (Constant)
        name = 'DVH constraint';
        parameterNames = {'d^{ref}', 'V^{min}', 'V^{max}'};
        %parameterIsDose = logical([1 0 0]);
        parameterTypes = {'dose','numeric','numeric'};
    end

    properties
        voxelScalingRatio = 1;
        referenceScalingVal = 0.01;
        parameters = {30,0,100};
    end

    methods
        function obj = matRad_MinMaxDVH(dRef,vMin,vMax)

            %If we have a struct in first argument
            if nargin == 1 && isstruct(dRef)
                inputStruct = dRef;
                initFromStruct = true;
            else
                initFromStruct = false;
                inputStruct = [];
            end

            %Call Superclass Constructor (for struct initialization)
            obj@DoseConstraints.matRad_DoseConstraint(inputStruct);

            %now handle initialization from other parameters
            if ~initFromStruct
                if nargin == 3 && isscalar(vMax)
                    obj.parameters{3} = vMax;
                end

                if nargin >= 1 && isscalar(dRef)
                    obj.parameters{1} = dRef;
                end

                if nargin >= 2 && isscalar(vMin)
                    obj.parameters{2} = vMin;
                end
            end
        end

        %Overloads the struct function to add constraint specific
        %parameters
        function s = struct(obj)
            s = struct@DoseConstraints.matRad_DoseConstraint(obj);
            s.voxelScalingRatio = 1;
            s.referenceScalingVal = 0.01;
        end

        function cu = upperBounds(obj,n)
            cu = obj.parameters{3} / 100;
        end
        function cl = lowerBounds(obj,n)
            cl = obj.parameters{2} / 100;
        end
        %% Calculates the Constraint Function value
        function cDose = computeDoseConstraintFunction(obj,dose)

            %Fast DVH point calculation
            cDose = sum(dose >= obj.parameters{1})/numel(dose);

            %cDose = 100 * cDose; %In Percent

            % alternative constraint calculation 3/4 %
            % % get reference Volume
            % refVol = cst{j,6}(k).volume/100;
            %
            % % calc deviation
            % deviation = d_i - d_ref;
            %
            % % calc d_ref2: V(d_ref2) = refVol
            % d_ref2 = matRad_calcInversDVH(refVol,d_i);
            %
            % % apply lower and upper dose limits
            % if isequal(cst{j,6}(k).type, 'max DVH constraint')
            %    deviation(d_i < d_ref | d_i > d_ref2) = 0;
            % elseif isequal(cst{j,6}(k).type, 'min DVH constraint')
            %    deviation(d_i > d_ref | d_i < d_ref2) = 0;
            % end
            %
            % %c = sum(deviation);                              % linear deviation
            % %c = deviation'*deviation;                        % square devioation
            % c = (1/size(cst{j,4},1))*(deviation'*deviation); % square deviation with normalization
            % %c = (deviation).^2'*(deviation).^2;               % squared square devioation
            % alternative constraint calculation 3/4 %
        end

        %% Calculates the Constraint jacobian
        function cDoseJacob  = computeDoseConstraintJacobian(obj,dose)
            %logistic approximation

            %Do we really need to sort two times?
            dose_sort = sort(dose);

            % calculate scaling
            NoVoxels     = max(obj.voxelScalingRatio*numel(dose),10);
            absDiffsort  = sort(abs(obj.parameters{1} - dose_sort));

            deltaDoseMax = absDiffsort(min(ceil(NoVoxels/2),numel(dose)));

            % calclulate DVHC scaling
            DVHCScaling = min((log(1/obj.referenceScalingVal-1))/(2*deltaDoseMax),250);

            d_diff = dose - obj.parameters{1};

            cDoseJacob = (2/numel(dose))*DVHCScaling*exp(2*DVHCScaling*d_diff)./(exp(2*DVHCScaling*d_diff)+1).^2;

            % alternative constraint calculation 4/4 %
            % % get reference Volume
            % refVol = cst{j,6}(k).volume/100;
            %
            % % calc deviation
            % deviation = d_i - d_ref;
            %
            % % calc d_ref2: V(d_ref2) = refVol
            % d_ref2 = matRad_calcInversDVH(refVol,d_i);
            %
            % % apply lower and upper dose limits
            % if isequal(cst{j,6}(k).type, 'max DVH constraint')
            %      deviation(d_i < d_ref | d_i > d_ref2) = 0;
            % elseif isequal(cst{j,6}(k).type, 'min DVH constraint')
            %      deviation(d_i > d_ref | d_i < d_ref2) = 0;
            % end
            %
            % %jacobVec = ones(size(cst{j,4}));             % linear deviation
            % %jacobVec = 2*deviation;                      % square deviation
            % jacobVec = (1/size(cst{j,4},1))*2*deviation; % square deviation with normalization
            % %jacobVec = 4*(deviation).^3;                  % squared square devioation
            % alternative constraint calculation 4/4 %
        end
    end

end


 function y = reale(x, arg2, arg3)
%| Return real part of complex data (with error checking).
%function y = reale(x, arg2, arg3)
%|
%| y = reale(x)
%| y = reale(x, tol) [default tol is 1e-13 for double, else 1e-6]
%| y = reale(x, 'warn', 'message')
%| y = reale(x, 'error')
%| y = reale(x, 'report')
%| y = reale(x, 'prompt')
%| y = reale(x, 'disp')
%|
%| Checks that imaginary part is negligible (warns etc. if not).
%|
%| Copyright Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if nargin == 1 && streq(x, 'test'), reale_test, return, end

com = 'error';
if isa(x, 'double')
	tol = 1e-13;
else
	tol = 1e-6;
end

if nargin > 1
	if ischar(arg2)
		com = arg2;
	elseif isnumeric(arg2)
		tol = arg2;
	end
end

switch com
case {'disp','prompt', 'report'}
	;
case 'warn'
	onlywarn = 1;
case 'error'
	onlywarn = 0;
otherwise
	fail('bad argument "%s"', com)
end

max_abs_x = max(abs(x(:)));
if max_abs_x == 0
	if any(imag(x(:)) ~= 0)
		fail 'max real 0, but imaginary!'
	else
		y = real(x);
		return
	end
end

frac = max(abs(imag(x(:)))) / max_abs_x;
if streq(com, 'report')
	printm('imaginary part %g%%', frac * 100)
	return
end

if frac > tol
	[cname line] = caller_name;
	t = sprintf('%s(%d): %s: imaginary fraction of %s [class %s] is %g', ...
		cname, line, mfilename, inputname(1), class(x), frac);
	if isvar('arg3')
		t = [t ', ' arg3];
	end
	if streq(com, 'disp')
		disp(t)

	elseif streq(com, 'prompt')
		printm('reale() called for input with imaginary part %g%%', frac * 100)
		printm('reale() called in context where a large imaginary part')
		printm('is likely an *error*.  proceed with caution!')
		t = input('proceed? [y|n]: ', 's');
		if isempty(t) || t(1) ~= 'y'
			printm('ok, aborting is probably wise!')
			error ' '
		end

	elseif onlywarn
		disp(t)
	else
		fail(t)
	end
end
y = real(x);

function reale_test
x = 7 + 1i*eps;
reale(x, 'warn');
reale(x, 'prompt');
%reale(x, 'report'); % check error reporting
%reale(x, eps/100) % check error faulting


function prctmus = gp_predprctmu(gp, x, y, varargin)
%GP_PREPRCTMU  Percentiles of the distribution of the location parameter
%
%  Description
%    PRCTMU = GP_PREDPRCTMU(GP, X, Y, XT, OPTIONS)
%    takes a GP structure together with matrix X of training inputs
%    and vector Y of training targets, and evaluates the
%    percentiles of the distribution of the location parameter at
%    test inputs XT.
%
%    PRCTMU = GP_PREDPRCTMU(GP, X, Y, OPTIONS)
%    evaluates the percentiles of the distribution of the location
%    parameter at training inputs X.
%
%    OPTIONS is optional parameter-value pair
%      prct   - percentiles to be computed (default = [5 50 95])
%      nsamp  - determines the number of samples used by GP_RND in case of
%               MCMC or IA (default = 5000).
%      predcf - index vector telling which covariance functions are
%               used for prediction. Default is all (1:gpcfn)
%      tstind - a vector defining, which rows of X belong to which
%               training block in *IC type sparse models. Default is [].
%               See also GP_PRED.
%      z      - optional observed quantity in triplet (x_i,y_i,z_i)
%               Some likelihoods may use this. For example, in case of
%               Poisson likelihood we have z_i=E_i, that is, expected value
%               for ith case.
%      zt     - optional observed quantity in triplet (xt_i,yt_i,zt_i)
%               Some likelihoods may use this. For example, in case of
%               Poisson likelihood we have z_i=E_i, that is, the expected
%               value for the ith case.
%
%  See also
%    GP_PRED, GP_PAK, GP_UNPAK
%
% Copyright (c) 2011 Ville Tolvanen
% Copyright (c) 2011-2012 Aki Vehtari

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

  ip=inputParser;
  ip.FunctionName = 'GP_PREDPRCTMU';
  ip.addRequired('gp',@(x) isstruct(x) || iscell(x));
  ip.addRequired('x', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addRequired('y', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addOptional('xt', [], @(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))))
  ip.addParamValue('z', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('zt', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('prct', [5 50 95], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('nsamp', 5000, @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('predcf', [], @(x) isempty(x) || ...
                   isvector(x) && isreal(x) && all(isfinite(x)&x>0))
  ip.addParamValue('tstind', [], @(x) isempty(x) || iscell(x) ||...
                   (isvector(x) && isreal(x) && all(isfinite(x)&x>0)))
  if numel(varargin)==0 || isnumeric(varargin{1})
    % inputParser should handle this, but it doesn't
    ip.parse(gp, x, y, varargin{:});
  else
    ip.parse(gp, x, y, [], varargin{:});
  end
  xt=ip.Results.xt;
  z = ip.Results.z;
  zt = ip.Results.zt;
  prct = ip.Results.prct;
  nsamp = ip.Results.nsamp;
  predcf=ip.Results.predcf;
  tstind=ip.Results.tstind;
  if isempty(xt)
    xt=x;
    if isempty(tstind)
      if iscell(gp)
        gptype=gp{1}.type;
      else
        gptype=gp.type;
      end
      switch gptype
        case {'FULL' 'VAR' 'DTC' 'SOR'}
          tstind = [];
        case {'FIC' 'CS+FIC'}
          tstind = 1:size(x,1);
        case 'PIC'
          if iscell(gp)
            tstind = gp{1}.tr_index;
          else
            tstind = gp.tr_index;
          end
      end
    end
    if isempty(zt)
      zt=z;
    end
  end

  % pass these forward
  options=struct();
  if ~isempty(z);options.z=z;end
  if ~isempty(zt);options.zt=zt;end
  if ~isempty(predcf);options.predcf=predcf;end
  if ~isempty(tstind);options.tstind=tstind;end

  [tn, nin] = size(x);

  if iscell(gp) || numel(gp.jitterSigma2)>1
    % gp_array or MCMC samples
    % the combined latent posterior is not Gaussian
    sampft = gp_rnd(gp, x, y, xt, 'nsamp', nsamp, options);
    if iscell(gp)
      % in the next step we need just one gp from the array
      gp=gp{1};
    end
    if isfield(gp, 'monotonic') && gp.monotonic
        [gp,x,y,z,xt,zt] = gp.fh.setUpDataForMonotonic(gp,x,y,z,xt,zt);
    end
    if isfield(gp.lik.fh,'trcov')
      % Gaussian likelihood
      prctmus = prctile(sampft', prct)';
    else
      prctmus = prctile(gp.lik.fh.invlink(gp.lik, sampft, zt)', prct)';
    end
  else
    % single GP
    if isfield(gp, 'monotonic') && gp.monotonic
        [gp,x,y,z,xt,zt] = gp.fh.setUpDataForMonotonic(gp,x,y,z,xt,zt);
    end
    % the latent posterior is Gaussian
    [Eft, Varft] = gp_pred(gp, x, y, xt, 'tstind', tstind, options);
    prct = prct./100;
    prct = norminv(prct, 0, 1);
    if isfield(gp.lik.fh,'trcov')
      % Gaussian likelihood
      prctmus = bsxfun(@plus, Eft, bsxfun(@times, sqrt(Varft), prct));
    else
      % Non-Gaussian likelihood
      np = length(prct);
      prctmus = zeros(size(Eft,1),np);
      for i=1:np
        prctmus(:,i) = gp.lik.fh.invlink(gp.lik, Eft+prct(i).*sqrt(Varft), zt);
      end
    end
  end

end


function ell_demo ( )

%*****************************************************************************80
%
%% ELL_DEMO demonstrates MESH2D on the L-shaped region.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ELL_DEMO:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Use MESH2D to mesh the L-shaped region.\n' );
  fprintf ( 1, '  Use MESH2D_TO_MEDIT to write the mesh to a file.\n' );

  warning off

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Set maximum element size HDATA.HMAX = 0.1\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  hdata = [];
  hdata.hmax = 0.1;

  [ p, t ] = mesh2d ( v, [], hdata );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );
  pause
%
%  Now call MESH2D_TO_MEDIT to write the data to a file.
%
  filename = 'ell.mesh';
  mesh2d_to_medit ( p, t, filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Wrote mesh data to MEDIT mesh file "%s"\n', filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ELL_DEMO:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end
function h = hfun1 ( x, y )

%*****************************************************************************80
%
%% HFUN1 is a size-function for the L-shaped region.
%
%  Discussion:
%
%    The smallest size is at (1.0,1.0), and sizes increase as their distance
%    from that point increases.
%
  h = 0.01 + 0.1 * sqrt ( ( x - 1.0 ).^2  + ( y - 1.0 ).^2 );

  return
end


function [pk, final_manip] = SCO_null_space(robot, pathT)
%global robot
global parameters
global hfigures
hfigures.hpaths = figure;
hfigures.hcosts = figure;
hfigures.hee = figure;
hfigures.htheta = figure;
hfigures.hbest_costs = figure;
hfigures.hdtheta = figure;

%N waypoints at each trajectory
%N = parameters.N;
%K particles
K = parameters.K;

%generate K different paths
%starting from arbitrary positions.
for k=1:K
  q0 = uniform(-pi, pi, 1, robot.DOF)';
  [pathq, pathT] = plan_path_moore_penrose(robot, q0);
  %generate particle
  G{k}.pathq = pathq;
  G{k}.pathT = pathT;
  animate_local(robot, pathq)
  figure, plot(pathq'), legend('q1', 'q2', 'q3', 'q4')
end
%pick arbitrarily one...
initial_manip = compute_manip(robot, pathq);
G_inicial = G;

%animate one of the particles (random)
%this may not be the best particle, of course
animate_local(robot, pathq)

i=0;
best_probs = [];
%Main loop
%till convergence of trajectory cost function
while i < 20
    i
    i=i+1;
    G = add_noise_to_particle_set(G, pathT);

    %best particle
    [pk, best_prob] = best_particle(G);
    best_probs = [best_probs, best_prob];
    plot_info(G, pk, best_probs)
end

%plot particle info
best_particle_info(pk)
figure,
plot(best_probs)
title('best PROB at each iteration (sum of probs-weights along trajectory)')
%animate best particle
animate_local(robot, pk.pathq)
final_manip = compute_manip(robot, pk.pathq);
figure, plot(initial_manip), hold
plot(final_manip)
legend('initial_manip', 'final_manip')
title('manip at each time step')

figure, hold
for k=1:K
    plot(compute_manip(robot, G{k}.pathq))
end

% function plot_particles_costs(G)
% K = size(G,2);
% figure, hold
% for k=1:K
%   pk = G{k};
%   plot(pk.P)
% end
% title('Probabilities')

function plot_info(G, best_pk, best_probs)
global hfigures parameters
K = size(G,2);
figure(hfigures.hpaths),
clf
hold on
title('robot paths')
pp=[];
for k=1:K
    pk = G{k};
    plot(pk.pathq')
    pp(k) = sum(pk.P);
end
figure(hfigures.hcosts)
plot(sort(pp))
title('Trajectory sum of probabilities for each particle')

figure(hfigures.hbest_costs), clf, hold on
plot(best_probs)
title('Best trajectory probability at each time step (sum of prob-weights)')

function best_particle_info(pk)
figure,
subplot(3,1,1)
plot(pk.pathq(1,:))
subplot(3,1,2)
plot(pk.pathq(2,:))
subplot(3,1,3)
plot(pk.pathq(3,:))
title('joint values')
figure,
plot(pk.P)
title('Prob at each time step of particle SHOULD BE >0')
figure,
plot(pk.pathq')

title('BEST PARTICLE PATH')

function animate_robot(pk)
global robot
N = size(pk.path,2);
for i=1:N
   qi = pk.path(:,i);
   drawrobot3d(robot, qi)
end

%generate a prior path for the particles
function path = generate_path(robot, q0, qN, N)
deltaq=(qN-q0)/(N-1);
path = [];
for i=0:N-1
    qi = q0 + i*deltaq;
    path = [path qi];
    %drawrobot3d(robot, qi)
end

function mcost = mean_cost(G)
pk = G{1};
K = size(G,2);
N = size(pk.path,2);
cc = [];
for k=1:K
    pk = G{k};
    cc(k) = sum(pk.costs);
end
mcost = mean(cc);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% OBTAIN the best particle according to the defined cost function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [pk, best_prob] = best_particle(G)
%global robot
pk = G{1};
K = size(G,2);
N = size(pk.pathq,2);
probs = [];

for k=1:K
    pk = G{k};
    probs(k) = sum(pk.P);
end
%we must minimize costs
%[val, index]=min(costs);
%alternatively maximize our defined weigths
[val, index]=max(probs);
fprintf('Best particle is index: k=%d\n', index)
pk = G{index};
best_prob = val;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute cost function to encode manipulability.
% Return:
% P: likelihood function
% q: the cost function it self
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [P, cost] = cost_function_manipulability(q)
global robot parameters
%compute nil-space!!
% J = manipulator_jacobian(robot, q);
manip = compute_manip(robot, q);
%define cost: cost is lower as manipulability is higher
cost = 1/(manip+0.01);
lambda = parameters.lambda_manip;
%return likelihood
P = exp(-(1/lambda)*cost);

function V = compute_link_velocity_1(theta, thetad)
V = 1*thetad(1)^2;

function V = compute_link_velocity_2(theta, thetad)
th1 = theta(1);
th2 = theta(2);

J = [-sin(th1)  -sin(th1+th2);
    cos(th1)    cos(th1+th2)];
V = J*[thetad(1) thetad(1)+thetad(2)]';
V = norm(V);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generate a noisy particle set
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function G=generate_initial_particle_set(K, path)
for k=1:K
    G{k} = generate_particle(path);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generate a path for each particle without noise
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function pk = generate_particle(path)
global robot
%each path is in each row for qi
DOF = robot.DOF;
for i = 1:DOF
    pk.path(i,:) = path(i,:);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% add noise to the
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function G=add_noise_to_particle_set(G, pathT)
K = size(G,2);
%for every particle in the set
for k=1:K
    pk = G{k};
    pathT = pk.pathT;
    pk = add_noise_to_particle_null_space(pk, pathT);
    G{k} = pk;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% !! Add noise along the null space!!!
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function pk = add_noise_to_particle_null_space(pk, pathT)
global parameters robot
N = size(pk.pathq, 2);
%each path is in each row for qi
DOF = robot.DOF;

% at each particle add some noise!
%move along the path of the particle
for i = 1:N
    %current joint position
    q1 = pk.pathq(:,i);
    %generate some samples along the nulls space
    qq = sample_from_null_space(q1);
    %populate with the current pose!!!
    qq = [qq q1];
    p = [];
    costs = [];
    %for all the new samples!
    for j=1:size(qq,2)
        %weight the samples,
        %compute cost of obstacles and manipulability
        [Pm, costm] = cost_function_manipulability(qq(:,j));
        if robot.DOF == 4
            [Po] = cost_function_distance_4dof(qq(:,j));
        else
            [Po] = cost_function_distance_7dof(qq(:,j));
        end
        p(j)=Pm*Po;
        %p(j)=Pm;
    end
    %select the best one
    %from the sample performed based on probability
    %max probatility!
    [prob, index] = max(p);
    q = qq(:,index);
    pk.pathq(:,i) = q;
    %and current probability
    pk.P(i) = prob;
end

%obtain samples from the null space
%near to q
function qq=sample_from_null_space(q1)
global robot
global parameters
%sigma_noise = parameters.noise_sigma_null_space;
alpha = parameters.alpha;
time_step = parameters.time_step;

q = q1;
the = 0;
qq=[];
%sigma_noise = 0.3;
%alpha = mvnrnd(0, sigma_noise);
%alpha = 0.3;
ss = +1;
%iterate at each time step: forward movement
while the < abs(alpha)
    %compute null_space
    if robot.DOF==4
        qdnull = null_space_4dof(robot, q);
    else
        qdnull = null_space_7dof(robot, q);
    end
    if norm(qdnull) < 0.01
        break
    end
    qdnull = qdnull/norm(qdnull);
    dq = ss*time_step*qdnull;
    the = the + norm(dq);
    q = q  + dq;
    qq = [qq q];
end

ss = -1;
q = q1;
the = 0;
%backward movement
while the < abs(alpha)
    %compute null_space
        %compute null_space
    if robot.DOF==4
        qdnull = null_space_4dof(robot, q);
    else
        qdnull = null_space_7dof(robot, q);
    end
    if norm(qdnull) < 0.01
        break
    end
    qdnull = qdnull/norm(qdnull);
    dq = ss*time_step*qdnull;
    the = the + norm(dq);
    q = q  + dq;
    qq = [qq q];
end

%
% Uniform value betweenn min_val and max_val
%
function delta = uniform(min_val, max_val, n, m)
delta = (max_val-min_val)*rand(n, m) + min_val;


function [ss,gg,tt,ff,zo]=ssubmmse(si,fsz,pp)
%SSUBMMSE performs speech enhancement using mmse estimate of spectral amplitude or log amplitude [SS,ZO]=(S,FSZ,P)
%
% Usage: y=ssubmmse(x,fs);   % enhance the speech using default parameters
%
% Inputs:
%   si      input speech signal
%   fsz     sample frequency in Hz
%           Alternatively, the input state from a previous call (see below)
%   pp      algorithm parameters [optional]
%
% Outputs:
%   ss        output enhanced speech
%   gg(t,f,i) selected time-frequency values (see pp.tf below)
%   tt        centre of frames (in seconds)
%   ff        centre of frequency bins (in Hz)
%   zo        output state (or the 2nd argument if gg,tt,ff are omitted)
%
% The algorithm operation is controlled by a small number of parameters:
%
%        pp.of          % overlap factor = (fft length)/(frame increment) [2]
%        pp.ti          % desired frame increment [0.016 seconds]
%        pp.ri          % set to 1 to round ti to the nearest power of 2 samples [0]
%        pp.ta          % time const for smoothing SNR estimate [0.396 seconds]
%        pp.gx          % maximum posterior SNR as a power ratio [1000 = +30dB]
%        pp.gn          % min posterior SNR as a power ratio when estimating prior SNR [1 = 0dB]
%        pp.gz          % min posterior SNR as a power ratio [0.001 = -30dB]
%        pp.xn          % minimum prior SNR [0]
%        pp.xb          % bias compensation factor for prior SNR [1]
%        pp.lg          % MMSE target: 0=amplitude, 1=log amplitude [1]
%        pp.ne          % noise estimation: 0=min statistics, 1=MMSE [0]
%        pp.bt          % threshold for binary gain or -1 for continuous gain [-1]
%        pp.mx          % input mixture gain [0]
%        pp.rf          % round output signal to an exact number of frames [0]
%        pp.tf          % selects time-frequency planes to output in the gg() variable ['g']
%                           'i' = input power spectrum
%                           'I' = input complex spectrum
%                           'n' = noise power spectrum
%                           'z' = "posterior" SNR (i.e. (S+N)/N )
%                           'x' = "prior" SNR (i.e. S/N )
%                           'g' = gain
%                           'o' = output power spectrum
%                           'O' = output complex spectrum
%
% The applied gain is mx+(1-mx)*optgain where optgain is calculated according to [1] or [2].
% If pp.bt>=0 then optgain is first thresholded with pp.bt to produce a binary gain 0 or 1.
%
% The default parameters implement the original algorithm in [1,2].
%
% Several parameters relate to the estimation of xi, the so-called "prior SNR",
%
%             xi=max(a*pp.xb*xu+(1-a)*max(gami-1,pp.gn-1),pp.xn);
%
% This is estimated as a smoothed version of 1 less than gami, the "posterior SNR"
% which is the noisy speech power divided by the noise power. This is
% clipped to a min of (pp.gn-1), smoothed using a factor "a" which corresponds to a
% time-constant of pp.ta and then clipped to a minimum of pp.xn. The
% previous value is taken to be pp.xb*xu where xu is the ratio of the
% estimated speech amplitude squared to the noise power.
%
% In addition it is possible to specify parameters for the noise estimation algorithm
% which implements reference [3] or [7] according to the setting of pp.ne
%
% Minimum statistics noise estimate [3]: pp.ne=0
%        pp.taca      % (11): smoothing time constant for alpha_c [0.0449 seconds]
%        pp.tamax     % (3): max smoothing time constant [0.392 seconds]
%        pp.taminh    % (3): min smoothing time constant (upper limit) [0.0133 seconds]
%        pp.tpfall    % (12): time constant for P to fall [0.064 seconds]
%        pp.tbmax     % (20): max smoothing time constant [0.0717 seconds]
%        pp.qeqmin    % (23): minimum value of Qeq [2]
%        pp.qeqmax    % max value of Qeq per frame [14]
%        pp.av        % (23)+13 lines: fudge factor for bc calculation  [2.12]
%        pp.td        % time to take minimum over [1.536 seconds]
%        pp.nu        % number of subwindows to use [3]
%        pp.qith      % Q-inverse thresholds to select maximum noise slope [0.03 0.05 0.06 Inf ]
%        pp.nsmdb     % corresponding noise slope thresholds in dB/second   [47 31.4 15.7 4.1]
%
% MMSE noise estimate [7]: pp.ne=1
%        pp.tax      % smoothing time constant for noise power estimate [0.0717 seconds](8)
%        pp.tap      % smoothing time constant for smoothed speech prob [0.152 seconds](23)
%        pp.psthr    % threshold for smoothed speech probability [0.99] (24)
%        pp.pnsaf    % noise probability safety value [0.01] (24)
%        pp.pspri    % prior speech probability [0.5] (18)
%        pp.asnr     % active SNR in dB [15] (18)
%        pp.psini    % initial speech probability [0.5] (23)
%        pp.tavini   % assumed speech absent time at start [0.064 seconds]
%
% If convenient, you can call specsub in chunks of arbitrary size. Thus the following are equivalent:
%
%                   (a) y=ssubmmse(s,fs);
%
%                   (b) [y1,z]=ssubmmse(s(1:1000),fs);
%                       [y2,z]=ssubmmse(s(1001:2000),z);
%                       y3=ssubmmse(s(2001:end),z);
%                       y=[y1; y2; y3];
%
% If the number of output arguments is either 2 or 5, the last partial frame of samples will
% be retained for overlap adding with the output from the next call to ssubmmse().
%
% See also specsub() for an alternative gain function
%
% Refs:
%    [1] Ephraim, Y. & Malah, D.
%        Speech enhancement using a minimum-mean square error short-time spectral amplitude estimator
%        IEEE Trans Acoustics Speech and Signal Processing, 32(6):1109-1121, Dec 1984
%    [2] Ephraim, Y. & Malah, D.
%        Speech enhancement using a minimum mean-square error log-spectral amplitude estimator
%        IEEE Trans Acoustics Speech and Signal Processing, 33(2):443-445, Apr 1985
%    [3] Rainer Martin.
%        Noise power spectral density estimation based on optimal smoothing and minimum statistics.
%        IEEE Trans. Speech and Audio Processing, 9(5):504-512, July 2001.
%    [4] O. Cappe.
%        Elimination of the musical noise phenomenon with the ephraim and malah noise suppressor.
%        IEEE Trans Speech Audio Processing, 2 (2): 345349, Apr. 1994. doi: 10.1109/89.279283.
%    [5] J. Erkelens, J. Jensen, and R. Heusdens.
%        A data-driven approach to optimizing spectral speech enhancement methods for various error criteria.
%        Speech Communication, 49: 530541, 2007. doi: 10.1016/j.specom.2006.06.012.
%    [6] R. Martin.
%        Statistical methods for the enhancement of noisy speech.
%        In J. Benesty, S. Makino, and J. Chen, editors,
%        Speech Enhancement, chapter 3, pages 4364. Springer-Verlag, 2005.
%    [7] Gerkmann, T. & Hendriks, R. C.
%        Unbiased MMSE-Based Noise Power Estimation With Low Complexity and Low Tracking Delay
%        IEEE Trans Audio, Speech, Language Processing, 2012, 20, 1383-1393

% Bugs/suggestions:
%   (1) sort out behaviour when si() is a matrix rather than a vector
%
%      Copyright (C) Mike Brookes 2004-2011
%      Version: $Id: ssubmmse.m 2460 2012-10-29 22:20:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if numel(si)>length(si)
    error('Input speech signal must be a vector not a matrix');
end
if isstruct(fsz)
    fs=fsz.fs;
    qq=fsz.qq;
    qp=fsz.qp;
    ze=fsz.ze;
    s=zeros(length(fsz.si)+length(si(:)),1); % allocate space for speech
    s(1:length(fsz.si))=fsz.si;
    s(length(fsz.si)+1:end)=si(:);
else
    fs=fsz;     % sample frequency
    s=si(:);
    % default algorithm constants

    qq.of=2;        % overlap factor = (fft length)/(frame increment)
    qq.ti=16e-3;    % desired frame increment (16 ms)
    qq.ri=0;        % round ni to the nearest power of 2
    qq.ta=0.396;    % Time const for smoothing SNR estimate = -tinc/log(0.98) from [1]
    qq.gx=1000;     % maximum posterior SNR = 30dB
    qq.gn=1;        % min posterior SNR as a power ratio when estimating prior SNR [1]
    qq.gz=0.001;    % min posterior SNR as a power ratio [0.001 = -30dB]
    qq.xn=0;        % minimum prior SNR = -Inf dB
    qq.xb=1;        % bias compensation factor for prior SNR [1]
    qq.lg=1;        % use log-domain estimator by default
    qq.ne=0;        % noise estimation: 0=min statistics, 1=MMSE [0]
    qq.bt=-1;       % suppress binary masking
    qq.mx=0;        % no input mixing
    qq.tf='g';      % output the gain time-frequency plane by default
    qq.rf=0;
    if nargin>=3 && ~isempty(pp)
        qp=pp;      % save for estnoisem call
        qqn=fieldnames(qq);
        for i=1:length(qqn)
            if isfield(pp,qqn{i})
                qq.(qqn{i})=pp.(qqn{i});
            end
        end
    else
        qp=struct;  % make an empty structure
    end
end
% derived algorithm constants
if qq.ri
    ni=pow2(nextpow2(ti*fs*sqrt(0.5)));
else
    ni=round(qq.ti*fs);    % frame increment in samples
end
tinc=ni/fs;         % true frame increment time
a=exp(-tinc/qq.ta); % SNR smoothing coefficient
gx=qq.gx;           % max posterior SNR as a power ratio
gz=qq.gz;           % min posterior SNR as a power ratio
kk=sqrt(2*pi);      % sqrt(8)*Gamma(1.5) - required constant
xn=qq.xn;           % floor for prior SNR, xi
ne=qq.ne;           % noise estimation: 0=min statistics, 1=MMSE [0]
gn1=max(qq.gn-1,0); % floor for posterior SNR when estimating prior SNR
xb=qq.xb;
tf=qq.tf;
rf=qq.rf || nargout==2 || nargout==5;            % round down to an exact number of frames

% calculate power spectrum in frames

no=round(qq.of);                  	% integer overlap factor
nf=ni*no;                           % fft length
w=sqrt(hamming(nf+1))'; w(end)=[];  % for now always use sqrt hamming window
w=w/sqrt(sum(w(1:ni:nf).^2));       % normalize to give overall gain of 1
if rf>0
    rfm='';                         % truncated input to an exact number of frames
else
    rfm='r';
end
[y,tt]=enframe(s,w,ni,rfm);
tt=tt/fs;                           % frame times
yf=rfft(y,nf,2);
yp=yf.*conj(yf);                    % power spectrum of input speech
[nr,nf2]=size(yp);                  % number of frames
ff=(0:nf2-1)*fs/nf;
if isstruct(fsz)
    if ne>0
        [dp,ze]=estnoiseg(yp,ze);       % estimate the noise using MMSE
    else
        [dp,ze]=estnoisem(yp,ze);       % estimate the noise using minimum statistics
    end
    ssv=fsz.ssv;
    xu=fsz.xu;                      % saved unsmoothed SNR
else
    if ne>0
        [dp,ze]=estnoiseg(yp,tinc,qp);	% estimate the noise using MMSE
    else
        [dp,ze]=estnoisem(yp,tinc,qp);	% estimate the noise using minimum statistics
    end
    ssv=zeros(ni*(no-1),1);        	% dummy saved overlap
    xu=1;                           % dummy unsmoothed SNR from previous frame
end
if ~nr                             	% no data frames
    ss=[];
    gg=[];
else
    gam=max(min(yp./dp,gx),gz);     % gamma = posterior SNR
    g=zeros(nr,nf2);                % create space for gain matrix
    x=zeros(nr,nf2);                % create space for prior SNR
    if qq.lg                        % use log domain estimator
        for i=1:nr
            gami=gam(i,:);
            xi=max(a*xb*xu+(1-a)*max(gami-1,gn1),xn);  % prior SNR
            xir=xi./(1+xi);
            gi=xir.*exp(0.5*expint(xir.*gami));
            g(i,:)=gi;             	% save gain for later
            x(i,:)=xi;              % save prior SNR
            xu=gami.*gi.^2;         % unsmoothed prior SNR
        end
    else
        for i=1:nr
            gami=gam(i,:);
            xi=max(a*xb*xu+(1-a)*max(gami-1,gn1),xn);  % prior SNR
            v=0.5*xi.*gami./(1+xi);	% note that this is 0.5*vk in [1]
            gi=(0.277+2*v)./gami; 	% accurate to 0.02 dB for v>0.5
            mv=v<0.5;
            if any(mv)
                vmv=v(mv);
                gi(mv)=kk*sqrt(vmv).*((0.5+vmv).*besseli(0,vmv)+vmv.*besseli(1,vmv))./(gami(mv).*exp(vmv));
            end
            g(i,:)=gi;              % save gain for later
            x(i,:)=xi;              % save prior SNR
            xu=gami.*gi.^2;         % unsmoothed prior SNR
        end
    end
    if qq.bt>=0
        g=g>qq.bt;
    end
    g=qq.mx+(1-qq.mx)*g;                    % mix in some of the input
    se=(irfft((yf.*g).',nf).').*repmat(w,nr,1);     % inverse dft and apply output window
    ss=zeros(ni*(nr+no-1),no);                      % space for overlapped output speech
    ss(1:ni*(no-1),end)=ssv;
    for i=1:no
        nm=nf*(1+floor((nr-i)/no));         % number of samples in this set
        ss(1+(i-1)*ni:nm+(i-1)*ni,i)=reshape(se(i:no:nr,:)',nm,1);
    end
    ss=sum(ss,2);
    if nargout>2 && ~isempty(tf)
        gg=zeros(nr,nf2,length(tf));  % make space
        for i=1:length(tf)
            switch tf(i)
                case 'i'            % 'i' = input power spectrum
                    gg(:,:,i)=yp;
                case 'I'            % 'i' = input power spectrum
                    gg(:,:,i)=yf;
                case 'n'            % 'n' = noise power spectrum
                    gg(:,:,i)=dp;
                case 'z'            % 'z' = posterior SNR (i.e. (S+N)/N )
                    gg(:,:,i)=gam;
                case 'x'            % 'x' = prior SNR
                    gg(:,:,i)=x;
                case 'g'            % 'g' = gain
                    gg(:,:,i)=g;
                case 'o'            % 'o' = output power spectrum
                    gg(:,:,i)=yp.*g.^2;
                case 'O'            % 'o' = output power spectrum
                    gg(:,:,i)=yf.*g;
            end
        end
    end
end
if nargout==2 || nargout==5
    if nr
        zo.ssv=ss(end-ni*(no-1)+1:end);    % save the output tail for next time
        ss(end-ni*(no-1)+1:end)=[];        % only output the frames that are completed
    else
        zo.ssv=ssv;  %
    end
    zo.si=s(length(ss)+1:end);      % save the tail end of the input speech signal
    zo.fs=fs;                       % save sample frequency
    zo.qq=qq;                       % save local parameters
    zo.qp=qp;                       % save estnoisem parameters
    zo.ze=ze;                       % save state of noise estimation
    zo.xu=xu;
    if nargout==2
        gg=zo;                      % 2nd of two arguments is zo
    end
elseif rf==0
    ss=ss(1:length(s));             % trim to the correct length if not an exact number of frames
end
if ~nargout && nr>0
    ffax=ff/1000;
    ax=zeros(4,1);
    ax(1)=subplot(223);
    imagesc(tt,ffax,20*log10(g)');
    colorbar;
    axis('xy');
    title(sprintf('Filter Gain (dB): ta=%.2g',qq.ta));
    xlabel('Time (s)');
    ylabel('Frequency (kHz)');

    ax(2)=subplot(222);
    imagesc(tt,ffax,10*log10(yp)');
    colorbar;
    axis('xy');
    title('Noisy Speech (dB)');
    xlabel('Time (s)');
    ylabel('Frequency (kHz)');

    ax(3)=subplot(224);
    imagesc(tt,ffax,10*log10(yp.*g.^2)');
    colorbar;
    axis('xy');
    title('Enhanced Speech (dB)');
    xlabel('Time (s)');
    ylabel('Frequency (kHz)');

    ax(4)=subplot(221);
    imagesc(tt,ffax,10*log10(dp)');
    colorbar;
    axis('xy');
    title('Noise Estimate (dB)');
    xlabel('Time (s)');
    ylabel('Frequency (kHz)');
    linkaxes(ax);
end