function [parameters, ll, ht, VCV, scores, diagnostics] = matrix_garch(data,dataAsym,p,o,q,startingvals,options)
% Estimation of symmetric and asymmetric MATRIX multivariate GARCH models
%
% USAGE:
%  [PARAMETERS,LL,HT,VCV,SCORES,DIAGNOSTICS] = matrix_garch(DATA,DATAASYM,P,O,Q,STARTINGVALS,OPTIONS)
%
% INPUTS:
%   DATA         - A T by K matrix of zero mean residuals -OR-
%                    K by K by T array of covariance estimators (e.g. realized covariance)
%   DATAASYM     - [OPTIONAL] K by K by T array of asymmetric covariance
%                    estimators (e.g. RC scaled by indicator functions)
%   P            - Positive, scalar integer representing the number of lags of the innovation process
%   O            - Non-negative scalar integer representing the number of asymmetric lags to include
%   Q            - Non-negative scalar integer representing the number of lags of conditional covariance
%   STARTINGVALS - [OPTIONAL] INCOMPLETE
%   OPTIONS      - [OPTIONAL] Options to use in the optimization (fminunc)
%
% OUTPUTS:
%   PARAMETERS   - K(K+1)/2*(1+P+O+Q) by 1 vector of parameters of the form
%                    [vech(C)' vech(A(1))' ... vech(A(P))' vech(G(1))' ...
%                    vech(G(O))' vech(B(1))' ... vech(B(Q))']'
%   LL           - The log likelihood at the optimum
%   HT           - A [K K T] dimension matrix of conditional covariances
%   VCV          - A numParams^2 square matrix of robust parameter covariances (A^(-1)*B*A^(-1)*t^(-1))
%   SCORES       - A T by numParams matrix of individual scores
%   DIAGNOSTICS  - Structure containing some diagnostic information
%
% COMMENTS:
%    The conditional variance, H(t), of a MATRIX GARCH is modeled as follows:
%
%      H(t) = CC' + A(1)A(1)'.*r_{t-1}'*r_{t-1} + ... + A(P)A(P)'.*r_{t-P}'*r_{t-P}
%                 + G(1)G(1)'.*n_{t-1}'*n_{t-1} + ... + G(O)G(O)'.*n_{t-P}'*n_{t-P}
%                  + B(1)B(1)'.*H(t-1) +...+ B(Q)B(Q)'.*H(t-q)
%
%    where n_{t} = r_{t} .* (r_{t}<0).  If using realized measures, the
%    RM_{t-1} replaces r_{t-1}'*r_{t-1}, and the asymmetric version
%    replaces n_{t-1}'*n_{t-1}
%
% EXAMPLES:
%     % Estimation of a symmetric MATRIX GARCH(1,0,1) model
%     parameters = matrix_garch(data,[],1,0,1);
%     % Estimation of an asymmetric MATRIX GARCH(1,0,1) model
%     parameters = matrix_garch(data,[],1,1,1);
%     % Estimation of a symmetric MATRIX GARCH(1,0,1) model using realized covariacne
%     data = RC % K by K by T 3D of realized covariacne
%     parameters = matrix_garch(RC,[],1,0,1);

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 10/28/2009

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Argument Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 3
        o=0;
        q=0;
        startingvals=[];
        options = [];
    case 4
        q=0;
        startingvals=[];
        options = [];
    case 5
        startingvals=[];
        options = [];
    case 6
        options = [];
    case 7
    otherwise
        error('Between 2 and 6 arguments required.')
end

%data should be TxK, T>K
if ndims(data)==2
    [t,k]=size(data);
    if ~isempty(dataAsym)
        error('If DATA is a T by K matrix, DATAASYM must be empty.');
    end
    temp = zeros(k,k,t);
    dataAsym = zeros(k,k,t);
    for i=1:t
        temp(:,:,i) = data(i,:)'*data(i,:);
        dataAsym(:,:,i) = (data(i,:).*(data(i,:)<0))'*(data(i,:).*(data(i,:)<0));
    end
    data = temp;
elseif ndims(data)==3
    [k,m,t] = size(data);
    if m~=k
        error('DATA must be K by K by T is a 3D array.');
    end
    if ~isempty(dataAsym)
        if ndims(dataAsym)~=3
            error('DATAASYM must be a 3D array with the same dimensions as DATA');
        end
        [k2,m2,t2]=size(dataAsym);
        if any([k m t]~=[k2 m2 t2])
            error('DATAASYM must be a 3D array with the same dimensions as DATA');
        end
    end
end
k2 = k*(k+1)/2;
if min(t,k)<2 || t<k
    error('DATA must be a T by K matrix or a K by K by T 3D array, T>K>1');
end

%p, o, q much be non-negative scalars
if length(p)>1 || any(p<1) || floor(p)~=p
    error('P must be a positive scalar');
end
if isempty(o)
    o = 0;
end
if length(o)>1 || any(o<0) || floor(o)~=o
    error('O must be a non-negative scalar');
end
if o>0 && isempty(dataAsym)
    error('DATAASYM must be non-empty if O>0.')
end

if isempty(q)
    q = 0;
end
if length(q)>1 || any(q<0) || floor(q)~=q
    error('Q must be a non-negative scalar');
end

% Startingvals must have (k*(k+1)/2)(1+p+o+q) parameters
if  ~isempty(startingvals)
    if size(startingvals,2)>size(startingvals,1)
        startingvals = startingvals';
    end
    if length(startingvals)<(p+o+q)
        error('STARTINGVALS should be a P+O+Q by 1 vector');
    end
    % Only validate if provided
    % FIXME: Fix Validation
    % TODO: Fix Validation
    kappa = 2;
    A=startingvals(1:p);
    G=startingvals(p+1:p+o);
    B=startingvals(p+o+1:p+o+q);
    if (sum(A)+sum(G)/kappa+sum(B))>=.999998
        error('Weighted sum of STATINGVALUES must be less than 1. See Comments.');
    end
    if any(A<0) || any(B<0) || any(G<0)
        error('STARTINGVALS must all be nonnegative.');
    end
end

%Make sure options is a valid option structure
if isempty(options)
    options=optimset('fminunc');
    options.Display='iter';
    options.Diagnostics='on';
    options.LargeScale='off';
    options.MaxFunEvals = 1000*k2*(1+p+o+q);
end
try
    optimset(options);
catch ME
    error('OPTIONS is not a valid options structure');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Argument Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Compute the backCast
backCast = zeros(k);
backCastAsym = zeros(k);
tau = max(ceil(sqrt(t)),k);
weights = .06 * .94.^(0:tau);
weights = weights / sum(weights);
for i=1:tau
    backCast = backCast + weights(i) * data(:,:,i);
    if o>0
        backCastAsym = backCastAsym + weights(i) * dataAsym(:,:,i);
    end
end

%Get starting values from scalar_vt_vech
if isempty(startingvals)
    startingOptions=optimset('fminunc');
    startingOptions.Display='off';
    startingOptions.Diagnostics='off';
    startingOptions.LargeScale='off';
    startingOptions.TolX = 1e-4;
    startingOptions.TolFun = 1e-4;
    [scalarVechStartingvals,~,~,~,~,~,diagnostics] = scalar_vt_vech(data,dataAsym,p,o,q,[],[],startingOptions);
    CpC = diagnostics.intercept;

    startingvals = zeros(k2 * (1+p+o+q),1);
    startingvals(1:k2) = chol2vec(chol(CpC)');
    index = k2;
    for i=1:(p+o+q)
        matrixParameters = scalarVechStartingvals(i)*(.02*eye(k) + .98*ones(k));
        startingvals(index+1:index+k2) = chol2vec(chol(matrixParameters)');
        index = index + k2;
    end
end

warning('off','MATLAB:illConditionedMatrix')
ll0 = matrix_garch_likelihood(startingvals,data,dataAsym,p,o,q,backCast,backCastAsym);
[parameters,ll,exitflag,output]=fminunc('matrix_garch_likelihood',startingvals,options,data,dataAsym,p,o,q,backCast,backCastAsym);

%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Estimation Robustification
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if exitflag<=0 &&  ll<ll0
    %Did not converge, but function improved
    options.MaxFunEvals=4*100*(p+q);
    options.MaxIter=2*100*(p+q);
    parameters=fminunc('matrix_garch_likelihood',parameters,options,data,dataAsym,p,o,q,backCast,backCastAsym);
end
warning('on','MATLAB:illConditionedMatrix')
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Estimation Robustification
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargout>1
    [ll,lls,ht]=matrix_garch_likelihood(parameters,data,dataAsym,p,o,q,backCast,backCastAsym);
    ll=-ll;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the VCV
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargout>3
    [VCV,A,B,scores]=robustvcv('matrix_garch_likelihood',parameters,0,data,dataAsym,p,o,q,backCast,backCastAsym);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the VCV
%%%%%%%%%%%%%%%%%%%%%%%%%%%
diagnostics = [];
diagnostics.EXITFLAG=exitflag;
diagnostics.ITERATIONS=output.iterations;
diagnostics.FUNCCOUNT=output.funcCount;
diagnostics.MESSAGE=output.message;
parameterMatrices = zeros(k,k,1+p+o+q);
index = 0;
for i=1:(1+p+o+1)
    temp = vec2chol(parameters(index+1:index+k2));
    parameterMatrices(:,:,i) = temp*temp';
    index=index+k2;
end
diagnostics.C = parameterMatrices(:,:,1);
diagnostics.A = parameterMatrices(:,:,2:p+1);
if o>0
    diagnostics.G = parameterMatrices(:,:,p+2:p+o+1);
else
    diagnostics.G = [];
end

if q>0
    diagnostics.B = parameterMatrices(:,:,p+o+2:p+o+q+1);
else
    diagnostics.B = [];
end


function x = line_cvt_lloyd ( n, a, b, it_num, header, x )

%*****************************************************************************80
%
%% LINE_CVT_LLOYD carries out the Lloyd algorithm.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 July 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of generators.
%
%    Input, real A, B, the left and right endpoints.
%
%    Input, integer IT_NUM, the number of iterations to take.
%
%    Input, string HEADER, an identifying string.
%
%    Input, real X(N), the initial point locations.
%
%    Output, real X(N), the final point locations.
%
  x = x(:);
%
%  Print the initial generators.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial generators:\n' );
  fprintf ( 1, '\n' );
  for k = 1 : n
    fprintf ( 1, '  %4d  %f\n', k, x(k,1) );
  end
%
%  Initialize the plotting arrays.
%
  step = 1 : it_num;
  e = nan ( it_num, 1 );
  xm = nan ( it_num, 1 );

  for it = 1 : it_num

    x_plot ( 1:n, it ) = x(1:n,1);

    x_new = line_cvt_lloyd_step ( n, a, b, x );

    e(it) = line_cvt_energy ( n, a, b, x );
    e(it) = max ( e(it), eps );
%
%  Display the energy.
%
    figure ( 1 )
    plot ( step, log ( e ), 'm-*' )
    title ( 'Log (Energy)' )
    xlabel ( 'Step' )
    ylabel ( 'Energy' )
    grid
%
%  Compute the generator motion.
%
    xm(it,1) = sum ( ( x_new(:) - x(:) ).^2 ) / n;
%
%  Display the generator motion.
%
    figure ( 2 )
    plot ( step, log ( xm ), 'm-*' )
    title ( 'Log (Average generator motion)' )
    xlabel ( 'Step' )
    ylabel ( 'Motion' )
    grid
%
%  Update the generators.
%
    x(1:n,1) = x_new(1:n,1);

  end

  x_plot(1:n,it_num+1) = x(1:n,1);
%
%  Print the current generators.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Current generators:\n' );
  fprintf ( 1, '\n' );
  for k = 1 : n
    fprintf ( 1, '  %4d  %f\n', k, x(k,1) );
  end
%
%  Plot the evolution of the locations of the generators.
%
  figure ( 3 )

  y = ( 0 : it_num );
  for k = 1 : n
    plot ( x_plot(k,1:it_num+1), y )
    hold on;
  end
  grid on
  hold off;

  title ( 'Generator evolution.' );
  xlabel ( 'Generator positions' );
  ylabel ( 'Iterations' );
%
%  Save the plots.
%
  figure ( 1 )
  filename = strcat ( header, '_energy.png' );
  print ( '-dpng', filename );
  figure ( 2 )
  filename = strcat ( header, '_motion.png' );
  print ( '-dpng', filename );
  figure ( 3 )
  filename = strcat ( header, '_evolution.png' );
  print ( '-dpng', filename );

  return
end


function [YY,XX] = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);

Ap(2,:) = 0;

for ii = 1:1:length(Ap)-1
    Ap(ii+1,ii) = 1;
end

inx = 1;
UUk = [Uk(inx); 0; 0; 0; 0];
PPk = (Xint_v*Xint_v');
VVk = [Vk(inx); 0; 0; 0; 0];
Qv = V*V';

for ii = 1:1:length(Xint_v)

XKk(ii,1) = Xint_v(ii)^2;                                             % FIRST STEP

end

PPk = Ap*PPk*Ap';                                                   % SECOND STEP

Kk = PPk*C'*inv( (C*PPk*C') + (V*Qv*V') );                          % THIRD STEP

for ii = 1:1:length(Xint_v)

XUPK(ii,1) = XKk(ii)^2 + UUk(ii);                                     % UPPER EQUATIONS.

Zk(ii,1) = cos(XUPK(ii)) +  VVk(ii);                                  % UPPER EQUATIONS.

end

for ii = 1:1:length(XKk)

XBARk(ii,1) = XKk(ii) + Kk(ii)*(Zk(ii) - (cos(XKk(ii)))) ;            % FOURTH STEP

end

II = eye(5,5);

Pk = ( II -  Kk*C)*PPk;                                             % FIFTH STEP

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

for ii = 1:1:n

UUk = [Uk(ii+1); 0; 0; 0; 0];
PPk = XBARk*XBARk';
VVk = [Vk(ii+1); 0; 0; 0; 0];

XKk = exp(-XBARk);                                                % FIRST STEP

PPkM = Ap*PPk*Ap';                                                 % SECOND STEP

Kk = PPkM*C'*inv( (C*PPkM*C') + (V*Qv*V') );                      % THIRD STEP

for nn = 1:1:length(XBARk)

XUPK(nn) = exp(-XKk(nn)) + UUk(nn);                              % UPPER EQUATIONS.

Zk(nn) = cos(XUPK(nn)) +  VVk(nn);                               % UPPER EQUATIONS.

end

for in = 1:1:length(XUPK)

XNEW(in) = XBARk(in) + Kk(in)*(Zk(in) - cos(XBARk(in)));           % FOURTH STEP

end

II = eye(5,5);

Pk = (II -  Kk*C)*PPkM;                                            % FIFTH STEP

XBARk = XNEW;

OUTX(ii) = XBARk(1,1);
OUTY(ii) = Zk(1,1);

end

YY = OUTY;
XX = OUTX;


function [jrt, jst]=jacprojRTS(j, i, rt, xyz, r0, a)
% symbolic projection function Jacobian
% code automatically generated with maple

  qr0=r0(j*4+1:(j+1)*4);

  t1 = (rt(1) ^ 2);
  t2 = (rt(2) ^ 2);
  t3 = (rt(3) ^ 2);
  t5 = sqrt((1 - t1 - t2 - t3));
  t6 = 0.1e1 / t5;
  t7 = t6 * qr0(2);
  t9 = -t7 * rt(1) + qr0(1);
  t11 = t6 * qr0(3);
  t13 = -t11 * rt(1) - qr0(4);
  t15 = t6 * qr0(4);
  t17 = -t15 * rt(1) + qr0(3);
  t19 = -t9 * xyz(1) - t13 * xyz(2) - t17 * xyz(3);
  t24 = -t5 * qr0(2) - qr0(1) * rt(1) - rt(2) * qr0(4) + rt(3) * qr0(3);
  t31 = t5 * qr0(3) + qr0(1) * rt(2) + rt(3) * qr0(2) - rt(1) * qr0(4);
  t37 = t5 * qr0(4) + qr0(1) * rt(3) + rt(1) * qr0(3) - rt(2) * qr0(2);
  t39 = t24 * xyz(1) - t31 * xyz(2) - t37 * xyz(3);
  t41 = t6 * qr0(1);
  t43 = -t41 * rt(1) - qr0(2);
  t48 = t5 * qr0(1) - rt(1) * qr0(2) - rt(2) * qr0(3) - rt(3) * qr0(4);
  t52 = t48 * xyz(1) + t31 * xyz(3) - t37 * xyz(2);
  t57 = t43 * xyz(1) + t13 * xyz(3) - t17 * xyz(2);
  t62 = t43 * xyz(2) + t17 * xyz(1) - t9 * xyz(3);
  t67 = t48 * xyz(2) + t37 * xyz(1) + t24 * xyz(3);
  t72 = t43 * xyz(3) + t9 * xyz(2) - t13 * xyz(1);
  t77 = t48 * xyz(3) - t24 * xyz(2) - t31 * xyz(1);
  t89 = -t19 * t31 - t39 * t13 + t43 * t67 + t48 * t62 + t72 * t24 - t77 * t9 + t57 * t37 + t52 * t17;
  t99 = -t19 * t37 - t39 * t17 + t43 * t77 + t48 * t72 - t57 * t31 - t52 * t13 - t62 * t24 + t67 * t9;
  t106 = -t39 * t37 + t48 * t77 - t52 * t31 - t67 * t24 + rt(6);
  t107 = 0.1e1 / t106;
  t119 = -t39 * t31 + t48 * t67 + t77 * t24 + t52 * t37 + rt(5);
  t123 = t106 ^ 2;
  t124 = 0.1e1 / t123;
  t125 = (a(1) * (t39 * t24 + t48 * t52 - t67 * t37 + t77 * t31 + rt(4)) + a(2) * t119 + a(3) * t106) * t124;
  t129 = -t7 * rt(2) + qr0(4);
  t132 = -t11 * rt(2) + qr0(1);
  t135 = -t15 * rt(2) - qr0(2);
  t137 = -t129 * xyz(1) - t132 * xyz(2) - t135 * xyz(3);
  t141 = -t41 * rt(2) - qr0(3);
  t146 = t141 * xyz(1) + t132 * xyz(3) - t135 * xyz(2);
  t151 = t141 * xyz(2) + t135 * xyz(1) - t129 * xyz(3);
  t157 = t141 * xyz(3) + t129 * xyz(2) - t132 * xyz(1);
  t170 = -t137 * t31 - t39 * t132 + t141 * t67 + t48 * t151 + t157 * t24 - t77 * t129 + t146 * t37 + t52 * t135;
  t180 = -t137 * t37 - t39 * t135 + t141 * t77 + t48 * t157 - t146 * t31 - t52 * t132 - t151 * t24 + t67 * t129;
  t187 = -t7 * rt(3) - qr0(3);
  t190 = -t11 * rt(3) + qr0(2);
  t193 = -t15 * rt(3) + qr0(1);
  t195 = -t187 * xyz(1) - t190 * xyz(2) - t193 * xyz(3);
  t199 = -t41 * rt(3) - qr0(4);
  t204 = t199 * xyz(1) + t190 * xyz(3) - t193 * xyz(2);
  t209 = t199 * xyz(2) + t193 * xyz(1) - t187 * xyz(3);
  t215 = t199 * xyz(3) + t187 * xyz(2) - t190 * xyz(1);
  t228 = -t195 * t31 - t39 * t190 + t199 * t67 + t48 * t209 + t215 * t24 - t77 * t187 + t204 * t37 + t52 * t193;
  t238 = -t195 * t37 - t39 * t193 + t199 * t77 + t48 * t215 - t204 * t31 - t52 * t190 - t209 * t24 + t67 * t187;
  t255 = (a(4) * t119 + a(5) * t106) * t124;
  jrt(1) = (a(1) * (t19 * t24 - t39 * t9 + t43 * t52 + t48 * t57 - t62 * t37 - t67 * t17 + t72 * t31 + t77 * t13) + a(2) * t89 + a(3) * t99) * t107 - t125 * t99;
  jrt(2) = (a(1) * (t137 * t24 - t39 * t129 + t141 * t52 + t48 * t146 - t151 * t37 - t67 * t135 + t157 * t31 + t77 * t132) + a(2) * t170 + a(3) * t180) * t107 - t125 * t180;
  jrt(3) = (a(1) * (t195 * t24 - t39 * t187 + t199 * t52 + t48 * t204 - t209 * t37 - t67 * t193 + t215 * t31 + t77 * t190) + a(2) * t228 + a(3) * t238) * t107 - t125 * t238;
  jrt(4) = a(1) * t107;
  jrt(5) = a(2) * t107;
  jrt(6) = a(3) * t107 - t125;
  jrt(7) = (a(4) * t89 + a(5) * t99) * t107 - t255 * t99;
  jrt(8) = (a(4) * t170 + a(5) * t180) * t107 - t255 * t180;
  jrt(9) = (a(4) * t228 + a(5) * t238) * t107 - t255 * t238;
  jrt(10) = 0.0e0;
  jrt(11) = a(4) * t107;
  jrt(12) = a(5) * t107 - t255;

  t1 = (rt(1) ^ 2);
  t2 = (rt(2) ^ 2);
  t3 = (rt(3) ^ 2);
  t5 = sqrt((1 - t1 - t2 - t3));
  t10 = -t5 * qr0(2) - qr0(1) * rt(1) - rt(2) * qr0(4) + rt(3) * qr0(3);
  t11 = t10 ^ 2;
  t16 = t5 * qr0(1) - rt(1) * qr0(2) - rt(2) * qr0(3) - rt(3) * qr0(4);
  t17 = t16 ^ 2;
  t22 = t5 * qr0(4) + qr0(1) * rt(3) + rt(1) * qr0(3) - rt(2) * qr0(2);
  t28 = -t5 * qr0(3) - qr0(1) * rt(2) - rt(3) * qr0(2) + rt(1) * qr0(4);
  t29 = t28 ^ 2;
  t32 = t10 * t28;
  t35 = -t16 * t22;
  t36 = 0.2e1 * t32 + t16 * t22 - t35;
  t38 = -t10 * t22;
  t39 = t16 * t28;
  t42 = t38 + 0.2e1 * t39 - t10 * t22;
  t48 = t10 * xyz(1) + t28 * xyz(2) - t22 * xyz(3);
  t53 = t16 * xyz(3) - t10 * xyz(2) + t28 * xyz(1);
  t58 = t16 * xyz(1) - t28 * xyz(3) - t22 * xyz(2);
  t63 = t16 * xyz(2) + t22 * xyz(1) + t10 * xyz(3);
  t65 = -t48 * t22 + t16 * t53 + t58 * t28 - t63 * t10 + rt(6);
  t66 = 0.1e1 / t65;
  t78 = t48 * t28 + t16 * t63 + t53 * t10 + t58 * t22 + rt(5);
  t82 = t65 ^ 2;
  t83 = 0.1e1 / t82;
  t84 = (a(1) * (t48 * t10 + t16 * t58 - t63 * t22 - t53 * t28 + rt(4)) + a(2) * t78 + a(3) * t65) * t83;
  t92 = t22 ^ 2;
  t93 = t29 + t17 - t10 ^ 2 - t92;
  t95 = -t28 * t22;
  t98 = t16 * t10;
  t99 = 0.2e1 * t95 - t16 * t10 - t98;
  t111 = t95 + 0.2e1 * t98 - t28 * t22;
  t114 = t92 + t17 - t28 ^ 2 - t11;
  t127 = (a(4) * t78 + a(5) * t65) * t83;
  jst(1) = (a(1) * (t11 + t17 - t22 ^ 2 - t29) + a(2) * t36 + a(3) * t42) * t66 - t84 * t42;
  jst(2) = (a(1) * (t32 + 0.2e1 * t35 + t10 * t28) + a(2) * t93 + a(3) * t99) * t66 - t84 * t99;
  jst(3) = (a(1) * (0.2e1 * t38 - t16 * t28 - t39) + a(2) * t111 + a(3) * t114) * t66 - t84 * t114;
  jst(4) = (a(4) * t36 + a(5) * t42) * t66 - t127 * t42;
  jst(5) = (a(4) * t93 + a(5) * t99) * t66 - t127 * t99;
  jst(6) = (a(4) * t111 + a(5) * t114) * t66 - t127 * t114;


function engine = kalman_inf_engine(bnet)
% KALMAN_INF_ENGINE Inference engine for Linear-Gaussian state-space models.
% engine = kalman_inf_engine(bnet)
%
% 'onodes' specifies which nodes are observed; these must be leaves.
% The remaining nodes are all hidden. All nodes must have linear-Gaussian CPDs.
% The hidden nodes must be persistent, i.e., they must have children in
% the next time slice. In addition, they may not have any children within the current slice,
% except to the observed leaves. In other words, the topology must be isomorphic to a standard LDS.
%
% There are many derivations of the filtering and smoothing equations for Linear Dynamical
% Systems in the literature. I particularly like the following
% - "From HMMs to LDSs", T. Minka, MIT Tech Report, (no date), available from
%    ftp://vismod.www.media.mit.edu/pub/tpminka/papers/minka-lds-tut.ps.gz

[engine.trans_mat, engine.trans_cov, engine.obs_mat, engine.obs_cov, engine.init_state, engine.init_cov] = ...
    dbn_to_lds(bnet);

% This is where we will store the results between enter_evidence and marginal_nodes
engine.one_slice_marginal = [];
engine.two_slice_marginal = [];

engine = class(engine, 'kalman_inf_engine', inf_engine(bnet));


function [VecTot, TOT, time] = rmt(P, Vec, Mat)
%rmt returns a matrix containing the best models for all the paths of the Replacement Method.
%TOT contains all the relative results showing the evolution of the method.
%
%
%	   Input:
%             P             Property vector
%             Vec           Initial descriptors vector
%             Mat           Descriptors matrix with descriptors pool
%
%
%     Returns:
%
%            VecTot           vector containing the best model for all the
%                               paths of the Replacement Method
%            TOT               contains all the relative results
%                               showing the evolution of the method.
%
% Andrew G. Mercader, Pablo R. Duchowicz
% INIFTA, La Plata, Argentina
% Created: 5 March 2007

TOT=[];
VecTot=[];
time=cputime;
warning off

[k_v,n_v]=size(Vec);

for k=1:n_v

Sr=rms(P,Vec,Mat);
TOT(k).A(1,:)=[Sr,Vec];

VecA=rmsr(P, Vec, Mat, k);
Po(1)=k;
VecI=VecA;
if n_v==1
    VecTot=[1,VecI];
    TOT=VecI;
    time=cputime-time
    return
end
VecI(1)=[];
COEF=rmder(P,VecI,Mat);
COER=COEF;
COER(Po)=[];
pos=find(COEF==max(COER));
Po(2)=pos;
TOT(k).A(2,:)=VecA;
for i=2:n_v;
    VecA=rmsr(P, VecI, Mat, pos);
    VecI=VecA;
    TOT(k).A(i+1,:)=VecA;
    VecI(1)=[];
    COEF=rmder(P,VecI,Mat);
    if i==n_v
        Po=[];
        break
    end
    COER=COEF;
    COER(Po)=[];
    pos=find(COEF==max(COER));
    Po(i+1)=pos;
end

for j=1:2;
COER=COEF;
COER(Po)=[];
pos=find(COEF==max(COER));
Po(1)=pos;
for i=1:n_v;
    VecA=rmsr(P, VecI, Mat, pos);
    VecI=VecA;
    TOT(k).A(i+(j*n_v),:)=VecA;
    VecI(1)=[];
    COEF=rmder(P,VecI,Mat);
    if i==n_v
        Po=[];
        break
    end
    COER=COEF;
    COER(Po)=[];
    pos=find(COEF==max(COER));
    Po(i+1)=pos;
end
end

for j=3:100;
COER=COEF;
COER(Po)=[];
pos=find(COEF==max(COER));
Po(1)=pos;
    for i=1:n_v;
    VecA=rmsr(P, VecI, Mat, pos);
    VecI=VecA;
    TOT(k).A(i+(j*n_v),:)=VecA;
    VecI(1)=[];
    COEF=rmder(P,VecI,Mat);
    if i==n_v
        Po=[];
        break
    end
    COER=COEF;
    COER(Po)=[];
    pos=find(COEF==max(COER));
    Po(i+1)=pos;
    end
   if TOT(k).A(i+(j*n_v),:)==TOT(k).A(i+(j*n_v)-(2*n_v),:)
        Po=[];
        break
   end

end
VecP=find(TOT(k).A==min(TOT(k).A(:,1)));
VecTot(k,:)=[k,TOT(k).A(VecP(1),:)];
VecTot=sortrows(VecTot,2);
end
time=cputime-time
warning on

% % End of rmt
%


function [week, sow] = time2weektow(time)

% SYNTAX:
%   [week, sow] = time2weektow(time);
%
% INPUT:
%   time = GPS time (continuous since 6-1-1980)
%
% OUTPUT:
%   week = GPS week
%   sow  = GPS seconds-of-week
%
% DESCRIPTION:
%   Conversion from GPS time in continuous format (similar to datenum) to
%   GPS time in week, seconds-of-week.

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

sec_in_week = 7*86400;

sow  = rem(time, sec_in_week);
week = (time - sow) / sec_in_week;


function [im, kspFull] = pocs( ksp, iter, watchProgress )
%Partial-Fourier Reconstruction with POCS
%
% [im, kspFull] = pocs( kspIn, iter, watchProgr )
%
% === Input ===
%
%   kspIn:      Reduced Cartesian MRI Data-Set
%               Any dimension may be reduced,
%               but only one reduction dim. is allowed due to Physics/Math.
%
%               Allowed shapes for kspIn are...
%                 ... Ny x Nx
%                 ... Nc x Ny x Nx
%                 ... Nc x Ny x Nx x Nz
%
%               With Nc == number of receive Channels / Coils.
%
%               kspIn can either be a zero-padded array, so the partial Fourier property is obvious.
%               Or kspIn can be the measured data only, then we try to find k-space centre automagically
%               and create a zero-padded array with the full size, first.
%               Errors are however more likely to occur in the latter case.
%
%
%   iter:       No. of iterations
%   (optional)  default: iter = 20
%               Try on your own if larger iter improves your results!
%
%   watchProgr: true/false; Whether the progress of the reconstruction should
%   (optional)  be monitored in an image window.
%               In 3D data, only the central partition will be shown.
%
%
% === Output ===
%
%   im:         Reconstructed Images (channels not combined)
%
%   kspFull:    Reconstructed full k-space data
%
%
%
% === About the code ===
%
%   (1) We find out whether input data is
%       a) already zero-filled or
%       b) the pure asymmetric dataset, only
%
%       If b) is true, we zero-fill the data ourselves, which means we have to
%       determine the dimension first, in which the partial Fourier reduction was done.
%       We therefor find the position of the max. intensity in k-space which should
%       be identical to k-space centre. If the k-space centre is different from the
%       centre of the matrix, we know the partial Fourier dimension.
%       We then enlarge the matrix to its desired full size and fill the new part
%       with zeros.
%       If a) was true, finding the partial Fourier dimension is easy:
%       It is the dimension with all the zeros. :-)
%
%   (2) We create one low resolution image per channel/coil:
%
%       We need a symmetrically sampled part around the central k-space. Think of a
%       small stripe of phase encoding lines in the central k-space.
%       We only use these symmetric data (setting the rest zero) to reconstruct
%       low-resolution images. In order to avoid Gibbs-Ringing, a Hamming-filter
%       with the width of the stripe is multiplied with the data.
%       Additionally, all the fully sampled dimensions get a Hamming filter, too,
%       since we increase SNR, reduce further Gibbs-ringing and do not lose much
%       resolution.
%
%   (3) The phase of the low-resolution images is saved
%
%       POCS uses the fact that k-space data of real objects (no imaginary part)
%       have a point symmetry:
%           S(-k)  =  S*(k)      with k = (kx, ky, kz)
%       Our MRI objects are always complex, but we assume that phase variations
%       are due to coil sensitivities and B0-inhomgeneities,
%       which are both slowly varying (no high res. required).
%       Small-scale phase pertubations will decrease the reconstruction quality.
%
%   (4) Reference phase is applied in image space
%
%       We...
%       ... transform our zero-filled data to image space (IFFT)
%       ... remove the phase --> abs(image)
%       ... set the phase of our reference phase map --> image .* exp(1i.*phase)
%       ... transform back to k-space (FFT)
%       ... re-insert the measured data (self-consistency!)
%       ... goto "We..."
%
%       Iterating through the above steps fills the missing k-space points
%       with reasonable values.
%       If the phase varies slowly and there is no aliasing, this works very well.
%
% Aliasing artifacts are very challenging for POCS.
% So try to prevent aliasing in the first place (sufficient Field of View).

% =========================================================================
% Original code by Martin Blaimer
% * changed by Uvo Hoelscher
% * changed by Michael Völker
%        -- auto-detect PF dimension
%        -- auto-find centre point/line/partition
%        -- accept zerofilled or "pure" data
%        -- for multichannel or plain 2D data (single-channel)
%        -- 2D and 3D
%        -- error handling
%        -- comments, comments, comments
%        -- added option to monitor progress
%        -- moved code to seperate functions
%        -- smooth transition between acquired signal and
%           reconstructed data
%
% Problems? Suggestions?
%  --> michael.voelker@mr-bavaria.de
% =========================================================================

    % ( ===================================================================
    % Input Handling
    %
        if ~exist( 'ksp', 'var' ) || isempty(ksp) || ~isnumeric(ksp)
            error('pocs:input', 'First input must be Cartesian k-space data.')
        end
        if ~exist('iter','var') || isempty(iter) || numel(iter) ~= 1 || ~isnumeric(iter)
            iter = 20;
        end
        if ~exist('watchProgress','var')  || isempty(watchProgress) ||  numel(watchProgress) ~= 1 || ~isfinite(watchProgress)
            watchProgress = false;
        else
            watchProgress = logical( watchProgress );
        end

        Ndim = ndims( ksp );

        if Ndim > 4 || Ndim < 2
            error('pocs:shape','First input ''kspace'' should have one of these shapes:\n\n\t... Ny x Nx\n\t... Nc x Ny x Nx\n\t... Nc x Ny x Nx x Nz')
        end
        if Ndim == 2    % Ny x Nx
            ksp = reshape( ksp, [1 size(ksp)] );    %  1 x Ny x Nx  --> now we have one channel...
            wasAddedCoilDim = true;
            Ndim = 3;
        else
            wasAddedCoilDim = false;
        end

        % read the properties of the data
        sz   = size( ksp );
        sz   = sz(2:end);           % the (k-)spatial size of the array (i.e. without channels)
        prec = class( ksp );        % single or double precision?
    % ) ===================================================================

    % First: Check the sampling pattern (which parts of input are actually data?)
    smplPtrn = reshape( sum(abs(ksp),1) ~= 0, sz);        % Ny x Nx x Nz

    % ( ===================================================================
    % If input data is not yet zero-filled, do it here
    %
        if nnz(smplPtrn) == numel(smplPtrn)     % only the sampled data were passed / |N|umber of |N|on |Z|ero elements

            [ ksp, pfDim, isUpper, isLower, Nsmp ] = zerofillPFdim( ksp, wasAddedCoilDim );

            sz = size( ksp );
            sz = sz(2:end);     % ignore channels
        else
            [ pfDim, isUpper, isLower, Nsmp ] = detectPFdim( smplPtrn, wasAddedCoilDim );
        end
        clear  smplPtrn
    % ) ===================================================================

    if numel(sz) < 3
        sz(3) = 1;
    end
    Ny = sz(1);
    Nx = sz(2);
    Nz = sz(3);

    % ( ===================================================================
    % Handle ugly problems.
    %
        if ~isUpper && ~isLower
            error('pocs:UnknownErrorFound', 'I thought we are partial Fourier, but things seem to make no sense... :-(')
        end
    % ) ===================================================================

    % =====================================================================
    %
    %        We can now be sure to operate with zero-padded data.
    %
    % =====================================================================

    % initialize a cell of subscripts
    subs = { ':', ':', ':', ':' };      % all channels / all Ny / all Nx / all Nz

    % If the first entries are zero-filled (instead of the trailing ones),
    % flip the entries so we can treat them as if we pf'ed the first half of kspace.
    if isLower
        subs{pfDim+1} = sz(pfDim):-1:1;     % ...esreveR
        ksp           = ksp(subs{:});       % !ecaps-k si sihT
        subs{pfDim+1} = 1:sz(pfDim);        % lalala, we didn't do anything...
    end

    % Find out which point is in the centre and which indices belong to the
    % symmetrically sampled part of k-space.
    [ centreLine, idxSym ] = findSymSampled( ksp, pfDim, Nsmp );

    szSym = numel( idxSym );                % 2 * (Nsmp - centreLine) + 1

    if isUpper
        fprintf('Using %g points around point %g\n', szSym,    centreLine    );
    else
        fprintf('Using %g points around point %g\n', szSym, sz(pfDim)-centreLine+1 );
    end

    % ( ===================================================================
    % build up a symmetric low-pass filter
    %
    filter = cast( 1, prec );
    for d = 1:Ndim-1

        reshRule = ones(1,Ndim);    % how the filter will be reshaped

        if d ~= pfDim   % Each standard dimension gets a simple low-pass filter

            filt1D = hamming( sz(d), 'periodic'  );

        else            % our partial Fourier dimension gets an extra nice filter

            % create a narrow filter and remove everything else
            filt1D          = zeros(sz(d), 1, prec);            % full-size filter
            tmp             = hann( szSym + 2, 'symmetric' );   % a very narrow window
            filt1D(idxSym)  = tmp(2:end-1);                     % cut out the zeros at the edges (we have data there!)

            % take a look:
            %figure, plot(filt1D)
        end

        % reshape the filter according to the dimension it represents
        reshRule(d+1) = sz(d);

        filt1D = reshape( filt1D, reshRule );
        filter = bsxfun( @times, filter, filt1D );      % iteratively build up a multidimensional filter
    end
    % ) ===================================================================

    % Apply the low-pass filter
    kspLowRes = bsxfun( @times, filter, ksp);
    clear  filt1D  filter  reshRule  idxSym

    % ( ===================================================================
    % prerequisites prior to the iteration loop
    %
    %  Set everything up here, do computations that you don't have
    %  to do in the loop, remove no longer needed variables...
    %
        % fftshift everything once before and after for-looping
        %  => less overhead during iteration
        ksp       = cmshiftnd(       ksp, [0  sz/2] );
        kspLowRes = cmshiftnd( kspLowRes, [0  sz/2] );

        % reorder arrays such that the fft-dimensions come first
        % => faster memory access
        ksp       = permute(       ksp, [2 3 4 1] );      % Ny x Nx x Nz x Nc
        kspLowRes = permute( kspLowRes, [2 3 4 1] );      %
        subs      = { subs{2}, subs{3}, subs{4}, subs{1} };

        % calc. initial image and the reference phase map
        im        =  fft( fft( fft( conj(ksp), [], 1), [], 2), [], 3);  % im's phase is wrong now, but we only want it's abs() to be correct
        phase     = ifft(ifft(ifft( kspLowRes, [], 1), [], 2), [], 3);
        phase     = exp(1i * angle(phase));

        % We use a trick in the loop to avoid using ifft (fft is faster).
        % We only need to calculate the factor 1/N ourselves, with N = prod(sz)
        phase = phase ./ prod(sz);      % 1/N is absorbed inside the phase array, once

        % create image with calculated phasemap from low res image
        im = abs(im) .* phase;

        % In the loop, we want to know where we have to copy the
        % measured data to, so we set the subscript of the pf dimension
        % accordingly.
        % We have to do this due to the ifftshift'ing above.
        tmp         = false( 1, sz(pfDim));
        tmp(1:Nsmp) = true;
        subs{pfDim} = find(ifftshift(tmp));

        % release RAM
        clear  tmp  kspLowRes

        % only keep the acquired data in memory
        ksp = ksp(subs{:});
    % ) ===================================================================

    % Helpers for pretty-printing:
    % Such a mess for such beautiful output!
    b = repmat('=',1,80);
    progress_str = 'starting POCS loop...';
    fprintf( '%s\n%s\n%s   %s', b, b(1), b(1), progress_str )
    edging = sprintf( '\n%s\n%s', b(1), b );
    fprintf( edging )

    % ( ===================================================================
    % iterative reconstruction POCS
    %
    tic
    for ii = double(~watchProgress) : iter

        if ii > 0

            % Fourier transform the image to k-space
            im = fft(fft(fft(  im  ,[],1),[],2),[],3);      % "im" is a really bad variable name now
                                                            % but we save a lot of RAM with this
            % Data Consistency:
            % insert original data where we have them
            im(subs{:}) = ksp;                              % "im" is still our reconstructed k-space signal

            % Fourier transform into image domain
            im = conj( im );
            im = fft(fft(fft(  im  ,[],1),[],2),[],3);      % Now, "im" is an image again.

            % create image with calculated phasemap from low res image
            im = abs(im) .* phase;

            prevLength = numel(progress_str) + numel(edging);
            t = toc;
            ETA = (t./ii) * iter  - t;
            progress_str = sprintf( 'Iteration %g/%g, in %g s,  ETA: %g s...', ii, iter, t, ETA );

            fprintf([repmat('\b',1,prevLength) '%s' '%s'], progress_str, edging );

        end % if ii > 0

        % a rough way to monitor the progress
        %
        if watchProgress
            tmp = ifftshift(sqrt(sum(abs(im(:,:,1,:).^2),4)));      % due to fftshift(), the 1st partition is the central one
            maxRange = sort( tmp(:), 'descend' );
            maxRange = maxRange( ceil(0.05 * numel(maxRange)) );    % ignore the "hottest" 5%

            if ~exist('pic','var')
                pic = [tmp tmp zeros(size(tmp),prec)];
                diffScale = 1;
            else
                delta = abs( pic(:,Nx+(1:Nx)) - tmp );
                diffScale = 0.5 * maxRange / median( delta(:) );
                pic(:,  Nx+(1:Nx)) = tmp;
                pic(:,2*Nx+(1:Nx)) = diffScale * delta;
                clear delta
            end

            figure(999)
            imagesc( pic, [0    maxRange ] )
            title(sprintf('\\bfiteration %g\ninitial     |    current     |     abs(previous - current) × %g', ii, diffScale ))
            axis image
            colormap(gray(256))
            drawnow
            clear tmp

            %if Nz == 1          % little pause for 2D (too fast otherwise)
            %    pause(2 / iter)
            %end
        end

    end % for ii = 1:iter
    fprintf([repmat('\b',1, numel(progress_str) + numel(edging)) 'POCS done! (%g s)' '%s\n\n'], t, edging );
    % ) ===================================================================

    clear  phase  pic

    % ( ===================================================================
    % The main part is over. Time for some thoughts.
    %
    % We began with a dataset that had fewer data samples than would be
    % necessary for an unambiguous image reconstruction. As a consequence,
    % an infinite number of images corresponds to the acquired data.
    % The above iteration picks that single image whose abs() fits the data
    % AND whose phase corresponds to the low-resolution phase, obtained
    % using the symmetric part of the data.
    %
    % Viewed in k-space, there is almost always a severe edge at the border
    % between acquired and interpolated data, which is due to imperfections
    % in the assumptions made.
    % Namely, phase often has some high frequency components which cannot be
    % accounted for in the low-resolution map. Additionally, there is noise
    % and we may have changing contrast or trajectory errors in our MRI
    % sequence.
    %
    %         ^
    %         | A A A A A A A A   \
    %         | A A A A A A A A
    %         | A A A A A A A A     acquired signal
    %      k2 | A A A A A A A A
    %         | A A A A A A A A   /
    %         | I I I I I I I I   \
    %         | I I I I I I I I     interpolated data
    %         | I I I I I I I I   /
    %         ----------------->
    %                 k1
    %
    % Empirically, it should be wise to create a smoother transition from
    % the acquired part of the signal to the interpolated data.
    %
        Ntrans = floor( (szSym-1)/3 );      % width of the transition zone

        % Create subscripts where we intend to keep the measured data, only.
        tmp                 = false( 1, sz(pfDim));
        tmp(1:Nsmp-Ntrans)  = true;
        subsPure            = subs;
        subsPure{pfDim}     = find(ifftshift(tmp));

        % Create subscripts where we want to have a smooth transition between
        % measured and phase-corrected data.
        subsTrans           = subs;
        subsTrans{pfDim}    = setdiff( subs{pfDim}, subsPure{pfDim} );

        % build a filter for the transition:
        tmp         = hann( 2*Ntrans+3, 'symmetric');
        filterTrans = tmp( Ntrans+3 : end-1 );
        filterTrans = reshape( filterTrans, [ ones(1,pfDim-1)  Ntrans  1] );

        % Seperate data in unfiltered part and transition zone.
        tmp = zeros( size(im), prec );
        tmp(subs{:}) = ksp;
        kspPure  = tmp(subsPure{:});
        kspTrans = tmp(subsTrans{:});
        clear  tmp  ksp

        im = fft(fft(fft(  im  ,[],1),[],2),[],3);      % "im" becomes k-space signal, again

        im(subsPure{:}) = kspPure;                      % strict data consistency for Nsmp-Ntrans samples
        im(subsTrans{:}) =  bsxfun( @times,   filterTrans,  kspTrans         )     ...
                          + bsxfun( @times, 1-filterTrans,  im(subsTrans{:}) );

        clear  subsPure  subsTrans  filterTrans  kspPure  kspTrans

        if nargout > 1
            kspFull = im;
        else
            kspFull = double.empty([sz 0]);     % kspFull exists, but no memory required
        end

        im = ifft(ifft(ifft(  im  ,[],1),[],2),[],3);   % "im" is an image, again
    % ) ===================================================================

    % ( ===================================================================
    % Undo the prerequisites (--> postrequisites???)
    %
        % undo the permutations
        im      = permute(      im, [4 1 2 3] );
        kspFull = permute( kspFull, [4 1 2 3] );
        subs    = { subs{4}, subs{1}, subs{2}, subs{3} };

        % undo the fftshifts
        im      = cmshiftnd(      im, [0  sz/2] );
        kspFull = cmshiftnd( kspFull, [0  sz/2] );

        % undo flipping
        if isLower
            subs{pfDim+1} = sz(pfDim):-1:1;
            im            = im(subs{:});
            kspFull       = kspFull(subs{:});
        end
    % ) ===================================================================

    if wasAddedCoilDim                              % we initially reshaped a simple 2D raw data matrix to be of size 1 x Ny x Nx
        im      = reshape(      im, Ny, Nx, [] );
        kspFull = reshape( kspFull, Ny, Nx, [] );
    end

end     % of pocs()

% =========================================================================
%                                                                         =
%                      SWAPPED CODE                                       =
%                                                                         =
% =========================================================================

function [ ksp, pfDim, isUpper, isLower, Nsmp ] = zerofillPFdim( ksp, wasAddedCoilDim )
    % Only the acquired data were passed and we have to find the asymmetric
    % dimension. Then we increase the size along this dimension and pad with 0.

    Ndim    = ndims( ksp ) - 1;     % one dimension was for the channels
    sz      = size(  ksp );
    sz      = sz(  2:end );         % ignore channel dimension
    Nc      = size(  ksp, 1 );
    prec    = class( ksp );

    % init some helper variables
    pfDim    = 0;               % partial Fourier reduction dimension
    isUpper  = false;
    isLower  = false;
    isPartialFourier = false(Ndim,1);

    % ( ===============================================================
    % autodetect the Partial Fourier dimension
    %
    for d = 1:Ndim

        centre = floor( sz(d)/2 ) + 1;

        tmp = squeeze( sum(abs(ksp),1) );
        for d2 = 1:Ndim
            if d2 ~= d
                tmp = max(tmp,[],d2);       % keep only the maximum of non-partial data points
            end
        end
        [ dummy, maxPos(d) ] = max( tmp(:) );   %#ok <-- don't use "~", for compatibility

        if abs(maxPos(d) - centre) >= 2      % significant asymmetry ==> partial Fourier acquisition

            isPartialFourier(d) = true;
            pfDim = d;
            Nsmp = sz(d);

            isUpper = maxPos(d) > centre;   % Did we sample the upper matrix part, so the lower part is missing...
            isLower = maxPos(d) < centre;   % ... or are the first data points missing (e.g. asymmetric echo)?
        end
    end % for d = 1:Ndim
    %
    % ) ===== (PF dim detection) ======================================

    switch nnz(isPartialFourier)    % |N|umber of |N|on |Z|ero elements
        case 0
            error( 'pocs:NoPfDim', 'No partial Fourier dimension found.' )
        case 1
            fprintf( 'Found partial Fourier along array dimension %d\n', pfDim + ~wasAddedCoilDim )
        otherwise
            error( 'pocs:TooManyPfDims', 'Partial Fourier only allowed in 1 dimension, but %g were found!', nnz(isPartialFourier) )
    end

    if pfDim == 0   % our init value above
        error('zerofillPF:NoPF','No partial Fourier property found!')
    end

    % initialize a cell of subscripts
    subs = { ':', ':', ':', ':' };      % all channels / all Ny / all Nx / all Nz

    c = maxPos(pfDim);

    if isUpper

        sz(pfDim) = 2 * (c - mod(c,2));         % determine the blown-up size we want to achieve
        subs{pfDim+1} = 1:Nsmp;

    elseif  isLower

        sz(pfDim) = 2 * (Nsmp - c + 1);
        c = floor( sz(pfDim)/2 ) + 1;
        sz(pfDim) = sz(pfDim) + 2*~mod(c,2);    % A hack for Stefan's data... keep an eye on this!
        subs{pfDim+1} = (1:Nsmp) + (sz(pfDim)-Nsmp);

    else
        error( 'zerofillPF:PFdimNotClassified', 'Could not tell how partial Fourier was implemented.' )
    end

    % do the zerofilling
    tmp = zeros( [Nc sz], prec );
    tmp(subs{:}) = ksp;
    ksp = tmp;

end % of zerofillPFdim()

function [ pfDim, isUpper, isLower, Nsmp ] = detectPFdim( smplPtrn, wasAddedCoilDim )
    % User passed already zero-padded data. This was nice, now it's easy
    % to find the partial Fourier dimension!

    Ndim = ndims( smplPtrn );
    sz = size( smplPtrn );

    % init some helper variables
    pfDim    = 0;               % partial Fourier reduction dimension
    isUpper  = false;
    isLower  = false;
    isPartialFourier = false( Ndim, 1 );

    % ( ===============================================================
    % Determine if this is a zerofilled partial Fourier measurement
    % and along which dimension the data is reduced.
    %
    % smplPtrn in Partial Fourier looks like this:
    %
    %     ^
    %     | 1 1 1 1 1 1 1 1     --->  sampling pattern is the same
    %     | 1 1 1 1 1 1 1 1           for all k1 points
    %     | 1 1 1 1 1 1 1 1
    %  k2 | 1 1 1 1 1 1 1 1             i.e. for programming:
    %     | 1 1 1 1 1 1 1 1             smplPtrn == repmat( smplPtrn(:,1,1), [1 Nx Nz] )
    %     | O O O O O 0 0 0
    %     | O O O O O 0 0 0
    %     | O O O O O 0 0 0
    %      ----------------->
    %           k1
    %
    for d = 1:Ndim

        subs = { ones(1,sz(d)),     ... % initialize a cell of subscripts we might be interested in
                 ones(1,sz(d)),     ...
                 ones(1,sz(d))  };
        subs{d} = 1:sz(d);              % we ask for all entries in the d'th dimension

        idx_d = sub2ind( sz, subs{:} ); % convert to linear array indices

        oneCol = smplPtrn( idx_d );     % one column of the d'th dimension

        % create a rule how to reshape oneCol
        reshRule = ones(1,Ndim);
        reshRule(d) = sz(d);                    % e.g. reshRule = [   1   1 128 ]
        oneCol = reshape( oneCol, reshRule);

        % create a rule how to replicate oneCol
        repRule = sz;
        repRule(d) = 1;                         % e.g. repRule  = [ 256 256   1 ]

        % Check if we get the sampling pattern again
        % just by replicating oneCol along the other dimensions
        isPartialFourier(d) = isequal( smplPtrn, repmat( oneCol, repRule ) );

        if isPartialFourier(d)
            pfDim = d;
            Nsmp  = nnz( oneCol );      % how many fully sampled lines do we have?

            % Sampled upper or lower part of k-space matrix?
            isUpper = isequal( oneCol(:).', [ true( 1,Nsmp)          false(1,sz(d)-Nsmp)    ]);
            isLower = isequal( oneCol(:).', [ false(1,sz(d)-Nsmp)    true( 1,Nsmp)          ]);
        end
    end
    % ) ===============================================================

    switch nnz(isPartialFourier)    % |N|umber of |N|on |Z|ero elements
        case 0
            error( 'pocs:NoPfDim', 'No partial Fourier dimension found.' )
        case 1
            fprintf( 'Found partial Fourier along array dimension %d\n', pfDim + ~wasAddedCoilDim )
        otherwise
            error( 'pocs:TooManyPfDims', 'Partial Fourier only allowed in 1 dimension!' )
    end

end % of detectPFdim()

function [ centreLine, idxSym ] = findSymSampled( ksp, pfDim, Nsmp )

    Ndim = ndims( ksp ) - 1;    % one for channels
    sz = size( ksp );
    sz = sz(2:end);

    % autodetect the central k-space line
    %if ~exist('centreLine', 'var') || isempty(centreLine)
        tmp = squeeze( sum(abs(ksp),1) );
        for d = 1:Ndim
           if d ~= pfDim
               tmp = max(tmp,[],d);     % keep only the maximum of non-partial data points
           end
        end
        [ dummy, centreLine] = max( tmp(:) );   %#ok the central line has the max intensity
    %end

    % calculate the size of the symmetric part and the full dataset
    startSym = centreLine - (Nsmp - centreLine);    % start of our symmetric sampling
    endSym   = centreLine + (Nsmp - centreLine);    % end of symmetric part
    idxSym   = startSym : endSym;

    if any(idxSym < 1)    ||   any(idxSym > sz(pfDim))
       error( 'pocs:BadDataProperty' , 'Symmetric part of k-space out of bounds.\nThe maximum k-space intensity is at index %g whereas it should be centred => near %g.\nThe way, zerofilling was done is probably wrong.\nCheck your input k-space.', centreLine, round(sz(pfDim)/2) )
    end

end % of findSymmetricSampled()

function x = cmshiftnd( x, shifts)
%Function to circularly shift N-D arrays

    if nargin < 2 || all(shifts(:) == 0)
       return                       % no shift
    end

    sz      = size( x );
    numDims = ndims(x);             % number of dimensions
    idx = cell(1, numDims);         % creates cell array of empty matrices,
                                    % one cell for each dimension

    for k = 1:numDims

        m = sz(k);
        p = ceil(shifts(k));

        if p < 0
            p = m + p;
        end

        idx{k} = [p+1:m  1:p];
    end

    % Use comma-separated list syntax for N-D indexing.
    x = x(idx{:});

end % of cmshiftnd()

% Avoid the need for the signal toolbox and implement
% hamming() and hann() manually:
%
function w = hamming( N, symFlag )
%Hamming window
%
% w = hamming(L) returns an L-point symmetric Hamming window in the column vector w.
% L should be a positive integer.
%
%  The coefficients of a Hamming window are computed from the following equation:
%
%       w(n) = 0.54  +  0.46 * cos(2*pi*n/N),   0 <= n <= N
%
%
% w = hamming( L, 'symFlag') returns an L-point Hamming window using the window sampling
% specified by 'symFlag', which can be either 'periodic'  or 'symmetric' (the default).
% The 'periodic' flag is useful for DFT/FFT purposes, such as in spectral analysis.
% The DFT/FFT contains an implicit periodic extension and the periodic flag enables a signal
% windowed with a periodic window to have perfect periodic extension.
% When 'periodic' is specified, hamming computes a length L+1 window and returns the first L points.
% When using windows for filter design, the 'symmetric' flag should be used.
%
% --> http://www.mathworks.de/de/help/signal/ref/hamming.html
% --> https://de.wikipedia.org/wiki/Hamming-Fenster

% implemented by Michael.Voelker@mr-bavaria.de, 2012

    if ~exist( 'N', 'var' ) || isempty(N) || numel(N) ~= 1 || ~isnumeric(N)  || ~isfinite(N) || N < 1 || floor(N) ~= N
        error( 'hamming:badSize', 'Window lenght must be a positive integer.' )
    end
    if ~exist( 'symFlag', 'var' ) || isempty(symFlag)
        symFlag = 'symmetric';
    end

    if N == 1
        w = 1;
        return
    end

    switch symFlag
        case 'symmetric'
            L = N-1;
        case 'periodic'
            L = N;
        otherwise
            error('hamming:symFlag', 'Unknown symmetry flag. Try ''symmetric'' (default) or ''periodic''.')
    end

    w = (0:N-1) - L/2;
    w = 0.54  +  0.46 * cos(2*pi * w(:)./L);

end % of hamming()

function w = hann( N, symFlag )
%von-Hann (Hanning) window
%
% w = hann(L) returns an L-point symmetric Hann window in the column vector w.
% L must be a positive integer.
%
% The coefficients of a Hann window are computed from the following equation:
%
%      w(n) = 0.5 * (1 + cos(2*pi*n/N)),   0 <= n <= N
%
% The window length is L = N+1.
%
% w = hann(L,'sflag') returns an L-point Hann window using the window sampling specified by 'sflag',
% which can be either 'periodic' or 'symmetric' (the default). The 'periodic' flag is useful for DFT/FFT purposes,
% such as in spectral analysis.
% The DFT/FFT contains an implicit periodic extension and the periodic flag enables a signal windowed
% with a periodic window to have perfect periodic extension.
% When 'periodic' is specified, hann computes a length L+1 window and returns the first L points.
% When using windows for filter design, the 'symmetric' flag should be used.
%
% --> http://www.mathworks.de/de/help/signal/ref/hann.html
% --> https://de.wikipedia.org/wiki/Hann-Fenster

% implemented by Michael.Voelker@mr-bavaria.de, 2012

    if ~exist( 'N', 'var' ) || isempty(N) || numel(N) ~= 1 || ~isnumeric(N)  || ~isfinite(N) || N < 1 || floor(N) ~= N
        error( 'hann:badSize', 'Window lenght must be a positive integer.' )
    end
    if ~exist( 'symFlag', 'var' ) || isempty(symFlag)
        symFlag = 'symmetric';
    end

    if N == 1
        w = 1;
        return
    end

    switch symFlag
        case 'symmetric'
            L = N-1;
        case 'periodic'
            L = N;
        otherwise
            error('hann:symFlag', 'Unknown symmetry flag. Try ''symmetric'' (default) or ''periodic''.')
    end

    w = (0:N-1) - L/2;
    w = 0.5 * ( 1 + cos(2*pi * w(:)./L) );

end % of hann()


function linpack_z_test18 ( )

%*****************************************************************************80
%
%% TEST18 tests ZPBDI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 3;
  m = 1;
  lda = m+1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST18\n' );
  fprintf ( 1, '  For a double precision complex (C)\n' );
  fprintf ( 1, '  positive definite hermitian band matrix (PB),\n' );
  fprintf ( 1, '  ZPBDI computes the determinant as\n' );
  fprintf ( 1, '    det = MANTISSA * 10**EXPONENT\n' );
  fprintf ( 1, '\n' );
%
%  Set the value of the superdiagonal and diagonal.
%
  a(1,1) = complex ( 0.0000,  0.0000 );
  a(1,2) = complex ( 2.1341, -0.2147 );
  a(1,3) = complex ( 2.0905,  1.1505 );

  a(2,1) = complex ( 4.5281,  0.0000 );
  a(2,2) = complex ( 5.0371,  0.0000 );
  a(2,3) = complex ( 4.7638,  0.0000 );

  [ a, info ] = zpbfa ( a, lda, n, m );

  if ( info ~= 0 )
    fprintf ( 1, '  Error!  ZPBFA returns INFO = %d\n', info );
    return
  end

  det = zpbdi ( a, lda, n, m );

  fprintf ( 1, '  Determinant = %f * 10^(%f)\n', det(1), det(2) );

  return
end


% em_test_setup.m
%
% create sample image, system matrix, and sinograms for examples
% and testing of Poisson emission maximum likelihood (ML) algorithms
% creates: ig sg xtrue G proj ci ytrue ri yi
%
% Copyright Jan 1998, Jeff Fessler, University of Michigan

% true emission image
if ~isvar('xtrue'), printm 'xtrue'
	if ~isvar('ig')
		ig = image_geom('nx', 64, 'ny', 60, 'fov', 500);
	end
	xtrue = read_zubal_emis('nx', ig.nx, 'ny', ig.ny);
	mumap = read_zubal_attn('nx', ig.nx, 'ny', ig.ny);
	im plc 2 3
	im(1, xtrue, 'emission image'), cbar
	im(2, mumap, 'attenuation map'), cbar

	% reconstruction mask (which pixels do we estimate?)
	ig.mask = ig.circ(220, 180) > 0;
	im(3, ig.mask + xtrue, 'support mask + xtrue')
end

% system matrix G
if ~isvar('G'), printm 'system'
	sg = sino_geom('par', 'nb', ig.nx+2, 'na', ig.ny*3/2, ...
		'dr', 528 / (ig.nx+2));

	% simple strip-integral system model
	G = Gtomo2_strip(sg, ig, 'single', 1);

	if isvar('f.wtf') && has_mex_jf
		if exist(f.wtf), delete(f.wtf), end
		wtf_write(f.wtf, G, ig.nx, ig.ny, sg.nb, sg.na);
	end
	if isvar('f.wtr') && has_mex_jf && has_aspire
		if exist(f.wtr), delete(f.wtr), end
		os_run(sprintf('wt -chat 0 col2row %s %s', f.wtr, f.wtf))
	end
end

% noisy measurements
if ~isvar('yi'), printm 'data yi'
	proj = G * xtrue;
	li = G * mumap;
	printm('Maximum line integral = %g', max(li(:)))
	if ~isvar('f.count'), f.count = 1e5; end
	% detector efficiency variations per CTI 931 PET scanner
	ci = exp(0.3 * randn(size(proj)));
	ci = ci .* exp(-li);
	ci = f.count / sum(ci(:) .* proj(:)) * ci;
	ci = dsingle(ci);
	ytrue = ci .* proj;
	if ~isvar('f.randpercent')
		f.randpercent = 10;
	end
	ri = f.randpercent / 100 * mean(ytrue(:)) * sg.ones;
	ri = dsingle(ri);
	rng(0)
	yi = poisson(ytrue + ri);

	im(4, ytrue, 'ytrue: true projections'), cbar
	im(5, yi, 'yi: noisy projections'), cbar
	clear ytrue proj
end

% FBP reconstruction
if ~isvar('xfbp'), printm 'fbp'
	xfbp = em_fbp(sg, ig, yi, ci, ri);
	xfbp = max(xfbp, 0);
	im(6, xfbp, 'FBP Reconstruction'), cbar
end

% save to files if needed
if isvar('f.yi')
	fld_write(f.yi, yi, 'check', 0)
end
if isvar('f.ci')
	fld_write(f.ci, ci, 'check', 0)
end
if isvar('f.ri')
	fld_write(f.ri, ri, 'check', 0)
end
if isvar('f.mask')
	fld_write(f.mask, ig.mask, 'check', 0)
end


function y = qinvjmul(labx,frmx,b,K)
% y = qinvjmul(labx,frmx,b,K)
%
% QINVJMUL  Inverse of Jordan multiply for Lorentz blocks
%
% **********  INTERNAL FUNCTION OF SEDUMI **********
%
% See also sedumi

% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA

lorN = length(K.q);
if lorN == 0
    y = zeros(0,1);
    return
end
if length(labx) > 2*lorN
    labx = labx(K.l+1:K.l+2*lorN);
end
detx = labx(1:lorN) .* labx(lorN+1:end);
x = qframeit(labx,frmx,K);
ix = K.mainblks;
if length(b) == ix(3)-ix(1);   % lorentz only ?
    ix = (1-ix(1)) + ix;
end
% ------------------------------------------------------------
% Let y1(k) = xk'Jbk/(sqrt2*detxk)
% ------------------------------------------------------------
y1 = x(1:lorN).*b(ix(1):ix(2)-1) - ddot(x(lorN+1:end),b,K.qblkstart);
y1 = y1./(sqrt(2)*detx);
% ------------------------------------------------------------
% Let y2[k] = (sqrt2/x1)*b2[k] - (y1/x1) * x2[k]
% ------------------------------------------------------------
y = [y1; qblkmul(sqrt(2)./x(1:lorN),b,K.qblkstart)...
    - qblkmul(y1./x(1:lorN),x(lorN+1:end),K.qblkstart)];


function [L,S,errHist] = solver_RPCA_constrained(AY,lambda_S, tau, A_cell, opts)
% [L,S,errHist] = solver_RPCA_constrained(Y,lambda_S, tau, A_cell, opts)
% Solves the problem
%   minimize_{L,S} .5|| L + S - Y ||_F^2
%   subject to
%   if opts.sum = true
%       (1)  ||L||_* + lambda_S ||S||_1 <= tau
%   if opts.max = true
%       (2) max(  ||L||_* , lambda_S ||S||_1 ) <= tau
%
%   if opts.max and opts.sum are false and tau is a negative number, then
%   we solve the problem:
%       minimize_{L,S} .5|| L + S - Y ||_F^2  + abs(tau)*( ||L||_* + lambda_S ||S||_1 )
%   (but see solver_RPCA_Lagrangian.m for a simpler interface)
%
%   or if A_cell is provided, where A_cell = {A, At}
%   (A is a function handle, At is a function handle to the transpose of A)
%   then
%
%   minimize_{L,S} .5|| A(L + S) - Y ||_F^2
%       subject to ...
%   (here, Y usually represents A(Y); if Y is not the same size
%    as A(L), then we will automatically set Y <-- A(Y) )
%
%   errHist(:,1) is a record of the residual
%   errHist(:,2) is a record of the full objective (that is, .5*resid^2 )
%   errHist(:,3) is the output of opts.errFcn if provided
%
% opts is a structure with options:
%   opts.sum, opts.max  (as described above)
%   opts.L0         initial guess for L (default is 0)
%   opts.S0         initial guess for S (default is 0)
%   opts.size       [n1,n2] where L and S are n1 x n2 matrices. The size is automatically
%       determined in most cases, but when providing a linear operator
%       it may be necessary to provide an explicit size.
%   opts.tol        sets stopping tolerance
%   opts.maxIts     sets maximum number of iterations
%   opts.printEvery will print information this many iterations
%   opts.displayTime will print out timing information (default is true for large problems)
%   opts.errFcn     a function of (L,S) that records information
%   opts.trueObj    if provided, this will be subtracted from errHist(2,:)
%   opts.Lip        Lipschitz constant, i.e., 2*spectralNorm(A)^2
%                       by default, assume 2 (e.g., good if A = P_Omega)
%   opts.FISTA      whether to use FISTA or not. By default, true
%     opts.restart  how often to restart FISTA; set to -Inf to make it automatic
%   opts.BB         whether to use the Barzilai-Borwein spectral steplength
%     opts.BB_type  which BB stepsize to take. Default is 1, the larger step
%     opts.BB_split whether to calculate stepslengths for S and L independently.
%       Default is false, which is recommended.
%   opts.quasiNewton  uses quasi-Newton-like Gauss-Seidel scheme.
%                     Only available in "max" mode
%     opts.quasiNewton_stepsize     stepsize length. Default is .8*(2/Lip)
%     opts.quasinewton_SLS          whether to take S-L-S sequence (default is true)
%                                   otherwise, takes a L-S Gauss-Seidel sequence
%   opts.SVDstyle   controls what type of SVD is performed.
%       1 = full svd using matlab's "svd". Best for small problems
%       2 = partial svd using matlab's "svds". Not recommended.
%       3 = partial svd using PROPACK, if installed. Better than option 2, worse than 4
%       4 = partial svd using randomized linear algebra, following
%           the Halko/Tropp/Martinnson "Structure in Randomness" paper
%       in option 4, there are additional options:
%       opts.SVDwarmstart   whether to "warm-start" the algorithm
%       opts.SVDnPower  number of power iterations (default is 2 unless warm start)
%       opts.SVDoffset  oversampling, e.g., "rho" in Tropp's paper. Default is 5
%
%   opts.L1L2      instead of using l1 penalty, e.g., norm(S(:),1), we can
%       also use block norm penalties, such as (if opts.L1L2 = 'rows')
%       the sum of the l2-norm of rows (i.e., l1-norm of rows),
%       or if opts.L1L2='cols', the sum of the l2-norms of colimns.
%       By default, or if opts.L1L2 = [] or false, then uses usual l1 norm.
%       [Feature added April 17 2015]
%
%  Features that may be added later: [email developers if these are
%    important to you]
%       - Allow Huber loss function.
%
% Stephen Becker, March 6 2014. Edited March 14 2014, April 2015.
%   stephen.becker@colorad.edu
% See also solver_RPCA_Lagrangian.m, solver_RPCA_SPGL1.m

% todo: allow S >= 0 constraints, since this is easy
% todo: allow Huber loss function

error(nargchk(3,5,nargin,'struct'));
if nargin < 5, opts = []; end
% == PROCESS OPTIONS ==
function out = setOpts( field, default )
    if ~isfield( opts, field )
        opts.(field)    = default;
    end
    out = opts.(field);
    opts    = rmfield( opts, field ); % so we can do a check later
end

if nargin < 4 || isempty(A_cell)
    A   = @(X) X(:);
    [n1,n2] = size(AY);
    At  = @(x) reshape(x,n1,n2);

    if ~iscell(tau)
        AY  = A(AY);
    end
    % The factor of 2 is since we are in both L and S
else
    A   = A_cell{1};
    At  = A_cell{2};
    % Y could be either Y or A(Y)
    if size(AY,2) > 1
        % AY is a vector, so it is probably Y and not AY
        disp('Changing Y to A(Y)');
        AY = A(AY);
        [n1,n2] = size(AY); % April 24 '14
    else
        % April 24, '14: we need to know the (n1,n2)
        sz      = setOpts('size',[] );
        if isempty(sz)
            error('Cannot determine the size of the variables; please specify opts.size=[n1,n2]');
        end
        n1 = sz(1);
        n2 = sz(2);
    end
    %[n1,n2] = size(AY); % comment out April 24 '14
end
normAY =    norm(AY(:));

vec = @(X) X(:);

% Some problem sizes. Feel free to tweak. Mainly affect the defaults
SMALL   = ( n1*n2 <= 50^2 );
MEDIUM  = ( n1*n2 <= 200^2 ) && ~SMALL;
LARGE   = ( n1*n2 <= 1000^2 ) && ~SMALL && ~MEDIUM;
HUGE    = ( n1*n2 > 1000^2 );

% -- PROCESS OPTIONS -- (some defaults depend on problem size )
tol     = setOpts('tol',1e-6*(SMALL | MEDIUM) + 1e-4*LARGE + 1e-3*HUGE );
maxIts  = setOpts('maxIts', 1e3*(SMALL | MEDIUM ) + 400*LARGE + 200*HUGE );
printEvery  = setOpts('printEvery',100*SMALL + 50*MEDIUM + 5*LARGE + 1*HUGE);
errFcn      = setOpts('errFcn', [] );
Lip         = setOpts('Lip', 2 );
restart     = setOpts('restart',-Inf);
trueObj     = setOpts('trueObj',0);
sumProject  = setOpts('sum', false );
maxProject  = setOpts('max', false );

if tau < 0
	Lagrangian = true;
	if sumProject || maxProject
		error('in Lagrangian mode (when tau<0 significies lambda=|tau|), turn off sum/maxProject');
	end
	lambda = abs(tau);
	tau = []; % help us track down bugs
else
	Lagrangian = false;
	if (sumProject && maxProject) || (~sumProject && ~maxProject), error('must choose either "sum" or "max" type projection'); end
end

QUASINEWTON = setOpts('quasiNewton', maxProject || Lagrangian );
FISTA       = setOpts('FISTA',~QUASINEWTON);
BB          = setOpts('BB',~QUASINEWTON);
% Note: BB with FISTA is sometimesm not so good
if BB && FISTA, warning('solver_RPCA:convergence','Convergence not guaranteed with FISTA if opts.BB=true'); end
BB_split    = setOpts('BB_split',false);
BB_type     = setOpts('BB_type',1); % 1 or 2
stepsizeQN  = setOpts('quasiNewton_stepsize', .8*2/Lip );
S_L_S       = setOpts('quasiNewton_SLS', true );
displayTime = setOpts('displayTime',LARGE | HUGE );
SVDstyle    = setOpts('SVDstyle', 1*SMALL + 4*(~SMALL) ); % 1 is full SVD
% and even finer tuning (only matter if SVDstyle==4)
SVDwarmstart= setOpts('SVDwarmstart', true );
SVDnPower   = setOpts('SVDnPower', 1 + ~SVDwarmstart ); % number of power iteratiosn
SVDoffset   = setOpts('SVDoffset', 5 );
SVDopts = struct('SVDstyle', SVDstyle,'warmstart',SVDwarmstart,...
    'nPower',SVDnPower,'offset',SVDoffset );

if QUASINEWTON
    if sumProject
        error('Can not run quasi-Newton mode when in "sum" formulation. Please change to "max"');
    elseif FISTA
        error('Can not run quasi-Newton with FISTA');
    elseif BB
        error('Can not run quasi-Newton with BB');
    end
end

% April 17 2015
L1L2        = setOpts('L1L2',0);
if isempty(L1L2), L1L2=0; end
if L1L2,
    if ~isempty(strfind(lower(L1L2),'row')),  L1L2 = 'rows';
    elseif ~isempty(strfind(lower(L1L2),'col')),  L1L2 = 'cols';
        % so col, COL, cols, columns, etc. all acceptable
    else
        error('unrecognized option for L1L2: should be row or column or 0');
    end
end

projNuclear(); % remove any persistent variables
if maxProject
    project = @(L,S,varargin) projectMax(L1L2,tau,lambda_S,SVDopts, L,S);
elseif sumProject
    if any(L1L2), error('with opts.sum=true, need opts.L1L2=0'); end
    project = @(L,S,varargin) projectSum(tau,lambda_S,L,S);
elseif Lagrangian
    project = @(L,S,varargin) projectMax(L1L2,lambda,lambda*lambda_S,SVDopts, L,S, varargin{:});
end

L           = setOpts('L0',zeros(n1,n2) );
S           = setOpts('S0',zeros(n1,n2) );

% Check for extra options that were not processed
if ~isempty( fieldnames(opts ) )
    disp( 'warning, found extra guys in opts');
    disp( opts )
    error('Found unprocessed options in "opts"');
end

stepsize    = 1/Lip;
errHist     = zeros(maxIts, 2 + ~isempty(errFcn) );
Grad        = 0;
if FISTA || BB || QUASINEWTON
    L_old   = L;
    S_old   = S;
end
L_fista     = L;
S_fista     = S;
BREAK   = false;
kk      = 0; % counter for FISTA
timeRef = tic;
for k = 1:maxIts
    % Gradient in (L,S) (at the fista point) is (R,R) where...
    R   = A(L_fista + S_fista) - AY;
    Grad_old    = Grad;
    Grad        = At(R);

    objL        = Inf;
    if QUASINEWTON
%         stepsizeQN  = 1 - min(0.1, 1/k );
%         stepsizeQN = 1 - .3/sqrt(k);

        if S_L_S
            % we solve for S, update L, then re-update S
            % Exploits the fact that projection for S is faster
            dL      = L - L_old;
            S_old   = S;
            [~,S_temp]   = project( [], S - stepsizeQN*( Grad + dL ), stepsizeQN ); % take small step...

            dS      = S_temp - S_old;
            L_old   = L;
            [L,~,rnk,objL]   = project( L - stepsizeQN*( Grad + dS ), [], stepsizeQN );

            dL      = L - L_old;
            [~,S]   = project( [], S - stepsizeQN*( Grad + dL ) , stepsizeQN);
        else
            % Gauss-Seidel update, starting with L, then S
            % Changing order seemed to not work as well
            dS      = S - S_old;
            L_old   = L;
            [L,~,rnk,objL]   = project( L - stepsizeQN*( Grad + dS ), [] , stepsizeQN);
            dL      = L - L_old;
            S_old   = S;
            [~,S]   = project( [], S - stepsizeQN*( Grad + dL ) , stepsizeQN);
        end
%         stepsizeQN = (1+3*stepsizeQN)/4;
    else
        if BB && k > 1
            [stepsizeL, stepsizeS]  = compute_BB_stepsize( Grad, Grad_old, L, L_old, S, S_old, BB_split, BB_type);
            if isnan(stepsizeL) || isnan(stepsizeS)
                fprintf(2,'Warning: no BB stepsize possible since iterates have not changed!\n');
                [stepsizeL, stepsizeS]   = deal( stepsize );
            end
        else
            [stepsizeL, stepsizeS]   = deal( stepsize );
        end

        % Now compute proximity step
        if FISTA || BB
            L_old   = L;
            S_old   = S;
        end
        L           = L_fista - stepsizeL*Grad;
        S           = S_fista - stepsizeS*Grad;
        if any(isnan(L(:))) || any(isnan(S(:))), fprintf(2,'DEBUG!\n'); keyboard; end
        [L,S,rnk,objL]   = project( L, S, stepsizeL, stepsizeS);
        if any(isnan(L(:))) || any(isnan(S(:))), fprintf(2,'DEBUG!\n'); keyboard; end
    end

    DO_RESTART = false;
    if FISTA
        if k>1 && restart > 0 && ~isinf(restart) && ~mod(kk,restart)
            kk = 0;
            DO_RESTART  = true;
        elseif restart==-Inf && kk > 5
            % In this case, we restart if the function has significantly increased
            if (errHist(k-1,2)-errHist(k-5,2)) > 1e-8*abs(errHist(k-5,2))
                DO_RESTART = true;
                kk = 0;
            end
        end
        L_fista = L + kk/(kk+3)*( L - L_old );
        S_fista = S + kk/(kk+3)*( S - S_old );
        kk      = kk + 1;
    else
        L_fista = L;
        S_fista = S;
    end

%     res          = norm(R(:)); % this is for L_fista, not L
    R            = A(L + S) - AY;
    % sometimes we already have R pre-computed, so if this turns out to
    %   be a signficant computational cost we can make fancier code...
    res          = norm(R(:));
    errHist(k,1) = res;
    errHist(k,2) = 1/2*(res^2); % + lambda_L*objL + lambda_S*objS;
    if Lagrangian
        errHist(k,2) = errHist(k,2) + lambda*objL;
        if any(L1L2)
            if strcmpi(L1L2,'rows')
                errHist(k,2) = errHist(k,2) + lambda*lambda_S*sum( sqrt( sum(S.^2,2) ) );
            else
                errHist(k,2) = errHist(k,2) + lambda*lambda_S*sum( sqrt( sum(S.^2,1) ) );
            end
        else
            errHist(k,2) = errHist(k,2) + lambda*lambda_S*norm(S(:),1);
        end
    end
    if k > 1 && abs(diff(errHist(k-1:k,1)))/res < tol
        BREAK = true;
    end
    PRINT   = ~mod(k,printEvery) | BREAK | DO_RESTART;
    if PRINT
        fprintf('Iter %4d, rel. residual %.2e, objective %.2e', k, res/normAY, errHist(k,2) -trueObj);
    end
    if ~isempty(errFcn)
        err     = errFcn(L,S);
        errHist(k,3)    = err;
        if PRINT, fprintf(', err %.2e', err ); end
    end
    if ~isempty(rnk) && PRINT, fprintf(', rank(L) %3d', rnk ); end
    if PRINT
        fprintf(', sparsity(S) %5.1f%%', 100*nnz(S)/numel(S) );
    end
    if displayTime && PRINT
        tm = toc( timeRef ); timeRef = tic;
        fprintf(', time %.1f s', tm );
    end
    if DO_RESTART, fprintf(' [restarted FISTA]'); end
    if PRINT, fprintf('\n'); end
    if BREAK
        fprintf('Reached stopping criteria (based on change in residual)\n');
        break;
    end
end
if BREAK
    errHist = errHist(1:k,:);
else
    fprintf('Reached maximum number of allowed iterations\n');
end

end

% subfunctions for projection
function [L,S,rnk,nuclearNorm] = projectMax( L1L2, tau, lambda_S,SVDopts, L, S , stepsize, stepsizeS)
 if nargin >= 7 && ~isempty(stepsize)
     % we compute proximity, not projection
     tauL 	= -abs( tau*stepsize );
     if nargin < 8 || isempty( stepsizeS ), stepsizeS = stepsize; end
     tauS 	= -abs( lambda_S*stepsizeS );
 else
     tauL 	= abs(tau);
     tauS 	= abs(tau/lambda_S );
 end

 % We project separately, so very easy
 if ~isempty(L)
     [L,rnk,nuclearNorm]  = projNuclear(tauL, L,SVDopts);
     if tauL > 0
         % we did projection, so this should be feasible
         nuclearNorm = 0;
     end
 else
     rnk = [];
     nuclearNorm = 0;
 end

 if ~isempty(S)
     if tauS > 0
         if ~any(L1L2)
             % use the l1 norm
             projS  = project_l1(tauS);
         elseif strcmpi(L1L2,'rows')
             projS  = project_l1l2(tauS,true);
         elseif strcmpi(L1L2,'cols')
             projS  = project_l1l2(tauS,false);
         else
             error('bad value for L1L2: should be [], ''rows'' or ''cols'' ');
         end
         S  = projS( S );
     else
         if ~any(L1L2)
             % use the l1 norm
             % simple prox
             S = sign(S).*max(0, abs(S) - abs(tauS));
         elseif strcmpi(L1L2,'rows')
             projS  = prox_l1l2(abs(tauS));
             S      = projS(S,1);
         elseif strcmpi(L1L2,'cols')
             projS  = prox_l1l2(abs(tauS));
             S      = projS(S',1)';
         else
             error('bad value for L1L2: should be [], ''rows'' or ''cols'' ');
         end

     end
 end
end

function [X,rEst,nrm] = projNuclear( tau, X, SVDopts )
 % Input must be a matrix, not a vector
 % Computes either the proximity operator of the nuclear norm (if tau<0)
 % or projection onto the nuclear norm ball of radius tau (if tau>0)

 persistent oldRank Vold iteration
 if nargin==0, oldRank=[]; Vold = []; iteration = 0; return; end
 if isempty(oldRank), rEst = 10;
 else, rEst = oldRank + 2;
 end
 if isempty(iteration), iteration = 0; end
 iteration   = iteration + 1;
 [n1,n2]     = size(X);
 minN       = min( [n1,n2] ); % could set smaller to make nonconvex
 % For the first few iterations, we constrain rankMax
 switch iteration
     case 1
         rankMax     = round(minN/4);
     case 2
         rankMax     = round(minN/2);
     otherwise
         rankMax     = minN;
 end

 style = SVDopts.SVDstyle;
 if tau==0, X=0*X; return; end

 switch style
     case 1
         % full SVD
         [U,S,V] = svd(X,'econ');
         s   = diag(S);
         if tau < 0 % we do prox
             s = max( 0, s - abs(tau) );
         else
             s = project_simplex( tau, s );
         end
         tt      = s > 0;
         rEst    = nnz(tt);
         U       = U(:,tt);
         S       = diag(s(tt));
         V       = V(:,tt);
         nrm     = sum(s(tt));
     case {2,3,4}
         % 2: use Matlab's sparse SVD
         % 3: use PROPACK
         % 4: use Joel Tropp's randomized SVD
         if style==2
             opts = struct('tol',1e-4);
             if rankMax==1, opts.tol = min(opts.tol,1e-6); end % important!
             svdFcn = @(X,rEst)svds(X,rEst,'L',opts);
         elseif style==3
             opts = struct('tol',1e-4,'eta',eps);
             opts.delta = 10*opts.eta;
             % set eta to eps, but not 0 otherwise reorth is very slow
             if rankMax==1, opts.tol = min(opts.tol,1e-6); end % important!
             svdFcn = @(X,rEst)lansvd(X,rEst,'L',opts);
         elseif style == 4
             opts = [];
             if isfield(SVDopts,'nPower') && ~isempty( SVDopts.nPower )
                 nPower = SVDopts.nPower;
             else
                 nPower = 2;
             end
             if isfield(SVDopts,'offset') && ~isempty( SVDopts.offset )
                 offset = SVDopts.offset;
             else
                 offset = 5;
             end
             if isfield( SVDopts, 'warmstart' ) && SVDopts.warmstart==true ...
                     && ~isempty(Vold)
                 opts = struct( 'warmStart', Vold );
             end
             ell     = @(r) min([r+offset,n1,n2]); % number of samples to take
             svdFcn = @(X,rEst)randomizedSVD(X,rEst,ell(rEst),nPower,[],opts );
         end

         ok  = false;
         while ~ok
             rEst    = min( [rEst,rankMax] );
             [U,S,V] = svdFcn(X,rEst);
             s       = diag(S);
             if tau < 0
                 % we are doing prox
                 lambda = abs(tau);
             else
                 lambda  = findTau(s,tau);
             end
             ok      = ( min(s) < lambda ) || (rEst == rankMax);
             if ok, break; end
             rEst    = 2*rEst;
         end
         rEst = min( length(find(s>lambda)), rankMax );
         S   = diag( s(1:rEst) - lambda );
         U   = U(:,1:rEst);
         V   = V(:,1:rEst);
         nrm = sum( s(1:rEst) - lambda );
     otherwise
         error('bad value for SVDstyle');
 end
 if isempty(U)
     X = 0*X;
 else
     X = U*S*V';
 end
 oldRank = size(U,2);
 if isfield( SVDopts, 'warmstart' ) && SVDopts.warmstart==true
     Vold = V;
 end
end

function x = project_simplex( q, x )
 % projects onto the constraints sum(x)=q and x >= 0
 % Update: projects onto sum(x) <= q and x >= 0

 x     = x.*( x > 0 ); % March 11 '14
 % March 11, fixing bug: we want to project onto the volume, not
 %   the actual simplex (surface)
 if sum(x) <= q, return; end
 if q==0, x = 0*x; return; end

 s     = sort( x, 'descend' );
 if q < eps(s(1))
     % eps(x) is the distance from abs(x) to the next larger
     %   floating point number, i.e., in floating point arithmetic,
     %   x + eps(x)/2 = x
     % eps(1) is about 2.2e-16

     error('Input is scaled so large compared to q that accurate computations are difficult');
     % since then cs(1) = s(1) - q is  not even guaranteed to be
     % smaller than q !
 end
 cs    = ( cumsum(s) - q ) ./ ( 1 : numel(s) )';
 ndx   = nnz( s > cs );
 x     = max( x - cs(ndx), 0 );
end

function tau = findTau( s, lambda )
 % Returns the shrinkage value necessary to shrink the vector s
 %   so that it is in the lambda scaled simplex
 %   Usually, s is the diagonal part of an SVD
 if all(s==0)||lambda==0, tau=0; return; end
 if numel(s)>length(s), error('s should be a vector, not a matrix'); end
 if ~issorted(flipud(s)), s = sort(s, 'descend'); end
 if any( s < 0 ), error('s should be non-negative'); end

 % project onto the simplex of radius lambda (not tau)
 % and use this to find "tau" (the shrinkage amount)
 % If we know s_i > tau for i = 1, ..., k, then
 %   tau = ( sum(s(1:k)) - lambda )/k
 % But we don't know k, so find it:
 cs  = (cumsum(s) - abs(lambda) )./(1:length(s))';
 ndx = nnz( s > cs ); % >= 1 as long as lambda > 0
 tau = max(0,cs(ndx));
 % We want to make sure we project onto sum(sigma) <= lambda
 %   and not sum(sigma) == lambda, so do not allow negative tau

end

function [L,S,rnk,nuclearNorm] = projectSum( tau, lambda_S, L, S )
  [m,n]           = size(L);
  [U,Sigma,V]     = svd(L,'econ');
  s       = diag(Sigma);
  wts     = [ ones(length(s),1); lambda_S*ones(m*n,1) ];
  proj    = project_l1(tau,wts);
  sS      = proj( [s;vec(S)] );
  sProj   = sS(1:length(s));
  S       = reshape( sS(length(s)+1:end), m, n );
  L       = U*diag(sProj)*V';
  rnk     = nnz( sProj );
  nuclearNorm = sum(sProj);
end

function [stepsizeL, stepsizeS]  = compute_BB_stepsize( ...
    Grad, Grad_old, L, L_old, S, S_old, BB_split, BB_type)

  if ~BB_split
      % we take a Barzilai-Borwein stepsize in the full variable
      yk  = Grad(:) - Grad_old(:);
      yk  = [yk;yk]; % to account for both variables
      sk  = [L(:) - L_old(:); S(:) - S_old(:) ];
      if BB_type == 1
          % Default. The bigger stepsize
          stepsize    = norm(sk)^2/(sk'*yk);
      elseif BB_type == 2
          stepsize    = sk'*yk/(norm(yk)^2);
      end
      [stepsizeL, stepsizeS] = deal( stepsize );

  elseif BB_split
      % treat L and S variables separately
      % Doesn't seem to work well.
      yk  = Grad(:) - Grad_old(:);
      skL  = L(:) - L_old(:);
      skS  = S(:) - S_old(:);
      if BB_type == 1
          % Default. The bigger stepsize
          stepsizeL   = norm(skL)^2/(skL'*yk);
          stepsizeS   = norm(skS)^2/(skS'*yk);
      elseif BB_type == 2
          stepsizeL   = skL'*yk/(norm(yk)^2);
          stepsizeS   = skS'*yk/(norm(yk)^2);
      end
  end
end

function op = project_l1( q , d)
%PROJECT_L1   Projection onto the scaled 1-norm ball.
%    OP = PROJECT_L1( Q ) returns an operator implementing the
%    indicator function for the 1-norm ball of radius q,
%    { X | norm( X, 1 ) <= q }. Q is optional; if omitted,
%    Q=1 is assumed. But if Q is supplied, it must be a positive
%    real scalar.
%
%    OP = PROJECT_L1( Q, D ) uses a scaled 1-norm ball of radius q,
%    { X | norm( D.*X, 1 ) <= 1 }. D should be the same size as X
%    and non-negative (some zero entries are OK).

% Note: theoretically, this can be done in O(n)
%   but in practice, worst-case O(n) median sorts are slow
%   and instead average-case O(n) median sorts are used.
%   But in matlab, the median is no faster than sort
%   (the sort is probably quicksort, O(n log n) expected, with
%    good constants, but O(n^2) worst-case).
%   So, we use the naive implementation with the sort, since
%   that is, in practice, the fastest.

 if nargin == 0,
     q = 1;
 elseif ~isnumeric( q ) || ~isreal( q ) || numel( q ) ~= 1 || q <= 0,
     error( 'Argument must be positive.' );
 end
 if nargin < 2 || isempty(d) || numel(d)==1
     if nargin>=2 && ~isempty(d)
         % d is a scalar, so norm( d*x ) <= q is same as norm(x)<=q/d
         if d==0
             error('If d==0 in proj_l1, the set is just {0}, so use proj_0');
         elseif d < 0
             error('Require d >= 0');
         end
         q = q/d;
     end
     op = @(varargin)proj_l1_q(q, varargin{:} );
 else
     if any(d<0)
         error('All entries of d must be non-negative');
     end
     op = @(varargin)proj_l1_q_d(q, d, varargin{:} );
 end

 % This is modified from TFOCS, Nov 26 2013, Stephen Becker
 % Note: removing "v" (value) output from TFOCS code
 %   Also removing extraneous inputs
    function x = proj_l1_q( q, x, varargin )
        myReshape   = @(x) x; % do nothing
        if size(x,2) > 1
            if ndims(x) > 2, error('You must modify this code to deal with tensors'); end
            myReshape     = @(y) reshape( y, size(x,1), size(x,2) );
            x   = x(:); % make it into a vector
        end
        s      = sort(abs(nonzeros(x)),'descend');
        cs     = cumsum(s);
        % ndx    = find( cs - (1:numel(s))' .* [ s(2:end) ; 0 ] >= q, 1 );
        ndx    = find( cs - (1:numel(s))' .* [ s(2:end) ; 0 ] >= q+2*eps(q), 1 ); % For stability
        if ~isempty( ndx )
            thresh = ( cs(ndx) - q ) / ndx;
            x      = x .* ( 1 - thresh ./ max( abs(x), thresh ) ); % May divide very small numbers
        end
        x   = myReshape(x);
    end

% Allows scaling. Added Feb 21 2014
    function x = proj_l1_q_d( q, d,  x, varargin )
        myReshape   = @(x) x; % do nothing
        if size(x,2) > 1
            if ndims(x) > 2, error('You must modify this code to deal with tensors'); end
            myReshape     = @(y) reshape( y, size(x,1), size(x,2) );
            x   = x(:); % make it into a vector
        end
        [goodInd,j,xOverD] = find( x./ d );
        [lambdas,srt]      = sort(abs(xOverD),'descend');
        s   = abs(x(goodInd).*d(goodInd));
        s   = s(srt);
        dd  = d(goodInd).^2;
        dd  = dd(srt);
        cs  = cumsum(s);
        cd  = cumsum(dd);
        ndx    = find( cs - lambdas.*cd >= q+2*eps(q), 1, 'first');
        if ~isempty( ndx )
            ndx     = ndx - 1;
            lambda  = ( cs(ndx) - q )/cd(ndx);
            x       = sign(x).*max( 0, abs(x) - lambda*d );
        end
        x   = myReshape(x);
    end

end

% Copied from TFOCS, April 17 2015
function op = project_l1l2( q, rowNorms )
%PROJ_L1L2    L1-L2 block norm: sum of L2 norms of rows.
%    OP = PROJ_L1L2( q ) implements the constraint set
%        {X | sum_{i=1:m} norm(X(i,:),2) <= 1 }
%    where X is a m x n matrix.  If n = 1, this is equivalent
%    to PROJ_L1. If m=1, this is equivalent to PROJ_L2
%
%    Q is optional; if omitted, Q=1 is assumed. But if Q is supplied,
%    then it must be positive and real and a scalar.
%
%   OP = PROJ_L1L2( q, rowNorms )
%     will either do the sum of the l2-norms of rows if rowNorms=true
%       (the default), or the sum of the l2-norms of columns if
%       rowNorms = false.
%
%   Known issues: doesn't yet work with complex-valued data.
%       Should be easy to fix, so email developers if this is
%       needed for your problem.
%
% Dual: prox_linfl2.m [not yet available]
% See also prox_l1l2.m, proj_l1.m, proj_l2.m

    if nargin == 0 || isempty(q),
        q = 1;
    elseif ~isnumeric( q ) || ~isreal( q ) || any(q <= 0) ||numel(q)>1,
        error( 'Argument must be positive and a scalar.' );
    end

    if nargin<2 || isempty(rowNorms)
        rowNorms = true;
    end

    if rowNorms
        op = @(x,varargin)prox_f_rows(q,x);
    else
        op = @(x,varargin)prox_f_cols(q,x);
    end

    function X = prox_f_rows(tau,X)
        nrms    = sqrt( sum( X.^2, 2 ) );
        % When we include a row of x, corresponding to row y of Y,
        % its contribution is norm(y)-lambda
        % So we have sum_{i=1}^m max(0, norm(y_0)-lambda)
        % So, basically project nrms onto the l1 ball...
        s      = sort( nrms, 'descend' );
        cs     = cumsum(s);

        ndx    = find( cs - (1:numel(s))' .* [ s(2:end) ; 0 ] >= tau+2*eps(tau), 1 ); % For stability

        if ~isempty( ndx )
            thresh = ( cs(ndx) - tau ) / ndx;
            % Apply to relevant rows
            d   = max( 0, 1-thresh./nrms );
            m   = size(X,1);
            X   = spdiags( d, 0, m, m )*X;
        end
    end

    function X = prox_f_cols(tau,X)
        nrms    = sqrt( sum( X.^2, 1 ) ).';
        s      = sort( nrms, 'descend' );
        cs     = cumsum(s);

        ndx    = find( cs - (1:numel(s))' .* [ s(2:end) ; 0 ] >= tau+2*eps(tau), 1 ); % For stability

        if ~isempty( ndx )
            thresh = ( cs(ndx) - tau ) / ndx;
            d   = max( 0, 1-thresh./nrms );
            n   = size(X,2);
            X   = X*spdiags( d, 0, n,n );
        end
    end

end % end projection_l1l2.m

% Copied from TRFOCS April 17 2015
function op = prox_l1l2( q )
%PROX_L1L2    L1-L2 block norm: sum of L2 norms of rows.
%    OP = PROX_L1L2( q ) implements the nonsmooth function
%        OP(X) = q * sum_{i=1:m} norm(X(i,:),2)
%    where X is a m x n matrix.  If n = 1, this is equivalent
%    to PROX_L1
%    Q is optional; if omitted, Q=1 is assumed. But if Q is supplied,
%    then it must be positive and real.
%    If Q is a vector, it must be m x 1, and in this case,
%    the weighted norm OP(X) = sum_{i} Q(i)*norm(X(i,:),2)
%    is calculated.
%
% Dual: proj_linfl2.m
% See also proj_linfl2.m, proj_l1l2.m

    if nargin == 0,
        q = 1;
    elseif ~isnumeric( q ) || ~isreal( q ) || any(q <= 0),
        error( 'Argument must be positive.' );
    end
    op = @(x,t)prox_f(q,x,t);

    function x = prox_f(q,x,t)
        if nargin < 3,
            error( 'Not enough arguments.' );
        end
        v = sqrt( sum(x.^2,2) );
        s = 1 - 1 ./ max( v ./ ( t .* q ), 1 );
        m = length(s);
        x = spdiags(s,0,m,m)*x;
    end

end


function r8_cscd_test ( )

%*****************************************************************************80
%
%% R8_CSCD_TEST tests R8_CSCD.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 January 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8_CSCD_TEST\n' );
  fprintf ( 1, '  R8_CSCD computes the cosecant of an angle\n' );
  fprintf ( 1, '  given in degrees.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ANGLE    R8_CSCD(ANGLE)\n' );
  fprintf ( 1, '\n' );

  for i = 0 : 15 : 360
    angle = i;
    if ( mod ( i, 180 ) == 0 )
      fprintf ( 1, '  %8.2f    Undefined\n', angle );
    else
      fprintf ( 1, '  %8.2f  %14.6g\n', angle, r8_cscd ( angle ) );
    end
  end

  return
end


function Pe = bit_channel_upgrading_procedure(W, z, miu)
N = length(z);
if N == 1
    Pe = 0.5 * sum(min(W));
    disp(['Bit index = ' num2str(z) '  ML detection Bit Error rate = ' num2str(Pe)])
else
    W_up = get_W_up(W);
    W_up = LR_sort(W_up);
    W_up_after_erasure_symbol_merge = erasure_symbol_merge(W_up);
    W_up_after_merge = upgrading_merge(W_up_after_erasure_symbol_merge, miu);
    Pe1 = bit_channel_upgrading_procedure(W_up_after_merge, z(1 : N/2), miu);

    W_down= get_W_down(W);
    W_down = LR_sort(W_down);
    W_down_after_erasure_symbol_merge = erasure_symbol_merge(W_down);
    W_down_after_merge = upgrading_merge(W_down_after_erasure_symbol_merge, miu);
    Pe2 = bit_channel_upgrading_procedure(W_down_after_merge, z(N/2 + 1 : end), miu);

    Pe = [Pe1 Pe2];
end
end

% function Pe = bit_channel_upgrading_procedure(W, z, miu)
% N = length(z);
% m = round(log2(N));
% Pe = zeros(N, 1);
% for k = N - 7 : N - 1
%     char_bin_expansion = dec2bin(k, m);
%     W_tmp = W;
%     for i_level = 1 : m
%         if char_bin_expansion(i_level) == '0'
%             W_up = get_W_up(W_tmp);
%             W_up = LR_sort(W_up);
%             W_up_after_erasure_symbol_merge = erasure_symbol_merge(W_up);
%             W_tmp = upgrading_merge(W_up_after_erasure_symbol_merge, miu);
%         else
%             W_down = get_W_down(W_tmp);
%             W_down = LR_sort(W_down);
%             W_down_after_erasure_symbol_merge = erasure_symbol_merge(W_down);
%             W_tmp = upgrading_merge(W_down_after_erasure_symbol_merge, miu);
%         end
%     end
%     Pe(k + 1) = 0.5 * sum(min(W_tmp));
%     disp(['Bit index = ' num2str(k + 1) ' ML detection Bit Error rate = ' num2str(Pe(k + 1))])
% end
% end


classdef TP10 < PROBLEM
% <multi> <real> <large/none> <constrained> <robust>
% Test problem for robust multi-objective optimization
% delta --- 0.05 --- Maximum disturbance degree
% H     ---   50 --- Number of disturbances

%------------------------------- Reference --------------------------------
% A. Gaspar-Cunha, J. Ferreira, and G. Recio, Evolutionary robustness
% analysis for multi-objective optimization: benchmark problems, Structural
% and Multidisciplinary Optimization, 2014, 49: 771-793.
%------------------------------- 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".
%--------------------------------------------------------------------------

    properties
        delta;      % Maximum disturbance degree
        H;          % Number of disturbances
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            [obj.delta,obj.H] = obj.ParameterSet(0.05,50);
            obj.M = 2;
            obj.D = 3;
            obj.lower    = [0,0,1];
            obj.upper    = [10,10,3];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(~,PopDec)
            PopObj(:,1) = PopDec(:,1).*sqrt(16+PopDec(:,3).^2) + PopDec(:,2).*sqrt(1+PopDec(:,3).^2);
            PopObj(:,2) = 20*sqrt(16+PopDec(:,3).^2)./PopDec(:,1)./PopDec(:,3);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            PopCon(:,1) = 20*sqrt(16+PopDec(:,3).^2) - 100*PopDec(:,1).*PopDec(:,3);
            PopCon(:,2) = 80*sqrt(1+PopDec(:,3).^2) - 100*PopDec(:,2).*PopDec(:,3);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = [100,100];
        end
        %% Calculate the metric value
        function score = CalMetric(obj,metName,Population)
            switch metName
                case {'Mean_IGD','Mean_HV','Worst_IGD','Worst_HV'}
                    score = feval(metName,Population,obj);
                otherwise
                    score = feval(metName,Population,obj.optimum);
            end
        end
        %% Perturb solutions multiple times
        function PopX = Perturb(obj,PopDec,N)
            if nargin < 3; N = obj.H; end
            Delta = repmat(obj.delta.*(obj.upper-obj.lower),N*size(PopDec,1),1);
            w     = UniformPoint(N,obj.D,'Latin');
            Dec   = 2*Delta.*w(reshape(repmat(1:end,size(PopDec,1),1),1,[]),:) + repmat(PopDec,N,1) - Delta;
            Dec   = obj.CalDec(Dec);
            PopX  = SOLUTION(Dec,obj.CalObj(Dec),obj.CalCon(Dec));
        end
    end
end


% EX_STOKES_BIFURCATION_RT_MP: data file for Stokes problem in a pipe with a bifurcation.

% 1) PHYSICAL DATA OF THE PROBLEM
problem_data  = struct ();

% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_bifurcation_mp.txt';

% Type of boundary conditions for each side of the domain
problem_data.drchlt_sides = 1:3;
problem_data.nmnn_sides = [];

% Physical parameters
problem_data.viscosity = @(x, y) ones (size (x));

% Force term
problem_data.f  = @(x, y) zeros ([2, size(x)]);

% Boundary terms
problem_data.h  = @test_stokes_bifurcation_mp_h_drchlt;

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
method_data = struct ();

method_data.element_name = 'rt';     % Element type for discretization

method_data.degree       = [3 3];  % Degree of the splines
method_data.regularity   = [2 2];  % Regularity of the splines
method_data.nsub         = [5 5];  % Number of subdivisions
method_data.nquad        = [5 5];  % Points for the Gaussian quadrature rule
method_data.Cpen = 10 * (method_data.degree(1)+1);

% 3) CALL TO THE SOLVER
[geometry, msh, space_v, vel, space_p, press] = mp_solve_stokes_div_conforming (problem_data, method_data);

% 4) POST-PROCESSING
% 4.1) EXPORT TO PARAVIEW
output_file  = 'bifurcation_2d_rt_mp_deg3_reg2_sub5';
vtk_pts = {linspace(0, 1, 20), linspace(0, 1, 20)};

fprintf ('results being saved in: %s_vel.pvd and %s_press.pvd\n', output_file, output_file)
sp_to_vtk (vel, space_v, geometry, vtk_pts, sprintf ('%s_vel', output_file), {'velocity', 'divergence'}, {'value', 'divergence'})
sp_to_vtk (press, space_p, geometry, vtk_pts, sprintf ('%s_press', output_file), 'pressure')

%!test
%! problem_data  = struct ();
%! problem_data.geo_name = 'geo_bifurcation_mp.txt';
%! problem_data.drchlt_sides = 1:3;
%! problem_data.nmnn_sides = [];
%! problem_data.viscosity = @(x, y) ones (size (x));
%! problem_data.f  = @(x, y) zeros ([2, size(x)]);
%! problem_data.h  = @test_stokes_bifurcation_mp_h_drchlt;
%! method_data = struct ();
%! method_data.element_name = 'rt';     % Element type for discretization
%! method_data.degree       = [3 3];  % Degree of the splines
%! method_data.regularity   = [2 2];  % Regularity of the splines
%! method_data.nsub         = [5 5];  % Number of subdivisions
%! method_data.nquad        = [5 5];  % Points for the Gaussian quadrature rule
%! method_data.Cpen = 10 * (method_data.degree(1)+1);
%! [geometry, msh, space_v, vel, space_p, press] = mp_solve_stokes_div_conforming (problem_data, method_data);
%! assert (msh.nel, 100)
%! assert (space_v.ndof, 552)
%! assert (space_p.ndof, 256)
%! for iptc = 1:4
%!   div = sp_eval (vel(space_v.gnum{iptc}) .* space_v.dofs_ornt{iptc}', space_v.sp_patch{iptc}, geometry(iptc), [20 20], 'divergence');
%!   assert (max (abs (div(:))) < 1e-12)
%! end