function [X,info] = IRsirt(A,b,varargin)
%IRsirt Simuletaneous Iterative Reconstruction Technique
%
% options  = IRsirt('defaults')
% [X,info] = IRsirt(A,b,K)
% [X,info] = IRsirt(A,b,K,options)
%
% This function calls the function 'sirt' in AIR Tools II that implements a
% number of Simultaneous Iterative Reconstruction Technique (SIRT) methods:
% cav, cimmino, drop, landweber and sart (see AIR Tools II for more details).
% The default method is sart which, in the CT community, is known as 'sirt'.
%
% With 'defaults' as input prints the default options.  Otherwise outputs
% the iterates specified in K, using max(K) as MaxIter, and using all other
% default options.  With options as input: uses the user- specified options
% and all the other default options.
%
% Inputs:
%  A : either (a) a full or sparse matrix
%             (b) a matrix object that performs the matrix*vector operation
%             (c) user-defined function m-file
%  b : right-hand side vector
%  K : (optional) integer vector that specifies which iterates are returned
%      in X; the maximum number of iterations is assumed to be max(K)
%      [ positive integer | vector of positive components ]
%  options : structure with the following fields (optional)
%      sirt_mthod   - the specific SIRT method to be used;
%                      [ 'cav' | 'cimmino' | 'drop' | 'landweber' | {'sart'} ]
%      x0            - initial guess for the iterations; default = zero vector
%                      [ array | {'none'} ]
%      MaxIter       - maximum allowed number of iterations
%                      [ {100} | positive integer ]
%      x_true        - true solution; allows us to returns error norms
%                      with respect to x_true at each iteration
%                      [ array | {'none'} ]
%      stopCrit      - stopping criterion for the iterations
%                      [ {'none'} | 'discrep' ]
%                      Note: 'discrep' requires NoiseLevel and eta
%      NoiseLevel    - norm of noise in rhs divided by norm of rhs
%                      [ {'none'} | nonnegative scalar]
%      eta           - safety factor for the discrepancy principle
%                      [ {1.01} | scalar greater than (and close to) 1 ]
%      relaxParam    - constant relaxation parameter, bounded above 2 divided
%                      by the spectral radius of the iteration matrix
%                      [ positive scalar | {'none'} ]
%                      If 'none' then a good value is chosed by the function
%      nonnegativity - apply nonnegativity constraints
%                      [ 'on' | {'off'} ]
%      Ubound        - apply box constraints in the interval [0,Ubound]
%                      [ positive scalar | {'off'} ]
%      IterBar       - shows the progress of the iterations
%                      [ {'on'} | 'off' ]
% Note: the options structure can be created using the function IRset.
%
% Outputs:
%   X : computed solutions, stored column-wise (at the iterations listed in K)
%   info : structure with the following fields:
%      its              - number of the last computed iteration
%      saved_iterations - iteration numbers of iterates stored in X
%      StopFlag         - a flag that describes the stopping condition:
%                           1 : reached maximum number of iterations
%                           2 : discrepancy principle satisfied
%      relaxParam       - the used relaxation parameter
%      Enrm             - relative error norms (requires x_true) for the
%                         stored iterations
%
% Note that this function provides a simplified call to the function "sirt"
% in AIR Tools II which has more features than we allow here.  To use the
% full power of the SIRT methods consider using AIR Tools II directly.
%
% See also: IRart, IRget, IRset

% Silvia Gazzola, University of Bath
% Per Christian Hansen, Technical University of Denmark
% James G. Nagy, Emory University
% April, 2018.

% This file is part of the IR Tools package and is distributed under the
% 3-Clause BSD License. A separate license file should be provided as part
% of the package.

% Set default values for options.
defaultopt = struct('x0', 'none', 'IterBar', 'on', 'stopCrit', 'none', ...
    'eta', 1.01, 'nonnegativity', 'off', 'NoiseLevel','none', ...
    'Ubound', 'none', 'relaxParam', 'none', 'sirt_method', 'sart', ...
    'MaxIter', 100, 'x_true', 'none');

% If input is 'defaults,' return the default options in X.
if nargin==1 && nargout <= 1 && isequal(A,'defaults')
    X = defaultopt;
    return;
end

% Check for acceptable number of optional input arguments.
switch length(varargin)
    case 0
        K = []; options = [];
    case 1
        if isa(varargin{1}, 'double')
            K = varargin{1}; options = [];
        else
            K = []; options = varargin{1};
        end
    case 2
        if isa(varargin{1}, 'double')
            K = varargin{1}; options = varargin{2};
        else
            K = varargin{2}; options = varargin{1};
        end
    otherwise
        error('Too many input parameters')
end

if isempty(options)
    options = defaultopt;
end

options = IRset(defaultopt, options);

if min(K) < 1, error('Number of iterations must be positive'), end

x0 = IRget(options, 'x0', [], 'fast');
if strcmpi(x0, 'none')
    x0 = [];
end

MaxIter       = IRget(options, 'MaxIter',       [], 'fast');
method        = IRget(options, 'sirt_method',   [], 'fast');
IterBar       = IRget(options, 'IterBar',       [], 'fast');
relaxParam    = IRget(options, 'relaxParam',    [], 'fast');
stopCrit      = IRget(options, 'stopCrit',      [], 'fast');
nonnegativity = IRget(options, 'nonnegativity', [], 'fast');
Ubound        = IRget(options, 'Ubound',        [], 'fast');
x_true        = IRget(options, 'x_true',        [], 'fast');

if isempty(K)
    K = MaxIter;
end
% Sorting the iteration numbers (in case they are shuffled in input).
K = sort(K,'ascend'); K = unique(K);
if ~((isreal(K) && (all(K > 0)) && all(K == floor(K))))
    error('K must be a vector of positive real integers')
end

% Set options for SIRT.  Always used a fixed lambda, either chosen by the
% user or set by the SIRT function.  Always use no stopping rule or stop
% by the size of the residual (discrepancy principle).

if strcmpi(relaxParam,'none')
    % The field lambda is not present.
else
   sirtoptions.relaxpar = relaxParam;
end

if strcmpi(stopCrit,'discrep')
    sirtoptions.stoprule.type = 'DP';
    if isempty(options.NoiseLevel) || strcmp(options.NoiseLevel,'none')
        error('When using discrepancy principle: options.NoiseLevel must be specified')
    end
    sirtoptions.stoprule.taudelta = options.eta*options.NoiseLevel*norm(b);
else
    sirtoptions.stoprule.type = 'none';
end

if strcmpi(nonnegativity,'on')
    sirtoptions.lbound = 0;
end

if (isreal(Ubound) && isscalar(Ubound) && Ubound > 0)
    sirtoptions.lbound = 0;
    sirtoptions.ubound = Ubound;
end

if strcmp(IterBar,'on')
    sirtoptions.waitbar = true;
end

% Call the requested SIRT method.
[X,sirtinfo] = sirt(method,A,b,K,x0,sirtoptions);

switch sirtinfo.stoprule
    case 0
        info.StopFlag = 1;
    case 2
        info.StopFlag = 2;
        if sirtinfo.finaliter==0
            warning(['No iterations were performed, beause the starting',...
                     ' vector x0 satisfies the discrepancy principle'])
        end
end
info.its = sirtinfo.finaliter;
info.saved_iterations = sirtinfo.itersaved;
info.relaxParam = sirtinfo.relaxpar;

% Compute relative error norms, if requested.
if ~strcmp(x_true,'none')
    Enrm = zeros(length(info.saved_iterations),1);
    for k=1:length(info.saved_iterations)
        Enrm(k) = norm(x_true-X(:,k));
    end
    info.Enrm = Enrm/norm(x_true);
end


function varargout = solver_sBPDN_WW( A, alpha, W1, beta, W2, b, epsilon, mu, x0, z0, opts, varargin )
% SOLVER_SBPDN_WW BPDN with two separate (weighted) l1-norm terms. Uses smoothing.
% [ x, out, opts ] = solver_sBPDN_WW( A, alpha, W_1, beta, W_2, b, epsilon, mu, x0, z0, opts )
%    Solves the smoothed basis pursuit denoising problem
%        minimize alpha*norm(W_1 x,1) + beta*norm(W_2 x, 1) + 0.5*mu*(x-x0).^2
%        s.t.     norm(A*x-b,2) <= epsilon
%    by constructing and solving the composite dual.
%    A, W_1 and W_2 must be a linear operator or matrix, and b must be a vector. The
%    initial points x0, z0 and the options structure opts are optional.
%    See also solver_sBPDN and solver_sBPDN_W

% Supply default values
error(nargchk(8,12,nargin));
if nargin < 9, x0 = []; end
if nargin < 10, z0 = []; end
if nargin < 11, opts = []; end
if ~isfield( opts, 'restart' ), opts.restart = 5000; end

if epsilon < 0
    error('TFOCS error: epsilon is negative');
end
if ~epsilon
    error('TFOCS error: cannot handle epsilon = 0.  Please call solver_sBP instead');
elseif epsilon < 100*builtin('eps')
    warning('TFOCS:badConstraint',...
        'TFOCS warning: epsilon is near zero; consider calling solver_sBP instead');
end

% Need to estimate the norms of A*A' and W*W' in order to be most efficient
if isfield( opts, 'noscale' ) && opts.noscale,
    normA2 = 1; normW12 = 1; normW22 = 1;
else
    normA2 = []; normW12 = []; normW22 = [];
    if isfield( opts, 'normA2'  )
        normA2 = opts.normA2;
        opts = rmfield( opts, 'normA2' );
    end
    if isfield( opts, 'normW12' )
        normW12 = opts.normW12;
        opts = rmfield( opts, 'normW12' );
    end
    if isfield( opts, 'normW22' )
        normW22 = opts.normW22;
        opts = rmfield( opts, 'normW22' );
    end
end
if isempty( normA2 ),
    normA2 = linop_normest( A ).^2;
end
if isempty( normW12 ),
    normW12 = linop_normest( W1 ).^2;
end
if isempty( normW22 ),
    normW22 = linop_normest( W2 ).^2;
end
if isempty(alpha),
    alpha = 1;
end
if isempty(beta),
    beta = 1;
end

proxScale1 = sqrt( normW12 / normA2 );
proxScale2 = sqrt( normW22 / normA2 );
prox       = { prox_l2( epsilon ), ...
               proj_linf( proxScale1 * alpha ),...
               proj_linf( proxScale2 * beta ) };
W1         = linop_compose( W1, 1 / proxScale1 );
W2         = linop_compose( W2, 1 / proxScale2 );
[varargout{1:max(nargout,1)}] = ...
    tfocs_SCD( [], { A, -b; W1, 0; W2, 0 }, prox, mu, x0, z0, opts, varargin{:} );

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function toms446_test02 ( )

%*****************************************************************************80
%
%% TOMS446_TEST02 tests MULTPLY, which multiplies two Chebyshev series.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2011
%
%  Author:
%
%    John Burkardt
%
  nf = 5;
  npl = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TOMS446_TEST02\n' );
  fprintf ( 1, '  Test MLTPLY, which computes the\n' );
  fprintf ( 1, '  product of two Chebyshev series.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Multiply series for SIN(X) and COS(X)\n' );
  fprintf ( 1, '  and compare with series for 1/2*SIN(2X).\n' );

  x = cheby ( nf, npl, @functn );

  for i = 1 : npl
    x1(i) = x(i,1);
    x2(i) = x(i,2);
    x(i,3) = 0.5 * x(i,3);
  end

  x3 = mltply ( x1, x2, npl );

  fprintf ( 1, '\n' );
  fprintf ( 1, '        Sin(x)      Cos(x)   1/2*Sin(2x)     RESULT\n' );
  fprintf ( 1, '\n' );

  for i = 1 : npl
    fprintf ( 1, '  %10.4f  %10.4f  %10.4f  %10.4f\n', x(i,1:3), x3(i) );
  end

  return
end


function [filt] = notchfilter(dat,Fs,Fl,N)

% NOTCHFILTER line noise reduction filter for EEG/MEG data
%
% [filt] = notchfilter(dat, Fsample, Fline)
%
% where
%   dat        data matrix (Nchans X Ntime)
%   Fsample    sampling frequency in Hz
%   Fline      line noise frequency (would normally be 50Hz)
%   N          optional filter order, default is 4
%
% if Fline is specified as 50, a band of 48-52 is filtered out
% if Fline is specified as [low high], that band is filtered out

% original      (c) 2003, Pascal Fries
% modifications (c) 2003, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if nargin<4
  % set the default filter order
  N = 4;
end

Nchans   = size(dat,1);
Nsamples = size(dat,2);

% use a digital FIR filter
Fn = Fs/2;           % Nyquist frequency
if length(Fl)==1
  % default use a notch-width of 2Hz in both directions
  % otherwise use the specified band
  Fl = [Fl-2 Fl+2];
end
[B, A] = butter(N, [min(Fl)/Fn max(Fl)/Fn], 'stop');
filt = filtfilt(B, A, dat')';


function [ R, subgrad, data ] = risk2( data, W )
%RISK evaluates risk term.
%
% Synopsis:
%  [R,subgrad,data] = risk(data)
%  [R,subgrad,data] = risk(data,W)
%
% Description:
%  Let the risk term be defined as
%
%  R(W) = 1/m sum_{i=1}^m [ max_{y \in Y}( (L(y_i, y) + <w, \Psi(x_i, y)> ) - <w, \Psi(x_i, y+i)> ]
%
%  This function returns value R and subgradient SUBGRAD of the
%  risk R(W) at W.
%
% 10-08-10 Michal Uricar
% 11-07-11 Michal Uricar, corners dataset (checked-only) + loss function modification

    options = data.options;

    if (nargin < 2)
        W = buildPsi2(options, data.tmpData, data.Y{1});
        W = sparse(length(W), 1);
    end

    psi_xiyhat = sparse(length(W), data.nImages);
    psi_xiyi = sparse(length(W), data.nImages);
    suma_all = zeros(1, data.nImages);

    for i = 1 : data.nImages
%     parfor i = 1 : data.nImages
        [lbpdat lbp_sparse] = getPsiMat(data, i);
        GT = data.Y{i};

        % \hat{y_i} = argmax_{y \in Y} [ L(y_i, y) + <w, \Psi(x_i, y)> ]
        L = computeL(options, GT, data.kappa(i));       % with normalization
%         L = computeL(options, GT);                      % without normalization
        y_hat = argmax_mex(options, W, data.mapTable, lbp_sparse, L);

        % \Psi(x_i, \hat{y_i})
        psi_xiyhat(:, i) = buildPsi2(options, lbpdat, y_hat);
        % \Psi(x_i, y_i)
        psi_xiyi(:, i) = buildPsi2(options, lbpdat, GT);

        % sum_{i = 1}^{m} ( max_{y \in Y}( L(y_i, y) + <w, \Psi(x_i, y_i)> )> ) - <w, \Psi(x_i, y_i)> )
        % with normalization coefficient kappa
        suma_all(i) = 100 * data.kappa(i) * ...
                        sum(1/data.options.M * sqrt(sum( (y_hat - GT).^2 ))) ...
                        + W'*psi_xiyhat(:, i) - W'*psi_xiyi(:, i);
%         % without normalization coefficient kappa
%         suma_all(i) = sum(1/data.options.M * sqrt(sum( (y_hat - GT).^2 ))) ...
%                         + W'*psi_xiyhat(:, i) - W'*psi_xiyi(:, i);
    end;

    suma = sum(suma_all);

    % \hat{R}(w) = 1/m \sum_{i = 1}^m [ max_{y \in Y}( L(y_i, y) + <w, \Psi(x_i, y_i)> )> ) - <w, \Psi(x_i, y_i)> ]
    R = 1/data.nImages * suma;
    % \partial_w \hat{R}(w) = 1/m \sum_{i = 1}^m ( \Psi(x_i, \hat{y_i}) - \Psi(x_i, y_i) )
    subgrad = 1/data.nImages * sum(psi_xiyhat - psi_xiyi, 2);

end


function x = singlehex2num(s)
%SINGLEHEX2NUM Convert single precision IEEE hexadecimal string to number.
%   SINGLEHEX2NUM(S), where S is a 8 character string containing
%   a hexadecimal number, returns a double type number
%   equal to the IEEE single precision
%   floating point number it represents.  Fewer than 8
%   characters are padded on the right with zeros.
%
%   If S is a character array, each row is interpreted as a single
%   precision number (and returned as a double).
%
%   NaNs, infinities and denorms are handled correctly.
%
%   Example:
%       hexsingle2num('40490fdb') returns Pi.
%       hexsingle2num('bf8') returns -1.
%
%   See also HEX2NUM.

% Based on Matlab's hex2num.
% Note: IEEE Standard 754 for floating point numbers
%
%  Floating point numbers are represented as:
%  x = +/- (1+f)*2^e
%
%  doubles: 64 bits
%           Bit 63       (1 bit)  = sign (0=positive, 1=negative)
%           Bit 62 to 52 (11 bits)= exponent biased by 1023
%           Bit 51 to 0  (52 bits)= fraction f of the number 1.f
%  singles: 32 bits
%           Bit 31       (1 bit)  = sign (0=positive, 1=negative)
%           Bit 30 to 23 (8 bits) = exponent biased by 127
%           Bit 22 to 0  (23 bits)= fraction f of the number 1.f
%
% Original file hexsingle2num from Mark Lubinski
% Changed on 19-may-05 by Matthias Noell: denormalized power set 2^-126

if iscellstr(s), s = char(s); end
if ~ischar(s)
    error('Input to hexsingle2num must be a string.')
end
if isempty(s), x = []; return, end

[row,col] = size(s);
blanks = find(s==' '); % Find the blanks at the end
if ~isempty(blanks), s(blanks) = '0'; end % Zero pad the shorter hex numbers.

% Convert characters to numeric digits.
% More than 8 characters are ignored
% For double: d = zeros(row,16);
d = zeros(row,8);
d(:,1:col) = abs(lower(s)) - '0';
d = d + ('0'+10-'a').*(d>9);
neg = d(:,1) > 7;
d(:,1) = d(:,1)-8*neg;

if any(d > 15) | any(d < 0)
    error('Input string to hexsingle2num should have just 0-9, a-f, or A-F.')
end

% Floating point exponent.
% For double: e = 16*(16*(d(:,1)-4) + d(:,2)) + d(:,3) + 1;
% For double: e = 256*d(:,1) + 16*d(:,2) + d(:,3) - 1023;
expBit = (d(:,3) > 7);
e = 32*d(:,1) + 2*d(:,2) + expBit - 127;
d(:,3) = d(:,3)-8*expBit;  % Remove most sig. bit of d(:,3) which belongs to exponent

% Floating point fraction.
% For double: sixteens = [16;256;4096;65536;1048576;16777216;268435456];
% For double: sixteens2 = 268435456*sixteens(1:6);
% For double: multiplier = 1./[sixteens;sixteens2];
% For double: f = d(:,4:16)*multiplier;
sixteens = [16;256;4096;65536;1048576;16777216];
multiplier = 2./[sixteens];
f = d(:,3:8)*multiplier;

x = zeros(row,1);
% Scale the fraction by 2 to the exponent.
% For double: overinf = find((e>1023) & (f==0));
overinf = find((e>127) & (f==0));
if ~isempty(overinf), x(overinf) = inf; end

% For double: overNaN = find((e>1023) & (f~=0));
overNaN = find((e>127) & (f~=0));
if ~isempty(overNaN), x(overNaN) = NaN; end

% For double: underflow = find(e<-1022);
underflow = find(e<-126);
if ~isempty(underflow), x(underflow) = pow2(f(underflow),-126); end

% For double: allothers = find((e<=1023) & (e>=-1022));
allothers = find((e<=127) & (e>=-126));
if ~isempty(allothers), x(allothers) = pow2(1+f(allothers),e(allothers)); end

negatives = find(neg);
if ~isempty(negatives), x(negatives) = -x(negatives); end


function [R,current_eps]=MC_USTM_Alamouti(snrdB,T,L,epsilon,prec,filename)
%
% metaconverse bound for the Alamouti ensemble
%-------------------------------------------------------------------
%                       SET-UP PARAMETERS
%-------------------------------------------------------------------

SAVE=1;
MAT=1;

K = 2^prec; % number of monte carlo simulations (at least 100 x 1/epsilon)
rho = 10.^(snrdB/10); % SNR in linear scale
Mt=2;
Mr=2;
%-------------------------------------------------------------------
%                       MONTE CARLO SIMULATION
%-------------------------------------------------------------------
Ip = zeros(K,1); %allocate for the montecarlo runs
% Iq = zeros(K,1); %allocate for the montecarlo runs
%-------------------------------------------------------------------
%                       CONSTANTS
%-------------------------------------------------------------------
rho_tilde = T*rho/Mt;

lambda=1+rho_tilde;
lambda1=1/lambda;
lambda2=rho_tilde*lambda1;

x1=rho_tilde;
x2=rho_tilde;

D= [diag([sqrt(1+x1), sqrt(1+x2)]), zeros(Mt,T-Mt);
zeros(T-Mt,Mt), eye(T-Mt)]; % D matrix (covariance matrix of equivalent noise)

%-------------------------------------------------------------------
%                       MONTE CARLO
%-------------------------------------------------------------------

%tic

norm=sqrt(.5);

for k=1:K

        i_L = 0;

        Z = randn(T,Mr,L)*norm+1i*randn(T,Mr,L)*norm;

        for l = 1:L %Create each realization

          %COMPUTE EVERYTHING THAT HAS TO DO WITH SINGULAR VALUES
          Sigma_alt= svd(D*Z(:,:,l)).^2;
          Sigma_alt=sort(Sigma_alt,1,'descend');
          TraceZ=abs(trace(Z(:,:,l)'*Z(:,:,l)));

          Y=D*Z(:,:,l);

          Ytilde=(zeros(T,4));

          Ytilde(:,[1,3])=Y;

          Ytilde(1:2:T,[2,4])= conj(Y(2:2:T,:));

          Ytilde(2:2:T,[2,4])= -conj(Y(1:2:T,:));

          Sigma=svd(Ytilde).^2;

          Sigma=[Sigma(1),Sigma(3)];

          Sigma=Sigma*lambda2;

           if (T>4),

               M=[gammainc(Sigma(1), T-5),  gammainc(Sigma(1), T-4), exp(Sigma(2)-Sigma(1))*gammainc(Sigma(2), T-5)/(Sigma(2)/Sigma(1))^(T-4), exp(Sigma(2)-Sigma(1))*gammainc(Sigma(2), T-4)/(Sigma(2)/Sigma(1))^(T-4) ;
                   (T-2)*Sigma(1), Sigma(1)^2,(T-2)*Sigma(2), Sigma(2)^2 ;
                   T-3, Sigma(1),T-3, Sigma(2);
                  (T-4)/Sigma(1),1,(T-4)/Sigma(2),1];

              logd=log(det(M))-(T-4)*log(Sigma(1))+Sigma(1);

            else

              M=[ 1, 2*Sigma(1), 1, 0 ; ...
                1,  Sigma(1)^2, Sigma(1), 1; ...
                exp(Sigma(2)-Sigma(1)), 2*Sigma(2), 1, 0; ...
                exp(Sigma(2)-Sigma(1)), Sigma(2)^2, Sigma(2), 1];

              logd=(log(det(M)))+Sigma(1);

            end

            log_exp_sum = logd -4*log(Sigma(1)-Sigma(2));

            i = - TraceZ  +sum(Sigma_alt) - log(gamma(T)) - log_exp_sum;

            i_L = i_L + i; %add it to the total i_L

        end

        Ip(k) =  i_L; %put all computations on a pile to compute the average later
end

if (SAVE==1)
  if (MAT==1)
    save(filename,'Ip')
  else
    save(filename,'Ip','-ascii','-append')
  end
end

%---------------------------------------
%   SEARCHING THE RATE
%---------------------------------------

% load saved data (to account for append possibilities)
if (SAVE==1 && MAT==0)

    Ip=load(filename);
end

Ip=sort(Ip);

Kcurrent=length(Ip); % redefine K to account for append

current_prec=floor(log2(Kcurrent)); % actual precision

K=2^(current_prec); % round off K to avoid search errors

% first find a suitable initial point for the linear search
step=K/2;
index=step;

onevec=ones(K,1);

while(step>1),

   th=Ip(index);

   current_eps=sum(Ip<=th)/K;

   step=step/2;

   if current_eps> epsilon,

       index=index-step;

   else

       index=index+step;

   end

end

current_eps=sum(Ip<=Ip(index))/K;

if(current_eps<epsilon)
    index=index+1;
end

% now perform linear search
% Rvect=zeros(1,K-index+1);
% for ii=1:K-index+1,
% %for ii=1:K-index+1,
%
%     current_rate=Ip(ii+index-1)-log(sum(Ip<=Ip(ii+index-1))/K-epsilon);
%
%     Rvect(ii)=current_rate;
%
% end

%faster search (potentially less tight)

count=0;

R=Ip(index)-log(sum(Ip<=Ip(index))/K-epsilon);

for ii=1:K-index+1,
    current_rate=Ip(ii+index-1)-log(sum(Ip<=Ip(ii+index-1))/K-epsilon);
    if current_rate<= R
        R=current_rate;
     else
       count=count+1;
    end

    if count==20,
      break
    end
end
R=R/(L*T*log(2));

end


classdef RWMOP11 < PROBLEM
% <multi> <real> <constrained>
% Water resource management problem

%------------------------------- Reference --------------------------------
% A. Kumar, G. Wu, M. Ali, Q. Luo, R. Mallipeddi, P. Suganthan, and S. Das,
% A benchmark-suite of real-world constrained multi-objective optimization
% problems and some baseline results, Swarm and Evolutionary Computation,
% 2021, 67: 100961.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        %% Initialization
        function Setting(obj)
            obj.M        = 5;
            obj.D        = 3;
            obj.lower    = [0.01 0.01 0.01];
            obj.upper    = [0.45 0.1 0.1];
            obj.encoding = ones(1,obj.D);
        end
        %% Evaluate multiple solutions
        function Population = Evaluation(obj,varargin)
            x  = varargin{1};
            x1 = x(:,1);
            x2 = x(:,2);
            x3 = x(:,3);
            % Objectives
            f(:,1) = 106780.37 .* (x2 + x3) + 61704.67 ;
            f(:,2) = 3000 .* x1 ;
            f(:,3) = 305700 .* 2289 .* x2 ./ power(0.06.*2289, 0.65) ;
            f(:,4) = 250 .* 2289 .* exp(-39.75.*x2+9.9.*x3+2.74) ;
            f(:,5) = 25 .* (1.39 ./(x1.*x2) + 4940.*x3 -80) ;
            % Constraints
            g(:,1) = 1 - (0.00139./(x1.*x2)+4.94.*x3-0.08);
            g(:,2) = 1 - (0.000306./(x1.*x2)+1.082.*x3-0.0986);
            g(:,3) = 50000 - (12.307./(x1.*x2) + 49408.24.*x3+4051.02);
            g(:,4) = 16000 - (2.098./(x1.*x2)+8046.33.*x3-696.71);
            g(:,5) = 10000 - (2.138./(x1.*x2)+7883.39.*x3-705.04);
            g(:,6) = 2000 - (0.417.*x1.*x2 + 1721.26.*x3-136.54);
            g(:,7) = 550 - (0.164./(x1.*x2)+631.13.*x3-54.48);
            g      = -g;
            Population = SOLUTION(varargin{1},f,g,varargin{2:end});
            obj.FE     = obj.FE + length(Population);
        end
         %% Generate a point for hypervolume calculation
        function R = GetOptimum(obj,~)
            R = [7.3450511e+04   1.3500000e+03   2.8534690e+06   6.6200320e+06   2.5000000e+04];
        end
    end
end


function value = c8_div ( z1, z2 )

%*****************************************************************************80
%
%% C8_DIV divides two C8's.
%
%  Discussion:
%
%    A C8 is a complex value.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, complex Z1, Z2, the arguments.
%
%    Output, complex VALUE, the function value.
%
  a = c8_real ( z1 );
  b = c8_imag ( z1 );
  c = c8_real ( z2 );
  d = c8_imag ( z2 );

  e = c * c + d * d;

  f = ( a * c + b * d ) / e;
  g = ( b * c - a * d ) / e;

  value = f + g * i;

  return
end


function [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (dec)
% by Sundar Krishnan
% 2003, Edited in June, 2004
%
% Description :
% This function Fr_dec2bin.m will convert a POSITIVE Decimal system
% Fraction (dec) to Binary system Fraction Fr_bin.
% Matlab itself has bin2dec.m and dec2bin.m, but there seems to be
% no standard Matlab function when fractions are involved.
%
% This function Fr_bin2dec.m and it's companion / dual function Fr_dec2bin.m
% were developed mainly with a view to get  quick results
% while learning Arithmetic (Entropy) Coding in School.
% (Now, more comments have been added to better explain the programme.)
%
% The results of this function are limited in accuracy due to the
% "precision" used in the function num2str.m in addition to
% Floating Point limits and Rounding errors.
%
% Accumulation of errors due to these limits can be seen
% when Fr_bin2dec and Fr_dec2bin are tested back-to-back in pairs.
%
% After experiments, I observed that the best precision is 16.
% If all the digits of the input bin are used for a pure fraction,
% the results are likely to be more accurate since we have more margin
% wrt the limit of 16 digits.
%
% Given below under "Usage Eg" are the many cases
% that have been tested during the development of this program,
% together with the results obtained in each case.
%
% Pl do forward me any new case that breaks the code
% beyond the aforesaid limitations.
%
% Outputs str_Fr and Fr_dec are intermediate results.
%
% See also : [Fr_dec, str_Fr, Fr_bin] = Fr_bin2dec (bin)
%
% Additional Test Cases involving pairs of dual tests
% are given towards the end.
%
%                                   ********************
%
% Usage Eg : (The foll have been tried out.)
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (0.6796875)         % 0.1010111
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (113.6796875)       % 1110001.1010111
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (113.68359374)
%   = 1110001.10101110111111111
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (1045.013671875)
%   = 10000010101.000000111
%
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (0.013671875)       % 0.000000111
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (0.0000131835937)
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (10099300.131835937)
%   = 100110100001101001100100.0010000111000000
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (1.0450137e+018)
%
% Also try this ! and enjoy the result :
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (1.0450137e+100)
%
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (2987.120089)
%   % = 101110101011.0001111010111110
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (1167892987.120089)
%   % = 1000101100111001010000111111011.0001111010111110
%
%                                       &&&&&&&&&&&&
%
% Usage Eg : Check in pairs :
% Fr_dec = Fr_bin2dec (10000010100.0010000111)  % = 1.044125000000000e+003
% Fr_bin = Fr_dec2bin (1.044125000000000e+003)  % = 10000010100.001
% Fr_bin = Fr_dec2bin ( Fr_bin2dec (10000010100.0010000111) )
%   returns Fr_bin = 10000010100.001
%
% Fr_bin = Fr_dec2bin ( Fr_bin2dec (101110101011.00011111) )
%   returns Fr_bin = 101110101011.0001 (corr to 2987.0625)
%   instead of the expected (same) 101110101011.00011111
%
% Fr_bin = Fr_dec2bin ( Fr_bin2dec ...
%          (1000101100111001010000111111011.0001111010111110) )
%   returns Fr_bin = 1000101100111001000000000000000.00000000000000000
%   (corr to 1167884288)
%   instead of the expected (same)
%   1000101100111001010000111111011.0001111010111110
%   which itself was obtained with Fr_dec2bin (1167892987.120089)
%
% Fr_bin = Fr_dec2bin ( Fr_bin2dec (101110101011.0001111010111110) )
%   returns Fr_bin = 101110101011.0001 (corr to 2987.0625)
%   instead of the expected (same) 101110101011.0001111010111110
%   which itself was obtained with Fr_dec2bin (2987.120089)
%
%
%                                   ********************

% 1) Inits :
Fr_bin = 0 ;
exp_power  = 0 ;

%                                       &&&&&&&&&&&&

% 2) Use num2str to convert the input to string :
%
% After experiments, I observed that the best precision is 16.
% For eg, with precision >= 17,
% str_Fr = num2str ( .1010111, 17 )  =  0.10101110000000001
% str_Fr = num2str ( .1010111, 16 )  =  0.1010111
%
% num2str.m's output will also contain "0" prefix before the decimal dot "."
% which we remove later.

% 2-a) Check if the input is greater than 1.
% If yes, can we use higher precision ?
% NO, I have found problems with precision > 16 even when the input  > 1 !
% So, commenting out the foll code, and retaining precision = 16 only.
% str_Fr = num2str (dec) ;
% if str_Fr > 1
%     precision = 48 ;
% else
%     precision = 16 ;
% end

precision = 16 ;            % See the note above.
str_Fr = num2str (dec, precision) ;
% Some egs of dec = 1045.0137 , 1.0450137e+018 , 0.131835937 , 0.0000131835937

% NOTE : For long input dec strings, pl note that even with precision > 16,
% say, with precision = 48, the input itself is accurately read
% only for the first 16 digits ; or, if it is converted to an exp format,
% then the input is accurately read only till 15 decimals after the dot.
% For eg, if dec = 116789292349873465787.120089,
% the whole integer part is taken as :
% 116789292349873470000 = % 1.1678929234987347e+016
% So, this will by itself creep in errors !

% In general, it is observed errors will creep in
% if the whole integer part > 999999999999999

%                                       &&&&&&&&&&&&

% 3) Now, if str_Fr above is in exp format, as for eg,
% '2.987062500000000e+003', we would like to get it in the form = 2987.0625
%
% I have observed that if the input no < 0.0001 (ie, < 0.0001000...)
% num2str.m's output is in the exp form ie, with powers less than e-005.
% For eg, dec = 0.000100000001 gives str_Fr = 0.000100000001
% But dec = 0.0000999999999999 gives str_Fr = 9.9999999999900001e-005
%
% Also, with precision = 16, num2str.m's output for nos > 1, upto 1.0e+015,
% is WITHOUT the exp form of power. For eg,
% with dec = 999999999999999.9999999999999999
% str_Fr = num2str (dec, 16)    % gives = 1.0e+015 = 1 0000 0000 0000 000
%
% For nos > 1e+016, num2str.m's output is in exp form.

if ~isempty ( findstr ( str_Fr, 'e') )

    exp_power = 0 ;
    [str_Fr_Bef_Exp, exp] = strtok ( str_Fr, 'e' ) ;
        % Some egs = str_Fr_Bef_Exp = 1.119996810555458,   exp = e-005

    [exp_power, ign ] = strtok ( exp, 'e' ) ;
        % exp starts with 'e', hence see LHS

    exp_power = abs ( str2num (exp_power) ) ;

    % Remove the dot at the 2nd place : (as in 1.119996810555458)
    % However, there is no dot when it's a pure fraction,
    % and is an exact submultiple of 2 !
    if length (str_Fr_Bef_Exp) >= 2  ;
        str_Fr_Bef_Exp (2) = [] ;
    end

    if exp (2) == '-'           % < 1e-005
        for k = 1 : exp_power - 1
            str_Fr_Init_Zeros(k) = '0' ;
        end

        str_Fr = strcat (  '0.', str_Fr_Init_Zeros, str_Fr_Bef_Exp ) ;

    elseif exp (2) == '+'       % > 1.0e+015

        str_Fr = str_Fr_Bef_Exp ;

        % Normally, the foll "if" loop should not be necessary
        % since exp format does not occur for powers <= 1.0e+015. Still ...
        if length ( str_Fr ) > exp_power + 1

            str_Fr ( end + 1 ) = str_Fr (end) ;

            for j = length (str_Fr) - 1  :  -1 :  ...
                    length (str_Fr) - (exp_power + 1) + 1

                str_Fr ( j ) = str_Fr (j-1) ;
            end

            str_Fr (exp_power + 2) = '.' ;

        end

        % Foll logic when exp_power > 1.0e+015, like for eg,
        % str_Fr = '1.0450137e+018'
        % implies str_Fr_Bef_Exp = 10450137 (length = 8)
        % ie, str_Fr should become 10450137 0000 0000 000 (length = 19)
        % ie, padding with 0s at the end is reqd.
        if length ( str_Fr ) < exp_power
            str_Fr = strcat ( str_Fr,  ...
                repmat ( ['0'], 1, exp_power - (length ( str_Fr ) - 1) ) ) ;
        end

    end

end

%                                       &&&&&&&&&&&&

% 4) Separate the whole integer and fraction parts of str_Fr.
[bef_dec, Fr_dec] = strtok ( str_Fr, '.' ) ;

%                                       &&&&&&&&&&&&

% Now, we have bef_dec as the whole integer part, and
% the Fractional part starting "."

% 5) Convert first the whole integer part to binary
% by calling the std Matlab's fn dec2bin.m
bef_bin = dec2bin ( str2num (bef_dec) ) ;

%                                       &&&&&&&&&&&&

% 6) Now, finally, deal with the Fractional Part.

len_strFr = length (Fr_dec) ;
% eg of Fr_dec = '.123456789'  or  = '.000000001'  or  = '.12402343750000'

% The Fractional Part Fr_bin should start here with the dot :
% We will later concatenate bef_bin and Fr_bin
%
% Note : The part about the Fractional Part Fr_bin is not as starightforward
% as the Fractional part Fr_dec in the dual file Fr_bin2dec.m
% It is more complex due to the fact that we need to find the decreasing
% powers of 2 that will match with Fr_dec.

Fr_bin = '.' ;

Fr_dec_Current = str2num (Fr_dec) ;

for k = 1 : 16
    if Fr_dec_Current  >=  2^(-k)
        % Fr_bin = strcat ( Fr_bin,  repmat (['0'], 1, k - length(Fr_bin)), ...
        %     '1'  ) ; % Old round about code, but it seems it still works !
        Fr_bin = strcat ( Fr_bin,  '1'  ) ;

        Fr_dec_Current = Fr_dec_Current - 2^-(k) ;

        % Don't go beyond the pt where the current decremented balance
        % is 0 or negative. This will happen if input dec is <= 2^(-16) !
        if Fr_dec_Current <= 0    % Uncomment foll when you want to see details
            % fprintf ( '\n **********  Fr_dec_Current <= 0  ********** \n' ) ;
            % fprintf ( '\n *******  Pausing ... Prees any Key  ******* \n' ) ;
            % pause
            break ;
        end

    else
        % Fr_bin = strcat ( Fr_bin,  repmat (['0'], 1, k - length(Fr_bin)), ...
        %     '0'  ) ; % Old round about code, but it seems it still works !
        Fr_bin = strcat ( Fr_bin,  '0'  ) ;
    end

end % for k = 1 : 16

% k, Fr_bin, Fr_dec_Current     % Uncomment for testing

% Note that since precision is set to 16, the limit in our code is :
% 2^(-16) = 0.0000152587890625
% So, if a fraction is less than 2^(-16), we will have Fr_bin = "."
% at this point.

%                                       ++++++++++++

% 6-b) Also, check at the next level 2^(-k-1) ie, beyond the above k
% to add 1 at the end if Fr_dec_Current >= the half mark.!
% At the limit of k = 16 above, 2^(-17) = 0.00000762939453125

% However, we need to take caution if the no is lower than 2^(-16)
% in which case Fr_bin at this point, would be just '.0000000000000000'

if length(Fr_bin) == 17  &  all ( Fr_bin == '.0000000000000000' )
% Note for R13 : If short-circuiting double && were used (not in R12),
% the 2nd expr will NOT be evaluated if the 1st is false
% ie, if false AND X is always false, so X is not computed.

% However, it is observed that even with this single &,
% the 2nd expr is not computed if the 1st expr is false.

    if Fr_dec_Current >= 2^-(17)
        % Fr_bin = strcat ( Fr_bin,  repmat ( ['0'], 1, 16 ),  '1'  ) ; % Old
        Fr_bin = strcat ( Fr_bin,  '1'  ) ;

        Fr_dec_Current = Fr_dec_Current - 2^-(17) ;
        if Fr_dec_Current  >=  2^(-18)
            Fr_bin = strcat (  Fr_bin, '1' ) ;
        end

    else
        % Fr_bin = strcat ( Fr_bin,  repmat ( ['0'], 1, 17 ),  '1'  ) ;  % Old
        Fr_bin = strcat ( Fr_bin,  '0'  ) ;

        Fr_dec_Current = Fr_dec_Current - 2^-(18) ;
        if Fr_dec_Current  >=  2^(-19)
            Fr_bin = strcat (  Fr_bin, '1' ) ;
        end
    end

elseif Fr_dec_Current  >=  2^(-k-1)
    % At this point, normally, k should be 16
    % unless at some point above, Fr_dec_Current <= 0
    Fr_bin = strcat (  Fr_bin, '1' ) ;
    % fprintf ( '\n      ************  Last 1 added.  ************ \n' ) ;
end

%                                       &&&&&&&&&&&&

% 7) Concatenate the whole integer part and the fraction parts.
Fr_bin = strcat ( bef_bin, Fr_bin ) ;

% Fr_bin

% class_Fr_bin = class(Fr_bin)        % = char (Note)
% But note that the dual function :
% Fr_dec = Fr_bin2dec (bin) returns a double !

%                                   ********************

% 8) Some additional Test Cases :

% dec < 2^(-16) = 0.0000152587890625 (nearer to 2^-16 than 2^-17)
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (0.0000131835937)
%   = 0.000000000000000011 (16 0s, 1, 1)
%
% Fr_bin = Fr_dec2bin ( Fr_bin2dec ( 0.000000000000000011 ) )
%   = 0.000000000000000011 (16 0s, 1, 1)
% Fr_bin2dec ( 0.000000000000000011 ) = 0.000011444091796875
% (= 2^-17 + 2^-18)         in place of 0.0000131835937

%                                       ++++++++++++

% dec < 2^(-16) = 0.0000152587890625 (nearer to 2^-17 than 2^-16)
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (0.0000101835937)
%   = 0.00000000000000001  (16 0s, 1)
%
% Fr_bin = Fr_dec2bin ( Fr_bin2dec ( 0.00000000000000001 ) )
%   = 0.00000000000000001  (16 0s, 1)
% Fr_bin2dec ( 0.00000000000000001 ) = 0.00000762939453125
% (= 2^-17)                in place of 0.0000101835937

%                                       ++++++++++++

% Midway betn 2^-17 and 2^-18 = 0.0000057220458984375
% dec < 2^(-17) = 0.00000762939453125   (nearer to 2^-18 than 2^-17)
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (0.0000056835937)
%   = 0.00000000000000000 (17 0s)
%
% Fr_bin = Fr_dec2bin ( Fr_bin2dec ( 0.00000000000000000 ) )
%   = 0.00000000000000000 (17 0s)
% Fr_bin2dec ( 0.00000000000000000 ) = 0.0
%                          in place of 0.0000056835937

%                                       ++++++++++++

% dec < 2^(-17) = 0.00000762939453125   (nearer to 2^-17 than 2^-18)
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (0.0000070835937)
%   = 0.000000000000000001 (17 0s, 1)
%
% Fr_bin = Fr_dec2bin ( Fr_bin2dec ( 0.000000000000000001 ) )
%   = 0.000000000000000001 (17 0s, 1)
% Fr_bin2dec ( 0.000000000000000001 ) = 0.000003814697265625
% (= 2^-18)                 in place of 0.0000070835937

%                                       ++++++++++++

% dec < 2^(-17) = 0.00000762939453125   (nearer to 2^-18 than 2^-17)
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (0.0000039935937)
%   = 0.00000000000000000  (17 0s)
%
% Fr_bin = Fr_dec2bin ( Fr_bin2dec ( 0.00000000000000000 ) )
%   = 0.00000000000000000 (17 0s)
% Fr_bin2dec ( 0.00000000000000000 ) = 0.0
%                          in place of 0.0000039935937
%                          in place of anything < (2^-17 - 2^-19)
%                          ie, < Midway betn 2^-17 and 2^-18
%                          ie, < 0.0000057220458984375

%                                       ++++++++++++

% dec = 0.00001652587890625  very slightly >  2^(-16) = 0.0000152587890625
% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (0.00001652587890625)
%   = 0.0000000000000001   (15 0s, 1)
%
% Fr_bin = Fr_dec2bin ( Fr_bin2dec ( 0.0000000000000001 ) )
%   = 0.0000000000000001   (15 0s, 1)
% Fr_bin2dec ( 0.0000000000000001 ) = 0.0000152587890625
%                         in place of 0.00001652587890625

%                                       ++++++++++++

% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (999 + 2^-11 + 2^-9)
%   = 1111100111.00000000101
%
% Fr_bin = Fr_dec2bin ( Fr_bin2dec ( 1111100111.00000000101 ) )
%   = 1111100111.00000000000000000
%
% Fr_bin2dec ( 1111100111.00000000101 ) = 999
% [Fr_dec, str_Fr, Fr_bin] = Fr_bin2dec ( 1111100111.00000000101 )
% gives Fr_dec = 999 in place of 999.00244140625 ,
% str_Fr = 1111100111 and an empty Fr_bin
% because of the precision = 16 limit !
%
% However, Fr_bin2dec ( .00000000101 ) = 0.00244140625
% This shows that if the all the digits of the input bin are used
% for a pure fraction, the results are likely to be more accurate
% since we have more margin wrt the limit of 16 digits.

%                                       ++++++++++++

% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (879.0010365625)
%   = 1101101111.00000000010000111
%
% Fr_bin = Fr_dec2bin ( Fr_bin2dec ( 1101101111.00000000010000111 ) )
%   = 1101101111.00000000000000000
% Fr_bin2dec ( 1101101111.00000000010000111 ) = 879
%                                   in place of 879.0010365625

%                                       ++++++++++++

% [Fr_bin, str_Fr, Fr_dec] = Fr_dec2bin (879.0012765625)
%   = 1101101111.00000000010100111
%
% Fr_bin = Fr_dec2bin ( Fr_bin2dec ( 1101101111.00000000010100111 ) )
%   = 1101101111.00000000000000000
% Fr_bin2dec ( 1101101111.00000000010100111 ) = 879
%                                   in place of 879.0012765625

%                                       ++++++++++++

%                                   ********************

% 9) Some useful values :
% (Pl note that the char length below in each line may cross 80 chars !
% But wrapping will not look nice nor easy to understand !)
%
% 2^-9    = 0.001953125
% 2^-10   = 0.0009765625
% 2^-11   = 0.00048828125
% 2^(-14) = 0.00006103515625
% 2^(-15) = 0.000030517578125
% 2^(-16) = 0.0000152587890625
% 2^(-17) = 0.00000762939453125
% 2^(-18) = 0.000003814697265625
% 2^(-19) = 0.0000019073486328125
%
% The pgm was tested with these values during development.
% These values can be spot-tested by testing the result of Fr_dec2bin ( dec). For eg :
% Fr_dec2bin ( 0.000285828865257397324183692319802554 ) ; = 0.00000000000100101
%
% Test Base = 2^(-15) :
% 2^(-15) + 2^(-16)    = 0.0000457763671875                         0.0000000000000011
%
% 2^(-15) + 2^(-16.9)  = 0.0000386945607188132718216934686662547    0.00000000000000101
% 2^(-15) + 2^(-17)    = 0.00003814697265625                        0.00000000000000101
% 2^(-15) + 2^(-17.1)  = 0.0000376360549281067460325725041667934    0.0000000000000010
%
% 2^(-15) + 2^(-18)    = 0.000034332275390625                       0.0000000000000010
% 2^(-15) + 2^(-18.01) = 0.0000343059253518563686429336937594913    0.0000000000000010
% 2^(-15)              = 0.000030517578125

% Test Base = 2^(-14) :
% 2^(-14) + 2^(-15)    = 0.000091552734375                          0.000000000000011
% 2^(-14) + 2^(-15.1)  = 0.0000895090634624269841302900166671735    0.00000000000001011
%
% 2^(-14) + 2^(-15.55) = 0.0000826143426875777442749281116365005    0.0000000000000101
% 2^(-14) + 2^(-16)    = 0.0000762939453125                         0.0000000000000101
%
% 2^(-14) + 2^(-16.55) = 0.0000714572163143493310459230799506385    0.00000000000001001
% 2^(-14) + 2^(-17)    = 0.00006866455078125                        0.00000000000001001
%
% 2^(-14) + 2^(-18)    = 0.000064849853515625                       0.0000000000000100
% 2^(-14) + 2^(-18.01) = 0.0000648235034768563686429336937594913    0.0000000000000100
% 2^(-14)              = 0.00006103515625                           0.00000000000001

% Test Base = 2^(-13) :

% 2^(-13) + 2^(-14)    = 0.00018310546875                           0.00000000000011
% 2^(-13) + 2^(-14.01) = 0.00018268386812970189828693910015186      0.00000000000010111
% 2^(-13) + 2^(-14.1)  = 0.000179018126924853968260580033334347     0.00000000000010111
% 2^(-13) + 2^(-14.45) = 0.000166750662107715619528003885783119     0.00000000000010101
%
% 2^(-13) + 2^(-14.75) = 0.000158362033538901399741134642656265     0.0000000000001010
% 2^(-13) + 2^(-15)    = 0.000152587890625                          0.000000000000101
%
% 2^(-13) + 2^(-15.75) = 0.000140216173019450699870567321328132     0.0000000000001001
% 2^(-13) + 2^(-16)    = 0.0001373291015625                         0.0000000000001001
% 2^(-13)              = 0.0001220703125                            0.0000000000001

% Test Base = 2^(-12) :
% 2^(-12) + 2^(-13)    = 0.0003662109375                            0.0000000000011
% 2^(-12) + 2^(-13.55) = 0.000327517105514794648367384639605108     0.0000000000010101
%
% 2^(-12) + 2^(-14)    = 0.00030517578125                           0.00000000000101
% 2^(-12) + 2^(-14.55) = 0.000285828865257397324183692319802554     0.00000000000100101
%
% 2^(-12) + 2^(-15)    = 0.000274658203125                          0.000000000001001
% 2^(-12)              = 0.000244140625                             0.000000000001

% Test Base = 2^(-1) :
% 2^(-1) + 2^(-16)     = 0.5000152587890625                         0.1000000000000001
% 2^(-1) + 2^(-16.1)   = 0.500014236953606213492065145008334        0.10000000000000001

% 2^(-1) + 2^(-16.9)   = 0.500008176982593813271821693468666        0.10000000000000001
% 2^(-1) + 2^(-17)     = 0.50000762939453125                        0.10000000000000001
% 2^(-1) + 2^(-17.1)   = 0.500007118476803106746032572504167        0.1000000000000000

% 2^(-1) + 2^(-17.99)  = 0.500003841230583407283053713600975        0.1000000000000000
% 2^(-1) + 2^(-18)     = 0.500003814697265625                       0.1000000000000000
% 2^(-1) + 2^(-18.01)  = 0.500003788347226856368642933693759        0.1000000000000000
% 2^(-1)               = 0.5                                        0.1

% Test Base = 2^(-16) :
% 2^(-16) + 2^(-16.99) = 0.0000229412502293145661074272019509372    0.00000000000000011
% 2^(-16) + 2^(-17)    = 0.00002288818359375                        0.00000000000000011
% 2^(-16) + 2^(-17.01) = 0.0000228354835162127372858673875189825    0.0000000000000001
%
% 2^(-16) + 2^(-17.99) = 0.0000191000196459072830537136009754686    0.0000000000000001
% 2^(-16) + 2^(-18)    = 0.000019073486328125                       0.0000000000000001
% 2^(-16) + 2^(-18.01) = 0.0000190471362893563686429336937594913    0.0000000000000001
%
% 2^(-16) + 2^(-19)    = 0.0000171661376953125                      0.0000000000000001
% 2^(-16)              = 0.0000152587890625                         0.0000000000000001

% Test Base = 2^(-17) :
% 2^(-17)              = 0.00000762939453125                        0.00000000000000001

% Test Base = 2^(-18) :
% 2^(-18)              = 0.000003814697265625                       0.0000000000000000

% Midway betn 2^-17 and 2^-18  =  0.0000057220458984375 is just above 0 ;
% 0.0000057220458984375  is the limit for this set of programmes.
% Anything < (2^-17 - 2^-19) ie, anything < 0.0000057220458984375 is 0.
%
%                                   ********************


function [A,B,D,E,I]=twt(X);
% subprogram for Twin T circuit
%
N=3; % N = no. of L's & C's
M=1; % M = no. of indep inputs
U=2; % U = no. of dep nodes in dc equivalent circuit
Y=2; % Y = output node in dc equiv ckt = V3
%
% X = [R1 R3 R5 R7 C2 C4 C6];
%      1  2  3  4  5  6  7
%
R1=X(1);R3=X(2);R5=X(3);R7=X(4);C2=X(5);C4=X(6);C6=X(7);
% Create array space
A1=zeros(U+N);B2=zeros(U+N,N+M);V=zeros(U+N,N+M);H=zeros(N,M+N);
%
% Build A1 matrix.
%
A1=[1/R5 0 0 -1 -1;
   0 -1/R3  1 0 0;
   0 1/R3+1/R7 0 0 1;
   1 0 0 0 0;
   -1 1 0 0 0];
%
% Fill in B2 array
E2=1;E4=1;E6=1;Ein=1;
%
B2=[0 0 0 0;
   -E2*(1/R1+1/R3) 0 0 Ein/R1;
   E2/R3 0 0 0;
   0 -E4 0 Ein;
   0 0 E6 0];
%
P=diag([C2 C4 C6]);
%
% As stated previously, the following code is
% the same for every circuit.
%
V=A1\B2;H=V(U+1:U+N,1:N+M);I=eye(N);
AB=P\H;A=AB(1:N,1:N);B=AB(1:N,N+1:N+M);
D=V(Y:Y,1:N);E=V(Y:Y,N+1:N+M);


% function Css = synsq_adm(type, opt)
%
% Calculate the Synchrosqueezing admissibility constant, the term
% R_\psi in Eq. 3 of [1].  Note, here we multiply R_\psi by the
% inverse of log(2)/nv (found in Alg. 1 of that paper).
%
% 1. E. Brevdo, N.S. Fučkar, G. Thakur, and H-T. Wu, "The
% Synchrosqueezing algorithm: a robust analysis tool for signals
% with time-varying spectrum," 2011.
%
% Uses numerical integration.
%
% Input:
%   type: type of wavelet (see help wfiltfn)
%   opt: options structure (wavelet parameters, see help wfiltfn)
%
% Output:
%   Css: proportional to 2*int(conj(f(w))/w, w=0..inf)
%
%---------------------------------------------------------------------------------
%    Synchrosqueezing Toolbox
%    Authors: Eugene Brevdo (http://www.math.princeton.edu/~ebrevdo/)
%---------------------------------------------------------------------------------
function Css = synsq_adm(type, opt)
    if nargin<2, opt=struct(); end
    switch type
      % case 'sombrero',
      %   if ~isfield(opt,'s'), s = 1; else s = opt.s; end
      %   Cpsi = (4/3)*s*sqrt(pi);
      % case 'shannon',
      %   Cpsi = log(2);
      otherwise
        psihfn = wfiltfn(type, opt);
        Css = quadgk(@(x) conj(psihfn(x))./x, 0, Inf);
    end

    % Normalization constant, due to logarithmic scaling in wavelet
    % transform
    Css = Css / (sqrt(2*pi)*2*log(2));
end


% ir_ct_roi_split1.m
% "frequency split" approach to ROI recon of Lin Fu et al. from Fully 3D 2015.
% Low frequency part of ROI sinogram comes from reprojecting FBP ROI image,
% whereas high frequency part comes from original sinogram.
% ROI reconstructed using PWLS-OS-LALM
% 2015-06-17 Jeff Fessler, University of Michigan

if ~isvar('Af'), printm 'setup geometry'
	f.down = 4;
	igf = image_geom('nx', 512, 'fov', 50, 'down', f.down);
	igf.mask = igf.circ > 0;
	sg = sino_geom('ge1', 'units', 'cm', 'strip_width', 'd', ...
		'down', f.down);

	igr = igf;
	igr.mask = igr.circ(11, 11, -10, 4) > 0;
	im(igf.mask + igr.mask)

	% system objects
	if has_mex_jf
		Af = Gtomo2_dscmex(sg, igf); % full
		Ar = Gtomo2_dscmex(sg, igr); % roi
	else
		Af = Gtomo_nufft_new(sg, igf);
		Ar = Gtomo_nufft_new(sg, igr);
	end
end

if ~isvar('xtrue'), printm 'xtrue, sinogram'
	% read image
    xtrue256 = ir_get_data('ncat_256_slice_140_ct_x100.fld');
	xtrue256 = single(xtrue256) / 200 * 0.4; % convert to 1/cm units

	if 1 % more realistic sinogram from finer image, avoid "inverse crime"
		ig_big = image_geom('nx', 512, 'fov', igf.fov, 'down', 2);
		if has_mex_jf
			Abig = Gtomo2_dscmex(sg, ig_big);
		else
			Abig = Gtomo_nufft_new(sg, ig_big);
		end
		sino_true = Abig * xtrue256;
	end
	xtrue = downsample2(xtrue256, f.down/2);

	im plc 2 2
	clim = [0 0.4];
	im(1, xtrue, 'x true', clim), cbar
	xlabelf('units: 1 / %s', sg.units)
	im(2, sino_true, 'sino true'), cbar
	im(3, xtrue + igf.mask + igr.mask)

	clear ddir ig_big Abig
drawnow
end

mask2 = conv2(single(igr.mask), ones(9)/9^2, 'same') > 0.999; % for roi rmse
im(3, igf.mask + igr.mask + mask2, 'ROIs')
xl = @(x) xlabelf('RMSE = %.3f / %s', rms(col(x - xtrue)), sg.units);
xr = @(x) xlabelf('RMSE = %.3f / %s', rms(x(mask2) - xtrue(mask2)), sg.units);

if ~isvar('sino'), printm 'noisy sinogram'
	rng(0)
	% transmission data:
	I0 = 1e5;
	yi = poisson(I0 * exp(-sino_true), 0, 'factor', 0.1);
	if any(yi(:) == 0)
		warn('%d of %d values are 0 in sinogram!', ...
			sum(yi(:)==0), length(yi(:)));
	end
	sino = log(I0 ./ max(yi,1)); % noisy fan-beam sinogram
	im(4, sino, 'sino noisy'), cbar
drawnow
%	ir_savefig ir_ct_roi_split1a
end

if ~isvar('fbp'), printm 'fbp 2d fan-beam reconstruction'
	tmp = fbp2(sg, igf);
	fbp = fbp2(sino, tmp, 'window', 'hanning,0.75');
	im(2, fbp, 'FBP Hanning', clim), cbar
	xl(fbp)
prompt
end

if ~isvar('kappa'), printm 'kappa: try to make resolution approximately uniform'
	wi = yi; % will give 0 weight to any ray where yi=0!
	kappa = sqrt( div0(Af' * wi, Af' * ones(size(wi))) );
	im(3, kappa, 'kappa'), cbar
prompt
end

% use local psf to help select beta
if ~isvar('R'), printm 'R'
	f.l2b = 10; % maybe a bit too big, but ok for now
	f.delta = 0.001;
%	f.pot_arg = {'lange3', f.delta}; % todo: why not as sharp as hyper3?
	f.pot_arg = {'hyper3', f.delta};
	R = Reg1(kappa, 'beta', 2^f.l2b, 'pot_arg', f.pot_arg);
	Rr = Reg1(kappa .* igr.mask, 'beta', 2^f.l2b, 'pot_arg', f.pot_arg);
%	qpwls_psf(A, R, 1, igf.mask, Gdiag(wi), 'loop', 1); % use this to choose beta
end

f.niter = 6;
f.nblock = 41; % 41 subsets

% OS-LALM
if ~isvar('xlalmf'), printm 'iterative reconstruction - lalm full'
	Ab = Gblock(Af, f.nblock);
	xlalmf = ir_pwls_os_lalm(fbp(igf.mask), Ab, sino, R, 'wi', wi, ...
		'isave', 'last', 'niter', f.niter);
	xlalmf = igf.embed(xlalmf);
	im(4, xlalmf(:,:,end), 'LALM full', clim), cbar
	xl(xlalmf(:,:,end))
%	ir_savefig ir_ct_roi_split1b
end

if ~isvar('fbpr'), printm 'fbp 2d fan-beam reconstruction - roi'
	tmp = fbp2(sg, igr);
	fbpr = fbp2(sino, tmp);
	im(1, xtrue .* igr.mask, 'True roi', clim), cbar
	im(2, fbpr, 'FBP roi', clim), cbar
	xr(fbpr)
prompt
end

if ~isvar('Hhi'), printm 'filters'
	nf = 2 * sg.nb;
	u = [-nf/2:nf/2-1]'/nf;
	cut = 0.1;
	Hhi = min((u/cut).^2, 1);
	Hlo = 1 - Hhi;
	plot(u, Hlo, '-o')
	clear u
end

if ~isvar('sino_roi'), printm 'sino_roi'
	sino_f = fft(sino, nf, 1);
	sino_hi = ifft(sino_f .* repmat(ifftshift(Hhi), [1 sg.na]), [], 1);
	sino_hi = sino_hi(1:sg.nb, :);
	im(1, fftshift(sino_f, 1))
	im(2, sino_hi, 'Hi-pass sino')

	sino_Ar = Ar * fbpr;
	sino_Arf = fft(sino_Ar, nf, 1);
	im(1, sino_Ar, 'Reproj FBP')
        sino_lo = ifft(sino_Arf .* repmat(ifftshift(Hlo), [1 sg.na]), [], 1);
	sino_lo = sino_lo(1:sg.nb, :);
	im(3, sino_lo, 'Low-pass sino')

	sino_roi = sino_lo + sino_hi;
	im(4, sino_roi, 'Synth ROI sino')
	clear sino_f sino_Arf
%	ir_savefig ir_ct_roi_split1c
prompt
end

% OS-LALM
if ~isvar('xlalmr'), printm 'iterative reconstruction - lalm roi'
	Ab = Gblock(Ar, f.nblock);
	xlalmr = ir_pwls_os_lalm(fbpr(igr.mask), Ab, sino_roi, Rr, 'wi', wi, ...
		'isave', 'last', 'niter', 2*f.niter);
	xlalmr = igr.embed(xlalmr);
end

if 1 % pics
	im(4, xlalmr(:,:,end), 'PWLS-OS-LALM roi', clim), cbar
	xr(xlalmr(:,:,end))

	im(1, xtrue .* igr.mask, 'True roi', clim), cbar
	im(2, fbpr, 'FBP Ramp roi', clim), cbar
	xr(fbpr)
	im(3, fbp .* igr.mask, 'FBP Hanning roi', clim), cbar
	xr(fbp)
%	ir_savefig ir_ct_roi_split1d
end


clear all
B0 = 1; Rtriple = 2; Damnio = 3;
B1 = 4; Ramnio = 5; Dabort = 6;
B2 = 7; U = 8;

N = 8;
dag = zeros(N,N);
dag(B0, [Rtriple B1 Ramnio]) = 1;
dag(Rtriple, [Damnio Dabort]) = 1;
dag(Damnio, [B1 Ramnio]) = 1;
dag(B1, B2) = 1;
dag(Ramnio, [Dabort U]) = 1;
dag(Dabort, B2) = 1;
dag(B2, U) = 1;

ns = zeros(1,N);
ns(B0) = 2;
ns(B1) = 3;
ns(B2) = 4;
ns(Rtriple) = 2;
ns(Ramnio) = 3;
ns(Damnio) = 2;
ns(Dabort) = 2;
ns(U) = 1;

limid = mk_limid(dag, ns, 'chance', [B0 B1 B2], ...
		 'decision', [Damnio Dabort], 'utility', [U]);

% states of nature
healthy = 1; downs = 2; miscarry = 3; aborted = 4;
% test results
pos = 1; neg = 2; unk = 3;
% actions
yes = 1; no = 2;

% Prior probability baby has downs syndrome
tbl = zeros(2,1);
p = 1/1000; % from www.downs-syndrome.org.uk figure
p = 24/10000; % www-personal.umich.edu/~bobwolfe/560/review/Downs.pdf (for women agen 35-40)
tbl(healthy) = 1-p;
tbl(downs) = p;
limid.CPD{B0} = tabular_CPD(limid, B0, tbl);

% Reliability of triple screen test
% Unreliable sensor
% B0 -> Rtriple
tbl = zeros(2,2); % Rtriple = pos, neg
p = 0.5;  % high false positive rate (guess)
tbl(healthy, :) = [p 1-p];
p = 0.6; % low detection rate (march of dimes figure)
tbl(downs, :) = [p 1-p];
limid.CPD{Rtriple} = tabular_CPD(limid, Rtriple, tbl);

limid.CPD{Damnio} = tabular_decision_node(limid, Damnio);

% Effect of amnio on baby  B0,Damnio -> B1
 % 1/200 risk of miscarry
p = 1/200; % (march of dimes figure)
tbl = zeros(2, 2, 3); % B1 = healthy, downs, miscarry
tbl(healthy, no, :) =  [1     0     0];
tbl(downs, no, :) =    [0     1     0];
tbl(healthy, yes, :) = [1-p     0   p];
tbl(downs, yes, :) =   [0     1-p   p];
limid.CPD{B1} = tabular_CPD(limid, B1, tbl);

% Reliability of amnio  B0, Damnio -> Ramnio
% Perfect sensor
tbl = zeros(2,2,3); % Ramnio = pos, neg, unk
tbl(:, no, :) =        repmat([0 0 1], 2 ,1);
tbl(healthy, yes, :) = [0 1 0];
tbl(downs, yes, :) =   [1 0 0];
limid.CPD{Ramnio} = tabular_CPD(limid, Ramnio, tbl);

limid.CPD{Dabort} = tabular_decision_node(limid, Dabort);

% Effect of abortion on baby  B1, Dabort -> B2
tbl = zeros(3, 2, 4); % B2 = healthy, downs, miscarry, aborted
tbl(:, yes, :) =       repmat([0 0 0 1], 3, 1);
tbl(healthy, no, :) =  [1 0 0 0];
tbl(downs, no, :) =    [0 1 0 0];
tbl(miscarry, no, :) = [0 0 1 0];
limid.CPD{B2} = tabular_CPD(limid, B2, tbl);

% Utility U(Ramnio, B2)
tbl = zeros(3, 4);
tbl(:, healthy) = 5000;
tbl(:, downs) = -50000;
tbl(:, miscarry) = -1000;
tbl(:, aborted) = -1000;

if 0
%tbl(unk, miscarry) = 0; % this case is impossible
tbl(pos, miscarry) = -1;
tbl(neg, miscarry) = -1000;
if 1
  tbl(unk, aborted) = -100;
  tbl(pos, aborted) = -1;
  tbl(neg, aborted) = -500;
else % pro-life utility fn
  tbl(unk, aborted) = -500000;
  tbl(pos, aborted) = -500000;
  tbl(neg, aborted) = -500000;
end
end

limid.CPD{U} = tabular_utility_node(limid, U,  tbl);

engine = jtree_limid_inf_engine(limid);
[strategy, MEU] = solve_limid(engine);

% Rtriple U(Damnio=1=yes)  U(Damnio=2=no)
% 1=pos    0               1
% 2=neg    0               1
dispcpt(strategy{Damnio})
if isequal(strategy{Damnio}(1,:), strategy{Damnio}(2,:))
  % Rtriple result irrelevant
  doAmnio = argmax(strategy{Damnio}(1,:))
else
  doAmnio = 1;
end

% Rtriple Ramnio U(Dabort=yes=1) U(Dabort=no=2)
% 1=pos   1=pos  1               0
% 2=neg   1=pos  1               0
% 1=pos   2=neg  0               1
% 2=neg   2=neg  0               1
% 1=pos   3=unk  0               1
% 2=neg   3=unk  0               1
dispcpt(strategy{Dabort})


% to test whether scg inference engine can handl dynameic BN
% Make a linear dynamical system
%   X1 -> X2
%   |     |
%   v     v
%   Y1    Y2

intra = zeros(2);
intra(1,2) = 1;
inter = zeros(2);
inter(1,1) = 1;
n = 2;

X = 2; % size of hidden state
Y = 2; % size of observable state

ns = [X Y];
dnodes = [];
onodes = [2];
eclass1 = [1 2];
eclass2 = [3 2];
bnet = mk_dbn(intra, inter, ns, dnodes, eclass1, eclass2);

x0 = rand(X,1);
V0 = eye(X);
C0 = rand(Y,X);
R0 = eye(Y);
A0 = rand(X,X);
Q0 = eye(X);

bnet.CPD{1} = gaussian_CPD(bnet, 1, 'mean', x0, 'cov', V0);
%bnet.CPD{2} = gaussian_CPD(bnet, 2, 'mean', zeros(Y,1), 'cov', R0, 'weights', C0, 'full', 'untied', 'clamped_mean');
%bnet.CPD{3} = gaussian_CPD(bnet, 3, 'mean', zeros(X,1), 'cov', Q0, 'weights', A0, 'full', 'untied', 'clamped_mean');
bnet.CPD{2} = gaussian_CPD(bnet, 2, 'mean', zeros(Y,1), 'cov', R0, 'weights', C0);
bnet.CPD{3} = gaussian_CPD(bnet, 3, 'mean', zeros(X,1), 'cov', Q0, 'weights', A0);

T = 5; % fixed length sequences

clear engine;
%engine{1} = kalman_inf_engine(bnet, onodes);
engine{1} = scg_unrolled_dbn_inf_engine(bnet, T, onodes);
engine{2} = jtree_unrolled_dbn_inf_engine(bnet, T);

N = length(engine);

% inference

ev = sample_dbn(bnet, T);
evidence = cell(n,T);
evidence(onodes,:) = ev(onodes, :);

t = 2;
query = [1 3];
m = cell(1, N);
ll = zeros(1, N);

engine{1} = enter_evidence(engine{1}, evidence);
[engine{2}, ll(2)] = enter_evidence(engine{2}, evidence);
m{1} = marginal_nodes(engine{1}, query);
m{2} = marginal_nodes(engine{2}, query, t);

% compare all engines to engine{1}
for i=2:N
  assert(approxeq(m{1}.mu, m{i}.mu));
  assert(approxeq(m{1}.Sigma, m{i}.Sigma));
%  assert(approxeq(ll(1), ll(i)));
end


function B=nancumsum(A,dim,nmode)
% NANCUMSUM: Cumulative sum of a matrix, with user-specified treatment of NaNs.
%     Computes the cumulative sum of matrix A along dimension DIM, allowing
%     the user to replace NaNs with zeros, to skip over them, or to reset
%     on NaNs, maintaining NaNs as placeholders.
%
% USAGE: B = nancumsum(A, DIM, NMODE)
%
% ARGUMENTS:
%
% A:    Input matrix.
%
% B:    Output cumulative sum matrix, treating NaNs as determined by nmode.
%
% DIM:  B = nancumsum(A, DIM) returns the nan-cumulative sum of the elements
%       along the dimension of A specified by scalar DIM. For example,
%       nancumsum(A,1) works down the columns, nancumsum(A,2) works
%       across the rows. If DIM is not specified, it defaults to the first
%       non-singleton dimension of A.
%
% NMODE: specifies how NaNs should be treated. Acceptable values are:
%       1: REPLACE NaNs with zeros (default).
%       2: MAINTAIN NaNs as position holders in B. (Skip NaNs without reset.)
%       3: RESET sum on NaNs, replacing NaNs with zeros.
%       4: RESET sum on NaNs, maintaining NaNs as position holders.
%
% EXAMPLES:
%
% 1) a = [NaN,2:5];
%
% nancumsum(a)
% ans =
%     0     2     5     9    14
%
% nancumsum(a,[],2)
% ans =
%   NaN     2     5     9    14
%
% nancumsum(a,[],3)
% ans =
%     2     5     9    14
%
% 2) a = magic(3); a(5)=NaN;
%
% b = nancumsum(a,2) % (Default NMode = 1)
% b =
%     8     9    15
%     3     3    10
%     4    13    15
%
% b = nancumsum(a,2,2)
% b =
%     8     9    15
%     3   NaN    10
%     4    13    15
%
% b = nancumsum(a,2,3)
% b =
%     8     9    15
%     3     0     7
%     4    13    15
%
% b = nancumsum(a,2,4)
% b =
%     8     9    15
%     3   NaN     7
%     4    13    15

% See also: cumsum, nansum, nancumprod, nanmean, nanmedian, ...
% (nancumprod is available from the FEX. Other nan* may require Toolboxes)

% Brett Shoelson
% brett.shoelson@mathworks.com
% 05/04/07
%
% Revision: 08/28/11
% Fixed bug in option 2 (faulty reset). Thanks to Andrew Stevens and Rick
% Patterson for reporting it. Also, eliminated old option 3 (deleting NaNs
% in a vector) as a trivial case and added two new options.
% Revision: 09/15/11
% Fixed a bug with multiple NaNs. Thanks to Tim Yates.
%
% Copyright The MathWorks, Inc. 2011

% Set defaults, check and validate inputs
if nargin < 3
    nmode = 1;
end

if ~ismember(nmode,1:4)
    error('NANCUMSUM: unacceptable value for nmode parameter.');
end

if nargin < 2 || isempty(dim)
    if ~isscalar(A)
        dim = find(size(A)>1);
        dim = dim(1);
    else
        % For scalar inputs (no nonsingleton dimension)
        dim = 1;
    end
end

% Calculate cumulative sum, depending on selection of nmode
switch nmode
    case 1
        % TREAT NaNs as 0's
        B = A;
        B(B~=B) = 0;
        B = cumsum(B, dim);
    case 2
        % DO NOT INCREMENT, BUT USE NaNs AS PLACEHOLDERS.
        B = nancumsum(A, dim, 1);
        B(A~=A) = NaN;
     case 3
        % RESET sum on NaNs, replacing NaNs with zeros.
        naninds = find(A~=A);
        for ii = 1:numel(naninds)
            B = nancumsum(A, dim, 1);
            A(naninds(ii)) = -B(naninds(ii));
        end
        B = cumsum(A,dim);
    otherwise %case 4
        % RESET sum on NaNs, maintaining NaNs as position holders.
        naninds = find(A~=A);
        B = nancumsum(A,dim,3);
        B(naninds)= NaN;
end


function linpack_d_test02 ( )

%*****************************************************************************80
%
%% TEST02 tests DCHEX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 5;
  lda = n;
  nz = 1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02\n' );
  fprintf ( 1, '  For double precision, general storage,\n' );
  fprintf ( 1, '  DCHEX can shift columns in a Cholesky factorization.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The number of equations is N = %d\n', n );
%
%  Set the values of the matrix A.
%
  a(1:n,1:n) = 0.0;

  for i = 1 : n
    a(i,i) = 2.0;
  end

  for i = 1 : n-1
    a(i,i+1) = -1.0;
  end

  for i = 2 : n
    a(i-1,i) = -1.0;
  end

  for i = 1 : n
    z(i,1) = i;
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix A:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  %14f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The vector Z:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %14f\n', z(i) );
  end
%
%  Decompose the matrix.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Decompose the matrix.\n' );

  job = 0;
  ipvt(1:n) = 0;

  [ a, ipvt, info ] = dchdc ( a, lda, n, ipvt, job );

  if ( info ~= n )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DCHDC returned INFO = %d\n', info );
    fprintf ( 1, '  This means the matrix is not positive definite.\n' );
  end
%
%  Zero out the lower diagonal.
%
  for i = 2 : n
    for j = 1 : i-1
      a(i,j) = 0.0;
    end
  end
%
%  Print the factorization.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The Cholesky factor U:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  %14f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Right circular shift columns L through K.
%
  k = 1;
  l = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '  Right circular shift columns K  = %d through L = %d\n', k, l );

  job = 1;
  [ a, z, c, s ] = dchex ( a, lda, n, k, l, z, n, nz, job );
%
%  Left circular shift columns K+1 through L.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '  Left circular shift columns K+1 = %d through L = %d\n', k+1, l );

  job = 2;
  [ a, z, c, s ] = dchex ( a, lda, n, k+1, l, z, n, nz, job );
%
%  Print the factorization.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The shifted Cholesky factor U:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  %14f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The shifted vector Z:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %14f\n', z(i) );
  end
%
%  Compute the Cholesky product.
%
  a(1:n,1:n) = a(1:n,1:n)' * a(1:n,1:n);

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The shifted product U'' * U:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  %14f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


%                           solveCoordinateDescent.m
%
%  Implementation of the Coordinate Descent algorithm proposed in the paper.
%
%% I/O
%  Inputs:
%     A:    m x n matrix or a function handle to a method that
%           returns A*x.
%     At:   The adjoint (transpose) of 'A'. If 'A' is a function handle, 'At'
%           must be provided.
%     b0:   m x 1 real,non-negative vector consists of all the measurements.
%     x0:   n x 1 vector. It is the initial guess of the unknown signal x.
%     opts: A struct consists of the options for the algorithm. For details,
%           see header in solvePhaseRetrieval.m or the User Guide.
%
%     Note: When a function handle is used, the
%     value of 'At' (a function handle for the adjoint of 'A') must be
%     supplied.
%
%  Outputs:
%     sol:  n x 1 vector. It is the estimated signal.
%     outs: A struct consists of the convergence info. For details,
%           see header in solvePhaseRetrieval.m or the User Guide.
%
%
%  See the script 'testCoordinateDescent.m' for an example of proper usage
%  of this function.
%
%% Notations
%  Notations mainly follow the paper's notation.
%
%% Algorithm Description
%  CD is an iterative procedure that successively minimizes the objective
%  function along coordinate directions. A single unknown is solved at each
%  iteration while all other variables are kept fixed. As a result, only
%  minimization of a univariate quartic polynomial is needed which is
%  easily achieved by finding the closed-form roots of a cubic equation.
%
%  Specifically, the method has the following steps: It keeps running until
%  the normalized gradient becomes smaller than the tolerance (1) At each
%  iteration, use the selected rule to choose an index i. (2) Minimize the
%  objective f with respect to the ith variable while keeping
%      all other 2n-1 (both real and imaginary parts are variables so there
%      are 2n in total) variables fixed by solving the cubic equation to
%      get alpha that minimize the objective along the ith variable.
%  (3) update the estimate along the ith variable by alpha.
%
%% References
%  Paper Title:   Coordinate Descent Algorithms for Phase Retrieval
%  Place:         Chapter II.B
%  Authors:       Wen-Jun Zeng, H. C. So
%  arXiv Address: https://arxiv.org/abs/1706.03474
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START-----------------------------------

function [sol, outs] = solveCoordinateDescent(A, At, b0, x0, opts)

    validateOptions(opts); % Check the validity of algorithm-specific options

    % Initialization
    m = length(b0);
    n = length(x0);
    sol = x0;
    Ax = A(sol);

    % Initialize values potentially computed at each round.
    currentTime = [];
    currentResid = [];
    currentReconError = [];
    currentMeasurementError = [];

    % Initialize vectors for recording convergence information
    [solveTimes,measurementErrors,reconErrors,residuals] = initializeContainers(opts);

    maxDiff = -inf;

    C = zeros(m, 3);

    % Used to compute gradient of objective function (used by greedy index
    % choice rule)
    f = @(z) (abs(z).^2 - b0.^2) .* z;

    startTime = tic; % Begin timer
    for iter = 1 : opts.maxIters
        switch lower(opts.indexChoice)
            case 'random'
                if opts.isComplex
                    index = randi(2*n);
                else
                    index = randi(n);
                end
            case 'cyclic'
                if opts.isComplex
                    index = mod(iter-1, 2*n) + 1;
                else
                    index = mod(iter-1, n) + 1;
                end
            case 'greedy'
                grad = At(f(A(sol)));

                if opts.isComplex
                    grad_bar = [real(grad); imag(grad)];
                else
                    grad_bar = grad;
                end
                [~, index] = max(abs(grad_bar));
        end

        vals = conj(A(double(1:n == mod(index-1, n)+1)'));
        for j = 1 : m
            if index > n
                C(j, 2) = 2*imag(Ax(j) * vals(j));
            else
                C(j, 2) = 2*real(Ax(j) * vals(j));
            end
            C(j, 3) = abs(vals(j))^2;
            C(j, 1) = abs(Ax(j))^2;
        end

        d_4 = sum(C(:, 3).^2);
        d_3 = sum(2 * C(:, 3) .* C(:, 2));
        d_2 = sum(C(:, 2).^2 + 2 * C(:, 3) .* (C(:, 1) - b0.^2));
        d_1 = sum(2 * C(:, 2) .* (C(:, 1) - b0.^2));

        % Desired alpha is a root of polynomial
        alphas = roots([4*d_4 3*d_3 2*d_2 d_1])';
        % Select only the real roots
        alphas = alphas(imag(alphas) == 0);
        % Function of alpha to be minimized
        g = @(x) d_4*x.^4 + d_3*x.^3 + d_2*x.^2 + d_1*x;
        % Find index of best alpha
        [~, idx] = min(g(alphas));
        alpha = alphas(idx);

        % Update x
        if (index > n)
            a_j = A(double(1:n == index - n)');
            sol(index - n) = sol(index - n) + 1i * alpha;
            Ax = Ax + (1i * alpha) * a_j;
        else
            a_j = A(double(1:n == index)');
            sol(index) = sol(index) + alpha;
            Ax = Ax + alpha * a_j;
        end

        diff = abs(alpha);
        maxDiff = max(diff, maxDiff);

        % Record convergence information and check stopping condition
        % If xt is provided, reconstruction error will be computed and used for stopping
        % condition. Otherwise, residual will be computed and used for stopping
        % condition.
        if ~isempty(opts.xt)
            x = sol;
            xt = opts.xt;
            %  Compute optimal rotation
            alpha = (x(:)'*xt(:))/(x(:)'*x(:));
            x = alpha*x;
            currentReconError = norm(x-xt)/norm(xt);
            if opts.recordReconErrors
                reconErrors(iter) = currentReconError;
            end
        end

        if isempty(opts.xt) | opts.recordResiduals
            currentResid = diff / max(maxDiff, 1.0e-30);
        end

        if opts.recordResiduals
            residuals(iter) = currentResid;
        end

        currentTime = toc(startTime);                % Record elapsed time so far
        if opts.recordTimes
            solveTimes(iter) = currentTime;
        end
        if opts.recordMeasurementErrors
            currentMeasurementError = norm(abs(A(sol)) - b0) / norm(b0);
            measurementErrors(iter) = currentMeasurementError;
        end

        % Display verbose output if specified
        if opts.verbose == 2
            displayVerboseOutput(iter, currentTime, currentResid, currentReconError, currentMeasurementError);
        end

        %  Test stopping criteria.
        if stopNow(opts, currentTime, currentResid, currentReconError)
            break;
        end

    end

    % Create output according to the options chosen by user
    outs = generateOutputs(opts, iter, solveTimes, measurementErrors, reconErrors, residuals);

    % Display verbose output if specified
    if opts.verbose == 1
        displayVerboseOutput(iter, currentTime, currentResid, currentReconError, currentMeasurementError);
    end
end

% Validate algorithm-specific options
function validateOptions(opts)
    validIndexChoices = {'cyclic', 'random', 'greedy'};
    checkIfInList('indexChoice',opts.indexChoice,validIndexChoices);
end