function hmm = updateOmega(hmm,Gamma,residuals,XX,XXGXX,XW,Tfactor,rangeK)

is_gaussian = hmm.train.order == 0; % true if Gaussian observation model is being used
K = hmm.K; ndim = hmm.train.ndim;
if nargin < 8 || isempty(rangeK), rangeK = 1:K; end
if nargin < 7, Tfactor = 1; end
setstateoptions
T = size(residuals,1);
if isfield(hmm.train,'B'), Q = size(hmm.train.B,2);
else Q = ndim; end

if (strcmp(hmm.train.covtype,'uniquediag') || strcmp(hmm.train.covtype,'shareddiag')) ...
        && hmm.train.uniqueAR
    % all are AR and there's a single covariance matrix
    hmm.Omega.Gam_rate = hmm.prior.Omega.Gam_rate;
    for k = rangeK
        if ~isempty(XW)
            XWk = XW(:,:,k);
        else
            XWk = zeros(size(residuals));
        end
        e = (residuals - XWk).^2;
        swx2 = zeros(T,ndim);
        for n=1:ndim
            ind = n:ndim:size(XX,2);
            tmp = XX(:,ind) * hmm.state(k).W.S_W;
            swx2(:,n) = sum(tmp .* XX(:,ind),2);
        end
        hmm.Omega.Gam_rate = hmm.Omega.Gam_rate + ...
            0.5 * Tfactor * sum( bsxfun(@times, e + swx2, Gamma(:,k)) );
    end
    hmm.Omega.Gam_shape = hmm.prior.Omega.Gam_shape + 0.5 * Tfactor * T;

elseif strcmp(hmm.train.covtype,'uniquediag') || strcmp(hmm.train.covtype,'shareddiag')
    hmm.Omega.Gam_rate(regressed) = hmm.prior.Omega.Gam_rate(regressed);
    for k = rangeK
        if ~isempty(XW)
            XWk = XW(:,:,k);
        else
            XWk = zeros(size(residuals));
        end
        e = (residuals(:,regressed) - XWk(:,regressed)).^2;
        swx2 = zeros(T,ndim);
        if ~isempty(hmm.state(k).W.Mu_W)
            for n = 1:ndim
                if ~regressed(n), continue; end
                if ndim==1
                    tmp = XX(:,Sind(:,n)) * hmm.state(k).W.S_W;
                else
                    tmp = XX(:,Sind(:,n)) * permute(hmm.state(k).W.S_W(n,Sind(:,n),Sind(:,n)),[2 3 1]);
                end
                swx2(:,n) = sum(tmp .* XX(:,Sind(:,n)),2);
            end
        end
        hmm.Omega.Gam_rate(regressed) = hmm.Omega.Gam_rate(regressed) + ...
            0.5 * Tfactor * sum( bsxfun(@times, e + swx2(:,regressed), Gamma(:,k)));
    end
    hmm.Omega.Gam_shape = hmm.prior.Omega.Gam_shape + 0.5 * Tfactor * T;

elseif strcmp(hmm.train.covtype,'uniquefull') || strcmp(hmm.train.covtype,'sharedfull')
    hmm.Omega.Gam_rate(regressed,regressed) = hmm.prior.Omega.Gam_rate(regressed,regressed);
    for k = rangeK
        if ~isempty(XW)
            XWk = XW(:,:,k);
        else
            XWk = zeros(size(residuals));
        end
        e = (residuals(:,regressed) - XWk(:,regressed));
        e = (bsxfun(@times,e,Gamma(:,k)))' * e;
        if all(S(:)==1)
            swx2 =  zeros(ndim,ndim);
            if ~isempty(hmm.state(k).W.Mu_W)
                for n1=find(regressed)
                    for n2=find(regressed)
                        if n2<n1, continue, end
                        if hmm.train.pcapred>0
                            index1 = (0:hmm.train.pcapred+(~hmm.train.zeromean)-1) * ndim + n1;
                            index2 = (0:hmm.train.pcapred+(~hmm.train.zeromean)-1) * ndim + n2;
                        else
                            index1 = (0:length(orders)*Q+(~hmm.train.zeromean)-1) * ndim + n1;
                            index2 = (0:length(orders)*Q+(~hmm.train.zeromean)-1) * ndim + n2;
                            index1 = index1(Sind(:,n1)); index2 = index2(Sind(:,n2));
                        end
                        swx2(n1,n2) = sum(sum(hmm.state(k).W.S_W(index1,index2) .* XXGXX{k}(Sind(:,n1),Sind(:,n2))));
                        swx2(n2,n1) = swx2(n1,n2);
                    end
                end
            end
            hmm.Omega.Gam_rate(regressed,regressed) = hmm.Omega.Gam_rate(regressed,regressed) ...
                + Tfactor * (e + swx2(regressed,regressed));
        else % multivariate regression model
            index_iv = sum(S,2)>0; % the independent variables
            xdim = sum(index_iv);
            ydim = sum(regressed);
            S_W = hmm.state(k).W.S_W(logical(S(:)),logical(S(:)));
            L = chol(S_W)';
            XGX = (bsxfun(@times,residuals(:,index_iv),Gamma(:,k)))' * residuals(:,index_iv);
            tracesum = zeros(ydim,ydim);
            for iL = 1:xdim*ydim
                vecinvL = reshape(L(:,iL),xdim,ydim);
                tracesum = tracesum + vecinvL'*XGX*vecinvL;
            end
            hmm.Omega.Gam_rate(regressed,regressed) = hmm.Omega.Gam_rate(regressed,regressed) + tracesum + e;
        end
    end
    hmm.Omega.Gam_rate(regressed,regressed) = (hmm.Omega.Gam_rate(regressed,regressed) + ...
        hmm.Omega.Gam_rate(regressed,regressed)') / 2;
    hmm.Omega.Gam_irate(regressed,regressed) = inv(hmm.Omega.Gam_rate(regressed,regressed));
    hmm.Omega.Gam_shape = hmm.prior.Omega.Gam_shape + Tfactor * T;

else % state dependent

    Gammasum = sum(Gamma);
    for k = rangeK
        if ~hmm.train.active, continue; end
        if ~isempty(XW)
            XWk = XW(:,:,k);
        else
            XWk = zeros(size(residuals));
        end
        if train.uniqueAR
            e = (residuals - XWk).^2;
            swx2 = zeros(T,ndim);
            for n = 1:ndim
                ind = n:ndim:size(XX,2);
                swx2(:,n) = sum(XX(:,ind) * hmm.state(k).W.S_W .* XX(:,ind),2);
            end
            hmm.state(k).Omega.Gam_rate = hmm.state(k).prior.Omega.Gam_rate + ...
                0.5 * Tfactor * sum( bsxfun(@times, e + swx2, Gamma(:,k)) );
            hmm.state(k).Omega.Gam_shape = hmm.state(k).prior.Omega.Gam_shape ...
                + 0.5 * Tfactor * Gammasum(k);

        elseif strcmp(train.covtype,'diag')
            e = (residuals(:,regressed) - XWk(:,regressed)).^2;
            swx2 = zeros(T,ndim);
            if ~isempty(hmm.state(k).W.Mu_W)
                for n=1:ndim
                    if ~regressed(n), continue; end
                    if ndim==1
                        swx2(:,n) = sum(XX(:,Sind(:,n)) * hmm.state(k).W.S_W(Sind(:,n),Sind(:,n)) ...
                            .* XX(:,Sind(:,n)),2);
                    else
                        swx2(:,n) = sum(XX(:,Sind(:,n)) * permute(hmm.state(k).W.S_W(n,Sind(:,n),Sind(:,n)),[2 3 1]) ...
                            .* XX(:,Sind(:,n)),2);
                    end
                end
            end
            hmm.state(k).Omega.Gam_rate(regressed) = hmm.state(k).prior.Omega.Gam_rate(regressed) + ...
                0.5 * Tfactor * sum( bsxfun(@times, e + swx2(:,regressed), Gamma(:,k)) );
            hmm.state(k).Omega.Gam_shape = hmm.state(k).prior.Omega.Gam_shape + 0.5 * Tfactor * Gammasum(k);

        else % full
            if is_gaussian
                if hmm.train.zeromean % If zeromean == 1 for Gaussian model, then XWk=0 and we don't need to subtract at all
                    e = residuals(:,regressed);
                else % If zeromean == 0 for Gaussian model, then XWk has all the same rows, and bsxfun() is a fair bit faster
                    e = bsxfun(@minus,residuals(:,regressed),XWk(1,regressed));
                end
            else
                e = (residuals(:,regressed) - XWk(:,regressed));
            end
            e = (bsxfun(@times,e,Gamma(:,k)))' * e;
            if all(S(:)==1)
                swx2 =  zeros(ndim,ndim);
                if ~isempty(hmm.state(k).W.Mu_W)
                    orders = formorders(train.order,train.orderoffset,train.timelag,train.exptimelag);
                    if isempty(orders)
                        swx2 = hmm.state(k).W.S_W * XXGXX{k};
                    else
                        for n1=find(regressed)
                            for n2=find(regressed)
                                if n2<n1, continue, end
                                if hmm.train.pcapred>0
                                    index1 = (0:hmm.train.pcapred+(~train.zeromean)-1) * ndim + n1;
                                    index2 = (0:hmm.train.pcapred+(~train.zeromean)-1) * ndim + n2;
                                else
                                    index1 = (0:length(orders)*Q+(~train.zeromean)-1) * ndim + n1;
                                    index2 = (0:length(orders)*Q+(~train.zeromean)-1) * ndim + n2;
                                    index1 = index1(Sind(:,n1)); index2 = index2(Sind(:,n2));
                                end
                                swx2(n1,n2) = sum(sum(hmm.state(k).W.S_W(index1,index2) .* XXGXX{k}(Sind(:,n1),Sind(:,n2))));
                                swx2(n2,n1) = swx2(n1,n2);
                            end
                        end
                    end
                end
                hmm.state(k).Omega.Gam_rate(regressed,regressed) = hmm.state(k).prior.Omega.Gam_rate(regressed,regressed) + ...
                    Tfactor * (e + swx2(regressed,regressed));
            else % multivariate regression model
                index_iv = sum(S,2)>0; % the independent variables
                xdim = sum(index_iv);
                ydim = sum(regressed);
                S_W = hmm.state(k).W.S_W(logical(S(:)),logical(S(:)));
                L = chol(S_W)';
                XGX = (bsxfun(@times,residuals(:,index_iv),Gamma(:,k)))' * residuals(:,index_iv);
                tracesum = zeros(ydim,ydim);
                for iL = 1:xdim*ydim
                    vecinvL = reshape(L(:,iL),xdim,ydim);
                    tracesum = tracesum + vecinvL'*XGX*vecinvL;
                end

                hmm.state(k).Omega.Gam_rate(regressed,regressed) = tracesum + e + ...
                    hmm.state(k).prior.Omega.Gam_rate(regressed,regressed);
            end

            hmm.state(k).Omega.Gam_shape = hmm.state(k).prior.Omega.Gam_shape + Tfactor * Gammasum(k);
            hmm.state(k).Omega.Gam_irate(regressed,regressed) = inv(hmm.state(k).Omega.Gam_rate(regressed,regressed));

            %if train.FC
            %    C0 = hmm.state(k).Omega.Gam_rate(regressed,regressed) / hmm.state(k).Omega.Gam_shape;
            %    C = hmm.train.A' * corrcov(hmm.train.A * C0 * hmm.train.A',1) * hmm.train.A;
            %    %iC = hmm.train.A' * pinv(corrcov(hmm.train.A * C0 * hmm.train.A'),1e-10) * hmm.train.A;
            %    iC = hmm.train.A' * ( (corrcov(hmm.train.A * C0 * hmm.train.A',1)+1e-4*eye(size(hmm.train.A,1))) ...
            %           \ hmm.train.A);
            %    hmm.state(k).Omega.Gam_rate(regressed,regressed) = C * hmm.state(k).Omega.Gam_shape;
            %    hmm.state(k).Omega.Gam_irate(regressed,regressed) = iC / hmm.state(k).Omega.Gam_shape;
            %end

            % ensuring symmetry
            hmm.state(k).Omega.Gam_rate(regressed,regressed) = (hmm.state(k).Omega.Gam_rate(regressed,regressed) + ...
                hmm.state(k).Omega.Gam_rate(regressed,regressed)') / 2;
            hmm.state(k).Omega.Gam_irate(regressed,regressed) = (hmm.state(k).Omega.Gam_irate(regressed,regressed) + ...
                hmm.state(k).Omega.Gam_irate(regressed,regressed)') / 2;

        end
    end

end

end


function F = le(X,Y)
% Internal class for constraint lists

% Try to evaluate
try
    if isa(X,'constraint')
        % (z > w) < y
        try
            Z = Y - X.List{end};
        catch
            Y = reshape(Y,[],1);
            Z = Y - X.List{end};
        end
        F = X;
        F.List{end+1} = '<=';
        F.List{end+1} = Y;
        F.Evaluated{end+1} = Z;
        F.ConstraintID(end+1) = yalmip('ConstraintID');
        F.strict(end+1) = 0;
    else
        % x < (w > y)
        try
            Z = Y.List{1} - X;
        catch
            X = reshape(X,[],1);
            Z = Y.List{1} - X;
        end
        F = Y;
        F.List = {X,'<=',F.List{:}};
        F.Evaluated = {Z,F.Evaluated{:}};
        F.ConstraintID = [yalmip('ConstraintID') F.ConstraintID];
        F.strict = [1 F.strict];
    end
catch
    error(lasterr);
end


function rtk=udtrop_rtkins(rtk,tt,bl) %#ok

global glc
INIT_ZWD=0.15; VAR_GRA=0.001^2;

for i=1:2
    if i==1,j=rtk.itr+1;else,j=rtk.itb+1;end

    if rtk.x(j)==0
        rtk=initx(rtk,INIT_ZWD,rtk.opt.std(3)^2,j);
        if rtk.opt.tropopt==glc.TROPOPT_ESTG
            for k=1:2
                rtk=initx(rtk,1e-6,VAR_GRA,j+k);
            end
        end
    else
        rtk.P(j,j)=rtk.P(j,j)+rtk.opt.prn(3)^2*abs(tt);
        if rtk.opt.tropopt==glc.TROPOPT_ESTG
            for k=1:2
                rtk.P(j+k,j+k)=rtk.P(j+k,j+k)+(rtk.opt.prn(3)*0.3)^2*abs(tt);
            end
        end
    end

end

return


function [f, energ, diagn] = scaled_mh(f, opt, gp, x, y, z)
%SCALED_MH  A scaled Metropolis-Hastings sampling for latent values
%
%  Description
%    [F, ENERG, DIAG] = SCALED_MH(F, OPT, GP, X, Y) takes the
%    current latent values F, options structure OPT, Gaussian
%    process structure GP, inputs X and outputs Y. Samples new
%    latent values and returns also energies ENERG and diagnostics
%    DIAG. The latent values are sampled from their conditional
%    posterior p(f|y,th).
%
%    The latent values are whitened with the prior covariance
%    before the sampling. This reduces the autocorrelation and
%    speeds up the mixing of the sampler. See (Neal, 1998) for
%    details on implementation.
%
%    The options structure should include the following fields:
%      repeat              - the number of MH-steps before
%                            returning a single sample (default 10)
%      sample_latent_scale - the scale for the MH-step (default 0.5)
%
%    OPT = SCALED_MH() Returns default options
%
%    OPT = SCALED_MH(OPT) Returns default options for fields not
%    yet set in OPT
%
%   Reference:
%
%    Neal, R. M. (1998) Regression and classification using
%    Gaussian process priors (with discussion), in J. M. Bernardo,
%    et al (editors) Bayesian Statistics 6, Oxford University
%    Press, pp. 475-501:
%
%  See also
%    GP_MC
%
% Copyright (c) 1999,2011 Aki Vehtari
% Copyright (c) 2006-2010 Jarno Vanhatalo

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

% set default options using hmc2_opt
  if nargin<=1
    if nargin==0
      f=scaled_mh_opt();
    else
      f=scaled_mh_opt(f);
    end
    return
  end

  [n,nout] = size(y);
  if isfield(gp.lik, 'nondiagW')
    switch gp.lik.type
      case {'LGP', 'LGPC'}
        % Do nothing
      case {'Softmax', 'Multinom'}
        % Do nothing
      otherwise
        nout=length(gp.comp_cf);
    end
    if isfield(gp, 'comp_cf')  % own covariance for each ouput component
      multicf = true;
      if length(gp.comp_cf) ~= nout
        error('SCALED_MH: the number of component vectors in gp.comp_cf must be the same as number of outputs.')
      end
    else
      multicf = false;
    end
  end
  f = reshape(f,n,nout);

  maxcut = -log(eps);
  mincut = -log(1/realmin - 1);
  lvs=opt.sample_latent_scale;
  a = max(min(f, maxcut),mincut);

  switch gp.type
    case {'FULL'}

      if ~isfield(gp.lik, 'nondiagW') || ismember(gp.lik.type, {'LGP' 'LGPC'})
        [K,C]=gp_trcov(gp, x);
        if isfield(gp,'meanf')
          [H_m,b_m,B_m]=mean_prep(gp,x,[]);
          C = C + H_m'*B_m*H_m;
        end
        L=chol(C)';
      else
        L = zeros(n,n,nout);
        if multicf
          for i1=1:nout
            [tmp, C] = gp_trcov(gp, x, gp.comp_cf{i1});
            L(:,:,i1)=chol(C, 'lower');
          end
        else
          for i1=1:nout
            [tmp, C] = gp_trcov(gp, x);
            L(:,:,i1)=chol(C, 'lower');
          end
        end
      end
      e = -gp.lik.fh.ll(gp.lik, y, f, z);
      ft = zeros(size(y));

      % Adaptive control algorithm to find such a value for lvs
      % that the rejection rate of Metropolis is optimal.
      slrej = 0;
      for li=1:100
        for i1 =1:nout
          ft(:,i1)=sqrt(1-lvs.^2).*f(:,i1)+lvs.*L(:,:,i1)*randn(n,1);
        end
        ed = -gp.lik.fh.ll(gp.lik, y, ft, z);
        a=e-ed;
        if exp(a) > rand(1)
          f=ft;
          e=ed;
          lvs=min(1,lvs*1.1);
        else
          lvs=max(1e-8,lvs/1.05);
        end
      end
      opt.sample_latent_scale=lvs;
      % Do the actual sampling
      for li=1:(opt.repeat)
        for i1 =1:nout
          ft(:,i1)=sqrt(1-lvs.^2).*f(:,i1)+lvs.*L(:,:,i1)*randn(n,1);
        end
        ed = -gp.lik.fh.ll(gp.lik, y, ft, z);
        a=e-ed;
        if exp(a) > rand(1)
          f=ft;
          e=ed;
        else
          slrej=slrej+1;
        end
      end
      diagn.rej = slrej/opt.repeat;
      diagn.lvs = lvs;
      diagn.opt=opt;
      energ=[];
      f = f(:)';

    case 'FIC'
      u = gp.X_u;
      m = size(u,1);
      % Turn the inducing vector on right direction
      if size(u,2) ~= size(x,2)
        u=u';
      end
      % Calculate some help matrices
      [Kv_ff, Cv_ff] = gp_trvar(gp, x);
      K_fu = gp_cov(gp, x, u);
      K_uu = gp_trcov(gp, u);
      Luu = chol(K_uu)';

      % Evaluate the Lambda (La)
      B=Luu\(K_fu');
      Qv_ff=sum(B.^2)';
      Lav = Cv_ff-Qv_ff;
      sLav = sqrt(Lav);

      n=length(y);
      e = -gp.lik.fh.ll(gp.lik, y, f, z);

      % Adaptive control algorithm to find such a value for lvs
      % so that the rejection rate of Metropolis is optimal.
      slrej = 0;
      for li=1:100
        ft=sqrt(1-lvs.^2).*f + lvs.*(sLav.*randn(n,1) + B'*randn(m,1));
        ed = -gp.lik.fh.ll(gp.lik, y, ft, z);
        a=e-ed;
        if exp(a) > rand(1)
          f=ft;
          e=ed;
          lvs=min(1,lvs*1.1);
        else
          lvs=max(1e-8,lvs/1.05);
        end
      end
      opt.sample_latent_scale=lvs;
      % Do the actual sampling
      for li=1:(opt.repeat)
        ft=sqrt(1-lvs.^2).*f + lvs.*(sLav.*randn(n,1) + B'*randn(m,1));
        ed = -gp.lik.fh.ll(gp.lik, y, ft, z);
        a=e-ed;
        if exp(a) > rand(1)
          f=ft;
          e=ed;
        else
          slrej=slrej+1;
        end
      end
      diagn.rej = slrej/opt.repeat;
      diagn.lvs = lvs;
      diagn.opt=opt;
      energ=[];
      f = f';

    case 'PIC'
      u = gp.X_u;
      m = size(u,1);
      ind = gp.tr_index;
      if size(u,2) ~= size(x,2)
        u=u';
      end

      % Calculate some help matrices
      [Kv_ff, Cv_ff] = gp_trvar(gp, x);  % 1 x f  vector
      K_fu = gp_cov(gp, x, u);         % f x u
      K_uu = gp_trcov(gp, u);    % u x u, noiseles covariance K_uu
      Luu = chol(K_uu)';

      % Evaluate the Lambda (La) for specific model
      % Q_ff = K_fu*inv(K_uu)*K_fu'
      % Here we need only the diag(Q_ff), which is evaluated below
      B=Luu\K_fu';
      iLaKfu = zeros(size(K_fu));  % f x u
      for i=1:length(ind)
        Qbl_ff = B(:,ind{i})'*B(:,ind{i});
        [Kbl_ff, Cbl_ff] = gp_trcov(gp, x(ind{i},:));
        La{i} = Cbl_ff - Qbl_ff;
        CLa{i} = chol(La{i})' ;
      end

      n=length(y);
      e = -gp.lik.fh.ll(gp.lik, y, f, z);

      % Adaptive control algorithm to find such a value for lvs
      % so that the rejection rate of Metropolis is optimal.
      slrej = 0;
      for li=1:100
        sampf = randn(size(f));
        for i=1:length(ind)
          sampf(ind{i},:) = CLa{i}*sampf(ind{i},:);
        end
        ft=sqrt(1-lvs.^2).*f + lvs.*(sampf + B'*randn(m,1));
        at = max(min(ft, maxcut),mincut);
        ed = -gp.lik.fh.ll(gp.lik, y, ft, z);
        a=e-ed;
        if exp(a) > rand(1)
          f=ft;
          e=ed;
          lvs=min(1,lvs*1.1);
        else
          lvs=max(1e-8,lvs/1.05);
        end
      end
      opt.sample_latent_scale=lvs;
      % Do the actual sampling
      for li=1:(opt.repeat)
        sampf = randn(size(f));
        for i=1:length(ind)
          sampf(ind{i},:) = CLa{i}*sampf(ind{i},:);
        end
        ft=sqrt(1-lvs.^2).*f + lvs.*(sampf + B'*randn(m,1));
        at = max(min(ft, maxcut),mincut);
        ed = -gp.lik.fh.ll(gp.lik, y, ft, z);
        a=e-ed;
        if exp(a) > rand(1)
          f=ft;
          e=ed;
        else
          slrej=slrej+1;
        end
      end
      diagn.rej = slrej/opt.repeat;
      diagn.lvs = lvs;
      diagn.opt=opt;
      energ=[];
      f = f';

    case 'CS+FIC'
      u = gp.X_u;
      cf_orig = gp.cf;
      ncf = length(gp.cf);
      n = size(x,1); m = size(u,1);

      cf1 = {};
      cf2 = {};
      j = 1;
      k = 1;
      for i = 1:ncf
        if ~isfield(gp.cf{i},'cs')
          cf1{j} = gp.cf{i};
          j = j + 1;
        else
          cf2{k} = gp.cf{i};
          k = k + 1;
        end
      end
      gp.cf = cf1;

      % First evaluate the needed covariance matrices
      % if they are not in the memory
      % v defines that parameter is a vector
      [Kv_ff, Cv_ff] = gp_trvar(gp, x);  % 1 x f  vector
      K_fu = gp_cov(gp, x, u);         % f x u
      K_uu = gp_trcov(gp, u);          % u x u, noiseles covariance K_uu
      K_uu = (K_uu+K_uu')./2;          % ensure the symmetry of K_uu
      Luu = chol(K_uu)';

      B=Luu\(K_fu');
      Qv_ff=sum(B.^2)';
      Lav = Cv_ff-Qv_ff;   % 1 x f, Vector of diagonal elements
      gp.cf = cf2;
      K_cs = gp_trcov(gp,x);
      La = sparse(1:n,1:n,Lav,n,n) + K_cs;
      gp.cf = cf_orig;

      LD = ldlchol(La);
      sLa = chol(La)';

      n=length(y);
      e = -gp.lik.fh.ll(gp.lik, y, f, z);

      % Adaptive control algorithm to find such a value for lvs
      % so that the rejection rate of Metropolis is optimal.
      slrej = 0;
      for li=1:100
        ft=sqrt(1-lvs.^2).*f + lvs.*(sLa*randn(n,1) + B'*randn(m,1));
        ed = -gp.lik.fh.ll(gp.lik, y, ft, z);
        a=e-ed;
        if exp(a) > rand(1)
          f=ft;
          e=ed;
          lvs=min(1,lvs*1.1);
        else
          lvs=max(1e-8,lvs/1.05);
        end
      end
      opt.sample_latent_scale=lvs;
      % Do the actual sampling
      for li=1:(opt.repeat)
        ft=sqrt(1-lvs.^2).*f + lvs.*(sLa*randn(n,1) + B'*randn(m,1));
        ed = -gp.lik.fh.ll(gp.lik, y, ft, z);
        a=e-ed;
        if exp(a) > rand(1)
          f=ft;
          e=ed;
        else
          slrej=slrej+1;
        end
      end
      diagn.rej = slrej/opt.repeat;
      diagn.lvs = lvs;
      diagn.opt=opt;
      energ=[];
      f = f';

  end
end

function opt = scaled_mh_opt(opt)
%SCALED_MH_OPT  Default options for scaled Metropolis-Hastings sampling
%
%  Description
%    OPT = SCALED_MH_OPT
%      return default options
%    OPT = SCALED_MH_OPT(OPT)
%      fill empty options with default values
%
%  The options and defaults are
%      repeat              - the number of MH-steps before
%                            returning a single sample (default 10)
%      sample_latent_scale - the scale for the MH-step (default 0.5)

  if nargin < 1
    opt=[];
  end

  if ~isfield(opt,'repeat')
    opt.repeat=10;
  end
  if ~isfield(opt,'sample_latent_scale')
    opt.sample_latent_scale=0.5;
  end

end


function putlabel(x,y,names,z,znames)

%PUTLABEL plots user-specified labels to the observations in a two- or
%  a three-dimensional figure.
%  If no labels are given, the indices are plotted.
%
% Required input arguments:
%    x : x-coordinates of the data
%    y : y-coordinates of the data
%
% Optional input arguments:
%   names : labels to be added on the plot. They must be listed in a
%           column vector.
%       z : z-coordinates of the data
%  znames : labels to be added on the 3D-plot. They must be listed in a
%           columnvector.
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% I/O: putlabel(x,y,names,z,znames)
%
% Written by S. Verboven on 01/10/2002
% Last update on 18/02/2004

xrange=get(gca,'Xlim');
range=xrange(2)-xrange(1);
if nargin<3
   for i=1:length(x)
      text(x(i)+range/50,y(i),num2str(i));
   end
else
   if nargin<4
      for i=1:length(x)
         text(x(i)+range/50,y(i),names(i,:));
      end
   else
      if nargin<5
         for i=1:length(x)
            text(x(i)+range/50,y(i),z(i),num2str(i));
         end
      else
         for i=1:length(x)
            text(x(i)+range/50,y(i),z(i),znames(i,:));
         end
      end
   end
end


function [] = get_quiver_data

ax = gca;
h = get(ax, 'children');
hq = findobj(h, 'flat', 'type', 'hggroup');

q = get(hq);

c = get(hq, 'children');

get(c(1) )
xb = get(c(1), 'XData');
yb = get(c(1), 'YData');
zb = get(c(1), 'ZData');

ax1 = newax;

plotmd(ax1, [xb; yb; zb] )

hold(ax1, 'on')

get(c(1) )
xh = get(c(2), 'XData');
yh = get(c(2), 'YData');
zh = get(c(2), 'ZData');

plotmd(ax1, [xh; yh; zh] )
axis equal


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	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 Modified: April 2021
% Current Institution: TCNJ
% IB2d Date Created: May 27th, 2015
% Institution Created: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (torsional springs or non-invariant beams)
% 	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: main2d is the function that gets called to run the code. It
%           itself, reads in parameters from the input2d file, and passes
%           them to the IBM_Driver function to run the simulation
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function main2d()

% This is the "main" file, which gets called to initialized the Immersed Boundary
% Simulation. It reads in all the parameters from "input2d", and sends them
% off to the "IBM_Driver.m" function to actual perform the simulation

%
% Path Reference to where Driving code is found %
%
warning('off','all');
addpath('../IBM_Blackbox/','../../IBM_Blackbox/','../../../IBM_Blackbox/','../../../../IBM_Blackbox/');

%
% THROW ERROR TO CORRECT PATH IF DRIVER IS NOT FOUND
%
assert( exist( 'IBM_Driver.m', 'file' ) == 2, 'IBM_Driver.m not found -> Please check path to IBM_Blackbox in main2d.m!' );

%
% READ-IN INPUT PARAMETERS %
%
[Fluid_Params, Grid_Params, Time_Params, Lag_Struct_Params, Output_Params, Lag_Name_Params,Con_Params] = please_Initialize_Simulation();

%
%-%-%-% DO THE IMMERSED BOUNDARY SOLVE!!!!!!!! %-%-%-%
%
[X, Y, U, V, xLags, yLags] = IBM_Driver(Fluid_Params,Grid_Params,Time_Params,Lag_Struct_Params,Output_Params,Lag_Name_Params,Con_Params);

%
% Print simulation has completed.
%
fprintf('\n\n');
fprintf(' |****** IMMERSED BOUNDARY SIMULATION HAS FINISHED! ******|\n\n')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Function to read in input files from "input2d" -> renames all
% quantities appropriately, just as they are in input file
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [params,struct_name] = give_Me_input2d_Parameters()

filename= 'input2d';  %Name of file to read in

fileID = fopen(filename); %Opens file for 'textscan' function
    % Read in the file, use 'CollectOutput' to gather all similar data together
    % and 'CommentStyle' to to end and be able to skip lines in file.
    C1 = textscan(fileID,'%s %s %f','CollectOutput',1,'CommentStyle','%');
    C2 = textscan(fileID,'%s %s %s','CollectOutput',1,'CommentStyle','%');
fclose(fileID);

params = C1{2};

struct_name = C2{1,1};


function I = imreadHistEq( rgbFile, lwirFile )
RGB = imread( rgbFile );
T = rgb2gray( imread( lwirFile ) );
I = cat(3, RGB, T);
% I(:,:,4) = histeq(I(:,:,4));
I(:,:,4) = I(:,:,4) .* (255.0 / 141);
end


function [Z, active_Z] = generate_reference(PopObj, original_active_number, Global, action, mode)

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

global LEARNING_DISABLE_FLAG;
if strcmp(mode, 'strict')
    active_number = 0;
    PopObj = normalization(PopObj);
    active_Z = [];
    while active_number < original_active_number
        instructed_action = action;
        [Z, received_action] = generate_Z(Global, instructed_action, 'commit');
        Z = unique([Z; active_Z], 'rows');
        [~, Allocation] = pair(PopObj, Z, 'sin');
        active_index = unique(Allocation);
        active_Z = Z(active_index, :);
        active_number = size(active_Z, 1);
        fprintf('%d(+%d)\n', active_number, numel(active_index)); % TODO
        if received_action ~= instructed_action
            LEARNING_DISABLE_FLAG = true;
            warning('reference generation malfunctions');
            break;
        end
    end
elseif strcmp(mode, 'normal')
    Z = generate_Z(Global, action, 'commit');
    active_Z = [];
end
end

function varargout = generate_Z(Global, action, commit_str)
global MAX_REFERENCE_NUM_FLAG current_density reference_generation_intial_flag;
persistent incremental_sequence pointer;
if reference_generation_intial_flag == true
    MAX_REFERENCE_NUM_FLAG = false;
    incremental_sequence = density_sequence(Global.M);
    pointer = 1;
    reference_generation_intial_flag = false;
    % TRIM
    TRIM_INDEX = find(incremental_sequence < Global.N);
    incremental_sequence(TRIM_INDEX(1: end)) = [];
end
original_pointer = pointer;
pointer = pointer + action;
if pointer >= numel(incremental_sequence)
    MAX_REFERENCE_NUM_FLAG = true;
end
pointer = min(numel(incremental_sequence), pointer);
pointer = max(1, pointer);
real_action = pointer - original_pointer;

Z = UniformPoint(incremental_sequence(pointer), Global.M);
current_density = size(Z, 1);

if ~strcmp(commit_str, 'commit')
    pointer = original_pointer;
end

varargout{1} = Z;
if nargout >= 2
    varargout{2} = real_action;
end
end

function incremental_sequence = density_sequence(M)
incremental_sequence = [];
R_MAX = 1e6;
step = 1;
R = nchoosek(M + step - 1, step);
while R < R_MAX
    incremental_sequence = [incremental_sequence, R];
    step = step + 1;
    R = nchoosek(M + step - 1, step);
end
end


function RegMeanSquareAffine(handles, metric)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

global planC stateS;
indexS = planC{end};

    [originF, spacingF] = getScanOriginSpacing(planC{indexS.scan}(stateS.imageRegistrationBaseDataset));

    [originM, spacingM] = getScanOriginSpacing(planC{indexS.scan}(stateS.imageRegistrationMovDataset));

    minstep = str2double(get(handles.affine_minstep, 'string'));
    maxstep = str2double(get(handles.affine_maxstep, 'string'));
    iternum = str2double(get(handles.affine_iternum, 'string'));
    tscale = str2double(get(handles.affine_tscale, 'string'));

    output = cell(1, 18);

    FImg = planC{indexS.scan}(stateS.imageRegistrationBaseDataset).scanArray;
    MImg = planC{indexS.scan}(stateS.imageRegistrationMovDataset).scanArray;

    dimF = size(FImg);
    dimM = size(MImg);
    %generate cliped base dataset
    clipBox = uint16(getappdata(stateS.handle.CERRSliceViewer, 'clipBox_baseTrans'));
    if ~isempty(clipBox)
        FImg = FImg(clipBox(2):clipBox(4), clipBox(1):clipBox(3), :);
        originF(1) = originF(1) + spacingF(1)*double(clipBox(1)-1);
        originF(2) = originF(2) + spacingF(2)*double(uint16(dimF(1)-clipBox(4)));
    end
    clipBox = uint16(getappdata(stateS.handle.CERRSliceViewer, 'clipBox_baseSag'));
    if ~isempty(clipBox)
        FImg = FImg(:, :, clipBox(2):clipBox(4));
        originF(3) = originF(3) + spacingF(3)*double(clipBox(2)-1);
    end

    %generate cliped move dataset
    clipBox = uint16(getappdata(stateS.handle.CERRSliceViewer, 'clipBox_movTrans'));
    if ~isempty(clipBox)
        MImg = MImg(clipBox(2):clipBox(4), clipBox(1):clipBox(3), :);
        originM(1) = originM(1) + spacingM(1)*double(clipBox(1)-1);
        originM(2) = originM(2) + spacingM(2)*double(uint16(dimM(1)-clipBox(4)));
    end
    clipBox = uint16(getappdata(stateS.handle.CERRSliceViewer, 'clipBox_movSag'));
    if ~isempty(clipBox)
        MImg = MImg(:, :, clipBox(2):clipBox(4));
        originM(3) = originM(3) + spacingM(3)*double(clipBox(2)-1);
    end

%downsample the input datasets
    dsampled = 0;
    if (get(handles.dsampleCheck,'Value') == get(handles.dsampleCheck,'Max'))

        tic;
        xyScale =  2; zScale = 2;
        spacingF = [spacingF(1)*xyScale spacingF(2)*xyScale spacingF(3)*zScale];
        spacingM = [spacingM(1)*xyScale spacingM(2)*xyScale spacingM(3)*zScale];
        disp('downSampling ...');
%         FImg = imdesample3d(FImg,xyScale,zScale);
%         MImg = imdesample3d(MImg,xyScale,zScale);
%         FImg=GPReduce2D(FImg,1);
%         MImg=GPReduce2(MImg,1);
        FImg = FImg(1:xyScale:end, 1:xyScale:end, 1:zScale:end);
        MImg = MImg(1:xyScale:end, 1:xyScale:end, 1:zScale:end);

        toc;
        dsampled = 1;
    end

    switch metric
        case 'mean squares'
            metricIndex = 0;
        case 'normalized correlation'
            metricIndex = 1;
    end

    tic;

%flip the source datasets on X for itk coordinate system
    fdim = 1;
    FImg = flipdim(FImg, fdim);
    MImg = flipdim(MImg, fdim);

% transform initializing modes 0:MomentON 1:GeometryOn 2:initail transform On
    initMode = get(handles.InitTrans, 'value');

%prepare the initial transform matrix
    transMB = planC{indexS.scan}(stateS.imageRegistrationBaseDataset).transM;
    transMM = planC{indexS.scan}(stateS.imageRegistrationMovDataset).transM;
    if isempty(transMM), transMM = eye(4); end;
    if isempty(transMB), transMB = eye(4); end;
    transM = inv(transMB) * transMM;

%flip the moving dataset to match the initial transM
    Tf = eye(4);
    flipM = 0;
    if (get(handles.flipX, 'value'))
        MImg = flipdim(MImg, 2);
        Tf = [-1 0 0 2*centerM(1); 0 1 0 0; 0 0 1 0; 0 0 0 1];
        flipM = 2;
    end
    if (get(handles.flipY, 'value'))
        MImg = flipdim(MImg, 1);
        Tf = [1 0 0 0; 0 -1 0 2*centerM(2); 0 0 1 0; 0 0 0 1];
        flipM = 1;
    end
    if (get(handles.flipZ, 'value'))
        MImg = flipdim(MImg, 3);
        Tf = [1 0 0 0; 0 1 0 0; 0 0 -1 2*centerM(3); 0 0 0 1];
        flipM = 3;
    end
    transM = transM * inv(Tf);
    rotM = transM(1:3, 1:3); transV = transM(1:3, 4);
    rotM(1,2) = -rotM(1,2); rotM(2,1) = -rotM(2,1); %rotM = rotM';
    transV(3) = -transV(3);%transV = -transV;

%call registration method
    try
        [im, Rotation, Offset] = Affine3D(int16(FImg), originF, spacingF, ...
                                      int16(MImg), originM, spacingM, ...
                                      minstep, maxstep, iternum, tscale, metricIndex, rotM, transV,initMode);
    catch
        [im, Rotation, Offset] = Affine3D_64(int16(FImg), originF, spacingF, ...
                                      int16(MImg), originM, spacingM, ...
                                      minstep, maxstep, iternum, tscale, metricIndex, rotM, transV,initMode);
    end

    toc;

    im = flipdim(im, fdim);
    %if (flipM), im = flipdim(im, flipM); end;

    output{1} = ['Angle (radians)   = ' num2str(Offset(4))];
    output{2} = ['Angle (degrees)   = ' num2str(Offset(5))];
    output{3} = ['Translation X = ' num2str(Offset(1))];
    output{4} = ['Translation Y = ' num2str(Offset(2))];
    output{5} = ['Translation Z = ' num2str(Offset(3))];
    output{6} = ['Iterations = ' num2str(Offset(6))];
    output{7} = ['Metric Value (mean square) = ' num2str(Offset(7))];
    output{8} = ['Rotation Center X = ' num2str(Offset(9))];
    output{9} = ['Rotation Center Y = ' num2str(Offset(10))];
    output{10} = ['Rotation Center Z = ' num2str(Offset(11))];
    output{11} = ['Offset X = ' num2str(Offset(12))];
    output{12} = ['Offset Y = ' num2str(Offset(13))];
    output{13} = ['Offset Z = ' num2str(Offset(14))];
    output{14} = ['Rotation Center X = ' num2str(Offset(15))];
    output{15} = ['Rotation Center Y = ' num2str(Offset(16))];
    output{16} = ['Rotation Center Z = ' num2str(Offset(17))];

    set(handles.OutputList, 'string', output);

%update the transM;
    Tv = [Offset(1) Offset(2) Offset(3)];
    Cv = [Offset(15) Offset(16) Offset(17)];
    Cv(3) = -Cv(3);

    rot = reshape(Rotation, 3,3);
    offset = [Offset(12) Offset(13) Offset(14)]';

    TM = eye(4);
    TM(:,4) = [offset; 1];

    RM = eye(4);
    RM(1:3, 1:3) = rot;

    newTransform = inv(TM*RM);
    newTransform =  transMB * newTransform * Tf;

    scanSetM = stateS.imageRegistrationMovDataset;
    scanSetF = stateS.imageRegistrationBaseDataset;
    planC{indexS.(stateS.imageRegistrationMovDatasetType)}(scanSetM).transM = newTransform;

% save the resampled dataset
    if (get(handles.saveCheck, 'value'))
        if (~dsampled)
            planC{indexS.scan}(end+1) = planC{indexS.scan}(scanSetF);
            planC{indexS.scan}(end).scanArray = im;
        else
            %imReg = resample(scanSetF, scanSetM); % need to up-sample im ????
            planC{indexS.scan}(end+1) = planC{indexS.scan}(scanSetF);
            planC{indexS.scan}(end).scanArray = imReg;
        end
        controlFrame('refresh');
    end

    sliceCallBack('refresh');

end


function [u, info] = solvebvpLinear(L, rhs, Ninit, pref, displayInfo)
%SOLVEBVP  Solve a linear CHEBOP BVP system.
%
% [U, INFO] = SOLVEBVPLINEAR(N, L, RHS, PREF, DISPLAYINFO), where:
%   N is a CHEBOP
%   L is a linear CHEBOP
%   RHS is a CHEBMATRIX
%   PREF is a CHEBOPPREF
%   DISPLAYINFO is a function handle
%
% attempts to solve the linear BVP
%
%       L*U = RHS + boundary conditions specified by L
%
% The output U is a CHEBMATRIX, and INFO is a MATLAB struct with useful
% information. This method should generally not called directly by the user,
% which should rather call the CHEBOP/MLDIVIDE or CHEBOP/SOLVEBVP methods.
%
% Observe that when this method is called, any affine parts of the original
% CHEBOP have been absorbed into RHS.
%
% See also: chebop/solvebvp

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

% Get defaults:
if ( nargin < 3 )
    pref = cheboppref();
end

% All values of the LINOPCONSTRAINT stored in L will be of incorrect sign when
% returned from LINEARIZE(), if we want to use it for a LINOP backslash. This is
% because when problems are solved with LINOP backslash, the solution to the
% problem is the output itself, while in a Newton iteration, we have to add the
% output of the LINOP solution to the current guess. Thus, flip the signs of the
% values of L.constraint:
L.constraint = -L.constraint;

% Solve the linear problem:
del = linsolve(L, rhs, pref);

if ( ~isempty(Ninit) )
    % If Ninit is not empty, N will have been linearized around Ninit. In that
    % case, we need to regard the solution del obtained above as a Newton
    % correction to Ninit.
    u = Ninit + del;
    u = simplify(u);
else
    u = del;
end

% Norm of residual:
normRes = norm(L*u - rhs, 'fro');

if ( nargin > 3 )
    % Print information after linear problem has been solved:
    displayInfo('linear', u, normRes, pref)
end

if ( nargout > 1 )
    % Return the norm of the residual in the INFO struct:
    info.error = normRes;
end

end


function [cnmfAnalysisOutput] = computeCnmfSignalExtraction_v2(inputMovie,numExpectedComponents,varargin)
	% Brapper function for CNMF, update for most recent versions.
	% Building off of demo_script.m in CNMF github repo
	% Most recent commit tested on: https://github.com/epnev/ca_source_extraction/commit/187bbdbe66bca466b83b81861b5601891a95b8d1
	% https://github.com/epnev/ca_source_extraction/blob/master/demo_script_class.m
	% Biafra Ahanonu
	% started: 2016.01.20
	% inputs
		% inputMovie - a string or a cell array of strings pointing to the movies to be analyzed (recommended). Else, [x y t] matrix where t = frames.
		% numExpectedComponents - number of expected components
	% outputs
		% cnmfAnalysisOutput - structure containing extractedImages and extractedSignals along with input parameters to the algorithm
	% READ BEFORE RUNNING
		% Get CVX from http://cvxr.com/cvx/doc/install.html
		% Run the below commands in Matlab after unzipping
		% cvx_setup
		% cvx_save_prefs (permanently stores settings)

	[cnmfAnalysisOutput] = ciapkg.signal_extraction.computeCnmfSignalExtraction_v2(inputMovie,numExpectedComponents,'passArgs', varargin);
end


function h = montageKPM(arg)
% montageKPM is like the built-in montage, but assumes input is MxNxK or filenames
%
% Converts patches (y,x,i) into patches(y,x,1,i)
% Also, adds a black border aroudn them

if iscell(arg)
  h= montageFilenames(arg);
else
  nr = size(arg,1); nc = size(arg,2); Npatches = size(arg,3);
  patchesColor = reshape(arg, [nr nc 1 Npatches]);
  patchesColor = patchesColor ./ max(patchesColor(:));

  if 1
    %put a black border around them for display purposes
    border = 5;
    bgColor = ones(1,1,class(patchesColor));
    patchesColorBig = bgColor*ones(nr+2*border, nc+2*border, 1, Npatches, class(patchesColor));
    %patchesColorBig = zeros(nr+2*border, nc+2*border, 1, Npatches, class(patchesColor));
    patchesColorBig(border+1:end-border, border+1:end-border, :, :) = patchesColor;
  else
    patchesColorBig = patchesColor;
  end
  montage(patchesColorBig)

end

%%%%%%%%%%%%%

function h = montageFilenames(filenames)

%[nRows, nCols, nBands, nFrames] = size(a);

% Estimate nMontageColumns and nMontageRows given the desired ratio of
% Columns to Rows to be one (square montage).
aspectRatio = 1;
nMontageCols = sqrt(aspectRatio * nRows * nFrames / nCols);

% Make sure montage rows and columns are integers. The order in the adjustment
% matters because the montage image is created horizontally across columns.
nMontageCols = ceil(nMontageCols);
nMontageRows = ceil(nFrames / nMontageCols);

% Create the montage image.
b = a(1,1); % to inherit type
b(1,1) = 0; % from a
b = repmat(b, [nMontageRows*nRows, nMontageCols*nCols, nBands, 1]);

rows = 1 : nRows;
cols = 1 : nCols;

for i = 0:nMontageRows-1
  for j = 0:nMontageCols-1,
    k = j + i * nMontageCols + 1;
    if k <= nFrames
      b(rows + i * nRows, cols + j * nCols, :) = a(:,:,:,k);
    else
      break;
    end
  end
end

if isempty(cm)
  hh = imshow(b);
else
  hh = imshow(b,cm);
end

if nargout > 0
    h = hh;
end

%--------------------------------------------------------------
%Parse Inputs Function

function [I,map] = parse_inputs(varargin)

% initialize variables
map = [];

iptchecknargin(1,2,nargin,mfilename);
iptcheckinput(varargin{1},{'uint8' 'double' 'uint16' 'logical' 'single' ...
                    'int16'},{},mfilename, 'I, BW, or RGB',1);
I = varargin{1};

if nargin==2
  if isa(I,'int16')
    eid = sprintf('Images:%s:invalidIndexedImage',mfilename);
    msg1 = 'An indexed image can be uint8, uint16, double, single, or ';
    msg2 = 'logical.';
    error(eid,'%s %s',msg1, msg2);
  end
  map = varargin{2};
  iptcheckinput(map,{'double'},{},mfilename,'MAP',1);
  if ((size(map,1) == 1) && (prod(map) == numel(I)))
    % MONTAGE(D,[M N P]) OBSOLETE
    eid = sprintf('Images:%s:obsoleteSyntax',mfilename);
    msg1 = 'MONTAGE(D,[M N P]) is an obsolete syntax.';
    msg2 = 'Use multidimensional arrays to represent multiframe images.';
    error(eid,'%s\n%s',msg1,msg2);
  end
end


%VGG_SEGMENT_GB  Graph-based image segmentation
%
%   S = vgg_segment_gb(A, sigma, K, min_sz[, compress])
%
% Segmentation of an image using the graph-based method described in:
%   "Efficient Graph-Based Image Segmentation.", Pedro F. Felzenszwalb and
%   Daniel P. Huttenlocher. International Journal of Computer Vision,
%   Volume 59, Number 2, September 2004.
% Code downloaded from:
%   http://people.cs.uchicago.edu/~pff/segment/
%
%IN:
%   A - HxWx3 uint8 image for segmentation.
%   sigma - scalar parameter on smoothing kernel to use prior to
%           segmentation.
%   k - scalar parameter on prefered segment size.
%   min_sz - scalar indicating the minimum number of pixels per segment.
%   compress - scalar boolean indicating whether the user wants the segment
%              indices compressed to the range [1 num_segments].
%              Default: 0.
%
%OUT:
%   S - HxW uint32 segmentation matrix, each value of which gives the index
%       of the region said pixel belongs to.

% $Id: vgg_segment_gb.m,v 1.1 2007/12/10 10:59:30 ojw Exp $

function varargout = vgg_segment_gb(varargin)
funcName = mfilename;
sd = 'seg_gb/';
sourceList = {['-I' sd], [funcName '.cxx']};
vgg_mexcompile_script; % Compilation happens in this script
return


function [pcaicaAnalysisOutput] = runPcaIca(inputMovie,nPCs,nICs,varargin)
	% Wrapper to run PCA-ICA (Mukamel, 2009) cell extraction using two existing versions on CIAPKG.
	% Biafra Ahanonu
	% started: 2019.10.31 [10:01:18]
	% inputs
		% inputMovie - Matrix ([x y frames]) or char string pointing to movie path.
		% nPCs - Int: number of principal components to request from PCA.
		% nICs - Int: number of independent components to request from ICA. Should be less than nPCs.
	% outputs
		% pcaicaAnalysisOutput - structure containing output filters and traces for the requested number of nICs.
		% pcaicaAnalysisOutput.IcaFilters - Matrix with dimensions [x y nICs].
		% pcaicaAnalysisOutput.IcaTraces - Matrix with dimensions [nICs frames].

	% changelog
		% 2020.10.14 [13:52:47] - Updated to complete conversion from class to independent function.
		% 2020.10.17 [19:05:12] - Additional updates to deal with matrix inputs and give additional information for each PCA-ICA version output.
		% 2021.08.08 [19:30:20] - Updated to handle CIAtah v4.0 switch to all functions inside ciapkg package.
	% TODO
		%

	import ciapkg.api.* % Import CIAtah package API

	%========================
	% Int:
		% 2 = Hakan/Tony version. (Preferred)
		% 1 = SpikeE version (Maggie Carr, Eran Mukamel, Jerome Lecoq, and Lacey Kitch, Biafra Ahanonu)
	options.version = 2;
	% Termination tolerance, e.g. 1e-5.
	options.TermTolICs = 10^(-5);
	% Str: hierarchy name in hdf5 where movie data is located
	options.inputDatasetName = '/1';
	% Float: parameter (between 0 and 1) specifying weight of temporal information in spatio-temporal ICA
	options.mu = 0.1;
	% Float: termination tolerance, e.g. 1e-5.
	options.term_tol = 5e-6;
	% Int: max iterations of FastICA, e.g. 750.
	options.max_iter = 1e3;
	% Str: output units, options: string of fluorescence ('fl'), standard deviation ('std'), '2norm', or variance ('var').
	options.outputUnits = 'fl';
	% Str: character string with regular expression if inputMovie is a folder path. In general ignore and give direct path to movie.
	options.fileFilterRegexp = 'concat';
	% Int vector: list of specific frames to load.
	options.frameList = [];
	% DEPRECIATED
		options.selectPCs = 0;
		options.selectICs = 0;
		options.displayIcImgs = 0;
	% get options
	options = getOptions(options,varargin);
	% display(options)
	% unpack options into current workspace
	% fn=fieldnames(options);
	% for i=1:length(fn)
	% 	eval([fn{i} '=options.' fn{i} ';']);
	% end
	%========================

	try
		if options.version==1
			disp('running PCA-ICA, old version...')
			startTime = tic;
			[PcaFilters, PcaTraces] = ciapkg.signal_extraction.pca_ica.runPCA(inputMovie, '', nPCs, options.fileFilterRegexp,'inputDatasetName',options.inputDatasetName,'frameList',options.frameList);
			if isempty(PcaFilters)
				disp('PCs are empty, skipping...')
				return;
			end
			[IcaFilters, IcaTraces, IcaInfo] = ciapkg.signal_extraction.pca_ica.runICA(PcaFilters, PcaTraces, '', nICs, '');
			pcaicaAnalysisOutput.IcaInfo = IcaInfo;
			traceSaveDimOrder = '[nComponents frames]';
			% reorder if needed
			options.IcaSaveDimOrder = 'xyz';
			if strcmp(options.IcaSaveDimOrder,'xyz')
				IcaFilters = permute(IcaFilters,[2 3 1]);
				imageSaveDimOrder = 'xyz';
			else
				imageSaveDimOrder = 'zxy';
			end
		elseif options.version==2
			disp('running PCA-ICA, new version...')
			startTime = tic;
			[PcaOutputSpatial, PcaOutputTemporal, PcaOutputSingularValues, PcaInfo] = ciapkg.signal_extraction.pca_ica_2.run_pca(inputMovie, nPCs, 'movie_dataset_name',options.inputDatasetName,'frameList',options.frameList);

			if isempty(PcaOutputTemporal)
				disp('PCs are empty, skipping...')
				return;
			end

			disp('+++')
			if ischar(inputMovie)==1
				movieDims = loadMovieList(inputMovie,'convertToDouble',0,'frameList',[],'inputDatasetName',options.inputDatasetName,'treatMoviesAsContinuous',1,'getMovieDims',1,'frameList',options.frameList);
			else
				movieDimsTmp = size(inputMovie);
				movieDims.x = movieDimsTmp(1);
				movieDims.y = movieDimsTmp(2);
				movieDims.z = movieDimsTmp(3);
			end

			% output_units = 'fl';
			% output_units = 'std';
			% options.PCAICA.term_tol = 5e-6;
			% options.PCAICA.max_iter = 1e3;
			[IcaFilters, IcaTraces, IcaInfo] = ciapkg.signal_extraction.pca_ica_2.run_ica(...
				PcaOutputSpatial,...
				PcaOutputTemporal,...
				PcaOutputSingularValues,...
				movieDims.x, movieDims.y,...
				nICs,...
				'output_units',options.outputUnits,...
				'mu',options.mu,...
				'term_tol',options.term_tol,...
				'max_iter',options.max_iter);
			IcaTraces = permute(IcaTraces,[2 1]);
			traceSaveDimOrder = '[nComponents frames]';
			% reorder if needed
			options.IcaSaveDimOrder = 'xyz';
			if strcmp(options.IcaSaveDimOrder,'xyz')
				imageSaveDimOrder = 'xyz';
			else
				IcaFilters = permute(IcaFilters,[3 1 2]);
				imageSaveDimOrder = 'zxy';
			end
			pcaicaAnalysisOutput.IcaInfo = IcaInfo;
			pcaicaAnalysisOutput.PcaInfo = PcaInfo;
		else
			disp('Incorrect version requested.')
			pcaicaAnalysisOutput.status = 0;
			return;
		end

		pcaicaAnalysisOutput.PcaIcaVersion = options.version;
		pcaicaAnalysisOutput.IcaFilters = IcaFilters;
		pcaicaAnalysisOutput.IcaTraces = IcaTraces;
		pcaicaAnalysisOutput.imageSaveDimOrder = imageSaveDimOrder;
		pcaicaAnalysisOutput.traceSaveDimOrder = traceSaveDimOrder;
		pcaicaAnalysisOutput.nPCs = nPCs;
		pcaicaAnalysisOutput.nICs = nICs;
		pcaicaAnalysisOutput.time.startTime = startTime;
		pcaicaAnalysisOutput.time.endTime = toc(startTime);
		pcaicaAnalysisOutput.time.dateTime = datestr(now,'yyyymmdd_HHMM','local');
		if ischar(inputMovie)
			pcaicaAnalysisOutput.movieList = inputMovie;
		else
			pcaicaAnalysisOutput.movieList = 'matrixInput';
		end
		pcaicaAnalysisOutput.status = 1;

	catch err
		disp(repmat('@',1,7))
		disp(getReport(err,'extended','hyperlinks','on'));
		disp(repmat('@',1,7))
	end
end


function [qrs_pos,sign,en_thres] = qrs_detect2(ecg,varargin)
% QRS detector based on the P&T method. This is an offline implementation
% of the detector.
%
% inputs
%   ecg:            one ecg channel on which to run the detector (required)
%                   in [mV]
%   varargin
%       THRES:      energy threshold of the detector (default: 0.6)
%                   [arbitrary units]
%       REF_PERIOD: refractory period in sec between two R-peaks (default: 0.250)
%                   in [ms]
%       fs:         sampling frequency (default: 1KHz) [Hz]
%       fid_vec:    if some subsegments should not be used for finding the
%                   optimal threshold of the P&Tthen input the indices of
%                   the corresponding points here
%       SIGN_FORCE: force sign of peaks (positive value/negative value).
%                   Particularly usefull if we do window by window detection and want to
%                   unsure the sign of the peaks to be the same accross
%                   windows (which is necessary to build an FECG template)
%       debug:      1: plot to bebug, 0: do not plot
%
% outputs
%   qrs_pos:        indexes of detected peaks (in samples)
%   sign:           sign of the peaks (a pos or neg number)
%   en_thres:       energy threshold used
%
%
%
% Physionet Challenge 2014, version 1.0
% Released under the GNU General Public License
%
% Copyright (C) 2014  Joachim Behar
% Oxford university, Intelligent Patient Monitoring Group
% joachim.behar@gmail.com
%
% Last updates:
%
% 13-09-2014
% Joachim Behar
% - bug on refrac period fixed
% - code a bit more tidy
% - condition added on flatline detection for overall segment (if flatline
% then returns empty matrices rather than some random stuff)
%
% 15-09-2014
% Julien Oster
% Sombrero hat instead of band-pass filter as the prefiltering step.
%
%
% When using this code, please reference the following papers:
%
% [1] Behar Joachim, Jonhson Alistair, Clifford Gari D., Oster Julien A
% Comparison of Single Channel Foetal ECG Extraction Methods.
% Annals of Biomedical Engineering. 42(6), 1340-53. 2014
%
% [2] Johnson Alistair E W, Behar Joachim, Oster Julien and Clifford Gari
% D. R-Peak Estimation using Multimodal Lead Switching.
% Accepted for Computing in Cardiology conference 2014.
%
%
% 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.

% == managing inputs
REF_PERIOD = 0.250;
THRES = 0.6;
fs = 1000;
fid_vec = [];
SIGN_FORCE = [];
debug = 0;

switch nargin
    case 2
        REF_PERIOD=varargin{1};
    case 3
        REF_PERIOD=varargin{1};
        THRES=varargin{2};
    case 4
        REF_PERIOD=varargin{1};
        THRES=varargin{2};
        fs=varargin{3};
    case 5
        REF_PERIOD=varargin{1};
        THRES=varargin{2};
        fs=varargin{3};
        fid_vec=varargin{4};
    case 6
        REF_PERIOD=varargin{1};
        THRES=varargin{2};
        fs=varargin{3};
        fid_vec=varargin{4};
        SIGN_FORCE=varargin{5};
    case 7
        REF_PERIOD=varargin{1};
        THRES=varargin{2};
        fs=varargin{3};
        fid_vec=varargin{4};
        SIGN_FORCE=varargin{5};
        debug=varargin{6};
    otherwise
        error('qrs_detect: wrong number of input arguments \n');
end

[a b] = size(ecg);
if(a>b); NB_SAMP=a; elseif(b>a); NB_SAMP=b; ecg=ecg'; end;
tm = 1/fs:1/fs:ceil(NB_SAMP/fs);

% == constants
MED_SMOOTH_NB_COEFF = round(fs/100);
INT_NB_COEFF = round(7*fs/256); % length is 7 for fs=256Hz
SEARCH_BACK = 1; % perform search back (FIXME: should be in function param)
MAX_FORCE = []; % if you want to force the energy threshold value (FIXME: should be in function param)
MIN_AMP = 0.1; % if the median of the filtered ECG is inferior to MINAMP then it is likely to be a flatline
               % note the importance of the units here for the ECG (mV)
NB_SAMP = length(ecg); % number of input samples

try
    % == Bandpass filtering for ECG signal
    % this sombrero hat has shown to give slightly better results than a
    % standard band-pass filter. Plot the frequency response to convince
    % yourself of what it does
    b1 = [-7.757327341237223e-05  -2.357742589814283e-04 -6.689305101192819e-04 -0.001770119249103 ...
         -0.004364327211358 -0.010013251577232 -0.021344241245400 -0.042182820580118 -0.077080889653194...
         -0.129740392318591 -0.200064921294891 -0.280328573340852 -0.352139052257134 -0.386867664739069 ...
         -0.351974030208595 -0.223363323458050 0 0.286427448595213 0.574058766243311 ...
         0.788100265785590 0.867325070584078 0.788100265785590 0.574058766243311 0.286427448595213 0 ...
         -0.223363323458050 -0.351974030208595 -0.386867664739069 -0.352139052257134...
         -0.280328573340852 -0.200064921294891 -0.129740392318591 -0.077080889653194 -0.042182820580118 ...
         -0.021344241245400 -0.010013251577232 -0.004364327211358 -0.001770119249103 -6.689305101192819e-04...
         -2.357742589814283e-04 -7.757327341237223e-05];

    b1 = resample(b1,fs,250);
    bpfecg = filtfilt(b1,1,ecg)';

    if (length(find(abs(bpfecg)>MIN_AMP))/NB_SAMP)>0.20
        % if 20% of the samples have an absolute amplitude which is higher
        % than MIN_AMP then we are good to go.

        % == P&T operations
        dffecg = diff(bpfecg');  % (4) differentiate (one datum shorter)
        sqrecg = dffecg.*dffecg; % (5) square ecg
        intecg = filter(ones(1,INT_NB_COEFF),1,sqrecg); % (6) integrate
        mdfint = medfilt1(intecg,MED_SMOOTH_NB_COEFF);  % (7) smooth
        delay  = ceil(INT_NB_COEFF/2);
        mdfint = circshift(mdfint,-delay); % remove filter delay for scanning back through ECG

        % look for some measure of signal quality with signal fid_vec? (FIXME)
        if isempty(fid_vec); mdfintFidel = mdfint; else mdfintFidel(fid_vec>2) = 0; end;

        % == P&T threshold
        if NB_SAMP/fs>90; xs=sort(mdfintFidel(fs:fs*90)); else xs = sort(mdfintFidel(fs:end)); end;

        if isempty(MAX_FORCE)
           if NB_SAMP/fs>10
                ind_xs = ceil(98/100*length(xs));
                en_thres = xs(ind_xs); % if more than ten seconds of ecg then 98% CI
            else
                ind_xs = ceil(99/100*length(xs));
                en_thres = xs(ind_xs); % else 99% CI
            end
        else
           en_thres = MAX_FORCE;
        end

        % build an array of segments to look into
        poss_reg = mdfint>(THRES*en_thres);

        % in case empty because force threshold and crap in the signal
        if isempty(poss_reg); poss_reg(10) = 1; end;

        % == P&T QRS detection & search back
        if SEARCH_BACK
            indAboveThreshold = find(poss_reg); % ind of samples above threshold
            RRv = diff(tm(indAboveThreshold));  % compute RRv
            medRRv = median(RRv(RRv>0.01));
            indMissedBeat = find(RRv>1.5*medRRv); % missed a peak?
            % find interval onto which a beat might have been missed
            indStart = indAboveThreshold(indMissedBeat);
            indEnd = indAboveThreshold(indMissedBeat+1);

            for i=1:length(indStart)
                % look for a peak on this interval by lowering the energy threshold
                poss_reg(indStart(i):indEnd(i)) = mdfint(indStart(i):indEnd(i))>(0.5*THRES*en_thres);
            end
        end

        % find indices into boudaries of each segment
        left  = find(diff([0 poss_reg'])==1);  % remember to zero pad at start
        right = find(diff([poss_reg' 0])==-1); % remember to zero pad at end

        % looking for max/min?
        if SIGN_FORCE
            sign = SIGN_FORCE;
        else
            nb_s = length(left<30*fs);
            loc  = zeros(1,nb_s);
            for j=1:nb_s
                [~,loc(j)] = max(abs(bpfecg(left(j):right(j))));
                loc(j) = loc(j)-1+left(j);
            end
            sign = mean(ecg(loc));  % FIXME: change to median?
        end

        % loop through all possibilities
        compt=1;
        NB_PEAKS = length(left);
        maxval = zeros(1,NB_PEAKS);
        maxloc = zeros(1,NB_PEAKS);
%%%%%%%% add two lines
        max_inv_val = zeros(1,NB_PEAKS);
        max_inv_loc = zeros(1,NB_PEAKS);
        for i=1:NB_PEAKS
            if sign>0
                % if sign is positive then look for positive peaks
                [maxval(compt) maxloc(compt)] = max(ecg(left(i):right(i)));
%%%%%%%%%%%%%%%% add 1 line
                [max_inv_val(compt) max_inv_loc(compt)] = min(ecg(left(i):right(i)));
            else
                % if sign is negative then look for negative peaks
                [maxval(compt) maxloc(compt)] = min(ecg(left(i):right(i)));
%%%%%%%%%%%%%%%% add 1 line
                [max_inv_val(compt) max_inv_loc(compt)] = max(ecg(left(i):right(i)));
            end
            maxloc(compt) = maxloc(compt)-1+left(i); % add offset of present location
%%%%%%%%%%%% add 1 line
            max_inv_loc(compt) = max_inv_loc(compt)-1+left(i); % add offset of present location

            % refractory period - has proved to improve results
            if compt>1
%%%%%%%%%%%%%%%%                if maxloc(compt)-maxloc(compt-1)<fs*REF_PERIOD && abs(maxval(compt))<abs(maxval(compt-1))
                if maxloc(compt)-maxloc(compt-1)<fs*REF_PERIOD && max(abs(maxval(compt)),abs(max_inv_val(compt)))<max(abs(maxval(compt-1)),abs(max_inv_val(compt-1)))
%%%%%%%%%%%%%%%% add 4 lines and modify the next one
                    if abs(maxval(compt-1))<abs(max_inv_val(compt-1))
                        maxloc(compt-1)=max_inv_loc(compt-1);
                        maxval(compt-1)=max_inv_val(compt-1);
                    end
                    maxloc(compt)=[]; maxval(compt)=[];max_inv_loc(compt)=[]; max_inv_val(compt)=[];
%%%%%%%%%%%%%%%%                elseif maxloc(compt)-maxloc(compt-1)<fs*REF_PERIOD && abs(maxval(compt))>=abs(maxval(compt-1))
                elseif maxloc(compt)-maxloc(compt-1)<fs*REF_PERIOD && max(abs(maxval(compt)),abs(max_inv_val(compt)))>=max(abs(maxval(compt-1)),abs(max_inv_val(compt-1)))
%%%%%%%%%%%%%%%% add 4 lines and modify the next one
                    if abs(maxval(compt))<abs(max_inv_val(compt))
                        maxloc(compt)=max_inv_loc(compt);
                        maxval(compt)=max_inv_val(compt);
                    end
                    maxloc(compt-1)=[]; maxval(compt-1)=[];max_inv_loc(compt-1)=[]; max_inv_val(compt-1)=[];
                else
                    compt=compt+1;
                end
            else
                % if first peak then increment
                compt=compt+1;
            end
        end

        qrs_pos = maxloc; % datapoints QRS positions
        R_t = tm(maxloc); % timestamps QRS positions
        R_amp = maxval; % amplitude at QRS positions
        hrv = 60./diff(R_t); % heart rate
    else
        % this is a flat line
        qrs_pos = [];
        R_t = [];
        R_amp = [];
        hrv = [];
        sign = [];
        en_thres = [];
    end
catch ME
    rethrow(ME);
    for enb=1:length(ME.stack); disp(ME.stack(enb)); end;
    qrs_pos = [1 10 20]; sign = 1; en_thres = 0.5;
end

% == plots
if debug
    figure;
    FONTSIZE = 20;
    ax(1) = subplot(4,1,1); plot(tm,ecg); hold on;plot(tm,bpfecg,'r')
        title('raw ECG (blue) and zero-pahse FIR filtered ECG (red)'); ylabel('ECG');
        xlim([0 tm(end)]);  hold off;
    ax(2) = subplot(4,1,2); plot(tm(1:length(mdfint)),mdfint);hold on;
        plot(tm,max(mdfint)*bpfecg/(2*max(bpfecg)),'r',tm(left),mdfint(left),'og',tm(right),mdfint(right),'om');
        title('Integrated ecg with scan boundaries over scaled ECG');
        ylabel('Int ECG'); xlim([0 tm(end)]); hold off;
    ax(3) = subplot(4,1,3); plot(tm,bpfecg,'r');hold on;
        plot(R_t,R_amp,'+k');
        title('ECG with R-peaks (black) and S-points (green) over ECG')
        ylabel('ECG+R+S'); xlim([0 tm(end)]); hold off;
    ax(4) = subplot(4,1,4); plot(R_t(1:length(hrv)),hrv,'r+')
        hold on, title('HR')
        ylabel('RR (s)'); xlim([0 tm(end)]);

    %linkaxes(ax,'x');
    set(gca,'FontSize',FONTSIZE);
    allAxesInFigure = findall(gcf,'type','axes');
    set(allAxesInFigure,'fontSize',FONTSIZE);
end

% NOTES
%   Finding the P&T energy threshold: in order to avoid crash due to local
%   huge bumps, threshold is choosen at 98-99% of amplitude distribution.
%   first sec removed for choosing the thres because of filter init lag.
%
%   Search back: look for missed peaks by lowering the threshold in area where the
%   RR interval variability (RRv) is higher than 1.5*medianRRv
%
%   Sign of the QRS (signForce): look for the mean sign of the R-peak over the
%   first 30sec when looking for max of abs value. Then look for the
%   R-peaks over the whole record that have this given sign. This allows to
%   not alternate between positive and negative detections which might
%   happen in some occasion depending on the ECG morphology. It is also
%   better than forcing to look for a max or min systematically.


function view = addROIrectMontage(view,sgn)
%
% view = addROIrectMontage(view,[sgn])
%
% If sgn~=0, adds user-specified rectangle to selected ROI in
% current slice. If sgn==0, removes the rectangle from the ROI.
%
% djh, 7/98
% ras, 9/04 -- updated for montage view
% If you change this function make parallel changes in:
%   all addROI*.m functions

% error if no current ROI
if view.selectedROI == 0
  myErrorDlg('No current ROI');
  return
end

if ~exist('sgn','var')
  sgn = 1;
end

% Get current ROI coords
curCoords = getCurROIcoords(view);

% Save prevSelpts for undo
view.prevCoords = curCoords;

% Get region from user.
% (rgn is short for region)
rgn = round(ginput(2));

% Make sure 2nd value is larger than the 1st.
for i=1:2
  if rgn(2,i) < rgn(1,i)
    rgn(:,i)=flipud(rgn(:,i));
  end
end

% Note: ginput hands them back in x, y order (1st col is x and
% 2nd col is y).  But we use them in the opposite order (y then
% x), so flip 'em.
rgn = fliplr(rgn);

% Get rgnSize
rgnSize = rgn(2,:) - rgn(1,:) + 1;

% Compute new coordinates
indices = 1:prod(rgnSize);
tmpCoords = indices2Coords(indices,rgnSize);
montageCoords = zeros(2,length(indices));
montageCoords(1,:) = tmpCoords(1,:) + rgn(1,1);
montageCoords(2,:) = tmpCoords(2,:) + rgn(1,2);
newCoords = montage2Coords(view,montageCoords);

% Do an (inverse) rotation if necessary
if (strcmp(view.viewType,'Flat'))
    % The '1' here signifies inverse rotation
	% (from screen to zero (internal) representation)
    newCoords = rotateCoords(view,newCoords,1);
end

% Convert coords to canonical frame of reference
newCoords = curOri2CanOri(view,newCoords);

% Merge/remove coordinates
if sgn
  coords = mergeCoords(curCoords,newCoords);
else
  coords = removeCoords(newCoords,curCoords);
end
view.ROIs(view.selectedROI).coords = coords;
view.ROIs(view.selectedROI).modified = datestr(now);

return


function plotExcitationMagnitude(varargin)
% Plots the excitation force magnitude for each hydro structure's bodies in
% the heave, surge and pitch degrees of freedom.
%
% Usage:
% ``plotExcitationMagnitude(hydro, hydro2, hydro3, ...)``
%
% Parameters
% ----------
%     varargin : struct(s)
%         The hydroData structure(s) created by the other BEMIO functions.
%         One or more may be input.
%

if isempty(varargin)
    error(['plotExcitationMagnitude: No arguments passed. Include one or more hydro ' ...
        'structures when calling: plotExcitationMagnitude(hydro1, hydro2, ...)']);
end

B=1;  % Wave heading index
figHandle = figure('Position',[950,500,975,521]);
titleString = ['Normalized Excitation Force Magnitude: ',...
    '$$\bar{X_i}(\omega,\theta) = {\frac{X_i(\omega,\theta)}{{\rho}g}}$$'];
subtitleString = {'Surge','Heave','Pitch'};
xString = {'$$\omega (rad/s)$$','$$\omega (rad/s)$$','$$\omega (rad/s)$$'};
yString = {['$$\bar{X_1}(\omega,\theta$$',' = ',num2str(varargin{1}.theta(B)),'$$^{\circ}$$)'],...
    ['$$\bar{X_3}(\omega,\theta$$',' = ',num2str(varargin{1}.theta(B)),'$$^{\circ}$$)'],...
    ['$$\bar{X_5}(\omega,\theta$$',' = ',num2str(varargin{1}.theta(B)),'$$^{\circ}$$)']};

notes = {''};

numHydro = length(varargin);
for ii = 1:numHydro
    numBod = varargin{ii}.Nb;
    tmp1 = strcat('X',num2str(ii));
    X.(tmp1) = varargin{ii}.w;
    tmp2 = strcat('Y',num2str(ii));
    a = 0;
    for i = 1:numBod
        m = varargin{ii}.dof(i);
        Y.(tmp2)(1,i,:) = squeeze(varargin{ii}.ex_ma(a+1,B,:));
        Y.(tmp2)(2,i,:) = squeeze(varargin{ii}.ex_ma(a+3,B,:));
        Y.(tmp2)(3,i,:) = squeeze(varargin{ii}.ex_ma(a+5,B,:));
        legendStrings{i,ii} = [varargin{ii}.body{i}];
        a = a + m;
    end
end

formatPlot(figHandle,titleString,subtitleString,xString,yString,X,Y,legendStrings,notes);
saveas(figHandle,'Excitation_Magnitude.png');

end


function F = vertcat(varargin)
%VERTCAT   Vertical concatenation of CHEBFUN3 objects.
%   VERTCAT(G, H, W) is the vertical concatenation of CHEBFUN3 objects F, G
%   and H. This function returns a CHEBFUN3V object.
%
%   [G; H; W] is equivalent to VERTCAT(G, H, W).
%
%   VERTCAT(G) returns the CHEBFUN3 object G.
%
% See also CHEBFUN3V.

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

if ( nargin == 1 )
    % VERTCAT of one argument just returns the same thing back to the user:
    F = varargin{1};
elseif ( nargin > 1 && nargin <= 3)
    if ( isa(varargin{2}, 'chebfun3v') )
        f = varargin{1};
        F = varargin{2};
        if ( F.nComponents > 2 )
            error('CHEBFUN:CHEBFUN3:vertcat:tooManyComponents', ...
                'Only CHEBFUN3V objects with 2 or 3 components are valid.');
        else
            Fc = F.components;
            g = Fc{1};
            h = Fc{2};
            F = chebfun3v({f, g, h});
        end

    elseif ( isa(varargin{2}, 'chebfun3' ) )
        % Call the CHEBFUN3V constructor:
        F = chebfun3v(varargin{:});
    else
        error('CHEBFUN:CHEBFUN3:vertcat:badInputs', ...
            'Inputs must be either CHEBFUN3 or CHEBFUN3V objects.');
    end

else
    error('CHEBFUN:CHEBFUN3:vertcat:tooManyInputs', ...
        'Cannot vertically concatenate more than three objects.');
end

end


function class_info=gen_class_info_cityscapes()

class_info=[];

class_info.class_names={'road', 'sidewalk', 'building', 'wall', 'fence', 'pole', 'trafficlight',...
     'trafficsign', 'vegetation', 'terrain', 'sky', 'person', 'rider', 'car', ...
     'truck', 'bus', 'train', 'motorcycle', 'bicycle', 'void'};

class_label_values=uint8([0:18 255]);

class_info.class_label_values=class_label_values;
class_info.background_label_value=uint8(0);
class_info.void_label_values=uint8(255);

cmap=load('cityscape_cmap.mat');
cmap=uint8(cmap.cityscape_cmap);
class_info.mask_cmap=im2double(cmap);

class_info=process_class_info(class_info);

end


% @authors:     Ahmad Humayun
% @contact:     ahumayun@cc.gatech.edu
% @affiliation: Georgia Institute of Technology
% @date:        Fall 2013 - Summer 2014

function [nonlambda_s, nonlambda_t, lambda_s, lambda_t, lambda_range, ...
          pairwise_edges, DISC_FACTOR, BIG_VALUE ] = ...
            preprocess_graphs(gp_obj, nonlambda_s, nonlambda_t, ...
                              lambda_s, lambda_t)
    l = -1;
    u = gp_obj.graph_sol_upper_bp;

    DISC_FACTOR = 1000; % original is 1000

    % i don't think it can be bigger (beyond certain value goes crazy)
    BIG_VALUE = 21475000000;

    % get the range of lambda values to use
    pmc_num_lambdas = gp_obj.segm_params.pmc_num_lambdas;

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

    lambda_range = ...
        GraphProcessor.generate_param_lambdas(l, u, DISC_FACTOR, ...
                                              pmc_num_lambdas, ...
                                              overrride_lambdas);

    % capping lambda weight and offsets (parametric capacities) to BIG_VALUE
    nonlambda_s = nonlambda_s * DISC_FACTOR;
    nonlambda_t = nonlambda_t * DISC_FACTOR;
    nonlambda_s(nonlambda_s > BIG_VALUE) = BIG_VALUE;
    nonlambda_t(nonlambda_t > BIG_VALUE) = BIG_VALUE;

    % capping non-lambda weights (non-parametric capacities) to BIG_VALUE
    lambda_s = lambda_s * DISC_FACTOR;
    lambda_t = lambda_t * DISC_FACTOR;
    lambda_s(lambda_s > BIG_VALUE) = BIG_VALUE;
    lambda_t(lambda_t > BIG_VALUE) = BIG_VALUE;

    pairwise_vals = gp_obj.pairwise_vals;
    pairwise_vals = pairwise_vals * DISC_FACTOR;
    pairwise_vals(pairwise_vals > BIG_VALUE) = BIG_VALUE;

    % a < from-node > < to-node > < capacity >
    pairwise_edges = [gp_obj.pairwise_edges pairwise_vals];
end


function obj = t_port_inj_power_acp(quiet)
%T_PORT_INJ_POWER_ACP  Tests of port_inj_power() derivatives wrt polar V.

%   MATPOWER
%   Copyright (c) 2019-2022, 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.

if nargin < 1
    quiet = 0;
end

tc = struct( ...        %% test cases
        'name', {'1', '2'}, ...
        'ec', { @mp.nme_gizmo_acp, ...
                {   {@mp.nme_gen_acp_nln, 'mp.nme_gen'}, ...
                    {@mp.nme_load_acp_nln, 'mp.nme_load'}, ...
                    {@mp.nme_branch_acp_nln, 'mp.nme_branch'}, ...
                    {@mp.nme_shunt_acp_nln, 'mp.nme_shunt'}, ...
                    @mp.nme_gizmo_acp_nln }    } ...
    );

t_begin(87*length(tc), quiet);

define_constants;
if quiet
    verbose = 0;
else
    verbose = 1;
end

casefile = 't_case9_gizmo';
mpopt = mpoption('out.all', 0, 'verbose', 0);
dmc = mp.dm_converter_mpc2().modify_element_classes(@mp.dmce_gizmo_mpc2).build();

for c = 1:length(tc)
    %% create network model object
    mpc = loadcase(casefile);
    dm = mp.data_model().modify_element_classes(@mp.dme_gizmo).build(mpc, dmc);
    ac = mp.net_model_acp().modify_element_classes(@mp.nme_gizmo_acp).build(dm);
    C = ac.C;
    D = ac.D;
    np = ac.np;
    nv = ac.nv/2;
    nz = ac.nz;
    A = [   C sparse(nv, nz);
            sparse(nz, np) D    ];
    A2 = [A sparse(nv+nz, np+nz); sparse(nv+nz, np+nz) A];

    %% other parameters
    dx = 1e-8;
    idx = randperm(np, fix(0.67*np))';
    lam = (1.5*rand(np, 1) + 0.5); k = randperm(np, fix(np/2)); lam(k) = -lam(k);
    e0 = zeros(np, 1);
    e1 = ones(np, 1);

    %% get ref bus index
    ref = find(mpc.bus(:, BUS_TYPE) == REF);

    %% construct initial system v1, v2, zr, zi, v_, z_, x_
    t = sprintf('%s : construct initial system v_, z_', tc(c).name);
    sv1 = ac.params_var('va');
    sv2 = ac.params_var('vm');
    szr = ac.params_var('zr');
    szi = ac.params_var('zi');

    %% randomize voltages a bit
    sv1 = sv1 + (0.6*rand(size(sv1)) - 0.3);    sv1(ref) = 0;
    sv2 = sv2 + (0.06*rand(size(sv1)) - 0.03);  sv2(ref) = 1;

    %% adjust values of z_
    szr(1) = 0.67;
    szi(1:3) = [0.1; 0.2; 0.3];

    %% initialize v_, z_, x_
    sv = sv2 .* exp(1j * sv1);
    sz = szr + 1j * szi;
    sx = [sv; sz];
    nx = length(sx);
    t_is(nx, nv+nz, 12, t);

    %%-----  tests using system voltages  -----
    t = sprintf('%s : ac.port_inj_power(x_) : ', tc(c).name);
    v10 = sv1; v20 = sv2; zr0 = szr; zi0 = szi; %% init w/ system v_, z_ components
    v0 = v20 .* exp(1j * v10);
    z0 = zr0 + 1j * zi0;
    x0 = [v0; z0];
    Nv = length(v0);
    Nz = length(z0);
    [S0, Sv1, Sv2, Szr, Szi] = ac.port_inj_power(x0);   %% analytical

    %% check matrix input/output
    SS0 = ac.port_inj_power(x0*ones(1,Nv));
    t_is(SS0, S0*ones(1,Nv), 12, [t 'matrix input']);

    %% Sv1
    v_ = (v20*ones(1,Nv)) .* exp(1j * (v10*ones(1,Nv) + dx*eye(Nv,Nv)));
    z_ = (zr0 + 1j * zi0) * ones(1,Nv);
    x_ = [v_; z_];
    SS = ac.port_inj_power(x_);
    num_Sv1b = (SS - SS0) / dx;
    t_is(full(Sv1), num_Sv1b, 6, [t 'Sv1']);

    %% Sv2
    v_ = (v20*ones(1,Nv) + dx*eye(Nv,Nv)) .* exp(1j * (v10*ones(1,Nv)));
    z_ = (zr0 + 1j * zi0) * ones(1,Nv);
    x_ = [v_; z_];
    SS = ac.port_inj_power(x_);
    num_Sv2b = (SS - SS0) / dx;
    t_is(full(Sv2), num_Sv2b, 6, [t 'Sv2']);

    SS0 = ac.port_inj_power(x0*ones(1,Nz));

    %% Szr
    v_ = (v20*ones(1,Nz)) .* exp(1j * (v10*ones(1,Nz)));
    z_ = (zr0 * ones(1,Nz) + dx*eye(Nz,Nz)) + 1j * (zi0 * ones(1,Nz));
    x_ = [v_; z_];
    SS = ac.port_inj_power(x_);
    num_Szrb = (SS - SS0) / dx;
    t_is(full(Szr), num_Szrb, 6, [t 'Szr']);

    %% Szi
    v_ = (v20*ones(1,Nz)) .* exp(1j * (v10*ones(1,Nz)));
    z_ = (zr0 * ones(1,Nz)) + 1j * (zi0 * ones(1,Nz) + dx*eye(Nz,Nz));
    x_ = [v_; z_];
    SS = ac.port_inj_power(x_);
    num_Szib = (SS - SS0) / dx;
    t_is(full(Szi), num_Szib, 6, [t 'Szi']);

    t = sprintf('%s : ac.port_inj_power(x_, 1, idx) : ', tc(c).name);
    [iS0, iSv1, iSv2, iSzr, iSzi] = ac.port_inj_power(x0, 1, idx);
    t_is(iS0, S0(idx), 12, [t 'S0']);
    t_is(iSv1, Sv1(idx, :), 12, [t 'Sv1']);
    t_is(iSv2, Sv2(idx, :), 12, [t 'Sv2']);
    t_is(iSzr, Szr(idx, :), 12, [t 'Szr']);
    t_is(iSzi, Szi(idx, :), 12, [t 'Szi']);

    t = sprintf('%s : ac.port_inj_power_hess(x0, ek) == ac.p_i_p_h(x0, 1, 1, k) : ', tc(c).name);
    for k = 1:length(lam)
        ek = e0; ek(k) = 1;
        H1 = ac.port_inj_power_hess(x0, ek);
        H2 = ac.port_inj_power_hess(x0, 1, 1, k);
        t_is(H1, H2, 12, sprintf('%s%d', t, k));
    end

    t = sprintf('%s : ac.port_inj_power_hess(x_, lam) : ', tc(c).name);
    H = ac.port_inj_power_hess(x0, lam);
    HH = sparse(size(H, 1), size(H, 2));
    for k = 1:length(lam)
        ek = e0; ek(k) = 1;
        HH = HH + lam(k) * ac.port_inj_power_hess(x0, ek);
    end
    t_is(H, HH, 12, [t 'weighted sum indiv Hessians']);

    t = sprintf('%s : ac.port_inj_power_hess(x_, lam, 1, idx) : ', tc(c).name);
    H = ac.port_inj_power_hess(x0, lam(idx), 1, idx);
    HH = sparse(size(H, 1), size(H, 2));
    for k = 1:length(idx)
        ek = e0; ek(idx(k)) = 1;
        HH = HH + lam(idx(k)) * ac.port_inj_power_hess(x0, ek);
    end
    t_is(H, HH, 12, [t 'weighted sum indiv Hessians']);

    t = sprintf('%s : ac.port_inj_power_hess(x_, lam) : ', tc(c).name);
    H = ac.port_inj_power_hess(x0, lam);
    [S0, Sv1, Sv2, Szr, Szi] = ac.port_inj_power(x0);
    numH = zeros(2*nx, 2*nx);
    for k = 1:Nv
        v1 = v10; v1(k) = v1(k) + dx;
        v_ = v20 .* exp(1j * v1);
        x_ = [v_; z0];
        [S0p, Sv1p, Sv2p, Szrp, Szip] = ac.port_inj_power(x_);
        numH(:, k) = ([Sv1p, Sv2p, Szrp, Szip]- [Sv1, Sv2, Szr, Szi]).' * lam / dx;

        v2 = v20; v2(k) = v2(k) + dx;
        v_ = v2 .* exp(1j * v10);
        x_ = [v_; z0];
        [S0p, Sv1p, Sv2p, Szrp, Szip] = ac.port_inj_power(x_);
        numH(:, Nv+k) = ([Sv1p, Sv2p, Szrp, Szip]- [Sv1, Sv2, Szr, Szi]).' * lam / dx;
    end
    for k = 1:Nz
        z_ = zr0 + 1j * zi0;
        z_(k) = z_(k) + dx;
        x_ = [v0; z_];
        [S0p, Sv1p, Sv2p, Szrp, Szip] = ac.port_inj_power(x_);
        numH(:, 2*Nv+k) = ([Sv1p, Sv2p, Szrp, Szip]- [Sv1, Sv2, Szr, Szi]).' * lam / dx;

        z_ = zr0 + 1j * zi0;
        z_(k) = z_(k) + 1j*dx;
        x_ = [v0; z_];
        [S0p, Sv1p, Sv2p, Szrp, Szip] = ac.port_inj_power(x_);
        numH(:, 2*Nv+Nz+k) = ([Sv1p, Sv2p, Szrp, Szip]- [Sv1, Sv2, Szr, Szi]).' * lam / dx;
    end
    t_is(full(H), numH, 5, [t 'numerical Hessian']);

    %%-----  tests using port voltages  -----
    t = sprintf('%s : ac.port_inj_power(x_, 0) : ', tc(c).name);
    v10 = C'*sv1; v20 = C'*sv2; zr0 = D'*szr; zi0 = D'*szi; %% init w/ port v_, z_ components
    v0 = v20 .* exp(1j * v10);
    z0 = zr0 + 1j * zi0;
    x0 = [v0; z0];
    Nv = length(v0);
    Nz = length(z0);
    [S0, Sv1, Sv2, Szr, Szi] = ac.port_inj_power(x0, 0);    %% analytical

    %% check matrix input/output
    SS0 = ac.port_inj_power(x0*ones(1,Nv), 0);
    t_is(SS0, S0*ones(1,Nv), 12, [t 'matrix input']);

    %% Sv1
    v_ = (v20*ones(1,Nv)) .* exp(1j * (v10*ones(1,Nv) + dx*eye(Nv,Nv)));
    z_ = (zr0 + 1j * zi0) * ones(1,Nv);
    x_ = [v_; z_];
    SS = ac.port_inj_power(x_, 0);
    num_Sv1b = (SS - SS0) / dx;
    t_is(full(Sv1), num_Sv1b, 6, [t 'Sv1']);

    %% Sv2
    v_ = (v20*ones(1,Nv) + dx*eye(Nv,Nv)) .* exp(1j * (v10*ones(1,Nv)));
    z_ = (zr0 + 1j * zi0) * ones(1,Nv);
    x_ = [v_; z_];
    SS = ac.port_inj_power(x_, 0);
    num_Sv2b = (SS - SS0) / dx;
    t_is(full(Sv2), num_Sv2b, 6, [t 'Sv2']);

    SS0 = ac.port_inj_power(x0*ones(1,Nz), 0);

    %% Szr
    v_ = (v20*ones(1,Nz)) .* exp(1j * (v10*ones(1,Nz)));
    z_ = (zr0 * ones(1,Nz) + dx*eye(Nz,Nz)) + 1j * (zi0 * ones(1,Nz));
    x_ = [v_; z_];
    SS = ac.port_inj_power(x_, 0);
    num_Szrb = (SS - SS0) / dx;
    t_is(full(Szr), num_Szrb, 6, [t 'Szr']);

    %% Szi
    v_ = (v20*ones(1,Nz)) .* exp(1j * (v10*ones(1,Nz)));
    z_ = (zr0 * ones(1,Nz)) + 1j * (zi0 * ones(1,Nz) + dx*eye(Nz,Nz));
    x_ = [v_; z_];
    SS = ac.port_inj_power(x_, 0);
    num_Szib = (SS - SS0) / dx;
    t_is(full(Szi), num_Szib, 6, [t 'Szi']);

    t = sprintf('%s : ac.port_inj_power(x_, 0, idx) : ', tc(c).name);
    [iS0, iSv1, iSv2, iSzr, iSzi] = ac.port_inj_power(x0, 0, idx);
    t_is(iS0, S0(idx), 12, [t 'S0']);
    t_is(iSv1, Sv1(idx, :), 12, [t 'Sv1']);
    t_is(iSv2, Sv2(idx, :), 12, [t 'Sv2']);
    t_is(iSzr, Szr(idx, :), 12, [t 'Szr']);
    t_is(iSzi, Szi(idx, :), 12, [t 'Szi']);

    t = sprintf('%s : ac.port_inj_power_hess(x0, ek, 0) == ac.p_i_p_h(x0, 1, 0, k) : ', tc(c).name);
    for k = 1:length(lam)
        ek = e0; ek(k) = 1;
        H1 = ac.port_inj_power_hess(x0, ek, 0);
        H2 = ac.port_inj_power_hess(x0, 1, 0, k);
        t_is(H1, H2, 12, sprintf('%s%d', t, k));
    end

    t = sprintf('%s : ac.port_inj_power_hess(x_, lam, 0) : ', tc(c).name);
    H = ac.port_inj_power_hess(x0, lam, 0);
    HH = sparse(size(H, 1), size(H, 2));
    for k = 1:length(lam)
        ek = e0; ek(k) = 1;
        HH = HH + lam(k) * ac.port_inj_power_hess(x0, ek, 0);
    end
    t_is(H, HH, 12, [t 'weighted sum indiv Hessians']);

    t = sprintf('%s : ac.port_inj_power_hess(x_, lam, 0, idx) : ', tc(c).name);
    H = ac.port_inj_power_hess(x0, lam(idx), 0, idx);
    HH = sparse(size(H, 1), size(H, 2));
    for k = 1:length(idx)
        ek = e0; ek(idx(k)) = 1;
        HH = HH + lam(idx(k)) * ac.port_inj_power_hess(x0, ek, 0);
    end
    t_is(H, HH, 12, [t 'weighted sum indiv Hessians']);

    t = sprintf('%s : ac.port_inj_power_hess(x_, lam, 0) : ', tc(c).name);
    H = ac.port_inj_power_hess(x0, lam, 0);
    [S0, Sv1, Sv2, Szr, Szi] = ac.port_inj_power(x0, 0);
    Nx = 2*Nv+2*Nz;
    numH = zeros(Nx, Nx);
    for k = 1:Nv
        v1 = v10; v1(k) = v1(k) + dx;
        v_ = v20 .* exp(1j * v1);
        x_ = [v_; z0];
        [S0p, Sv1p, Sv2p, Szrp, Szip] = ac.port_inj_power(x_, 0);
        numH(:, k) = ([Sv1p, Sv2p, Szrp, Szip]- [Sv1, Sv2, Szr, Szi]).' * lam / dx;

        v2 = v20; v2(k) = v2(k) + dx;
        v_ = v2 .* exp(1j * v10);
        x_ = [v_; z0];
        [S0p, Sv1p, Sv2p, Szrp, Szip] = ac.port_inj_power(x_, 0);
        numH(:, Nv+k) = ([Sv1p, Sv2p, Szrp, Szip]- [Sv1, Sv2, Szr, Szi]).' * lam / dx;
    end
    for k = 1:Nz
        z_ = zr0 + 1j * zi0;
        z_(k) = z_(k) + dx;
        x_ = [v0; z_];
        [S0p, Sv1p, Sv2p, Szrp, Szip] = ac.port_inj_power(x_, 0);
        numH(:, 2*Nv+k) = ([Sv1p, Sv2p, Szrp, Szip]- [Sv1, Sv2, Szr, Szi]).' * lam / dx;

        z_ = zr0 + 1j * zi0;
        z_(k) = z_(k) + 1j*dx;
        x_ = [v0; z_];
        [S0p, Sv1p, Sv2p, Szrp, Szip] = ac.port_inj_power(x_, 0);
        numH(:, 2*Nv+Nz+k) = ([Sv1p, Sv2p, Szrp, Szip]- [Sv1, Sv2, Szr, Szi]).' * lam / dx;
    end
    t_is(full(H), numH, 5, [t 'numerical Hessian']);
end

t_end;

if nargout
    obj = ac;
end


% Faust Synthetic
test_idx=[80:99];
pairs_list = [];
for i=1:20
    for j = 1:20
        pairs_list = [pairs_list; test_idx(i),test_idx(j)];
    end
end

% delete_idx = floor(pairs_list(:,1)/10) == floor(pairs_list(:,2)/10);
% pairs_list(delete_idx,:)=[];

fileID = fopen('test_pairs.txt','w');
for i=1:size(pairs_list,1)
    id1 = sprintf('%03d', pairs_list(i,1));
    id2 = sprintf('%03d', pairs_list(i,2));
    fprintf(fileID,['tr_reg_', id1, '.mat tr_reg_' ,id2, '.mat\n']);
end
fclose(fileID)


function [elem] = subsref(tt,s)
% SUBSREF V2
% Evaluate elements, cores of TT-formats and fields of the TT-structure
%   A=TT{I} computes I-th core of the TT-representation
%
%   ELEM=TT(IND) computes element with index IND of the tensor TT
%
% TT-Toolbox 2.2.1, 2009-2016
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com

% Possibility to extract subtensor added by Alexey Boyko,
% Skolkovo Institute of Science and Technology
% For questions email alexey.boyko@skolkovotech.ru or a.boyko@skoltech.ru

% Example 1
% y =tt_x(2,10)
% y2=y(1,2,1,2,2,:,:,:,:,:), or, alternatively,
% y2=y(1,2,1,2,2) or
% y2=y([1,2,1,2,2]) or
% y2=y({1,2,1,2,2,':',':',':',':',':'}) or
% y2=y({1,2,1,2,2}) or
% will return 5-dimensional tt_tensor, corresponding to
% fist 5 modes\harmonics being frozen to indices 1,2,1,2,2 respectively

% Example 2
% y =tt_x(2,10)
% y2=y(1,:,1,2,2,:,:,1,:,:), or, alternatively,
% y2=y({1,':',1,2,2,':',':',1}) or
% y2=y({1,':',1,2,2,':',':',1,':',':'}) or

% will also return 5-dimensional tt_tensor, corresponding to freezing
% respective modes\harmonics

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

switch s(1).type
    case '()'
    %Evaluate element of a tensor in position s
    d=tt.d; n=tt.n; ps=tt.ps; cr=tt.core; r=tt.r;

    %ind=double(s);
    pp=s.subs;
    mn=numel(pp);

    if mn == 1
        if iscell( pp{1})
            %one argument, cell array
            pp=pp{1};
        else
            % one argument, ordinary array
            pp=num2cell(pp{1});
        end
    end
    mn=numel(pp);

    if ( mn > d )
        error('Invalid number of indices specified: given %d, need no more than %d \n',mn,d);
    end

    idxs=[];
    vals=[];
    for i=1:mn
        if isnumeric(pp{i})
            idxs=[idxs,i];
            vals=[vals,pp{i}];
        end
    end

    mn=numel(idxs);
    if(mn < d)
        elem=tt_subtensor(tt,idxs,vals);
    else

        %               if ( mn == 1 ) %Special case
        %                  ind=pp{1};
        %                  if ( is_array(ind) )
        %                    for i=1:d
        %                      pp{i}=ind(i);
        %                    end
        %                  else
        %                     pp=ind; %Supposedly it was a cell array of index subsets
        %                  end
        %               end

        elem=tt_tensor;
        elem.r=r;
        elem.d=d;
        n0=zeros(d,1);
        for i=1:d
            ind=pp{i};
            if ( strcmp(ind, ':'))
                pp{i}=1:n(i);
                ind=pp{i};
            end
            n0(i)=numel(ind);
        end
        ps1=cumsum([1;n0.*r(1:d).*r(2:d+1)]);
        cr1=zeros(ps1(d+1)-1,1);

        for i=1:d
            ind=pp{i};
            crx=cr(ps(i):ps(i+1)-1);
            crx=reshape(crx,[r(i),n(i),r(i+1)]);
            crx=crx(:,ind,:);
            cr1(ps1(i):ps1(i+1)-1)=crx(:);
        end

        if ( prod(n0) == 1 ) %single element
            v=1;
            for i=1:d
                crx=cr1(ps1(i):ps1(i+1)-1); crx=reshape(crx,[r(i),r(i+1)]);
                v=v*crx;
            end
            elem=v;
        else
            elem.n=n0;
            elem.core=cr1;
            elem.ps=ps1;
        end

    end

    case '.'
        switch s(1).subs
            case 'r'
                elem = tt.r;
                if (numel(s)>1)
                    s = s(2:end);
                    elem = subsref(elem, s);
                end;
%                 if (numel(s)>1)&&(strcmp(s(2).type,'()'))
%                     elem = tt.r(s(2).subs{1});
%                 else
%                     elem=tt.r;
%                 end;
            case 'over'
                elem = tt.over;
                if (numel(s)>1)
                    s = s(2:end);
                    elem = subsref(elem, s);
                end;
            case 'core'
                if (numel(s)>1)&&(strcmp(s(2).type,'()'))
                    elem = tt.core(s(2).subs{1});
                else
                    elem=tt.core;
                end;
            case 'd'
                elem=tt.d;
            case 'ps'
                elem = tt.ps;
                if (numel(s)>1)
                    s = s(2:end);
                    elem = subsref(elem, s);
                end;

%                 if (numel(s)>1)&&(strcmp(s(2).type,'()'))
%                     elem = tt.ps(s(2).subs{1});
%                 else
%                     elem=tt.ps;
%                 end;
            case 'n'
                elem = tt.n;
                if (numel(s)>1)
                    s = s(2:end);
                    elem = subsref(elem, s);
                end;

%                 if (numel(s)>1)&&(strcmp(s(2).type,'()'))
%                     elem = tt.n(s(2).subs{1});
%                 else
%                     elem=tt.n;
%                 end;
            otherwise
                error(['No field ', s.subs, ' is here.']);
        end

    case '{}'
        %Return the core in the old (not exactly!!! r1-n-r2 here) format
        pp=s.subs;
        mn=numel(pp);
        if ( mn > 1 )
          error('Invalid number of cores asked');
        end
        elem=core(tt,pp{1});

        if (numel(s)>1)
            s = s(2:end);
            elem = subsref(elem, s);
        end;
%         if (pp{1}~=1)
%             elem=permute(elem,[2,1,3]);
%         end;

    otherwise
        error('Invalid subsref.');
end
end


function t = getLinesearch(problem, x, d, storedb, key)
% Returns a hint for line-search algorithms.
%
% function t = getLinesearch(problem, x, d)
% function t = getLinesearch(problem, x, d, storedb)
% function t = getLinesearch(problem, x, d, storedb, key)
%
% For a line-search problem at x along the tangent direction d, computes
% and returns t such that retracting t*d at x yields a good point around
% where to look for a line-search solution. That is: t is a hint as to
% "how far to look" along the line.
%
% storedb is a StoreDB object, key is the StoreDB key to point x.
%
% See also: canGetLinesearch

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, July 17, 2014.
% Contributors:
% Change log:
%
%   April 3, 2015 (NB):
%       Works with the new StoreDB class system.

    % Allow omission of the key, and even of storedb.
    if ~exist('key', 'var')
        if ~exist('storedb', 'var')
            storedb = StoreDB();
        end
        key = storedb.getNewKey();
    end

    if isfield(problem, 'linesearch')
    %% Compute the line-search hint function using linesearch.

        % Check whether this function wants to deal with storedb or not.
        switch nargin(problem.linesearch)
            case 2
                t = problem.linesearch(x, d);
            case 3
                % Obtain, pass along, and save the store for x.
                store = storedb.getWithShared(key);
                [t, store] = problem.linesearch(x, d, store);
                storedb.setWithShared(store, key);
            case 4
                % Pass along the whole storedb (by reference), with key.
                t = problem.linesearch(x, d, storedb, key);
            otherwise
                up = MException('manopt:getLinesearch:badfun', ...
                    'linesearch should accept 2, 3 or 4 inputs.');
                throw(up);
        end

    else
    %% Abandon computing the line-search function.

        up = MException('manopt:getLinesearch:fail', ...
            ['The problem description is not explicit enough to ' ...
             'compute a line-search hint.']);
        throw(up);

    end

end