%%*******************************************************************
%%  Read in a problem in SDPA sparse format.
%%
%%  [blk,At,C,b] = read_sdpa(fname)
%%
%%  Input: fname = name of the file containing SDP data in
%%                 SDPA foramt.
%%  Important: the data is assumed to contain only
%%             semidefinite and linear blocks.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%******************************************************************

function [blk,At,C,b] = read_sdpa(fname)

%%
%%  Open the file for input
%%
compressed = 0;
if exist(fname,'file')
    fid = fopen(fname,'r');
elseif exist([fname,'.Z'],'file');
    compressed = 1;
    unix(['uncompress ',fname,'.Z']);
    fid = fopen(fname,'r');
elseif exist([fname,'.gz'],'file');
    compressed = 2;
    unix(['gunzip ',fname,'.gz']);
    fid = fopen(fname,'r');
else
    fprintf('*** Problem not found, please specify the correct path or problem name. \n');
    blk = []; At = []; C = []; b = [];
    return;
end
%%
%%  Clean up special characters and comments from the file
%%
[datavec,count] = fscanf(fid,'%c'); %#ok
linefeeds = strfind(datavec,char(10));
comment_chars = '*"=';
cumidx = [];
for i=1:length(comment_chars)
    idx = strfind(datavec,comment_chars(i));
    cumidx = [cumidx,idx]; %#ok
end
for j=length(cumidx):-1:1
    if (cumidx(j)==1) || (strcmp(datavec(cumidx(j)-1),char(10)))
        datavec(cumidx(j):linefeeds(find(cumidx(j)<linefeeds,1,'first')))='';
    else
        datavec(cumidx(j):linefeeds(find(cumidx(j)<linefeeds,1,'first'))-1)='';
    end
end
special_chars = ',{}()';
cumidx=[];
for i=1:length(special_chars)
    idx = strfind(datavec,special_chars(i));
    cumidx = [cumidx,idx]; %#ok
end
datavec(cumidx) = blanks(length(cumidx));
clear linefeeds;
%%
%% Close the file
%%
fclose('all');
if compressed==1; unix(['compress ',fname]); end;
if compressed==2; unix(['gzip ',fname]); end;
%%
%%  Next, read in basic problem size parameters.
%%
datavec = sscanf(datavec,'%f');
if size(datavec,1) < size(datavec,2); datavec = datavec'; end;
m = datavec(1);
numblk  = datavec(2);
blksize = datavec(3:numblk+2);
if size(blksize,1) > size(blksize,2); blksize = blksize'; end
%%
%% Get input  b.
%%
idxstrt = 2+numblk;
b = datavec(idxstrt+1:idxstrt+m);
idxstrt = idxstrt+m;
b = -b;
%%
%% Construct blk
%%
deblksize = 100;
spblkidxtmp = find( (blksize>1) & (blksize < deblksize) );
spblkidxtmp = sort(spblkidxtmp);
deblkidx = find( (blksize<=1) | (blksize >= deblksize) );
denumblk = length(deblkidx);
linblkidx = zeros(1,denumblk);
for p = 1:denumblk
    n = blksize(deblkidx(p));
    if (n > 1);
        blk{p,1} = 's'; blk{p,2} = n; %#ok
        n2 = n*(n+1)/2;
        At{p,1} = sparse(n2,m); %#ok
        C{p,1} = sparse(n,n); %#ok
    else
        linblkidx(p) = p;
        blk{p,1} = 'l'; blk{p,2} = abs(n); %#ok
        At{p,1} = sparse(abs(n),m); %#ok
        C{p,1} = sparse(abs(n),1); %#ok
    end
end
if ~isempty(spblkidxtmp)
    maxnumblk = 200;
    spnumblk = ceil(length(spblkidxtmp)/maxnumblk);
    for q = 1:spnumblk
        if (q < spnumblk)
            spblkidxall{q} = spblkidxtmp([(q-1)*maxnumblk+1: q*maxnumblk]); %#ok
        else
            spblkidxall{q} = spblkidxtmp([(q-1)*maxnumblk+1: length(spblkidxtmp)]); %#ok
        end
        tmp = blksize(spblkidxall{q});
        blk{denumblk+q,1} = 's'; %#ok
        blk{denumblk+q,2} = tmp; %#ok
        n2 = sum(tmp.*(tmp+1))/2;
        At{denumblk+q,1} = sparse(n2,m); %#ok
        C{denumblk+q,1} = sparse(sum(tmp),sum(tmp)); %#ok
    end
else
    spnumblk = 0;
end
linblkidx(denumblk+1:denumblk+spnumblk) = 0;
%%
%% Construct single blocks of A,C
%%
len = length(datavec);
Y = reshape(datavec(idxstrt+1:len),5,(len-idxstrt)/5)';
clear datavec;
Y = sortrows(Y,[1 2]);
matidx = [0; find(diff(Y(:,1)) ~= 0); size(Y,1)];
%%
for k = 1:length(matidx)-1
    idx = matidx(k)+1 : matidx(k+1);
    Ytmp  = Y(idx,1:5);
    matno = Ytmp(1,1);
    Ytmp2 = Ytmp(:,2);
    for p = 1:denumblk
        n  = blksize(deblkidx(p));
        idx = find(Ytmp2 == deblkidx(p));
        ii = Ytmp(idx,3); jj = Ytmp(idx,4); vv =Ytmp(idx,5);
        len = length(idx);
        if (n > 1)
            idxtmp = find(ii > jj);
            if ~isempty(idxtmp);
                tmp = jj(idxtmp);
                jj(idxtmp) = ii(idxtmp); ii(idxtmp) = tmp;
            end
            tmp = -sparse(ii,jj,vv,n,n);
            tmp = tmp + triu(tmp,1)';
        else
            tmp = -sparse(ii,ones(len,1),vv,abs(n),1);
        end
        if (matno == 0)
            C{p,1} = tmp; %#ok
        else
            if (n > 1)
                At{p,1}(:,matno) = svec(blk(p,:),tmp,1); %#ok
            else
                At{p,1}(:,matno) = tmp; %#ok
            end
        end
    end
end
%%
%% Construct big sparse block of A,C
%%
if (spnumblk > 0)
    Y1 = Y(:,1);
    diffY1 = find(diff([-1; Y1; inf]));
    for kk = 1:length(diffY1)-1
        idx = diffY1(kk) : diffY1(kk+1)-1;
        matno = Y1(diffY1(kk));
        Ytmp  = Y(idx,1:5);
        Ytmp2 = Ytmp(:,2);
        maxYtmp2  = Ytmp2(length(Ytmp2));
        minYtmp2  = Ytmp2(1);
        diffYtmp2 = Ytmp2(diff([-1; Ytmp2])~=0);
        for q = 1:spnumblk
            spblkidx    = spblkidxall{q};
            maxspblkidx = spblkidx(length(spblkidx));
            minspblkidx = spblkidx(1);
            count = 0;
            if (minYtmp2 <= maxspblkidx) && (maxYtmp2 >= minspblkidx)
                tmpblksize = blksize(spblkidx);
                n = sum(tmpblksize);
                cumspblksize = [0 cumsum(tmpblksize)];
                n2 = sum(tmpblksize.*(tmpblksize+1))/2;
                idx = zeros(n2,1); offset = zeros(n2,1);
                for t = 1:length(diffYtmp2)
                    p = find(spblkidx == diffYtmp2(t));
                    if ~isempty(p)
                        idxtmp = find(Ytmp2 == spblkidx(p));
                        len = length(idxtmp);
                        idx(count+1:count+len) = idxtmp;
                        offset(count+1:count+len) = cumspblksize(p);
                        count = count + len;
                    end
                end
                idx = idx(1:count); offset = offset(1:count);
                ii = Ytmp(idx,3)+offset; jj = Ytmp(idx,4)+offset; vv = Ytmp(idx,5);
                idxtmp = find(ii > jj);
                if ~isempty(idxtmp);
                    tmp = jj(idxtmp);
                    jj(idxtmp) = ii(idxtmp); ii(idxtmp) = tmp;
                end
                idxeq = find(ii==jj);
                tmp = spconvert([ii jj -vv; jj ii -vv; n n 0]) ...
                    + spconvert([ii(idxeq) jj(idxeq) vv(idxeq); n n 0]);
                if (matno == 0)
                    C{denumblk+q,1} = tmp; %#ok
                else
                    At{denumblk+q,1}(:,matno) = svec(blk(denumblk+q,:),tmp,1); %#ok
                end
            end
        end
    end
end
%%
%% put all linear blocks together as a single linear block
%%
idx = find(linblkidx);
if (length(idx) > 1)
    sdpidx = find(linblkidx==0);
    blktmp = 0; Atmp = []; Ctmp = [];
    for k = 1:length(idx)
        tmp = linblkidx(idx(k));
        blktmp = blktmp+blk{tmp,2};
        Atmp = [Atmp; At{tmp}]; %#ok
        Ctmp = [Ctmp; C{tmp}]; %#ok
    end
    At = At(sdpidx); C = C(sdpidx); blk = blk(sdpidx,:);
    len = length(sdpidx);
    blk(2:len+1,:) = blk;
    blk{1,1} = 'l'; blk{1,2} = blktmp;
    At(2:len+1,1) = At; C(2:len+1,1) = C;
    At{1,1} = Atmp; C{1,1} = Ctmp;
end
%%******************************************************************


filename='Gripping_triangle_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'nonadjoint_compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
constraint_case = 'INEQUALITY';
optimizer = 'IPOPT'; incrementFactor = 1;
filterType = 'P1';

nsteps = 1;
Vfrac_final = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function value = r8vec_in_ab ( n, x, a, b )

%*****************************************************************************80
%
%% R8VEC_IN_AB is TRUE if the entries of an R8VEC are in the range [A,B].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries.
%
%    Input, real X(N), the vector.
%
%    Input, real A, B, the limits, with A <= B.
%
%    Output, logical VALUE, is TRUE if every entry is
%    between A and B.
%
  if ( any ( x(1:n) < a | b < x(1:n) ) )
    value = 0;
  else
    value = 1;
  end

  return
end


function [C,phi,S12,S1,S2,f,zerosp,confC,phistd,Cerr]=coherencysegcpb(data1,data2,win,params,segave,fscorr)
% Multi-taper coherency,cross-spectrum and individual spectra with segmenting
%   computed by segmenting two univariate time series into chunks - continuous and binned point process
%
% Usage:
% [C,phi,S12,S1,S2,f,zerosp,confC,phistd,Cerr]=coherencysegcpb(data1,data2,win,params,segave,fscorr)
% Input:
% Note units have to be consistent. See chronux.m for more information.
%       data1 (column vector, continuous data) -- required
%       data2 (column vector, binned point process data) -- required
%       win   (length of segments) - required
%       params: structure with fields tapers, pad, Fs, fpass, err
%       - optional
%           tapers : precalculated tapers from dpss or in the one of the following
%                    forms:
%                   (1) A numeric vector [TW K] where TW is the
%                       time-bandwidth product and K is the number of
%                       tapers to be used (less than or equal to
%                       2TW-1).
%                   (2) A numeric vector [W T p] where W is the
%                       bandwidth, T is the duration of the data and p
%                       is an integer such that 2TW-p tapers are used. In
%                       this form there is no default i.e. to specify
%                       the bandwidth, you have to specify T and p as
%                       well. Note that the units of W and T have to be
%                       consistent: if W is in Hz, T must be in seconds
%                       and vice versa. Note that these units must also
%                       be consistent with the units of params.Fs: W can
%                       be in Hz if and only if params.Fs is in Hz.
%                       The default is to use form 1 with TW=3 and K=5
%
%	        pad		    (padding factor for the FFT) - optional (can take values -1,0,1,2...).
%                    -1 corresponds to no padding, 0 corresponds to padding
%                    to the next highest power of 2 etc.
%			      	 e.g. For N = 500, if PAD = -1, we do not pad; if PAD = 0, we pad the FFT
%			      	 to 512 points, if pad=1, we pad to 1024 points etc.
%			      	 Defaults to 0.
%           Fs   (sampling frequency) - optional. Default 1.
%           fpass    (frequency band to be used in the calculation in the form
%                                   [fmin fmax])- optional.
%                                   Default all frequencies between 0 and Fs/2
%           err  (error calculation [1 p] - Theoretical error bars; [2 p] - Jackknife error bars
%                                   [0 p] or 0 - no error bars) - optional. Default 0.
%       segave (average over segments for 1, don't average for 0)
%       fscorr   (finite size corrections, 0 (don't use finite size corrections) or
%                1 (use finite size corrections) - optional
%                (available only for spikes). Defaults 0.
% Output:
%       C (magnitude of coherency - frequencies x segments if segave=0; dimension frequencies if segave=1)
%       phi (phase of coherency - frequencies x segments if segave=0; dimension frequencies if segave=1)
%       S12 (cross spectrum -  frequencies x segments if segave=0; dimension frequencies if segave=1)
%       S1 (spectrum 1 - frequencies x segments if segave=0; dimension frequencies if segave=1)
%       S2 (spectrum 2 - frequencies x segments if segave=0; dimension frequencies if segave=1)
%       f (frequencies)
%       zerosp (1 for trials where no spikes were found, 0 otherwise)
%       confC (confidence level for C at 1-p %) - only for err(1)>=1
%       phistd - theoretical/jackknife (depending on err(1)=1/err(1)=2) standard deviation for phi
%                Note that phi + 2 phistd and phi - 2 phistd will give 95% confidence
%                bands for phi - only for err(1)>=1
%       Cerr  (Jackknife error bars for C - use only for Jackknife - err(1)=2)

if nargin < 3; error('Need data1 and data2 and size of segment'); end;
if nargin < 4; params=[]; end;
[tapers,pad,Fs,fpass,err,trialave,params]=getparams(params);
clear tapers pad fpass trialave
if nargin < 5 || isempty(segave); segave=1;end;
if nargin < 6 || isempty(fscorr); fscorr=0; end;
if nargout > 7 && err(1)==0;
%   Errors computed only if err(1) is non-zero. Need to change params and run again.
    error('When errors are desired, err(1) has to be non-zero.');
end;
if nargout > 9 && err(1)~=2;
    error('Cerr computed only for Jackknife. Correct inputs and run again');
end;

N=check_consistency(data1,data2);
dt=1/Fs; % sampling interval
T=N*dt; % length of data in seconds
E=0:win:T-win; % fictitious event triggers
win=[0 win]; % use window length to define left and right limits of windows around triggers
data1=createdatamatc(data1,E,Fs,win); % segmented data 1
data2=createdatamatpb(data2,E,Fs,win); % segmented data 2
params.trialave=segave;
if nargout==7;
  [C,phi,S12,S1,S2,f,zerosp]=coherencycpb(data1,data2,params,fscorr); % compute coherency for segmented data
elseif nargout==9;
  [C,phi,S12,S1,S2,f,zerosp,confC,phistd]=coherencycpb(data1,data2,params,fscorr); % compute coherency for segmented data
elseif nargout==10;
  [C,phi,S12,S1,S2,f,zerosp,confC,phistd,Cerr]=coherencycpb(data1,data2,params,fscorr); % compute coherency for segmented data
end;


function bel = tree_protocol(engine, evidence, pot_type, local_kernel, msg)

bnet = bnet_from_engine(engine);
ns = bnet.node_sizes;
onodes = find(~isemptycell(evidence));

ndoms = length(engine.gdl.doms);
prod_of_msg = cell(1, ndoms);
bel = cell(1, ndoms);

% collect to root (node to parents)
for n=engine.postorder
  % absorb msgs from children
  prod_of_msg{n} = mk_initial_pot(pot_type, engine.gdl.doms{n}, ns, bnet.cnodes, onodes);
  for c=children(engine.tree, n)
    prod_of_msg{n} = multiply_by_pot(prod_of_msg{n}, msg{c,n});
  end
  % send msg to parents
  for p=parents(engine.tree, n)
    if iter==1
      temp = prod_of_msg{n};
    else
      temp = divide_by_pot(prod_of_msg{n}, old_msg{p,n});
    end
    temp = multiply_by_pot(temp, local_kernel{n});
    temp2 = marginalize_pot(temp, engine.gdl.sepset{n,p}, engine.maximize);
    %fprintf('%d sends %d\n', n, p);
    msg{n,p} = normalize_pot(temp2);
  end
end

% distribute from root (node to children)
for n=engine.preorder
  % absorb from parents
  %prod_of_msg{n} = mk_initial_pot(pot_type, doms{n}, ns, cnodes, onodes);
  for p=parents(engine.tree, n)
    prod_of_msg{n} = multiply_by_pot(prod_of_msg{n}, msg{p,n});
  end
  bel{n} = normalize_pot(multiply_pots(prod_of_msg{n}, local_kernel{n}));
  % send msg to children
  for c=children(engine.tree, n)
    temp = divide_by_pot(prod_of_msg{n}, msg{c,n});
    temp = multiply_by_pot(temp, local_kernel{n});
    temp2 = marginalize_pot(temp, engine.gdl.sepset{n,c}, engine.maximize);
    %fprintf('%d sends %d\n', n, c);
    msg{n,c} = normalize_pot(temp2);
  end
end


classdef form_acp < mp.form_ac
%MP.FORM_ACP  MATPOWER Formulation class for AC polar voltage formulations
%   Each concrete Network Model Element class must inherit, at least
%   indirectly, from both MP.NM_ELEMENT and MP.FORM.
%
%   Subclass of MP.FORM_AC.
%   MP.FORM provides properties and methods related to the specific
%   formulation (e.g. DC version, AC polar power version, etc.)
%
%   Properties
%       (model parameters inherited from MP.FORM_AC)
%
%   Methods
%       form_name() - returns string w/name of formulation ('AC-polar formulation')
%       form_tag() - returns string w/short label for formulation ('acp')

%   MATPOWER
%   Copyright (c) 2019-2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%     properties
%     end

    methods
        function name = form_name(obj)
            name = 'AC-polar';
        end

        function tag = form_tag(obj)
            tag = 'acp';
        end

        function vtypes = model_vvars(obj)
            vtypes = {'va', 'vm'};
        end

        function [Iva, Ivm] = port_inj_current_jac(obj, ...
                n, v_, Y, M, invdiagvic, diagSlincJ)
            % [Iva, Ivm] = obj.port_inj_current_jac(...)

            %% intermediate terms
            diagv = sparse(1:n, 1:n, v_, n, n);
            C = invdiagvic * (diagSlincJ - conj(M * diagv));
            D = sparse(1:n, 1:n, 1 ./ abs(v_), n, n);

%             %% linear current term
%             Ivm = Y * diagv;
%             Iva = 1j * Ivm;
%             Ivm = Ivm * D;
%
%             %% + current from linear power term
%             Iva = Iva + 1j * C;
%             Ivm = Ivm - C * D;

            A = Y * diagv;
            Iva = 1j * (A + C);
            Ivm = (A - C) * D;
        end

        function [Ivava, Ivavm, Ivmvm] = port_inj_current_hess_v(obj, x_, lam, v_, z_, diaginvic, Y, M, diagSlincJ, dlamJ)
            % [Ivava, Ivavm, Ivmvm] = obj.port_inj_current_hess_v(x_, lam)
            % [Ivava, Ivavm, Ivmvm] = obj.port_inj_current_hess_v(x_, lam, sysx)
            % [Ivava, Ivavm, Ivmvm] = obj.port_inj_current_hess_v(x_, lam, sysx, idx)
            % [...] = obj.port_inj_current_hess_vz(x_, lam, v_, z_, diaginvic, Y, M, diagSlincJ, dlamJ)

            if nargin < 10
                sysx = v_;
                if nargin < 5
                    idx = []
                else
                    idx = z_;
                end
                [Y, M, N, s] = obj.get_params(idx, {'Y', 'M', 'N', 's'});
                [v_, z_, vi_] = obj.x2vz(x_, sysx, idx);

                %% compute linear power injections
                if isempty(z_)
                    Slin = M*v_ + s;
                else
                    Slin = M*v_ + N*z_ + s;
                end

                n  = length(v_);    %% number of all port voltages
                ni = length(vi_);   %% number of selected port voltages
                diaginvic = sparse(1:ni, 1:ni, 1 ./ conj(vi_), ni, ni);
                if isempty(idx)     %% all ports
                    diagSlincJ = sparse(1:n, 1:n, conj(Slin), n, n);
                    dlamJ      = sparse(1:n, 1:n, lam, n, n);
                else                %% selected ports
                    diagSlincJ = sparse(1:ni, idx, conj(Slin), ni, n);
                    dlamJ      = sparse(1:ni, idx, lam, ni, n);
                end
            else
                n  = length(v_);    %% number of all port voltages
                ni = length(lam);
            end

            %% intermediate terms
            diagv  = sparse(1:n, 1:n, v_, n, n);
            A = diaginvic * diagSlincJ;
            B = diaginvic * conj(M);
            % B2 = B * conj(diagv);
            % C = A - B2;
            D = sparse(1:n, 1:n, 1 ./ abs(v_), n, n);
            % E = diaginvic * conj(N);
            F = dlamJ.';
            G = F * B * conj(diagv);    %% F * B2
            dBtlam = sparse(1:n, 1:n, B.' * lam, n, n);
            H = dBtlam * conj(diagv);
            K = (F * A).';
            GG = G + G.';
            LL = GG - H - K;
            MM = D * (2*K - GG) * D;

            %% linear current term
            dYtlam = sparse(1:n, 1:n, Y.' * lam, n, n);
            Ivava = -dYtlam * diagv;
            Ivavm = -j * Ivava * D;

            %% + current from linear power term
            Ivava = Ivava + LL;
            Ivavm = Ivavm + 1j * LL.' * D;
            Ivmvm = MM;
        end

        function [Ivazr, Ivazi, Ivmzr, Ivmzi] = port_inj_current_hess_vz(obj, x_, lam, v_, z_, diaginvic, N, dlamJ)
            % [Ivazr, Ivazi, Ivmzr, Ivmzi] = obj.port_inj_current_hess_vz(x_, lam)
            % [...] = obj.port_inj_current_hess_vz(x_, lam, sysx)
            % [...] = obj.port_inj_current_hess_vz(x_, lam, sysx, idx)
            % [...] = obj.port_inj_current_hess_vz(x_, lam, v_, z_, diaginvic, N, dlamJ)

            if nargin < 8
                sysx = v_;
                if nargin < 5
                    idx = []
                else
                    idx = z_;
                end
                N = obj.get_params(idx, 'N');
                [v_, z_, vi_] = obj.x2vz(x_, sysx, idx);

                n  = length(v_);    %% number of all port voltages
                ni = length(vi_);   %% number of selected port voltages
                diaginvic = sparse(1:ni, 1:ni, 1 ./ conj(vi_), ni, ni);
                if isempty(idx)     %% all ports
                    dlamJ = sparse(1:n, 1:n, lam, n, n);
                else                %% selected ports
                    dlamJ = sparse(1:ni, idx, lam, ni, n);
                end
            else
                n  = length(v_);    %% number of all port voltages
                ni = length(lam);
            end

            %% intermediate terms
            D = sparse(1:n, 1:n, 1 ./ abs(v_), n, n);
            E = diaginvic * conj(N);
            NN = dlamJ.' * E;

            %% current from linear power term
            Ivazr = 1j * NN;
            Ivazi = NN;
            Ivmzr = -D * NN;
            Ivmzi = -1j * Ivmzr;
        end

        function [Sva, Svm] = port_inj_power_jac(obj, ...
                n, v_, Y, M, diagv, diagvi, diagIlincJ)
            % [Sva, Svm] = obj.port_inj_power_jac(...)

            %% intermediate terms
            A = diagvi * diagIlincJ;
%             B = diagvi * conj(Y);
%             C = B * conj(diagv);
            C = diagvi * conj(Y * diagv);
            D = sparse(1:n, 1:n, 1 ./ abs(v_), n, n);

%             %% linear power term
%             Svm = M * diagv;
%             Sva = 1j * Svm;
%             Svm = Svm * D;
%
%             %% + power from linear current term
%             Sva = Sva + 1j * (A - C);
%             Svm = Svm + (A + C) * D;

            Svm = M * diagv + A;
            Sva = 1j * (Svm - C);
            Svm = (Svm + C) * D;
        end

        function [Svava, Svavm, Svmvm] = port_inj_power_hess_v(obj, x_, lam, v_, z_, diagvi, Y, M, diagIlincJ, dlamJ)
            % [Svava, Svavm, Svmvm] = obj.port_inj_power_hess_v(x_, lam)
            % [Svava, Svavm, Svmvm] = obj.port_inj_power_hess_v(x_, lam, sysx)
            % [Svava, Svavm, Svmvm] = obj.port_inj_power_hess_v(x_, lam, sysx, idx)
            % [...] = obj.port_inj_power_hess_v(x_, lam, v_, z_, diagvi, Y, M, diagIlincJ, dlamJ)

            if nargin < 10
                sysx = v_;
                if nargin < 5
                    idx = []
                else
                    idx = z_;
                end
                [Y, L, M, i] = obj.get_params(idx, {'Y', 'L', 'M', 'i'});
                [v_, z_, vi_] = obj.x2vz(x_, sysx, idx);

                %% compute linear current injections
                if isempty(z_)
                    Ilin = Y*v_ + i;
                else
                    Ilin = Y*v_ + L*z_ + i;
                end

                n  = length(v_);    %% number of all port voltages
                ni = length(vi_);   %% number of selected port voltages
                diagvi = sparse(1:ni, 1:ni, vi_, ni, ni);
                if isempty(idx)     %% all ports
                    diagIlincJ = sparse(1:n, 1:n, conj(Ilin), n, n);
                    dlamJ      = sparse(1:n, 1:n, lam, n, n);
                else                %% selected ports
                    diagIlincJ = sparse(1:ni, idx, conj(Ilin), ni, n);
                    dlamJ      = sparse(1:ni, idx, lam, ni, n);
                end
            else
                n  = length(v_);    %% number of all port voltages
                ni = length(lam);
            end

            %% intermediate terms
            diagv  = sparse(1:n, 1:n, v_, n, n);

            A = diagvi * diagIlincJ;
            B = diagvi * conj(Y);
            C = B * conj(diagv);
            D = sparse(1:n, 1:n, 1 ./ abs(v_), n, n);
            % E = diagvi * conj(L);
            F = dlamJ.';
            G = F * C;
            dBtlam = sparse(1:n, 1:n, B.' * lam, n, n);
            H = conj(diagv) * ((F*B).' - dBtlam);
            K = F * (C - A);

            %% linear power term
            dMtlam = sparse(1:n, 1:n, M.' * lam, n, n);
            Svava = -dMtlam * diagv;
            Svavm = -j * Svava * D;

            %% + power from linear current term
            Svava = Svava + H + K;
            Svavm = Svavm + 1j * (H - K).' * D;
            Svmvm = D * (G + G.') * D;
        end

        function [Svazr, Svazi, Svmzr, Svmzi] = port_inj_power_hess_vz(obj, x_, lam, v_, z_, diagvi, L, dlamJ)
            % [Svazr, Svazi, Svmzr, Svmzi] = obj.port_inj_power_hess_vz(x_, lam)
            % [...] = obj.port_inj_power_hess_vz(x_, lam, sysx)
            % [...] = obj.port_inj_power_hess_vz(x_, lam, sysx, idx)
            % [...] = obj.port_inj_power_hess_vz(x_, lam, v_, z_, diagvi, L, dlamJ)

            if nargin < 8
                sysx = v_;
                if nargin < 5
                    idx = []
                else
                    idx = z_;
                end
                L = obj.get_params(idx, 'L');
                [v_, z_, vi_] = obj.x2vz(x_, sysx, idx);

                n  = length(v_);    %% number of all port voltages
                ni = length(vi_);   %% number of selected port voltages
                diagvi = sparse(1:ni, 1:ni, vi_, ni, ni);
                if isempty(idx)     %% all ports
                    dlamJ = sparse(1:n, 1:n, lam, n, n);
                else                %% selected ports
                    dlamJ = sparse(1:ni, idx, lam, ni, n);
                end
            else
                n  = length(v_);    %% number of all port voltages
                ni = length(lam);
            end

            %% intermediate terms
            D = sparse(1:n, 1:n, 1 ./ abs(v_), n, n);
            E = diagvi * conj(L);
            LL = dlamJ.' * E;
            M = D * LL;

            %% power from linear current term
            Svazr = 1j * LL;
            Svazi = LL;
            Svmzr = M;
            Svmzi = -1j * M;
        end

        function [va, vm] = aux_data_va_vm(obj, ad)
            va = ad.va;
            vm = ad.vm;
        end
    end     %% methods
end         %% classdef


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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Reads in (x,y) positions of the immersed boundary from .vtk
%           format
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = read_Lagrangian_Data_From_vtk(path,simNums)

cd(path);

filename = ['lagsPts.' num2str(simNums) '.vtk'];  % desired lagsPts.xxxx.vtk file

fileID = fopen(filename);
if ( fileID== -1 )
    error('\nCANNOT OPEN THE FILE!');
end

str = fgets(fileID); %-1 if eof
if ~strcmp( str(3:5),'vtk');
    error('\nNot in proper VTK format');
end

% read in the header info %
str = fgets(fileID);
str = fgets(fileID);
str = fgets(fileID);
str = fgets(fileID);
str = fgets(fileID); % Up to white space in previous lagsPts.X files (pre June 2021)

% Check whether VTK file has time info. This is a VTK file with time,
%       need to read the next 3 lines to have read in appropriate
%       number of header lines.
if ~strcmp( str(1), 'P')
	str = fgets(fileID);
    str = fgets(fileID);
    str = fgets(fileID);
end

% stores # of Lagrangian Pts. as stated in .vtk file
numLagPts = sscanf(str,'%*s %f %*s',1);

% read in the vertices %
[mat,count] = fscanf(fileID,'%f %f %f',3*numLagPts);
if count ~= 3*numLagPts
   error('\nProblem reading in Lagrangian Pts.');
end

mat = reshape(mat, 3, count/3); % Reshape vector -> matrix (every 3 entries in vector make into matrix row)
vertices = mat';              % Store vertices in new matrix

fclose(fileID);               % Closes the data file.

xLag = vertices(:,1);         % x-Lagrangian Pts.
yLag = vertices(:,2);         % y-Lagrangian Pts.

cd ..;                        % Change directory back to ../viz_IB2d/ directory

clear mat str filename fileID count;


function saveResults(destDir, canonicalImageSize)
% alignment results
load(fullfile(destDir, 'final.mat'),'A','E','O','M') ;

%% display
mkdir([destDir '\Err']);
mkdir([destDir '\Res']);
mkdir([destDir '\O']);
FACT = 255;
for i=1:size(A,2)
    im = reshape(A(:,i), canonicalImageSize);
    imMin = min(im(:));
    imMax = max(im(:));
    im = (im - imMin)./(imMax - imMin);
    im = uint8(255*im);
    imwrite(im,[destDir '\Res\' sprintf('Image_%.4d.bmp',i)],'bmp');

    % E
    im = reshape(E(:,i), canonicalImageSize);
    im = abs(im) ./ max(max(abs(im)));
    im = uint8(FACT.*abs(im));
    imwrite(im,[destDir '\Err\' sprintf('Er_%.4d.bmp',i)],'bmp');

    % O
    im = reshape(O(:,i), canonicalImageSize);
    im = abs(im) ./ max(max(abs(im)));
    im = uint8(FACT.*abs(im));
    imwrite(im,[destDir '\O\' sprintf('O_%.4d.bmp',i)],'bmp');
end;


function n = double(W, option)
   %DOUBLE returns a waveform's data as a double type
   %   N = double(waveform  [, option] )
   %   this is just a fancy way of saying get(W,'data')
   %   but can handle vectors of W. The data lengths of the vectors do not
   %   need to be the same.  By default it will zero-pad the end of all short
   %   vectors.  Using the OPTION, you can specify a NaN padding instead.
   %
   %   Input Arguments
   %       WAVEFORM: a waveform object   assumed as 1xN DIMENSIONAL
   %               (If not, it will be treated as though it were 1xN!)
   %       OPTION: Handle to any function that creates an array of a
   %               dictated size.  The chosen function should work like:
   %               A = (nRows,nCols); and return a double array of the
   %               appropriate size. ex. @ones, @zeros, @nan.
   %
   %   Output Arguments
   %       N: the columns of data
   %
   %   example:
   %       % let W(1) be a waveform with data [0 1 2 5 3]
   %       % let W(2) be a waveform with data [2 5]
   %
   %       Y = double(W);
   %       % Y is now [[ 0 2; 1 5; 2 0; 5 0; 3 0]
   %
   %       Z = double(W,@nan); pads with NaN instead of zeros
   %       % Z is now [ 0 2; 1 5; 2 NaN; 5 NaN; 3 NaN]
   %
   % See also WAVEFORM/GET, NAN

   % AUTHOR: Celso Reyes, Geophysical Institute, Univ. of Alaska Fairbanks
   % $Date$
   % $Revision$

   if numel(W) <= 1
      n = W.data; %  assumption --> data is already a column of double!
      return
   end

   if nargin == 1
      createDefaultArray = @zeros;
   else
      if ischar(option)
         createDefaultArray = str2func(option);
      elseif isa(option,'function_handle')
         createDefaultArray = option;
      else
         error('waveform:double:invalidOption',...
            '''option'' must be either a function handle or function name.');
      end
   end

   m = get(W,'data_length');
   if isempty(m)
      n = [];
      return
   end

   m=m(:);
   if all(m == m(1))
      n = [W(:).data]; % easy-peazy since all data is of same length.
   else
      n = createDefaultArray(max(m),numel(W));
      for x=1:numel(W)
         n(1:m(x),x) = W(x).data; % assumes that data is a column of double!
      end
   end
end


function img = connectivity_constraint(img, thr, sz, ind_max)
% remove small nonzero pixels
if ~exist('thr', 'var') || isempty(thr)
    thr = 0.01;
end
if ~exist('sz', 'var')||isemtpy(sz)
    sz = 5;
end
if ~exist('ind_max', 'var')||isemtpy(ctr)
    [~, ind_max] = max(img(:));
end
se = strel('square', sz);
ai_open = imopen(img, se);

temp = full(ai_open>max(img(:))*thr);
l = bwlabel(temp, 4);   % remove disconnected components
% [~, ind_max] = max(ai_open(:));

img(l(:)~=l(ind_max)) = 0;


function test3
%TEST3 test sparse on int8, int16, and logical
% Example:
%   test3
% See also cholmod_test

% Copyright 2006-2007, Timothy A. Davis, University of Florida

fprintf ('=================================================================\n');
fprintf ('test3: test sparse on int8, int16, and logical\n') ;

clear all
c =  ['a' 'b' 0 'd']							    %#ok
sparse(c)
sparse2(c)
sparse(c')
sparse2(c')
whos
nzmax(ans)  %#ok

try % this will fail
    sparse(int8(c))
catch
    fprintf ('sparse(int8(c)) fails in MATLAB\n') ;
end
sparse2(int8(c))

sparse2 (int16(c))
whos
s = logical(rand(4) > .5)						    %#ok
sparse (s)
whos
sparse2(s)
whos

x = rand(4)								    %#ok
sparse (x > .5)								    %#ok
whos
sparse2 (x > .5)							    %#ok
whos

fprintf ('test3 passed\n') ;


function [payload, interproc_data ] = aip_detector( ECG_matrix, ECG_header, ECG_start_offset, progress_handle, payload_in, interproc_data)
% aip_detector Arbitrary impulsive pseudoperiodic detector
%   Detailed explanation goes here
    payload = [];

    payload.series_quality.AnnNames = {};
    payload.series_quality.ratios = [];
    payload.series_quality.estimated_labs = {};

    %% config parsing

    if( ~isfield(payload_in, 'trgt_width') )
        % Default QRS width
        payload_in.trgt_width = 0.06; % seconds
    end

    if( ~isfield(payload_in, 'trgt_min_pattern_separation') )
        % Default QRS width
        payload_in.trgt_min_pattern_separation = 0.3; % seconds
    end

    if( ~isfield(payload_in, 'trgt_max_pattern_separation') )
        % Default QRS width
        payload_in.trgt_max_pattern_separation = 2; % seconds
    end

    if( ~isfield(payload_in, 'stable_RR_time_win') )
        % Default time window to consider an stable rhythm
        payload_in.stable_RR_time_win = 2; % seconds
    end

    if( ~isfield(payload_in, 'final_build_time_win') )
        % Default time window to build the final detection based on each
        % pattern found in the recording
        payload_in.final_build_time_win = 20; % seconds
    end

    if( ~isfield(payload_in, 'powerline_interference') )
        % Default auto
        payload_in.powerline_interference = nan; % Hz
    end

    if( ~isfield(payload_in, 'max_patterns_found') )
        % Default QRS width
        payload_in.max_patterns_found = 3; % # patterns
    end

    if( ~isfield(payload_in, 'sig_idx') )
        % Default QRS width
        payload_in.sig_idx = 1:ECG_header.nsig; % # patterns
    end

    %% preprocessing

    % lead names desambiguation
    str_aux = regexprep(cellstr(ECG_header.desc), '\W*(\w+)\W*', '$1');
    lead_names = regexprep(str_aux, '\W', '_');

    [str_aux2, ~ , aux_idx] = unique( lead_names );
    aux_val = length(str_aux2);

    if( aux_val ~= ECG_header.nsig )
        for ii = 1:aux_val
            bAux = aux_idx==ii;
            aux_matches = sum(bAux);
            if( sum(bAux) > 1 )
                lead_names(bAux) = strcat( lead_names(bAux), repmat({'v'}, aux_matches,1), cellstr(num2str((1:aux_matches)')) );
            end
        end
    end

    lead_names = regexprep(lead_names, '\W*(\w+)\W*', '$1');
    lead_names = regexprep(lead_names, '\W', '_');

    progress_handle.checkpoint('Powerline filtering');

    ECG_matrix = double(ECG_matrix);

    if( isnan(payload_in.powerline_interference) )
        f_armonic = 1:3;
        f_armonic = f_armonic*50;
        f_armonic = f_armonic( f_armonic < (ECG_header.freq / 2) );
        f50_idx = round( f_armonic * ECG_header.nsamp / ECG_header.freq ) + 1;
        pwl50_e = abs(goertzel( ECG_matrix(:,payload_in.sig_idx), f50_idx ));

        f_armonic = 1:3;
        f_armonic = f_armonic*60;
        f_armonic = f_armonic( f_armonic < (ECG_header.freq / 2) );
        f60_idx = round( f_armonic * ECG_header.nsamp /ECG_header.freq ) + 1;
        pwl60_e = abs(goertzel( ECG_matrix(:,payload_in.sig_idx), f60_idx ));

        if( sum(sum(pwl50_e)) > sum(sum(pwl60_e)) )
            payload_in.powerline_interference = 50; % Hz
        else
            payload_in.powerline_interference = 60; % Hz
        end
    end

    % Resample for the closest smaller sampling freq
    target_fs = floor(ECG_header.freq/payload_in.powerline_interference) * payload_in.powerline_interference;

    if( target_fs == ECG_header.freq )
        ECG_matrix_res = ECG_matrix(:,payload_in.sig_idx);
    else
        [ups, downs] = rat(target_fs / ECG_header.freq);
        ECG_matrix_res = resample(ECG_matrix(:,payload_in.sig_idx), ups, downs );
    end

    pwl_size = round(target_fs / payload_in.powerline_interference);

    ECG_matrix_res = filter( ones(pwl_size,1)/pwl_size, 1, flipud(ECG_matrix_res) );
    ECG_matrix_res = filter( ones(pwl_size,1)/pwl_size, 1, flipud(ECG_matrix_res) );

    if( target_fs ~= ECG_header.freq )
        ECG_matrix_res = resample(ECG_matrix_res, downs, ups );
        aux_val = size(ECG_matrix_res,1);

        % Pad or trim after resampling
        if( aux_val < ECG_header.nsamp )
            ECG_matrix_res = [ECG_matrix_res; zeros( ECG_header.nsamp - aux_val, size(ECG_matrix_res,1) )];
        elseif( aux_val > ECG_header.nsamp )
            ECG_matrix_res = ECG_matrix_res(1:ECG_header.nsamp,:);
        end
    end

    ECG_matrix(:,payload_in.sig_idx) = ECG_matrix_res;

    %% Start
    pattern_size = 2*round(payload_in.trgt_width/2*ECG_header.freq)+1; % force odd number

    pattern_coeffs = diff(gausswin(pattern_size+1)) .* gausswin(pattern_size);
    first_pattern_coeffs = pattern_coeffs;

    progress_handle.Loops2Do = length(payload_in.sig_idx);

    lp_size = round(1.2*pattern_size);

    for this_sig_idx = rowvec(payload_in.sig_idx)

        progress_handle.start_loop();

        pb_str_prefix = [ lead_names{this_sig_idx} ': ' ];
        %% Initial Pattern search

        progress_handle.checkpoint([ pb_str_prefix 'First pattern guess' ]);

        rise_detector = filter( first_pattern_coeffs, 1, flipud(ECG_matrix(:,this_sig_idx)) );
        rise_detector = filter( first_pattern_coeffs, 1, flipud(rise_detector) );
        rise_detector = filter( ones(lp_size,1)/lp_size, 1, flipud(abs(rise_detector)) );
        rise_detector = filter( ones(lp_size,1)/lp_size, 1, flipud(rise_detector) );

        % figure(4); plot(rise_detector ./ max(rise_detector)); hold on; plot(ECG_matrix(:,this_sig_idx) ./ max(ECG_matrix(:,this_sig_idx))); legend('rise detector','ECG');

        % figure(3); plot(ECG_matrix(:,1) ); xlims = xlim(); ylims = ylim(); box_h = ylims + [0.01 -0.1] * diff(ylims); aux_sc = 0.8*diff(ylims)/(max(rise_detector)-min(rise_detector));aux_off = ylims(1) + 0.6*diff(ylims); hold on; plot(rise_detector * aux_sc + aux_off); hold off; ylim(ylims);

        progress_handle.checkpoint([ pb_str_prefix 'Peak detection first guess' ]);

        actual_thr = thr_calc(rise_detector, payload_in.trgt_min_pattern_separation, ECG_header.freq);

        if isnan(actual_thr)
            cprintf('Red', '\nRecording %s, lead %s, no patterns above threshold found.(Current %f seconds). \n\n',ECG_header.recname , lead_names{this_sig_idx}, payload_in.stable_RR_time_win);
            continue
        end

        first_detection_idx = modmax(rise_detector, 1, actual_thr, 1, round(payload_in.trgt_min_pattern_separation * ECG_header.freq));

        %% Look for stable segments
        if numel(first_detection_idx) <= 1
            cprintf('Red', '\nRecording %s, lead %s, Not enough patterns found over noise floor. \n\n',ECG_header.recname , lead_names{this_sig_idx});
            continue
        end

        RRserie = RR_calculation(first_detection_idx, ECG_header.freq);

        RRserie_filt = MedianFiltSequence(first_detection_idx, RRserie, round(5 * ECG_header.freq));

        RR_scatter = abs(RRserie - RRserie_filt)./RRserie_filt;

        % Find the most stable regions
        RR_thr = prctile(RR_scatter, 50);

        str_aux = [ 'aip_guess' '_' lead_names{this_sig_idx} ];
        payload.(str_aux).time = first_detection_idx + ECG_start_offset - 1;
        payload.series_quality.AnnNames = [ payload.series_quality.AnnNames ; {str_aux} {'time'} ];
        RRserie_mean_sq_error = mean((RRserie - RRserie_filt).^2);
        payload.series_quality.ratios = [payload.series_quality.ratios; RRserie_mean_sq_error];
        payload.series_quality.estimated_labs = [ payload.series_quality.estimated_labs; {[]} ];

%         stable_rhythm_regions = get_segments_from_sequence(first_detection_idx, RR_scatter, round(payload_in.stable_RR_time_win * ECG_header.freq), RR_thr );
%
% %     figure(3); plot(first_detection_idx, [RRserie RRserie_filt] ); ylims = ylim(); ylims = ylims + [0.1 -0.1] * diff(ylims); hold on; plot( [ stable_rhythm_regions'; flipud(stable_rhythm_regions'); stable_rhythm_regions(:,1)' ],  repmat([ylims(1); ylims(1); ylims(2);ylims(2);ylims(1)],1, size(stable_rhythm_regions,1) ), 'b:' ); hold off;
% %     figure(3); plot(first_detection_idx, [RR_scatter], ':xb' ); xlims = xlim(); ylims = ylim(); ylims = ylims + [0.01 -0.1] * diff(ylims); hold on; plot(xlims, [RR_thr RR_thr], '--r'); plot( [ stable_rhythm_regions'; flipud(stable_rhythm_regions'); stable_rhythm_regions(:,1)' ],  repmat([ylims(1); ylims(1); ylims(2);ylims(2);ylims(1)],1, size(stable_rhythm_regions,1) ), 'b:' ); hold off;
% %     figure(3); plot(first_detection_idx, [RRserie RRserie_filt] ); xlims = xlim(); ylims = ylim(); ylims = ylims + [0.1 -0.1] * diff(ylims); aux_sc = 0.25*diff(ylims)/(max(RR_scatter)-min(RR_scatter));aux_off = ylims(1) + 0.65*diff(ylims); hold on; plot(first_detection_idx, (RR_scatter*aux_sc) +aux_off , ':xb' ); plot(xlims, ([RR_thr RR_thr]*aux_sc ) + aux_off, '--r'); plot( [ stable_rhythm_regions'; flipud(stable_rhythm_regions'); stable_rhythm_regions(:,1)' ],  repmat([ylims(1); ylims(1); ylims(2);ylims(2);ylims(1)],1, size(stable_rhythm_regions,1) ), 'b:' ); hold off;
%
%         if( isempty(stable_rhythm_regions) )
%             cprintf('Red', '\nRecording %s, lead %s, no stable rhythm segments found. Consider decreasing "stable_RR_time_win" (Current %f seconds). \n\n',ECG_header.recname , lead_names{this_sig_idx}, payload_in.stable_RR_time_win);
%             continue
%         end
%
%         lstable_rhythm_regions = size(stable_rhythm_regions,1);
%
%         % Get the larger or more stable segments first
%         [~, aux_idx] = sort(diff(stable_rhythm_regions,1,2), 'descend');
%
%         larger_stable_rank = arrayfun(@(a)(find(a==aux_idx)), 1:lstable_rhythm_regions);
%
%         pattern_prewin = round(payload_in.trgt_width/2*ECG_header.freq); % force odd number
%
%         % Get the segments with less change in morphology
%         pack_variance = repmat(realmax,lstable_rhythm_regions,1);
%
%         % From the longest - most rhythm stable regions
%         for ii = 1: round(lstable_rhythm_regions/4)
%
%             jj = aux_idx(ii);
%
%             % Woody method refinement
%             aux_idx2 = find(first_detection_idx >= stable_rhythm_regions(jj,1) & first_detection_idx <= stable_rhythm_regions(jj,2));
%             avg_pack = pack_signal(ECG_matrix(:,1), first_detection_idx(aux_idx2), [ pattern_prewin pattern_prewin ], true);
%
%             % Mean variance across the ensemble
%             pack_variance(jj) = nanmean(nanvar(squeeze(avg_pack), [], 2));
%
%         end
%         [~, aux_idx] = sort(pack_variance);
%
%         morphology_rank = arrayfun(@(a)(find(a==aux_idx)), 1:lstable_rhythm_regions);
%
%         % Final rank
%         [~, aux_idx] = sort(larger_stable_rank + morphology_rank);
%
%         stable_rhythm_regions = stable_rhythm_regions(aux_idx( 1:min(payload_in.max_patterns_found , size(stable_rhythm_regions,1))), : );
%
%         % figure(3); plot(first_detection_idx, [RRserie RRserie_filt] ); xlims = xlim(); ylims = ylim(); ylims = ylims + [0.1 -0.1] * diff(ylims); aux_sc = 0.25*diff(ylims)/(max(RR_scatter)-min(RR_scatter));aux_off = ylims(1) + 0.65*diff(ylims); hold on; plot(first_detection_idx, (RR_scatter*aux_sc) +aux_off , ':xb' ); plot(xlims, ([RR_thr RR_thr]*aux_sc ) + aux_off, '--r'); plot( [ stable_rhythm_regions'; flipud(stable_rhythm_regions'); stable_rhythm_regions(:,1)' ],  repmat([ylims(1); ylims(1); ylims(2);ylims(2);ylims(1)],1, size(stable_rhythm_regions,1) ), 'b:' ); arrayfun(@(a)(text( stable_rhythm_regions(a,1), ylims(2), num2str(a))), 1:min(payload_in.max_patterns_found )); hold off;
%
%         %% save first detections as a separate time serie
%
%         str_aux = [ 'aip_guess' '_' lead_names{this_sig_idx} ];
%         payload.(str_aux).time = first_detection_idx + ECG_start_offset - 1;
%         payload.series_quality.AnnNames = [ payload.series_quality.AnnNames ; {str_aux} {'time'} ];
%         RRserie_mean_sq_error = mean((RRserie - RRserie_filt).^2);
%         payload.series_quality.ratios = [payload.series_quality.ratios; RRserie_mean_sq_error];
%         payload.series_quality.estimated_labs = [ payload.series_quality.estimated_labs; {[]} ];
%
%         k_woody = 3;
%         win_limits = [ pattern_prewin (pattern_prewin+1) ];
%         extended_win_limits = [k_woody * win_limits(1), k_woody * win_limits(1)+1];
%         aux_offset = (extended_win_limits(1) - win_limits(1) + 1); % ???
%
%         aux_pat_coeff = zeros(sum(win_limits),size(stable_rhythm_regions,1));
%
%         for ii = 1:size(stable_rhythm_regions,1)
%
%             % refinamos con el metodo de woody
%             aux_idx2 = find(first_detection_idx >= stable_rhythm_regions(ii,1) & first_detection_idx <= stable_rhythm_regions(ii,2));
%
%     % figure(3); avg_pack = pack_signal(ECG_matrix(:,this_sig_idx), first_detection_idx(aux_idx2), 10*[ pattern_prewin pattern_prewin ], true); plot_ecg_mosaic(avg_pack)
%
%             if isempty(aux_idx2)
%                 % Cannot found any stable regions to apply the woody_method
%                 cprintf('Red', '\nRecording %s, lead %s, Cannot found any stable regions. \n\n',ECG_header.recname , lead_names{this_sig_idx});
%                 continue
%             end
%
%             pattern_coeffs = woody_method(ECG_matrix(:,this_sig_idx), first_detection_idx(aux_idx2), extended_win_limits, 0.95);
%             % trim the pattern
%             aux_pat_coeff(:,ii) = pattern_coeffs( aux_offset:(aux_offset+size(aux_pat_coeff,1)-1) );
%
%             % figure(5); plot(first_pattern_coeffs ./ max(abs(first_pattern_coeffs))); hold on; plot(aux_pat_coeff(:,ii) ./ max(abs(aux_pat_coeff(:,ii))))
%
%             progress_handle.checkpoint([ pb_str_prefix 'Pattern match ' num2str(ii) ]);
%
%         %         rise_detector = filter(ones(lp_size,1)/lp_size,1, abs([0; diff(double(ECG_matrix(:,ss)))]));
%         %         rise_detector = [zeros(((lp_size-1)/2)-1,1); rise_detector((lp_size-1)/2:end)];
%             rise_detector = filter( pattern_coeffs, 1, flipud(ECG_matrix(:,this_sig_idx)) );
%             rise_detector = filter( pattern_coeffs, 1, flipud(rise_detector) );
%             rise_detector = filter( ones(lp_size,1)/lp_size, 1, flipud(abs(rise_detector)) );
%             rise_detector = filter( ones(lp_size,1)/lp_size, 1, flipud(rise_detector) );
%
%             % figure(6); plot(rise_detector ./ max(rise_detector)); hold on; plot(ECG_matrix(:,this_sig_idx) ./ max(ECG_matrix(:,this_sig_idx)))
%
%             progress_handle.checkpoint([ pb_str_prefix 'Peak detection pattern ' num2str(ii) ]);
%
%             actual_thr = thr_calc(rise_detector, payload_in.trgt_min_pattern_separation, ECG_header.freq);
%
%             if isnan(actual_thr)
%                 cprintf('Red', '\nRecording %s, lead %s, no patterns above threshold found.(Current %f seconds). \n\n',ECG_header.recname , lead_names{this_sig_idx}, payload_in.stable_RR_time_win);
%                 continue
%             end
%
%             % Here the MIN time restriction is important
%             this_idx = modmax(rise_detector, 1, actual_thr, 1, round(payload_in.trgt_min_pattern_separation * ECG_header.freq));
%
%             if numel(first_detection_idx) <= 1
%                 cprintf('Red', '\nRecording %s, lead %s, Not enough patterns found over noise floor. \n\n',ECG_header.recname , lead_names{this_sig_idx});
%                 continue
%             end
%
%             RRserie = RR_calculation(this_idx, ECG_header.freq);
%
%             this_RRserie_filt = colvec(MedianFiltSequence(this_idx, RRserie, round(5 * ECG_header.freq)));
%
%             RRserie_mean_sq_error = mean((RRserie - this_RRserie_filt).^2);
%
%             % Structure of detection loading
%             str_aux = [ 'aip_patt_' num2str(ii) '_' lead_names{this_sig_idx} ];
%             payload.(str_aux).time = this_idx + ECG_start_offset - 1;
%             payload.series_quality.AnnNames = [ payload.series_quality.AnnNames ; {str_aux} {'time'} ];
%             payload.series_quality.ratios = [payload.series_quality.ratios; RRserie_mean_sq_error];
%             payload.series_quality.estimated_labs = [ payload.series_quality.estimated_labs; {[]} ];
%
%         end

        % Como el ratio se usa para rankear mediciones, hacemos un ratio
        % ficticio para cada detección por el ranking de "parsimoniosidad" que
        % sacó
        payload.series_quality.ratios = 1-(payload.series_quality.ratios * 1./max(payload.series_quality.ratios));

        progress_handle.end_loop();

    end
end

function [actual_thr] = thr_calc(rise_detector, trgt_min_pattern_separation, freq)
    initial_thr = 30;

    actual_thr = prctile(rise_detector, initial_thr);

    [~, max_values ] = modmax(rise_detector, 1, actual_thr, 1, round(trgt_min_pattern_separation * freq));

    if isempty(max_values)

        % possible flat recording.
        actual_thr = nan;

    else

        prctile_grid = prctile(max_values,1:100);

        grid_step = median(diff(prctile_grid));

        if grid_step == 0
            actual_thr = nan;
        else
            thr_grid = actual_thr:grid_step:max(max_values);

            hist_max_values = histcounts(max_values, thr_grid);

            first_bin_idx = 1;

            [thr_idx, thr_max ] = modmax( colvec( hist_max_values ) , first_bin_idx, 0, 0, [], [] );

            if( isempty(thr_max) )
                actual_thr = nan;
            else
                % Mass center of the distribution, probably the value where the
                % patterns under search are located.
                thr_idx_expected = floor(rowvec(thr_idx) * colvec(thr_max) *1/sum(thr_max));

                aux_seq = 1:length(thr_grid);

                % MPS0018 recording from Basel VIII database forced to add the
                % lesser or equal to aux_seq <= thr_idx_expected. Probably a
                % recording where no clear maximum above the noise threshold were
                % found.
                min_hist_max_values = min( hist_max_values( aux_seq >= first_bin_idx & aux_seq < thr_idx_expected) );

                % In case several indexes match the boolean condition, the mean
                % index is the center of all those indexes. Other criteria such as
                % min or max can be explored
                % thr_min_idx = round(mean(find(bAux & [hist_max_values 0] == min_hist_max_values)));
                thr_min_idx = round(mean(find(aux_seq >= first_bin_idx & aux_seq < thr_idx_expected & [hist_max_values 0] == min_hist_max_values)));

                actual_thr = thr_grid(thr_min_idx);
            end
        end

    end

end


function wNew = response_apply(w,filterObj,sourceType,source)

%RESPONSE_APPLY Deconvolve an instrument response from a waveform object.
% W = RESPONSE_APPLY(W,FILTEROBJ,SOURCETYPE,SOURCE) applies an instrument
% response to waveform W, filtered on the filter range specified by
% filterobject FILTEROBJ. The response is read from SOURCE which is of type
% SOURCETYPE. SOURCETYPE can be:
%   polezero:  A polezero structure. See HELP RESPONSE_GET_FROM_POLEZERO
%   antelope:  An Antelope database. See HELP RESPONSE_GET_FROM_DB
%  structure:  A response structure. See HELP RESPONSE_GET_FROM_DB
%
%  Example uses
%     w = response_apply(w,filterObj,'polezero',polezero)
%     w = response_apply(w,filterObj,'antelope',dbName)
%     w = response_apply(w,filterObj,'structure',response)
%
%
% see also response_get_from_db, response_get_from_polezero

% Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
%         (borrowing liberally from codes written by M. Haney)
% $Date$
% $Revision$

% CHECK INPUTS
if ~ischar(sourceType) || size(sourceType,1)~=1
    error('SOURCETYPE must be the same for all waveforms');
end
if strncmpi(sourceType,'antelope',3) && size(sourceType,1)~=1
    error('All responses must be sourced from a single database');
end

% PREPARE INDIVIDUAL WAVEFORMS

filterObj = repmat(filterObj,size(w));
source = repmat({source},size(w));
for n = 1:numel(w)
    wNew(n) = response_apply_one(w(n),filterObj(n),sourceType,source(n));
end

wNew = reshape(wNew,size(w));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PROCESS INDIVIDUAL WAVEFORMS

function w = response_apply_one(w,filterObj,sourceType,source)

if numel(w)~=1
    error('Only one waveform should be passed to this subroutine.');
end

% % CHECK THE SOURCE OF THE RESPONSE
% if isa(source,'char')                 % antelope db
%     disp(['Attempting to retreive response information from database: ' source ' ...']);
% elseif  isa(source,'struct')          % polezero structure
%     disp('Using explicit poles/zeros ...');
% else
%     error('response source argument not recognized');
% end

% PREPARE VARIABLE SPACE
filterBand = get(filterObj,'CUTOFF');
period = get(w,'PERIOD');
nyquist = get(w,'NYQ');
rawData = double(w);
dataLength = numel(rawData);
rawData = reshape(rawData,1,dataLength);

% PREPARE TRACE DATA
% Create a zero padded Tukey taper
%    Taper size is determined by the high pass filter frequency
%    Tapered waveform is zeros padded on either end to triple the trace length
% Glenn 20160519: could the follow commands be simplified using tukeywin?
taperFullWidth = round(0.5/(filterBand(1)*period))*2; % guarantees an even number
taperAmp = hanning(taperFullWidth)';
taperAmp = [taperAmp(1:(taperFullWidth/2)) ones(1,(dataLength-taperFullWidth)) taperAmp(((taperFullWidth/2)+1):taperFullWidth)];
taperAmp = [ zeros(1,dataLength) taperAmp zeros(1,dataLength) ];
rawData = [ zeros(1,dataLength) rawData zeros(1,dataLength) ].*taperAmp;
dataLength = numel(rawData);

% CREATE FULL INVERSE RESPONSE VECTORS
% Seperate routines for even and odd trace length
% Full responses are reconstituted from positive only side
% ws and respInv are positive frequencies only
% wsFull and respInvFull have negative frequencies as well
if floor(dataLength/2) == ceil(dataLength/2)
    halfDataLength=dataLength/2;
    ws = 2*pi*[0:(halfDataLength)]*(1/(halfDataLength))*nyquist;
    [respInv,response] = prep_inverse_response(w,ws,sourceType,source);
    wsFull = [ -ws(halfDataLength+1:-1:2) ws(1:halfDataLength) ];
    respInvFull = ([ real(respInv(end:-1:2)) real(respInv(1:end-1)) ] + 1i*[ -imag(respInv(end:-1:2)) imag(respInv(1:end-1)) ]);
else
    halfDataLength = (dataLength+1)/2;
    ws = 2*pi * [0:(halfDataLength-1)] * (2/dataLength) * nyquist;
    [respInv,response] = prep_inverse_response(w,ws,sourceType,source);
    wsFull = [ -ws(end:-1:2) ws(1:end) ];
    respInvFull = ([ real(respInv(end:-1:2)) real(respInv(1:end)) ] + 1i*[ -imag(respInv(end:-1:2)) imag(respInv(1:end)) ]);
end

% APPLY THE RESPONSE
newData = real(ifft(ifftshift(fftshift(fft(rawData)).*(respInvFull))));

% BANDPASS FILTER THE DATA
[z q] = butter(get(filterObj,'POLES'),[(filterBand(1)/nyquist) (filterBand(2)/nyquist)]);
% Glenn 20160519 could the follow 3 lines be replaced using filtfilt?
newData = filter(z,q,newData);
newData = filter(z,q,fliplr(newData));
newData = fliplr(newData);

% REMOVE TRACE PADDING
newData = newData( (dataLength/3)+1 : 2*(dataLength/3) );

% RETURN INSTURMENT CORRECTED WAVEFORM
w = set(w,'DATA',newData);
w = addhistory(w,'A frequency response was deconvolved using RESPONSE_APPLY. See RESPONSE field.');
w = addhistory(w,'Butterworth filter was doubly applied (FILTFILT). See FILTER field.');
w = addfield(w,'RESPONSE',response);
w = addfield(w,'FILTER',filterObj);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SET UP THE INVERSE RESPONSE FUNCTIONS
% could facilitate calls to a different response loader

function [respInv,response] = prep_inverse_response(w,ws,sourceType,source)

switch lower(sourceType)

    case {'antelope'}
        sta = get(w,'STATION');
        chan = get(w,'CHANNEL');
        time = get(w,'START_MATLAB');
        response = response_get_from_db(sta,chan,time,(ws/(2*pi)),source);
        respInv = 1./response.values;
        respInv(1) = 0;
        respInv = reshape(respInv,1,numel(respInv));

    case {'polezero'}
        response = response_get_from_polezero(ws,source);
        respInv = 1./response.values;
        respInv(1) = 0;
        respInv = reshape(respInv,1,numel(respInv));

    case {'structure'}
        disp('Structure is not yet implemented')
    otherwise
        error('response source argument not recognized');

end


function mdnet_prepare_model()
% MDNET_PREPARE_MODEL
% Prepare a initial CNN model for learning MDNet.
%
% conv1-3 are adopted from VGG-M network.
% fc4-fc6 are randomly initialized.
% fc6 will be replaced by multiple domain-specific layers when training MDNet.
%
% Hyeonseob Nam, 2015
%

% conv1-3 layers from VGG-M network pretrained on ImageNet
src_model = './models/imagenet-vgg-m-conv1-3.mat';
% output network
dst_model = './models/mdnet_init.mat';

if exist(dst_model,'file')
    return;
end

%% load conv layers
load(src_model);

new_layers = {};
for i=1:numel(layers)
    if strcmp(layers{i}.name,'conv4'), break; end
    switch (layers{i}.type)
        case 'conv'
            layers{i}.filters = layers{i}.weights{1};
            layers{i}.biases = layers{i}.weights{2};
            layers{i} = rmfield(layers{i},'weights');
            layers{i}.pad = 0;
            last_dim = size(layers{i}.biases,2);
        case 'pool'
            layers{i}.pad = 0;
    end
    new_layers{end+1} = layers{i};
end

%% init fc layers
scal = 1 ;
init_bias = 0.1;

% Block 4
new_layers{end+1} = struct('type', 'conv', ...
                           'name', 'fc4', ...
                           'filters', 0.01/scal * randn(3,3,last_dim,512,'single'),...
                           'biases', init_bias*ones(1,512,'single'), ...
                           'stride', 1, ...
                           'pad', 0, ...
                           'filtersLearningRate', 10, ...
                           'biasesLearningRate', 20, ...
                           'filtersWeightDecay', 1, ...
                           'biasesWeightDecay', 0) ;
new_layers{end+1} = struct('type', 'relu', 'name', 'relu4') ;
new_layers{end+1} = struct('type', 'dropout', 'name', 'drop4', 'rate', 0.5) ;

% Block 5
new_layers{end+1} = struct('type', 'conv', ...
                           'name', 'fc5', ...
                           'filters', 0.01/scal * randn(1,1,512,512,'single'),...
                           'biases', init_bias*ones(1,512,'single'), ...
                           'stride', 1, ...
                           'pad', 0, ...
                           'filtersLearningRate', 10, ...
                           'biasesLearningRate', 20, ...
                           'filtersWeightDecay', 1, ...
                           'biasesWeightDecay', 0) ;
new_layers{end+1} = struct('type', 'relu', 'name', 'relu5') ;
new_layers{end+1} = struct('type', 'dropout', 'name', 'drop5', 'rate', 0.5) ;

% Block 6
new_layers{end+1} = struct('type', 'conv', ...
                           'name', 'fc6', ...
                           'filters', 0.01/scal * randn(1,1,512,2,'single'), ...
                           'biases', zeros(1, 2, 'single'), ...
                           'stride', 1, ...
                           'pad', 0, ...
                           'filtersLearningRate', 10, ...
                           'biasesLearningRate', 20, ...
                           'filtersWeightDecay', 1, ...
                           'biasesWeightDecay', 0) ;
new_layers{end+1} = struct('type', 'softmaxloss', 'name', 'loss') ;

clear layers; layers = new_layers;
save(dst_model,'layers');


function S = size(v)
% SIZE  Size of the expansion coefficients of a BALLFUNV
%   S = SIZE(V) returns a 3 by 3 matrix whose vertical entries represent
%   the sizes of Vx, Vy and Vz
%
%   See also BALLFUN/SIZE.

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

% Return the list [m,n,p] corresponding to the size of the ballfun
% functions in f

if isempty( v )
    S = [];
    return
end

V = v.comp;

S = [ size( V{1} );
      size( V{2} );
      size( V{3} ); ];
end


% INTLAB  INTerval LABoratory.
% Version 7.1   June-24-2013   966 files, 849 m-files, 32,346 lines of Matlab code w/o comments
%                 (54,324 with comments),
%             supported by a test-case library with more than 60,000 lines of Matlab code w/o comments
%
%===> For license information and terms of use, see README.TXT.
%
%
%Directories and toolboxes
%  intval       - Interval package
%  gradient     - Automatic differentiation package
%  hessian      - Automatic Hessian package
%  taylor       - Automatic Taylor package
%  slope        - Automatic slope package
%  polynom      - Polynomial package (univariate and multivariate)
%  long         - Rudimentary long package
%  utility      - Some useful functions
%  AccSum       - Reference implementations for sum/dot routines
%  demos        - several demo routines
%
%Auxiliary files
%  intlablogo   - INTLAB logo display
%  startintlab  - Initialization of INTLAB
%  startup      - Calls startintlab: adapt to your local installation
%  intlabsetting   - Current setting of INTLAB control variables
%  INTLAB_Version_2     - Additions and changes in version 2
%  INTLAB_Version_3     - Additions and changes in version 3
%  INTLAB_Version_3.1   - Additions and changes in version 3.1
%  INTLAB_Version_4     - Additions and changes in version 4
%  INTLAB_Version_4.1   - Additions and changes in version 4.1
%  INTLAB_Version_4.1.1 - Additions and changes in version 4.1.1
%  INTLAB_Version_4.1.2 - Additions and changes in version 4.1.2
%  INTLAB_Version_5     - Additions and changes in version 5
%  INTLAB_Version_5.1   - Additions and changes in version 5.1
%  INTLAB_Version_5.2   - Additions and changes in version 5.2
%  INTLAB_Version_5.3   - Additions and changes in version 5.3
%  INTLAB_Version_5.4   - Additions and changes in version 5.4
%  INTLAB_Version_5.5   - Additions and changes in version 5.5
%  INTLAB_Version_6     - Additions and changes in version 6
%  INTLAB_Version_7     - Additions and changes in version 7
%  INTLAB_Version_7.1   - Additions and changes in version 7.1
%  FAQ          - Frequently asked questions
%  Readme       - Installation, a little tutorial and miscellaneous
%
%
%INTLAB based on
%  S.M. Rump: INTLAB - INTerval LABoratory, in "Developments in
%    Reliable Computing", ed. T. Csendes, Kluwer Academic Publishers,
%    77-104, 1999.
%
%INTLAB implementation of interval arithmetic is based on
%  S.M. Rump: Fast and Parallel Interval Arithmetic,
%    BIT 39(3), 539-560, 1999.
%and
%  S. Oishi, S.M. Rump: Fast verification of solutions of matrix equations,
%    Numerische Mathametik 90, 755-773, 2002.
%
%Real interval standard functions based on
%  S.M. Rump: Rigorous and portable standard functions,
%    BIT 41(3), 540-56, 2001.
%
%Complex interval standard functions based on
%  N.C. Boersken: Komplexe Kreis-Standardfunktionen, Freiburger
%    Intervallberichte 78/2.
%
%Accurate summation and dot product with specified _precision_
%  T. Ogita, S.M. Rump, and S. Oishi. Accurate Sum and Dot Product,
%     SIAM Journal on Scientific Computing (SISC), 26(6):1955-1988, 2005.
%  S.M. Rump, T. Ogita, and S. Oishi. Fast High Precision Summation.
%     Nonlinear Theory and Its Applications (NOLTA), IEICE, 1(1), 2010.
%  S.M. Rump: Ultimately Fast Accurate Summation, SIAM Journal on
%     Scientific Computing (SISC), 31(5):3466-3502, 2009.
%
%Accurate summation and dot product with specified _accuracy_
%  S.M. Rump, T. Ogita, and S. Oishi: Accurate Floating-point Summation I:
%     Faithful Rounding. SIAM Journal on Scientific Computing (SISC),
%     31(1): 189-224, 2008.
%  S.M. Rump, T. Ogita, and S. Oishi: Accurate Floating-point Summation II:
%     Sign, K-fold Faithful and Rounding to Nearest. SIAM Journal on Scientific
%     Computing (SISC), 31(2):1269-1302, 2008.
%  S.M. Rump: Ultimately Fast Accurate Summation, SIAM Journal on
%     Scientific Computing (SISC), 31(5):3466-3502, 2009
%
%The linear system solver including inner inclusions is completely redesigned and
%now based on
%  S.M. Rump. Accurate solution of dense linear systems, Part II: Algorithms using
%    directed rounding. Journal of Computational and Applied Mathematics (JCAM),
%    242:185–212, 2013.
%
%Least squares problems and underdetermined linear systems are based on
%  S.M. Rump. Verified Bounds for Least Squares Problems and Underdetermined Linear Systems.
%    SIAM J. Matrix Anal. Appl. (SIMAX), 33(1):130–148, 2012.
%and new componentwise error estimates on
%  S.M. Rump: Improved componentwise verified error bounds for least squares problems
%    and underdetermined linear systems, to appear.
%
%For references to verification algorithms cf. the corresponding routines. Many
%of them are discussed in
%  S.M. Rump: Verification methods: Rigorous results using floating-point arithmetic.
%    Acta Numerica, 19:287-449, 2010.
%
%Slopes based on
%  R. Krawzcyk, A. Neumaier: Interval slopes for rational functions
%    and associated centered forms, SIAM J. Numer. Anal. 22, 604-616
%    (1985)
%with improvements based on
%  S.M. Rump: Expansion and Estimation of the Range of Nonlinear Functions,
%    Math. Comp. 65(216), pp. 1503-1512, 1996.
%
%Gradients, Hessians and Taylor expansion based on standard forward mode, cf. for example
%  L.B. Rall: Automatic Differentiation: Techniques and Applications,
%    Lecture Notes in Computer Science 120, Springer, 1981.
%
%Long floating point and interval arithmetic based on standard techniques
%  with adaptation and speed up for interpretative systems.
%

% written  12/30/98     S.M. Rump
% modified 03/06/99     S.M. Rump  Version 2
% modified 11/16/99     S.M. Rump  Version 3
% modified 03/07/02     S.M. Rump  Version 3.1
% modified 12/08/02     S.M. Rump  Version 4
% modified 12/27/02     S.M. Rump  Version 4.1
% modified 01/22/03     S.M. Rump  Version 4.1.1
% modified 11/18/03     S.M. Rump  Version 4.1.2
% modified 04/04/04     S.M. Rump  Version 5
% modified 06/04/05     S.M. Rump  Version 5.1
% modified 12/20/05     S.M. Rump  Version 5.2
% modified 05/26/06     S.M. Rump  Version 5.3
% modified 05/31/07     S.M. Rump  Version 5.4
% modified 11/05/08     S.M. Rump  Version 5.5
% modified 05/08/09     S.M. Rump  Version 6
% modified 12/12/12     S.M. Rump  Version 7
% modified 06/24/13     S.M. Rump  Version 7.1
%

% Copyright (c) Siegfried M. Rump, head of the Institute for Reliable Computing,
%               Hamburg University of Technology
%
% For license information and terms of use, see README.TXT.
%
% :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
% ::  Some references to papers using INTLAB are collected on the
% ::    INTLAB homepage   http://www.ti3.tuhh.de/
% ::  If you have additional references to add, please send me
% ::  a mail ( rump [at] tuhh.de )
%


function [varargout]=uniqueIntegerRow(F)

Fs=sort(F,2); %Sort so faces with same nodes have the same rows
[~,IND_F,IND_F_2]=unique(Fs,'rows');
F_uni=F(IND_F,:);

%%

varargout{1}=F_uni;
varargout{2}=IND_F;
varargout{3}=IND_F_2;

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


close all;
clear all;
clc;

data_file_path = 'bin/mmv_phase_transition_snr_30db_s_4.mat';
options.export = true;
options.export_dir = 'bin';
options.export_name = 'mmv_snr_30_db_s_4';
options.chosen_ks = [2, 4, 8, 16, 32, 64];
options.subtitle = 'MMV, SNR=30dB s=4';
spx.pursuit.PhaseTransitionAnalysis.print_results(data_file_path, ...
    'CoSaMP', options);


function [Ip] = go_prepare_image_Aplus(I, x1,y1,x2,y2,s,K1, K2)

Ic = imcrop(I,[x1 y1 x2 y2]);

I(y1,x1:x1+x2+1,:) = 255;
I(y1+1,x1:x1+x2+1,:) = 255;

I(y1+y2,x1:x1+x2+1,:) = 255;
I(y1+y2+1,x1:x1+x2+1,:) = 255;

I(y1:y1+y2,x1,:) = 255;
I(y1:y1+y2,x1+1,:) = 255;
I(y1:y1+y2,x1+x2,:) = 255;
I(y1:y1+y2,x1+x2+1,:) = 255;

Is = imresize(Ic,s,'nearest');

Ip = uint8(255*ones(size(I,1)+K1, size(I,2)+K2, 3));

Ip(1:size(I,1),1:size(I,2),:) = I;

Is(1:end,1,:) = 255;
Is(1:end,end,:) = 255;
Is(1,1:end,:) = 255;
Is(end,1:end,:) = 255;

Ip(size(Ip,1)-size(Is,1)+1:size(Ip,1),size(Ip,2)-size(Is,2)+1:size(Ip,2),:)=Is;


function [Vm]=rigidTransformVertices(V,T,v)

Vm=V;
[I,J,K]=cart2im(V(:,1),V(:,2),V(:,3),v); %Convert to image coordinates
IJK=[I(:) J(:) K(:) ones(size(I(:)))]; %Prepare for mapping
IJK_mapped=(T*IJK')'; %Do mapping
[Vm(:,1),Vm(:,2),Vm(:,3)]=im2cart(IJK_mapped(:,1),IJK_mapped(:,2),IJK_mapped(:,3),v); %Convert mapped image coordinates back to "Cartesian" coordinates

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


function out=erf(x)

precision=x(1).precision;
out_rval=cell(size(x));
out_ival=cell(size(x));

for ii=1:numel(x)
 imag=false;
 [xrval,xival]=getVals(x,ii);
 if hasimag(xival), imag=true; end
 if imag
  error('argument of erf function for mp objects must be real');
 else
  out_rval{ii}=mpfr_erf(precision,xrval);
 end
end % for ii=1:max(ex,
out=class(struct('rval',out_rval,...
                 'ival',out_ival,...
                 'precision',precision),'mp');


function CPD = gaussian_CPD(bnet, self, varargin)
% GAUSSIAN_CPD Make a conditional linear Gaussian distrib.
%
% CPD = gaussian_CPD(bnet, node, ...) will create a CPD with random parameters,
% where node is the number of a node in this equivalence class.

% To define this CPD precisely, call the continuous (cts) parents (if any) X,
% the discrete parents (if any) Q, and this node Y. Then the distribution on Y is:
% - no parents: Y ~ N(mu, Sigma)
% - cts parents : Y|X=x ~ N(mu + W x, Sigma)
% - discrete parents: Y|Q=i ~ N(mu(i), Sigma(i))
% - cts and discrete parents: Y|X=x,Q=i ~ N(mu(i) + W(i) x, Sigma(i))
%
% The list below gives optional arguments [default value in brackets].
% (Let ns(i) be the size of node i, X = ns(X), Y = ns(Y) and Q = prod(ns(Q)).)
% Parameters will be reshaped to the right size if necessary.
%
% mean       - mu(:,i) is the mean given Q=i [ randn(Y,Q) ]
% cov        - Sigma(:,:,i) is the covariance given Q=i [ repmat(100*eye(Y,Y), [1 1 Q]) ]
% weights    - W(:,:,i) is the regression matrix given Q=i [ randn(Y,X,Q) ]
% cov_type   - if 'diag', Sigma(:,:,i) is diagonal [ 'full' ]
% tied_cov   - if 1, we constrain Sigma(:,:,i) to be the same for all i [0]
% clamp_mean - if 1, we do not adjust mu(:,i) during learning [0]
% clamp_cov  - if 1, we do not adjust Sigma(:,:,i) during learning [0]
% clamp_weights - if 1, we do not adjust W(:,:,i) during learning [0]
% cov_prior_weight - weight given to I prior for estimating Sigma [0.01]
% cov_prior_entropic - if 1, we also use an entropic prior for Sigma [0]
%
% e.g., CPD = gaussian_CPD(bnet, i, 'mean', [0; 0], 'clamp_mean', 1)

if nargin==0
  % This occurs if we are trying to load an object from a file.
  CPD = init_fields;
  clamp = 0;
  CPD = class(CPD, 'gaussian_CPD', generic_CPD(clamp));
  return;
elseif isa(bnet, 'gaussian_CPD')
  % This might occur if we are copying an object.
  CPD = bnet;
  return;
end
CPD = init_fields;

CPD = class(CPD, 'gaussian_CPD', generic_CPD(0));

args = varargin;
ns = bnet.node_sizes;
ps = parents(bnet.dag, self);
dps = myintersect(ps, bnet.dnodes);
cps = myintersect(ps, bnet.cnodes);
fam_sz = ns([ps self]);

CPD.self = self;
CPD.sizes = fam_sz;

% Figure out which (if any) of the parents are discrete, and which cts, and how big they are
% dps = discrete parents, cps = cts parents
CPD.cps = find_equiv_posns(cps, ps); % cts parent index
CPD.dps = find_equiv_posns(dps, ps);
ss = fam_sz(end);
psz = fam_sz(1:end-1);
dpsz = prod(psz(CPD.dps));
cpsz = sum(psz(CPD.cps));

% set default params
CPD.mean = randn(ss, dpsz);
CPD.cov = 100*repmat(eye(ss), [1 1 dpsz]);
CPD.weights = randn(ss, cpsz, dpsz);
CPD.cov_type = 'full';
CPD.tied_cov = 0;
CPD.clamped_mean = 0;
CPD.clamped_cov = 0;
CPD.clamped_weights = 0;
CPD.cov_prior_weight = 0.01;
CPD.cov_prior_entropic = 0;
nargs = length(args);
if nargs > 0
  CPD = set_fields(CPD, args{:});
end

% Make sure the matrices have 1 dimension per discrete parent.
% Bug fix due to Xuejing Sun 3/6/01
CPD.mean = myreshape(CPD.mean, [ss ns(dps)]);
CPD.cov = myreshape(CPD.cov, [ss ss ns(dps)]);
CPD.weights = myreshape(CPD.weights, [ss cpsz ns(dps)]);

% Precompute indices into block structured  matrices
% to speed up CPD_to_lambda_msg and CPD_to_pi
cpsizes = CPD.sizes(CPD.cps);
CPD.cps_block_ndx = cell(1, length(cps));
for i=1:length(cps)
  CPD.cps_block_ndx{i} = block(i, cpsizes);
end

%%%%%%%%%%%
% Learning stuff

% expected sufficient statistics
CPD.Wsum = zeros(dpsz,1);
CPD.WYsum = zeros(ss, dpsz);
CPD.WXsum = zeros(cpsz, dpsz);
CPD.WYYsum = zeros(ss, ss, dpsz);
CPD.WXXsum = zeros(cpsz, cpsz, dpsz);
CPD.WXYsum = zeros(cpsz, ss, dpsz);

% For BIC
CPD.nsamples = 0;
switch CPD.cov_type
 case 'full',
  % since symmetric
    %ncov_params = ss*(ss-1)/2;
    ncov_params = ss*(ss+1)/2;
  case 'diag',
    ncov_params = ss;
  otherwise
    error(['unrecognized cov_type ' cov_type]);
end
% params = weights + mean + cov
if CPD.tied_cov
  CPD.nparams = ss*cpsz*dpsz + ss*dpsz + ncov_params;
else
  CPD.nparams = ss*cpsz*dpsz + ss*dpsz + dpsz*ncov_params;
end

% for speeding up maximize_params
CPD.useC = exist('rep_mult');

clamped = CPD.clamped_mean & CPD.clamped_cov & CPD.clamped_weights;
CPD = set_clamped(CPD, clamped);

%%%%%%%%%%%

function CPD = init_fields()
% This ensures we define the fields in the same order
% no matter whether we load an object from a file,
% or create it from scratch. (Matlab requires this.)

CPD.self = [];
CPD.sizes = [];
CPD.cps = [];
CPD.dps = [];
CPD.mean = [];
CPD.cov = [];
CPD.weights = [];
CPD.clamped_mean = [];
CPD.clamped_cov = [];
CPD.clamped_weights = [];
CPD.cov_type = [];
CPD.tied_cov = [];
CPD.Wsum = [];
CPD.WYsum = [];
CPD.WXsum = [];
CPD.WYYsum = [];
CPD.WXXsum = [];
CPD.WXYsum = [];
CPD.nsamples = [];
CPD.nparams = [];
CPD.cov_prior_weight = [];
CPD.cov_prior_entropic = [];
CPD.useC = [];
CPD.cps_block_ndx = [];


function [ y, j ] = j_carry_islamic ( y, j )

%*****************************************************************************80
%
%% J_CARRY_ISLAMIC carries year-days to years in an Islamic date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, J, a YJ date.
%
  while ( 1 )

    days = year_length_islamic ( y );

    if ( j < days )
      break
    end

    j = j - days;
    y = y + 1;

  end

  return
end