%% ODE_POOL uses the MATLABPOOL command to run the ODE code.
%
%  Discussion:
%
%    Output printed by the function appears directly on the screen.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 March 2011
%
%  Author:
%
%    John Burkardt
%

%
%  Initialize the k and b ranges.
%
  bVals = 0.1 : 0.05 : 5;
  kVals = 1.5 : 0.05 : 5;
%
%  Begin the parameter sweep.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ODE_POOL\n' );
  fprintf ( 1, '  Sweep through sets of values of parameters B and K,\n' );
  fprintf ( 1, '  computing the solution of the ODE corresponding to each set.\n' );
  fprintf ( 1, '  For each solution X(T), determine the maximum value over time.\n' );
  fprintf ( 1, '  Construct a contour plot of XMAX(B,K).\n' );
  fprintf ( 1, '  Use the PARFOR command to carry out these computations in parallel.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of K values = %d\n', length ( kVals ) );
  fprintf ( 1, '  Number of B values = %d\n', length ( bVals ) );
  fprintf ( 1, '  Number of times the ODE must be solved = %d\n', ...
    length ( kVals ) * length ( bVals ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Begin computation\n' );

  matlabpool open local 4
%
%  Solve the ODE for every pair of K and B values and return the maximum
%  value over the time interval.
%
  tic
  peakVals = ode_fun ( bVals, kVals );
  toc

  matlabpool close
%
%  Now display am image of the data.
%
  ode_display ( bVals, kVals, peakVals );


 function [xs, info] = qpwls_pcg1(x, A, W, yi, C, varargin)
%function [xs, info] = qpwls_pcg1(x, A, W, yi, C, [options])
%|
%| quadratic penalized weighted least squares (QPWLS) via
%| preconditioned conjugate gradients (PCG) algorithm
%| cost(x) = (y-Ax)'W(y-Ax) / 2 + x'C'Cx / 2
%|
%| in
%|	x	[np 1]		initial estimate
%|	A	[nd np]		system matrix
%|	W	[nd nd]		data weighting matrix, usually Gdiag(wi)
%|	yi	[nd 1]		noisy data
%|	C	[nc np]		penalty 'differencing matrix' (0 for unregularized)
%|
%| options
%|	niter			# total iterations (default: 1)
%|					(max # if tol used)
%|	isave	[]		list of iterations to archive (default: 'last')
%|	userfun	@		user defined function handle (see default below)
%|					taking arguments (x, userarg{:})
%|	userarg {}		user arguments to userfun (default {})
%|	precon	[np np]		preconditioner (matrix or object) (or 1)
%|	dircheck 0|1		check descent direction? (default: 1)
%|				set to 0 to save time, if you dare...
%|	stop_diff_tol		stop iterations if norm(xnew-xold)/norm(xnew)
%|				is less than this unitless value.  default: 0
%|	stop_diff_norm		use norm(.,type) for stop rule
%|				choices: 1 | 2 (default) | inf
%|	stop_grad_tol		stop if norm(grad) / y'W y < tol; default: 0
%|	stop_grad_norm		which norm(grad) to use.  default: 2
%|	chat	0|1		verbosity (default 0)
%|
%| out
%|	xs	[np niter]	estimates each iteration
%|	info	[niter 3]	gamma, step size, time each iteration
%|
%| Copyright Jan 1998, Jeff Fessler, University of Michigan

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

% defaults
arg.precon = 1;
arg.niter = 1;
arg.isave = [];
arg.userfun = @userfun_default;
arg.userarg = {};
arg.key = 1;
arg.dircheck = true; % default is to check descent direction
arg.stop_diff_tol = 0;
arg.stop_diff_norm = 2;
arg.stop_grad_tol = 0;
arg.stop_grad_norm = 2;
arg.chat = 0;

arg = vararg_pair(arg, varargin, 'subs', ...
{'stop_threshold', 'stop_diff_tol'; 'stop_norm_type', 'stop_diff_norm'});

arg.isave = iter_saver(arg.isave, arg.niter);
if arg.stop_diff_tol
	norm_diff = @(x) norm(x, arg.stop_diff_norm);
end
if arg.stop_grad_tol
	norm_grad = @(g) norm(g, arg.stop_grad_norm) / reale(yi' * (W * yi));
end

if ~isreal(yi) && ~isequal(arg.precon, 1)
	persistent warned
	if isempty(warned), warned = 0; end
	if ~warned
		warning 'not 100% sure about the complex preconditioned case'
		warned = 1;
	end
end

cpu etic
if isempty(x), x = zeros(ncol(A),1); end

x = x(:);
np = length(x);
xs = zeros(np, length(arg.isave), 'single');
if any(arg.isave == 0)
	xs(:, arg.isave == 0) = single(x);
end

%info = zeros(arg.niter, ?); % trick: do not initialize because size may change

% initialize projections
ticker(mfilename, 1, arg.niter)
Ax = A * x;
Cx = C * x;

% iterate
for iter = 1:arg.niter
	ticker(mfilename, iter, arg.niter)

	% (negative) gradient
	ngrad = A' * (W * (yi-Ax)) - C' * Cx;

	if arg.stop_grad_tol && norm_grad(ngrad) < arg.stop_grad_tol
		if arg.chat
			printm('stop at iteration %d with grad %g < %g', ...
				iter, norm_grad(ngrad), arg.stop_grad_tol)
		end
		if isequal(arg.isave, arg.niter) % saving last iterate only?
			xs = single(x); % save 'final' iterate
		else % saving many iterates?
			xs(:, arg.isave > iter) = []; % clear out unused
		end
	return
	end

	% preconditioned gradient
	pregrad = arg.precon * ngrad;

	% search direction
	newinprod = ngrad' * pregrad;
	% fix: should i take the real part?
	newinprod = reale(newinprod, 'warn', 'inprod');
	if iter == 1
		ddir = pregrad;
		gamma = 0;
	else
		if oldinprod == 0
			warn 'inprod=0. going nowhere!'
			gamma = 0;
		else
			gamma = newinprod / oldinprod;	% Fletcher-Reeves
%			gamma = (newinprod - oldgrad' * pregrad) / oldinprod;
		end
		ddir = pregrad + gamma * ddir;
	end
	oldgrad = ngrad;
	oldinprod = newinprod;

	% check if descent direction
	if arg.dircheck && real(ddir' * ngrad) < 0
		warn 'wrong direction; try using stop_grad_tol'
		ratio = norm(ngrad(:), arg.stop_grad_norm) / (yi'*W*yi);
		pr ratio % see how small it is
		if arg.key, keyboard, end
	end

	% step size in search direction
	Adir = A * ddir;
	Cdir = C * ddir;

	denom = Adir'*(W*Adir) + Cdir'*Cdir;
	denom = reale(denom, 'error', 'denom');
	if denom == 0
		warning 'found exact solution??? step=0 now!?'
		step = 0;
	else
		step = (ddir' * ngrad) / denom;
%		step = reale(step, 'warn', 'step');
		step = real(step); % real step sizes seems only logical
	end

	if step < 0
		warning 'downhill?'
		if arg.key, keyboard, end
	end

	% update
	Ax = Ax + step * Adir;
	Cx = Cx + step * Cdir;
	x = x + step * ddir;

	if any(arg.isave == iter)
		xs(:, arg.isave == iter) = single(x);
	end
	info(iter,:) = arg.userfun(x, arg.userarg{:});

	% check norm(xnew-xold) / norm(xnew) vs threshold
	if arg.stop_diff_tol && ...
		norm_diff(step * ddir) / norm_diff(x) < arg.stop_diff_tol
		if arg.chat
			ratio = norm_diff(step * ddir) / norm_diff(x);
			printm('stop at iteration %d with diff %g < %g', ...
				iter, ratio, arg.stop_diff_tol)
		end
		if isequal(arg.isave, arg.niter) % saving last iterate only?
			xs = single(x); % save the 'final' iterate
		else % saving many iterates?
			xs(:, arg.isave > iter) = []; % clear out unused
		end
	return
	end
end

% default user function.
% using this evalin('caller', ...) trick, one can compute anything of interest
function out = userfun_default(x, varargin)
gamma = evalin('caller', 'gamma');
step = evalin('caller', 'step');
out = [gamma step cpu('etoc')];

% qpwls_pcg1_test0()
function qpwls_pcg1_test0
mask = true([8 7]); mask(1) = false;
A = Gblur(mask, 'psf', ones(3)/9);
tmp = ones(size(mask));
A = Gdiag(tmp(mask), 'mask', mask);
xtrue = zeros(size(mask), 'single');
xtrue(end/2, round(end/2)) = 1;
y = A * xtrue(mask);
beta = 2^-7;
beta = 2^-2;
R = Reg1(mask, 'beta', beta, 'order', 1);
qpwls_psf(A, R.C, 1, mask, 1, 'loop', 0);
hess = full(A' * A + R.C' * R.C);
xhat = hess \ (A' * y);
xhat = embed(xhat, mask);
pr fwhm2(xhat)
im clf, im(xhat)

% user functions for tracking time and distance to a reference image
f.userfun = @(x, xref) [cpu('etoc') norm(x(:) - xref(:))];
f.userarg = {xhat(mask)}; % reference image just for testing

xinit = 0 * mask;
xpcg = qpwls_pcg1(xinit(mask(:)), A, 1, y, R.C, 'niter', 100, ...
	'userfun', f.userfun, 'userarg', f.userarg, ...
        'stop_grad_tol', 1e-8, 'stop_grad_norm', 2, ...
        'stop_diff_tol', 0e-6, 'stop_diff_norm', 2, 'chat', 1);
xpcg = embed(xpcg, mask);

im plc 2 2
im(1, xtrue)
im(2, xhat)
im(3, xpcg)
im(4, xpcg - xhat)
equivs(xpcg, xhat)


function [error_x, error_y, fex, fey, ae] = stokespost_q1q1_p(q1q1sol,jmpx,jmpy,els,xy,ev)
%stokespost_q1q1_p  computes Poisson error estimator for Q1-Q1
%   [err_x, err_y, fex, fey, ae] = stokespost_q1p0_p(q1q1sol,jmpx,jmpy,els,xy,ev);
%   input
%          q1q1sol        Q1-Q1 flow solution
%          jmpx, jmpy     component elementwise edge stress jumps
%          els            elementwise edge lengths
%          xy             vertex coordinate vector
%          ev             element mapping matrix
%   output
%          err_x, err_y   component of velocity elementwise error estimate
%          fex, fey       component elementwise rhs vectors
%          ae             elementwise Poisson problem matrices
%
%   IFISS function: DJS; 8 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      fprintf('computing local error estimator... ')
      x=xy(:,1); y=xy(:,2);nvtx=length(x);
      nel=length(ev(:,1));
      error_x=zeros(nel,1);  error_y=zeros(nel,1);
      psol=q1q1sol(2*nvtx+1:end);
%
% set up 3x3 Gauss points
      gpt=sqrt(0.6);
      s(1) = -gpt; t(1) = -gpt; wt(1)=25/81;
      s(2) =  gpt; t(2) = -gpt; wt(2)=25/81;
      s(3) =  gpt; t(3) =  gpt; wt(3)=25/81;
      s(4) = -gpt; t(4) =  gpt; wt(4)=25/81;
      s(5) =  0.0; t(5) = -gpt; wt(5)=40/81;
      s(6) =  gpt; t(6) =  0.0; wt(6)=40/81;
      s(7) =  0.0; t(7) =  gpt; wt(7)=40/81;
      s(8) = -gpt; t(8) =  0.0; wt(8)=40/81;
      s(9) =  0.0; t(9) =  0.0; wt(9)=64/81;
%
% inner loop over elements
        for ivtx = 1:4
        xl_v(:,ivtx) = x(ev(:,ivtx));
        yl_v(:,ivtx) = y(ev(:,ivtx));
        psol_v(:,ivtx) = psol(ev(:,ivtx));
		end

        ae = zeros(nel,5,5); elerrx=zeros(5,nel);  elerry=zeros(5,nel);
        fex = zeros(nel,5); fey = zeros(nel,5);
% loop over Gauss points
         for igpt = 1:9
         sigpt=s(igpt);
         tigpt=t(igpt);
         wght=wt(igpt);
% evaluate derivatives etc
         [jac_v,invjac_v,phi_v,dphidx_v,dphidy_v] = deriv(sigpt,tigpt,xl_v,yl_v);
         [psi_v,dpsidx_v,dpsidy_v] = qderiv(sigpt,tigpt,xl_v,yl_v);

            for j = 1:5
               for i = 1:5
               ae(:,i,j) = ae(:,i,j)+wght*dpsidx_v(:,i+4).*dpsidx_v(:,j+4).*invjac_v(:);
               ae(:,i,j) = ae(:,i,j)+wght*dpsidy_v(:,i+4).*dpsidy_v(:,j+4).*invjac_v(:);
               end
               for ss=1:4
               fex(:,j) = fex(:,j)-wght*dphidx_v(:,ss).*psol_v(:,ss).*psi_v(:,j+4);
               fey(:,j) = fey(:,j)-wght*dphidy_v(:,ss).*psol_v(:,ss).*psi_v(:,j+4);
               end
            end
% end of Gauss point loop
         end
%
% include edge jumps (evaluated at the midpoint)
         for ee = 1:4
         fex(:,ee) = fex(:,ee) - jmpx(:,ee) .* els(:,ee)*(1/3);
         fey(:,ee) = fey(:,ee) - jmpy(:,ee) .* els(:,ee)*(1/3);
         end
%
% solve for local estimate
         for ielem = 1:nel
		 elerrx(:,ielem) = squeeze(ae(ielem,1:5,1:5))\(fex(ielem,1:5)');
		 elerry(:,ielem) = squeeze(ae(ielem,1:5,1:5))\(fey(ielem,1:5)');
	     end
%%
         for ivtx=1:5,
	     error_x(:) = error_x(:) + fex(:,ivtx) .* elerrx(ivtx,:)';
		 error_y(:) = error_y(:) + fey(:,ivtx) .* elerry(ivtx,:)';
         end
%%
         fprintf('done.\n')
		 return


function keep = newtonpolytope(exponent_m,exponent_p)
%NEWTONPOLYTOPE  Internal function to remove monimials in SOS programs using Newton polytope

% WARNING : THIS CODE SUCKS AND IS ONLY USED AS A BACK UP PLAN
% IF EVERYTHING ELSE FAILS (CRASHING LP SOLVERS ETC)

% *************************************
% TRY TO CALCULATE CONVEX HULL
% *************************************
try
    cnvhull = convhulln(full(exponent_p));
catch
    keep = 1:size(exponent_m,1);
    return
end

% ***************************************
% GET THE UNIQUE POINTS OF IN CONVEX HULL
% ***************************************
unique_points = unique(cnvhull);

% ***************************************
% CALCULATE A POINT IN INTERIOR
% ***************************************
p_c = sum(exponent_p(unique_points,:),1)'/length(unique_points);

% ***************************************
% CALCULATE HYPER-PLANES Ai^T(x-bi)=0
% ***************************************
j = 1;
A = [];
for i = 1:size(cnvhull,1)
    X = exponent_p(cnvhull(i,:),:)';
    y = X(:,1);
    dX = X(:,2:end)-repmat(X(:,1),1,size(X,2)-1);
    Atemp = null(full(dX'));
    % Is this a full-dimensional facet
    if size(Atemp,2)==1
        direction = (p_c-y)'*Atemp;
        if direction > 0
           A{j}=-Atemp;
       else
           A{j}=Atemp;
       end
        b{j}=y;
        j=j+1;
    end
end

% ***************************************
% CHECK IF MONOMIAL IS IN NEWTON POLYTOPE
% ***************************************
if ~isempty(A)
    keep = [];
    for j = 1:size(exponent_m,1)
        inside = 1;
        y = exponent_m(j,:)';
        if isempty(findrows(exponent_p,y)) % Numerically safe on border
            i = 1;
            while inside & (i<=length(A))
                inside = inside & ((A{i}'*(y-b{i}))<=1e-9);
                i = i+1;
            end
        end
        if inside
            keep = [keep j];
        end
    end
else
    keep = 1:size(exponent_m,1);
end


function [zPred, PzPred,otherInfo]=cubKalMeasPred(xPred,PPred,zDim,h,xi,w,innovTrans,measAvgFun,stateDiffTrans,stateTrans)
%%CUBKALMEASPRED Perform the measurement prediction part of the measurement
%           update step of the cubature Kalman filter with additive
%           measurement noise. The function cubKalUpdateWithPred can be
%           used to complete the measurement update. Separating the
%           measurement prediction step from the rest of the update step
%           can make the creation of multiple measurement association
%           hypotheses from a single target prediction more efficient.
%           The full measurement update function is cubKalUpdate.
%
%INPUTS: xPred The xDimXnumComp predicted target states.
%        PPred The xDimXxDimXnumComp predicted state covariance matrices.
%         zDim The dimensionality of the output of h.
%            h A function handle for the measurement function that takes
%              the state as its argument.
%           xi An xDimXnumCubPoints matrix of cubature points. If this
%              and the next parameter are omitted or empty matrices are
%              passed, then fifthOrderCubPoints(xDim) is used. It is
%              suggested that xi and w be provided to avoid needless
%              recomputation of the cubature points.
%            w A numCubPointsX1 vector of the weights associated with the
%              cubature points.
%   innovTrans An optional function handle that computes and optionally
%              transforms the value of the difference between the
%              observation and any predicted points. This is called as
%              innovTrans(a,b) and the default if omitted or an empty
%              matrix is passed is @(a,b)bsxfun(@minus,a,b). This must be
%              able to handle sets of values. For a zDimX1 measurement,
%              either of the inputs could be zDimXN in size while one of
%              the inputs could be zDimX1 in size.  This only needs to be
%              supplied when a measurement difference must be restricted
%              to a certain range. For example, the innovation between two
%              angles will be 2*pi if one angle is zero and the other
%              2*pi, even though they are the same direction. In such an
%              instance, a function handle to the
%              wrapRange(bsxfun(@minus,a,b),-pi,pi) function with the
%              appropriate parameters should be passed for innovTrans.
%   measAvgFun An optional function handle that, when given N measurement
%              values with weights, produces the weighted average. This
%              function only has to be provided if the domain of the
%              measurement is not linear. For example, when averaging
%              angular values, then the function meanAng should be used.
% stateDiffTrans An optional function handle that takes an xDimXN matrix of
%              N differences between states and transforms them however
%              might be necessary. If not transformation is necessary, this
%              parameter can be omitted or an empty matrix passed.
%   stateTrans An optional function that takes a state estimate and
%              transforms it. This is useful if one wishes the elements of
%              the state to be bound to a certain domain. For example, if
%              an element of the state is an angle, one might generally
%              want to bind it to the region +/-pi.
%
%OUTPUTS: zPred The zDimXnumComp measurement predictions from the filter.
%        PzPred The zDimXzDimXnumComp covariance matrices associated with
%               zPred.
%     otherInfo A structure containing members of intermediate results of
%               this function that can be passed to cubKalUpdateWithPred
%               when updating with a measurement.
%
%The mathematics behind the cubature Kalman filter are described in more
%detail in Section IX of [1] and in [2]. See the comments to cubKalUpdate
%for more information.
%
%EXAMPLE:
%With this example, we demonstrate that one gets the same result using
%cubKalUpdate in one step as with using cubKalMeasPred followed by
%cubKalUpdateWithPred.
% xPred=[1e3;-2e3;100;200];
% h=@(x)([sum(x.^2);x(1)-x(4)^(3/2)]);
% PPred=[28,   3.5,    6,  8.5;
%      3.5,    23,  8.5,   11;
%        6,   8.5,   18, 13.5;
%      8.5,    11, 13.5,   13];
% z=1e6*[5.050000548964568;
%       -0.001829553054023];
% zDim=size(z,1);
% R=eye(zDim,zDim);%Measurement covariance matrix.
% %The update in one step.
% [xUpdate,PUpdate,innov,Pzz,W]=cubKalUpdate(xPred,PPred,z,R,h);
% %The update in two steps.
% [zPred, PzPred,otherInfo]=cubKalMeasPred(xPred,PPred,zDim,h);
% [xUpdate1,PUpdate1,innov1,Pzz1,W1]=cubKalUpdateWithPred(z,R,zPred,PzPred,otherInfo);
% %One will see that the one and two step updates agree.
% max(abs([xUpdate1-xUpdate;PUpdate1(:)-PUpdate(:);innov1(:)-innov;Pzz1(:)-Pzz(:);W1(:)-W(:)]))
%
%REFERENCES:
%[1] D. F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems Magazine,
%    vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%[2] I. Arasaratnam and S. Haykin, "Cubature Kalman filters," IEEE
%    Transactions on Automatic Control, vol. 54, no. 6, pp. 1254-1269,
%    Jun. 2009.
%
%June 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    xDim=size(xPred,1);
    numComp=size(xPred,2);

    if(nargin<5||isempty(xi))
        [xi,w]=fifthOrderCubPoints(xDim);
    end

    if(nargin<7||isempty(innovTrans))
        %The function just returns the input.
        innovTrans=@(a,b)bsxfun(@minus,a,b);
    end

    if(nargin<8||isempty(measAvgFun))
        measAvgFun=@(zPoints,w)calcMixtureMoments(zPoints,w);
    end

    if(nargin<9||isempty(stateDiffTrans))
        stateDiffTrans=@(x)x;
    end

    if(nargin<10||isempty(stateTrans))
        stateTrans=@(x)x;
    end

    numCubPoints=size(xi,2);

    zPred=zeros(zDim,numComp);
    PzPred=zeros(zDim,zDim,numComp);
    xPredCenPoints=zeros(xDim,numCubPoints,numComp);
    zPredCenPoints=zeros(zDim,numCubPoints,numComp);
    Pxz=zeros(xDim,zDim,numComp);

    for k=1:numComp
        %cholSemiDef is used instead of chol in case a positive semi-definite
        %covariance matrix is passed.
        SPred=cholSemiDef(PPred,'lower');
        %Predicted cubature state points
        xPredPoints=stateTrans(transformCubPoints(xi,xPred,SPred));

        %Predicted cubature measurement points
        zPredPoints=zeros(zDim,numCubPoints);
        for curP=1:numCubPoints
            zPredPoints(:,curP)=h(xPredPoints(:,curP));
        end

        %Measurement prediction.
        zPred=measAvgFun(zPredPoints,w);

        %Centered, predicted cubature measurement points, transformed as
        %necessary to keep the values within a desired range.
        zPredCenPoints(:,:,k)=innovTrans(zPredPoints,zPred);
        xPredCenPoints(:,:,k)=stateDiffTrans(bsxfun(@minus,xPredPoints,xPred));
        for curP=1:numCubPoints
            diff=zPredCenPoints(:,curP);
            PzPred(:,:,k)=PzPred(:,:,k)+w(curP)*(diff*diff');
            Pxz(:,:,k)=Pxz(:,:,k)+w(curP)*xPredCenPoints(:,curP)*diff';
        end
        %Pxz is not needed for the measurement prediction, but we compute it
        %here, so that it need not be recomputed again and again if
        %cubKalUpdateWithPred is called for multiple measurements.
    end

    otherInfo.innovTrans=innovTrans;
    otherInfo.stateDiffTrans=stateDiffTrans;
    otherInfo.stateTrans=stateTrans;
    otherInfo.xPredCenPoints=xPredCenPoints;
    otherInfo.zPredCenPoints=zPredCenPoints;
    otherInfo.xPred=xPred;
    otherInfo.w=w;
    otherInfo.Pxz=Pxz;
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [tt] = move_tt_block(tt, spos, epos, eps)
%Performs a bubble movement of a block inside a train
%   [TT] = MOVE_TT_BLOCK(TT, SPOS, EPOS, EPOS) Performs the bubble movement
%   of the SPOS-th block to the position EPOS, the intermediate blocks
%   shift to the vacant places, i.e. by (-1) if spos<eps, and +1,
%   otherwise. Requires truncation with the L2-norm accuracy EPSs
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et. al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

if (spos==epos)
    return;
end;

d = tt.d;
n = tt.n;
r = tt.r;

% QR to spos
for i=1:spos-1
    cr = reshape(tt{i}, r(i)*n(i), r(i+1));
    [cr, rv]=qr(cr, 0);
    cr2 = reshape(tt{i+1}, r(i+1), n(i+1)*r(i+2));
    cr2 = rv*cr2;
    r(i+1) = size(cr, 2);
    tt{i} = reshape(cr, r(i), n(i), r(i+1));
    tt{i+1} = reshape(cr2, r(i+1), n(i+1), r(i+2));
end;
for i=d:-1:spos+1
    cr = reshape(tt{i}, r(i), n(i)*r(i+1));
    [cr, rv]=qr(cr.', 0);
    cr2 = reshape(tt{i-1}, r(i-1)*n(i-1), r(i));
    cr2 = cr2*(rv.');
    r(i) = size(cr, 2);
    tt{i} = reshape(cr.', r(i), n(i), r(i+1));
    tt{i-1} = reshape(cr2, r(i-1), n(i-1), r(i));
end;

% Now, start permutation
if (spos<epos) % From left to right
    for i=spos:epos-1
        cr1 = reshape(tt{i}, r(i)*n(i), r(i+1));
        cr2 = reshape(tt{i+1}, r(i+1), n(i+1)*r(i+2));
        cr = cr1*cr2;
        cr = reshape(cr, r(i), n(i), n(i+1), r(i+2));
        cr = permute(cr, [1, 3, 2, 4]);
        tempvar = n(i); n(i) = n(i+1); n(i+1) = tempvar;
        cr = reshape(cr, r(i)*n(i), n(i+1)*r(i+2));
        [u,s,v]=svd(cr, 'econ');
        s = diag(s);
        nrm = norm(s);
        r(i+1) = my_chop2(s, eps*nrm/sqrt(d-1));
        tt{i} = reshape(u(:,1:r(i+1)), r(i), n(i), r(i+1));
        v = diag(s(1:r(i+1)))*(v(:,1:r(i+1))');
        tt{i+1} = reshape(v, r(i+1), n(i+1), r(i+2));
    end;
end;

if (epos<spos) % From right  to left
    for i=spos:-1:epos+1
        cr1 = reshape(tt{i}, r(i), n(i)*r(i+1));
        cr2 = reshape(tt{i-1}, r(i-1)*n(i-1), r(i));
        cr = cr2*cr1;
        cr = reshape(cr, r(i-1), n(i-1), n(i), r(i+1));
        cr = permute(cr, [1, 3, 2, 4]);
        tempvar = n(i); n(i) = n(i-1); n(i-1) = tempvar;
        cr = reshape(cr, r(i-1)*n(i-1), n(i)*r(i+1));
        [u,s,v]=svd(cr, 'econ');
        s = diag(s);
        nrm = norm(s);
        r(i) = my_chop2(s, eps*nrm/sqrt(d-1));
        tt{i} = reshape(v(:,1:r(i))', r(i), n(i), r(i+1));
        u = u(:,1:r(i))*diag(s(1:r(i)));
        tt{i-1} = reshape(u, r(i-1), n(i-1), r(i));
    end;
end;

end


function [ varargout ] = TrimmedICP(Md, MovData, RefData, Tf0, MaxIter,TrMin,TrMax,lamda)
if nargin == 0
    clc; close all;
    %load ../../StanfordData/bunny;
    RefData = rand(3, 100); %bunny{1}';
    MovData = rand(3, 100);% bunny{2}';
    Md = createns(RefData');
    Tf0 = [eye(3) zeros(3, 1)];
    MaxIter = 50;
    TrMin = 0.35;
    TrMax = 1.0;
    lamda = 2.0;
end
Dim = size(MovData, 1);
PreMSE= 10^5;   CurMSE= 10^6;  Iter = 1;
RelErr = 1.0;
R0 = Tf0(1:Dim, 1:Dim);
T0 = Tf0(1:Dim, end);
Tf = [R0 T0; zeros(1, Dim) 1 ];
for Iter = 1 : 1 : MaxIter       % (abs(CurMSE-PreMSE)>10^(-9))   % this threshold is sensitive to resolutions....for bunny, this value should be 1e-12.
    TData = Loc2Glo(MovData, Tf(1:Dim, 1:Dim)', Tf(1:Dim, end) );
    [corr,TD] = knnsearch( Md,TData(1:Dim, :)');
    SortTD2 = sortrows(TD.^2); % Sort the correspongding points
    minTDIndex = floor(TrMin*length(TD)); % Get minimum index of TD
    maxTDIndex = ceil(TrMax*length(TD)); % Get maxmum index of TD
    TDIndex = [minTDIndex : maxTDIndex]';
    mTr = TDIndex./length(TD);
    mCumTD2 = cumsum(SortTD2);
    mMSE = mCumTD2(minTDIndex : maxTDIndex)./TDIndex;
    mPhi = ObjectiveFunction(mMSE, mTr);
    PreMSE=CurMSE;
    [CurMSE, nIndex] = min(mPhi);
    Trim = mTr(nIndex); % Update Tr for next step
    corr(:,2) = [1 : length(corr)]';
    % Sort the corresponding points
    corrTD = [corr, TD];
    SortCorrTD = sortrows(corrTD, 3);

    TrLength = floor(Trim*size(SortCorrTD,1)); % The number of corresponding points after trimming
    TCorr = SortCorrTD(1:TrLength, 1:2);     % Trim the corresponding points according to overlap parameter Tr
    % Register MData with TData
    dM = RegFun(RefData(:, TCorr(:, 1)), TData(:, TCorr(:, 2)) );
    % dM = reg(RefData, TData, TCorr);
    Tf = dM * Tf;
    % [M, TCorr, scan] = CalRtPhi(model, data, SortCorrTD, Trim);
    Err = [norm(dM(1:Dim, 1:Dim)-eye(Dim)) norm(dM(1:Dim, end))];
    if max(Err) <= 1e-5
        break;
    end
    RelErr = (PreMSE - CurMSE) / abs(PreMSE);
    if abs(RelErr) <= 1e-3
        break;
    end
end
IS_SHOW = 0;
if IS_SHOW
    Res = CalRes(MovData);
    h = figure;
    ICP_PlotFun(MovData, RefData, Tf, Res, h);
    title('Trimmed ICP');
end;
if nargout == 1
    varargout{1} = Tf;
end
if nargout == 2
    varargout{1} = Tf(1:Dim, 1:Dim);
    varargout{2} = Tf(1:Dim, end);
end
bTest = 1;

end

%%%%%%%%%%%%%%%%%%%%Integrated Function%%%%%%%%%%%%%%%%%%%%
% %% Calculate R,t,Phi based on current overlap parameter
% function [M,TCorr,TData] = CalRtPhi(Model, scan, SortCorrTD,Tr)
%
% TrLength = floor(Tr*size(SortCorrTD,1)); % The number of corresponding points after trimming
% TCorr = SortCorrTD(1:TrLength, 1:2);     % Trim the corresponding points according to overlap parameter Tr
% % Register MData with TData
% [M] = reg(Model(1:3,:), scan(1:3,:), TCorr);
% % To obtain the transformation data
% TData = M*scan;
%
% end
%
% function [M,TCorr,TData] = CalRtPhi(Model, scan, SortCorrTD,Tr)
%
% TrLength = floor(Tr*size(SortCorrTD,1)); % The number of corresponding points after trimming
% TCorr = SortCorrTD(1:TrLength, 1:2);     % Trim the corresponding points according to overlap parameter Tr
% % Register MData with TData
% [M] = reg(Model(1:3,:), scan(1:3,:), TCorr);
% % To obtain the transformation data
% TData = M*scan;
%
% end
%%%%%%%%%%%%%%% Calculate the registration matrix %%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%% T(TData)->MData %%%%%%%%%%%%%%%%%%%%%%%%%
% SVD solution
function [M] = reg(Model, Data, corr)

n = length(corr);
M = Model(:,corr(:,1));
mm = mean(M,2);
S = Data(:,corr(:,2));
ms = mean(S,2);
Sshifted = [S(1,:)-ms(1); S(2,:)-ms(2); S(3,:)-ms(3)];
Mshifted = [M(1,:)-mm(1); M(2,:)-mm(2); M(3,:)-mm(3)];
K = Sshifted*Mshifted';
K = K/n;
[U A V] = svd(K);
R1 = V*U';
if det(R1)<0
    B = eye(3);
    B(3,3) = det(V*U');
    R1 = V*B*U';
end
t1 = mm - R1*ms;
M=[];
M(1:3,1:3)=R1;
M(1:3,4)=t1;
M(4,:)=[0,0,0,1];
end

function [Phi] = ObjectiveFunction(MSE, TrB)
lamga= 2;
Phi = MSE./((TrB).^((1+lamga)));
end


function [Rot,v,x,PosAmers,P] = iekfPropagation(dt,Rot,v,x,omega_b,a_b,...
    PosAmers,P,omega,acc,Q,g)
NbAmers = size(PosAmers,2);

% state propagation
Rot = Rot*expSO3((omega-omega_b)*dt);
v = v+(Rot*(acc-a_b)+g)*dt;
x = x+v*dt;

% covariance propagation
F = eye(size(P));
F(4:6,1:3) = vecto(g)*dt;
F(7:9,1:3) = vecto(g)*dt*dt;
F(7:9,4:6) = eye(3)*dt;
F(1:3,10:12) = -Rot*dt;
F(4:6,10:12) = -vecto(v)*Rot*dt;
F(7:9,10:12) = -vecto(x)*Rot*dt;
for i = 1:NbAmers
    posAmers_i = PosAmers(:,i);
    F(13+3*i:15+3*i,10:12) = -vecto(posAmers_i)*Rot*dt;
end
F(4:6,13:15) = -Rot*dt;
F(7:9,13:15) = -Rot*dt*dt;

G = zeros(size(P,1),size(Q,1));
G(1:3,1:3) = Rot;
G(4:6,1:3) = vecto(v)*Rot;
G(7:9,1:3) = vecto(x)*Rot;
G(4:6,4:6) = Rot;
G(7:9,4:6) = Rot*dt;
G(10:15,7:12) = eye(6);
G(1:3,7:9) = Rot*dt;
G(4:6,7:9) = vecto(v)*Rot*dt*dt;
G(7:9,7:9) = vecto(x)*Rot*dt*dt*dt;
G(4:6,10:12) = Rot*dt*dt;
G(7:9,10:12) = Rot*dt*dt*dt;

P = F*P*F' + G*(Q*dt)*G'*dt;
end


function y = log(x)
%LOG          Implements  log(x)  for intervals
%
%   y = log(x)
%
%interval standard function implementation
%

% written  12/30/98     S.M. Rump
% modified 08/31/99     S.M. Rump  complex allowed, following N.C. Boersken:
%                                  Komplexe Kreis-Standardfunktionen,
%                                  Freiburger Intervallberichte 78/2,
%                                  NaN input, sparse input, log(0),
%                                  major revision, improved accuracy
% modified 12/06/99                branch cut with warning
% modified 09/02/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 12/04/05     S.M. Rump  'realstdfctsexcptnignore' added and some
%                                     improvements, tocmplx replaced by cintval,
%                                     extreme values for approximate part
% modified 09/06/07     S.M. Rump  approximate std fcts removed
% modified 10/23/07     S.M. Rump  complex numbers
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/20/08     S.M. Rump  check for zero omitted, improved performance
% modified 10/18/08     S.M. Rump  StdFctsException ignore/NaN
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/13/12     S.M. Rump  INTLAB_INTVAL_STDFCTS
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if x.complex
    if issparse(x.mid)
      x.mid = full(x.mid);
      x.rad = full(x.rad);
    end
    y = x;

%   y.mid = log(x.mid);
%   y.rad = - log( 1 - x.rad./abs(x.mid) );

    INTLAB_STDFCTS_PI = getappdata(0,'INTLAB_STDFCTS_PI');

    xmidre = real(x.mid);
    xmidim = imag(x.mid);
    Xmidim = intval(xmidim);
    Mim = atan(Xmidim./xmidre);      % -pi/2 <= Mim <= pi/2

    % special treatment of imaginary axis
    index = ( xmidre==0 );
    if any(index(:))
      Mim.inf(index) = INTLAB_STDFCTS_PI.PI2INF;          % pi/2
      Mim.sup(index) = INTLAB_STDFCTS_PI.PI2SUP;
      indexneg = index & ( xmidim<0 );
      if any(indexneg(:))
        Mim.inf(indexneg) = -INTLAB_STDFCTS_PI.PI2SUP;    % -pi/2
        Mim.sup(indexneg) = -INTLAB_STDFCTS_PI.PI2INF;
      end
    end

    index = ( xmidre < 0 );
    if any(index(:))
      % correct to atan2:  -pi <= Mim <= pi
      Pi = infsup( INTLAB_STDFCTS_PI.PIINF, ...
                   INTLAB_STDFCTS_PI.PISUP );
      signxmidim = sign(xmidim(index));
      corr = Pi.*(signxmidim+(signxmidim==0));
      Mim.inf(index) = Mim.inf(index) + corr.inf;
      Mim.sup(index) = Mim.sup(index) + corr.sup;
    end

    % x.mid = r*exp(j*phi)  ==>  log(x.mid) = log(r) + j*phi
    R = abs(x.mid);
    Mreinf = log_rnd(R,-1);
    Mresup = log_rnd(R,1);

    setround(1)
    mre = Mreinf + 0.5*(Mresup-Mreinf);
    mim = Mim.inf + 0.5*(Mim.sup-Mim.inf);
    y.mid = mre + j*mim;
    mrad = abs( mre-Mreinf + j*(mim-Mim.inf) );
    % log(R)  in  mre + j*mim +/- mrad

    wng = warning;
    warning off
    % x.rad < R ,  otherwise zero interval
    y.rad = log_rnd( -( x.rad./R - 1 ) , -1);
    warning(wng);
    y.rad = -y.rad + mrad;
    setround(0)

    % special treatment of branch cut
    index0 = ( R <= x.rad );               % zero interval
    index = ( ~index0 ) & ...
            (   ( ( xmidre<0 ) & ( xmidim>=0 ) & ( x.rad>xmidim ) ) ...
              | ( ( xmidre<0 ) & ( xmidim<0 ) & ( x.rad>=-xmidim ) ) ...
            );
    if any(index(:))
      warning('Complex Log: Input interval intersects with branch cut')
    end

    if any(index0(:))
      y.mid(index0) = complex(NaN,NaN);
      y.rad(index0) = NaN;
    end

    if rndold
      setround(rndold)
    end

    return
  end

  if issparse(x.inf)
    x.inf = full(x.inf);
    x.sup = full(x.sup);
  end
  % input x real and full
  % real range of definition:  [0,inf]
  INTLAB_STDFCTS_EXCPTN = getappdata(0,'INTLAB_STDFCTS_EXCPTN');
  index = ( x.inf<0 );                  % (partially) exceptional indices
  if any(index(:))                      % handle input out-of-range
    if INTLAB_STDFCTS_EXCPTN<=1  % out-of-range input handled as complex
      if INTLAB_STDFCTS_EXCPTN==1
        warning('LOG: Real interval input out of range changed to be complex')
      end
      y = x;
      %VVVV  y(index) = log(cintval(x(index)));
      s.type = '()'; s.subs = {index}; y = subsasgn(y,s,log(cintval(subsref(x,s))));
      %AAAA  Matlab bug fix
      index = ~index;
      if any(index(:))
        %VVVV  y(index) = log(x(index));
        s.type = '()'; s.subs = {index}; y = subsasgn(y,s,log(subsref(x,s)));
        %AAAA  Matlab bug fix
      end
      if rndold
        setround(rndold)
      end
      return
    end
    setappdata(0,'INTLAB_STDFCTS_EXCPTN_',1);
    if INTLAB_STDFCTS_EXCPTN==3    % ignore input out of range (ignore-mode)
      x.inf(index) = realmin*eps;;        % completely exceptional indices treated below
      indexneg = index & ( x.sup<0);      % completely exceptional indices
    end
  else
    indexneg = [];                        % make sure indexneg is not undefined
  end

  % input x real and full
  y = x;
  wng = warning;
  warning off

  % treat non-exceptional cases
  y.inf = log_rnd(x.inf,-1);
  y.sup = log_rnd(x.sup,1);

  if INTLAB_STDFCTS_EXCPTN==3      % ignore input out of range (ignore-mode)
    if ~isempty(find(indexneg))           % completely exceptional arguments to NaN
      y.inf(indexneg) = NaN;
      y.sup(indexneg) = NaN;
    end
  else                                    % any input out of range to NaN (NaN-mode)
    if ~isempty(find(index))              % exceptional arguments to NaN
      y.inf(index) = NaN;
      y.sup(index) = NaN;
    end
  end

  warning(wng)
  setround(rndold)


% hessMultdalgl - function that computes H*x for DAL with grouped
%                 L1 regularization
%
% Copyright(c) 2009 Ryota Tomioka
% This software is distributed under the MIT license. See license.txt

function yy = hessMultdalgl(xx, A, eta, Hinfo)

yy = Hinfo.hloss*xx;
if ~isempty(Hinfo.precomp)
    % general case
    for p=Hinfo.precomp
        % project & reshape xx's for all spans
        xk = reshape(p.AJ'*xx,[],numel(p.jj));
        % dot product between vn's and xk's, scale by ff's and multiply by vn's (also add xk*(1-ff))
        tmp = bsxfun(@times,p.vn,sum(p.vn.*xk).*p.ff) + bsxfun(@times,xk,p.omff);
        % block-diagonalize tmp
        bd = sparse(p.idxu,p.idxv,tmp,numel(xk),numel(p.jj));
        % map through AJ again, sum and multiply by eta(1), add results to yy
        yy = yy + sum(p.AJ*bd,2)*eta(1);
    end
else
    % very sparse case
    blks =Hinfo.blks;
    hloss=Hinfo.hloss;
    I    =Hinfo.I;
    vv   =Hinfo.vv;
    nm   =Hinfo.nm;
    lambda=Hinfo.lambda;
    for kk=1:length(I)
        jj=I(kk);
        J=Hinfo.blkival{jj};
        vn=vv(J)/nm(jj);
        ff=lambda/nm(jj);
        AJ=A.slice(J);
        xk=AJ'*xx;
        yy = yy + eta(1)*(AJ*((1-ff)*xk + ff*(vn'*xk)*vn));
    end
end

B=Hinfo.B;
if ~isempty(B)
  yy = yy + eta(2)*(B*(B'*xx));
end


 function [xs, ni] = eml_sps(x, Gt, yi, ci, ri, niter, curv)
%function [xs, ni] = eml_sps(x, Gt, yi, ci, ri, niter, curv)
%	One iteration of the ML-SPS algorithm for emission Poisson problem
%	(separable paraboloidal surrogates)
%	model: Y_i ~ Poisson(c_i [G x]_i + r_i)		WITH r_i > 0 REQUIRED!
%	in:
%		Gt	transpose of system matrix
%		see em_fbp.m for model, G, yi, ci, ri
%	out:
%		x [np,niter]	updated image vectors each iteration
%
%	Copyright Mar 2000, Jeff Fessler, The University of Michigan

if nargin < 3, ir_usage, end

[nb, na] = size(yi);

if ~isvar('ci') || isempty(ci)
	ci = ones(size(yi));
end
if ~isvar('ri') || isempty(ri)
	ri = zeros(size(yi));
end
if ~isvar('niter') || isempty(niter)
	niter = 1;
end

eml_check(yi, ci, ri)

	gi = sum(Gt)';	% g_i = sum_j g_ij

xs = zeros(numel(x), niter);
xs(:,1) = x(:);
%
%	loop over iterations
%
for ii=2:niter
	li = reshape(Gt' * x(:), size(yi));	% l=G*x "line integrals"
	yb = ci .* li + ri;			% predicted measurement means

	%	curvatures
	ni = eml_curvature(yi, ci, ri, li, yb, curv);

	dothi = ci .* (yi ./ yb - 1);

	x = x + (Gt * dothi(:)) ./ (Gt * (gi .* ni(:)));
	x = max(x,0);

	xs(:,ii) = x;
end


function degree = dunavant_degree ( rule )

%*****************************************************************************80
%
%% DUNAVANT_DEGREE returns the degree of a Dunavant rule for the triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    David Dunavant,
%    High Degree Efficient Symmetrical Gaussian Quadrature Rules
%    for the Triangle,
%    International Journal for Numerical Methods in Engineering,
%    Volume 21, 1985, pages 1129-1148.
%
%    James Lyness, Dennis Jespersen,
%    Moderate Degree Symmetric Quadrature Rules for the Triangle,
%    Journal of the Institute of Mathematics and its Applications,
%    Volume 15, Number 1, February 1975, pages 19-32.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%
%    Output, integer DEGREE, the polynomial degree of exactness of
%    the rule.
%
  if ( 1 <= rule & rule <= 20 )
    degree = rule;
  else

    degree = -1;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DUNAVANT_DEGREE - Fatal error!\n' );
    fprintf ( 1, '  Illegal RULE = %d\n', rule );
    error ( 'DUNAVANT_DEGREE - Fatal error!' )

  end

  return
end


function rhs_spiral_test ( )

%*****************************************************************************80
%
%% RHS_SPIRAL_TEST samples the right hand side at the initial time.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/navier_stokes_2d_exact/rhs_spiral_test.m
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2015
%
%  Author:
%
%    John Burkardt
%
  nu = 1.0;
  rho = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'RHS_SPIRAL_TEST\n' );
  fprintf ( 1, '  Spiral Flow:\n' );
  fprintf ( 1, '  Sample the Navier-Stokes right hand sides\n' );
  fprintf ( 1, '  at the initial time T = 0, using the unit square.\n' );
  fprintf ( 1, '  Kinematic viscosity NU = %g\n', nu );
  fprintf ( 1, '  Fluid density RHO = %g\n', rho );

  n = 1000;
  xy_lo = 0.0;
  xy_hi = 1.0;
  seed = 123456789;
  [ x, seed ] = r8vec_uniform_ab ( n, xy_lo, xy_hi, seed );
  [ y, seed ] = r8vec_uniform_ab ( n, xy_lo, xy_hi, seed );
  t = 0.0;

  [ f, g, h ] = rhs_spiral ( nu, rho, n, x, y, t );

  fprintf ( 1, '\n' );
  fprintf ( 1, '           Minimum       Maximum\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F:  %14.6g  %14.6g\n', min ( f ), max ( f ) );
  fprintf ( 1, '  G:  %14.6g  %14.6g\n', min ( g ), max ( g ) );
  fprintf ( 1, '  H:  %14.6g  %14.6g\n', min ( h ), max ( h ) );

  return
end


function c = tmat_mxm ( a, b )

%*****************************************************************************80
%
%% TMAT_MXM multiplies two geometric transformation matrices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Foley, van Dam, Feiner, Hughes,
%    Computer Graphics, Principles and Practice,
%    Addison Wesley, Second Edition, 1990.
%
%  Parameters:
%
%    Input, real A(4,4), the first geometric transformation matrix.
%
%    Input, real B(4,4), the second geometric transformation
%    matrix.
%
%    Output, real C(4,4), the product A * B.
%
  c(1:4,1:4) = a(1:4,1:4) * b(1:4,1:4);

  return
end


function [CRLB,J]=getEstimatorMinMSEBound(RInv,stateJacob,statJacob,biasVec,biasJacob,numMeasDims,checkPosDef)
%%GETESTIMATORMINMSEBOUND This evaluates a generalization of the Cramer-Rao
%       lower bound (CRLB) specifically for the case where measurements are
%       corrupted with multivariate Gaussian noise. The measurement model
%       is z=h(x)+w, where x is the (deterministic) state, h is the
%       (possible nonlinear) measurement function and w is zero-mean
%       Gaussian noise with covariance matrix R. h can vary between
%       measurements that are being fused. This offers a lower bound on the
%       mean-squared error of a biased multivariate statistic. The
%       statistic is just a transformation of the state, so zStat=f(x).
%       Biased means that if T(x) is the estimator, then the expecttation
%       E{T(x)-f(x)}=b(x), where b(x) is the bias. If one doesn't know the
%       bias and its gradient, omitting those terms provides a lower bound
%       on the covariance matrix of an unbiased statistic. If the statistic
%       Jacobian is omitted, then the bound is directly on the state x.
%       The bias term can be useful when one already has an estimator with
%       a known or approximated bias and they want to approximate its
%       accuracy with the MSE matrix bound rather than analytically
%       evaluating its true MSE.
%
%INPUTS: RInv The zDimXzDimXnumMeas set of inverse covariance matrices
%          associated with the multivariate Gaussian noise corrupting each
%          of the numMeas measurements. If the dimensionality of the
%          measurements varies, then zDim is the maximum
%          dimensionality of any measurement and then numMeasDims is
%          required so that if sel=1:numMeasDims(k), then RInv(sel,sel,k)
%          is the submatrix used for the kth measurement.
% stateJacob The zDimXxDimXnumMeas Jacobian matrices of derivatives of the
%          measurement function h taken with respect to the elements of
%          the target state for every measurement. If a single zDimXxDim
%          matrix is passed, then it is assumed that this is the same for
%          all numMeas measurement. If the dimensionality of the
%          measurements varies, then zDim is the maximum
%          dimensionality of any measurement and then numMeasDims is
%          required so that if sel=1:numMeasDims(k), then
%          stateJacob(sel,:,k) is the submatrix used for the kth
%          measurement.
% statJacob The statDimXxDim matrix of derivatives of the the statistic
%          function f(x) taken with respect to the state x. If this is
%          omitted or an empty matrix is passed, then an xDimXxDim identity
%          matrix is used, which is the same as h(x)=x.
%  biasVec A statDimX1 vector, if provided. This is the bias of the assumed
%          estimator of the statistics. If this value and the next value
%          are omitted or empty matrices are provided, then the CRLB for an
%          unbiased statistic is computed.
% biasJacob A statDimXxDim matrix of derivatives of the estimator bias b(x)
%          taken with respect to the state x. If this is provided, then
%          biasVec must be provided. If omitted or an empty matrix is
%          passed, then this is assumed to be 0.
% numMeasDims If all measurement shave the same dimensionality , then this
%          input should be omitted or an empty matrix passed. Otherwise,
%          this is a length numMeas vector that specified how many
%          dimensions each measurement has.
% checkPosDef If this is true, then a check is performed as to whether the
%          Fisher information matrix in the algorithm is full rank.
%          If it is not, then the CRLB matrix will be returned empty and no
%          attempt will be made to invert it. The default if omitted or an
%          empty matrix is passed is false.
%
%OUTPUTS: CRLB The statDimXstatDim CRLB matrix or, if the FIM was singular,
%              and checkPosDef is true, an empty matrix.
%            J The Fisher infromation matrix that went into computing CRLB.
%
%A simple derivation of the standard multivariate CRLB is derived in [1].
%If in the derivation there, one replaces the difference T_k(x)-x_k (the
%difference of the best estimator of the kth component of x and the state)
%with T_k(x)-f_k(x), where f_k(x) is the kth component of a desired
%statistic and thus T_k(x) becomes an estimator of a component of a
%statistic, then one will derive the CRLB of a statistic instead of just of
%the state. In the scalar case, this was already in Rao's original paper in
%[2].
%
%To consider a biased statistic, one then replaces the assumption in the
%paper of a zero expected value: E{T_k(x)-x_k}=0 with
%E{T_k(x)-f_k(x)}=b_k(x). The rest of the derivation in [1] is pretty much
%the same, making sure that one is evaluating Cov{Z} and not just E{Z*Z'}.
%Completing the derivation just using E{Z*Z'} and not Cov{Z} will lead one
%to have a lower bound that is smaller by a bias*bias' term.
%
%Note that is is possible for a biased estimator to have a lower MSE than
%an unbiased estimator. One such example is given in [3].
%
%EXAMPLE 1:
%This is a simple example, where z=x+w with w having the identity matrix as
%the covariance matrix. We take x to be 2X1. We want to estimate
%f(x)=x'*x. The assumed biased estimator is T(z)=z'*z+sum(z)/100. (Note
%that even T(z)=z'*z is biased, but we choose to make the bias non-
%constant). The bias in 2D can be found to be b(x)=2+sum(x)/100.
%The analytic mean squared error in 2D is
%MSE=(80002+40001*x(1)^2+2*x(1)*(400+x(2))+x(2)*(800+40001*x(2)))/10000
%We plot the MSE and the CRLB for a fixed x(1), while varying x(2). One can
%see a reasonably good level of agreement, though the bound is not tight.
% MSE=@(x)(80002+40001*x(1)^2+2*x(1)*(400+x(2))+x(2)*(800+40001*x(2)))/10000;
% biasVec=@(x)(2+sum(x)/100);
% biasJacob=@(x)[1/100,1/100];
% statJacob=@(x)2*x';
% stateJacob=eye(2,2);
% RInv=eye(2,2);
% numPts=200;
% x1=-1/2;
% x2=linspace(-5,5,numPts);
% MSEVal=zeros(1,numPts);
% CRLB=zeros(1,numPts);
% for curPt=1:numPts
%     xCur=[x1;x2(curPt)];
%     MSEVal(curPt)=MSE(xCur);
%     CRLB(curPt)=getEstimatorMinMSEBound(RInv,stateJacob,statJacob(xCur),biasVec(xCur),biasJacob(xCur));
% end
% figure(1)
% clf
% hold on
% plot(x2,MSEVal,'-b','linewidth',2)
% plot(x2,CRLB,'--k','linewidth',2)
% legend('Actual MSE','CRLB','location','north')
%
%REFERENCES:
%[1] A. Papoulis and S. U. Pillai, Probability, Random Variables and
%    Stochastic Processes, 4th ed. Boston: McGraw Hill, 2002.
%[2] C. R. Rao, "Information and the accuray attainable in the estimation
%    of statistical parameters," Bulletin of the Calcutta Mathematical
%    Society, vol. 37, no. 3, pp. 81-91, 1945.
%[3] P. Stoica and R. L. Moses, "On biased estimators and the unbiased
%    Cramér-Rao lower bound," Signal Processing, vol. 21, no. 4, pp. 349-
%    350, Dec. 1990.
%
%November 2022 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numMeas=size(RInv,3);
xDim=size(stateJacob,2);
statDim=size(statJacob,1);

if(nargin<7||isempty(checkPosDef))
    checkPosDef=false;
end

if(nargin<6)
    numMeasDims=[];
end

if(nargin<5||isempty(biasJacob))
    biasJacob=zeros(statDim,xDim);
end

if(nargin<4||isempty(biasVec))
    biasVec=zeros(statDim,1);
end

if(nargin<3||isempty(statJacob))
    statJacob=eye(xDim,xDim);
end

if(numMeas>1&&size(stateJacob,3)==1)
    stateJacob=repmat(stateJacob,[1,1,numMeas]);
end

%First get the FIM
J=zeros(xDim,xDim);
if(isempty(numMeasDims))
    %If all measurements are zDim in size
    for k=1:numMeas
        J=J+stateJacob(:,:,k)'*RInv(:,:,k)*stateJacob(:,:,k);
    end
else
    for k=1:numMeas
        zDimCur=numMeasDims(k);
        sel=1:zDimCur;
        J=J+stateJacob(sel,:,k)'*RInv(sel,sel,k)*stateJacob(sel,:,k);
    end
end

if(checkPosDef)
    %Check whether or not J is positive definite.
    if(matrixRank(J)<xDim)
        CRLB=[];
        return;
    end
end

CRLB=(statJacob+biasJacob)*inv(J)*(statJacob+biasJacob)'+biasVec*biasVec';

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function  prices = SABR_EurBarAmer_func(call, M, T, S0, Kvec, r, CTMCParams, ModParams, contract_type, L)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Pricing Function for European, American, and Barrier Options using
% double Layer CTMC approximation for SABR
%
% Models Supported: SABR
% Returns: price of contract (for vector of strikes)
% Author: Justin Lars Kirkby
%
% References:  (1) General Valuation Framework for SABR and Stochastic Local Volatility
%                   Models. SIAM J. Financial Mathematics, 2018. (w/ Z. Cui
%                   and D. Nguyen)
%
% ----------------------
% Contract/Model Params
% ----------------------
% call = 1 for call option, else Put
% contract_type = type of contact: % 1 = European, 2 = American, 3 = Down and Out Barrier
% Kvec  = strike vector
% S0 = initinal underlying value
% r   = interest rate (e.g. 0.05)
% T   = time remaining until maturity (in years, e.g. T=1)
% ModParams = model parameters: .v0, .alpha, .beta, .rho
% L =  For barrier contract, this is the barrier
%
% ----------------------
% Numerical (CTMC) Params
% ----------------------
% CTMCParams: .m_0 = grid/state size for variance process
%             .N = grid/state stize for underlying
%             .gridMult_v = grid non-uniformity multiplier (for variance)
%             .gridMult_s = grid non-uniformity multiplier (for underlying)
%             .gamma = Grid width param for variance grid
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

m_0        = CTMCParams.m_0;
N          = CTMCParams.N;
gridMult_v = CTMCParams.gridMult_v;
gridMult_s = CTMCParams.gridMult_s;  %Grid mult param for S
gamma      = CTMCParams.gamma;          %Grid width param for variance grid

gridMethod_v = 5;   %%% ALWAYS use 5 for this one (puts v0 on grid)
gridMethod_s = 4;   %%% 5 puts S_0 on grid, but 4 seems better (requires interpolation)

v0    = ModParams.v0;
alpha = ModParams.alpha;
beta  = ModParams.beta;
rho   = ModParams.rho;

%%%%%%%%%%%%%%%%%%%%%%
%%%   Set Asset Grid bounds
%%%%%%%%%%%%%%%%%%%%%%
if S0 < 0.5
    ls = .01*S0;   %lower bound in asset Grid (S_t) space
else
    ls = 0.001*S0;
end

us = max(4.5*S0, S0 + 10*(v0*(S0)^beta)*sqrt(T));  %upper bound in asset Grid (S_t) space

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%   Step 1: Variance Grid / Generators
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = T/M;
t = sqrt(T)/2;    %NOTE: this is different than we used to use...

mu_func = @(u) 0*u;
sig_func = @(u) alpha*u;
mu_H = v0;
sig2_H = v0^2*(exp(alpha^2*t) - 1);

lx = max(0.0001,mu_H - gamma*sqrt(sig2_H));
ux = mu_H + gamma*sqrt(sig2_H);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%   Step 1: Variance Grid / Generators
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
center_v = v0;
[Q,v] = General_Q_Matrix_Newest(m_0,mu_func,sig_func,lx,ux,gridMethod_v,center_v, gridMult_v);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%   Step 2: Asset Grid / Grid For Xtilde
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
g = @(s) (s ).^(1-beta)/(1-beta);
invOneBet = 1/(1-beta);

center_s = S0;   %center of asset grid (e.g. center points around the strike)
manualPoint_s = center_s;  %manually places S0 on grid
Xgrid = g(getNonUniformGrid(N, ls, us, gridMethod_s, center_s, manualPoint_s, gridMult_s)) - rho/alpha*v0;  %Grid for Xtilde

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%   Step 3: Generators (for Xtilde process)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Nm = N*m_0;
G = zeros(Nm, Nm);  %Big Generator matrix
I = eye(N,N);

Payoff = zeros(Nm, 1);  %Terminal payoff
sqrtRho = sqrt(1-rho^2);

for j = 1:m_0 %loop through rows of big G matrix
    %%%%%%%%%%
    % Step(1): Find G_j (generator with v(j) fixed)
    %%%%%%%%%%
    nu_j = v(j);
    muX_func_nu  = @(x) -.5*beta*(nu_j)^2.*((1-beta)*(x + rho*nu_j/alpha)).^(-1) ;  %Drift function of Xtilde with v(j) fixed
    sigX_func_nu = @(x) sqrtRho*nu_j*[x>-100];  %Constant function for each fixed variance state

    Gnu = getGenerator_Q_MatrixOnly(Xgrid, muX_func_nu, sigX_func_nu, gridMethod_v);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%% FORCE absorbing vs reflecting
    Gnu(1,1) = 0; Gnu(1,2) =0;
    %Gnu(N,N) = 0; Gnu(N,N-1) = 0;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    %%%%%%%%%%
    % Step(2): Populate the Generator matrix (recall it is block tridiagonal,
    %%%%%%%%%%
    for k = max(1,j-1):min(m_0,j+1)  %%% NOTE: we skip the ones that are known to be zeros (matrix is block tridiagonal)
        lamjk = Q(j,k);
        if j==k  %diagonal block element of G matrix
            G((j-1)*N + 1:j*N, (k-1)*N + 1:k*N ) = Gnu + lamjk*I;
        else
            G((j-1)*N + 1:j*N, (k-1)*N + 1:k*N ) = lamjk*I;
        end
    end
end

%%%% Find bracketing variance gridpoint
k_0 = 2;
while v0 >= v(k_0) && k_0 < m_0
    k_0 = k_0+1;
end
k_0 = k_0 - 1;  %left bracketing point:  v(k_0) <= v0 < v(k_0 +1)

%%%% Find bracketing Xtilde gridpoint
x0 = g(S0) - rho*v0/alpha;  %initial value on Xtilde grid for (S_0, v0)
j_0 = 2;
while x0 >= Xgrid(j_0) && j_0 < N
    j_0 = j_0+1;
end
j_0 = j_0 - 1;  %left bracketing point:   Xtilde(j_0) <= x0 < Xtilde(j_0 +1)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% VALUE : using recursive method
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

P = expm(G*dt);   %transition matrix of one dimensional CTMC
initialIndex = (k_0-1)*N + j_0;  %index corresponding to initial conditions

prices = zeros(length(Kvec),1);

for k = 1:length(Kvec)
    K = Kvec(k);
    %%% Calculate Payoff
    if call == 1
        for j = 1:m_0
            Payoff((j-1)*N +1:j*N) = max(0, ((1-beta)*(max(0,Xgrid + rho*v(j)/alpha) )).^invOneBet - K) ;  %Portion of payoff corresponding to v(j)
        end
    else
       for j = 1:m_0
            Payoff((j-1)*N +1:j*N) = max(0, K - ((1-beta)*(max(0,Xgrid + rho*v(j)/alpha) )).^invOneBet );  %Portion of payoff corresponding to v(j)
       end
    end
    %%% Now Price
    if contract_type == 3  % Down and out for now
        %determine which states remain alive
        alive = zeros(Nm,1);
        for j = 1:m_0
            cons = g(L) - v(j)*rho/alpha;
            alive((j-1)*N +1:j*N) = (Xgrid > cons);
        end
        pVec = alive.*Payoff;
        for m = M-1:-1:0
            pVec = exp(-r*dt)*alive.*(P*pVec);
        end
    else %% either American or European
        pVec = exp(-r*dt)*(P*Payoff);  %initialize the value (at last period)
        if contract_type == 2
            for m = M-2:-1:0
                pVec = exp(-r*dt)*(P*pVec);  %continuation value
                pVec = max(pVec, Payoff);   %max of continuation and intrinsic value
            end
        elseif contract_type == 1
            for m = M-2:-1:0
                pVec = exp(-r*dt)*(P*pVec);  %continuation value
            end
        end
    end

    if gridMethod_s == 5  %Both v0 and x0 are on grid  (we assume v0 is a member of vol grid, ie as long as gridMethod_v = 5)
        prices(k) = pVec(initialIndex);
    elseif gridMethod_s == 4 %x0 is not on grid (though we assume v0 is, ie as long as gridMethod_v = 5)
        price1 = pVec(initialIndex);    %corresponds to j_0
        price2 = pVec(initialIndex+1);  %corresponds to j_0 + 1
        prices(k) = price1 + (price2 - price1)*(x0 - Xgrid(j_0))/(Xgrid(j_0+1) - Xgrid(j_0));  %LINEAR INTERPOLATION
    end
end

end


function [A,B,C,D] = vibsBlockOperator(omega,U,sigma,u,cL,cT,rhoS,c0,rho0,f)
%+========================================================================+
%|                                                                        |
%|                 OPENVIBS - LIBRARY FOR VIBRO-ACOUSTIC                  |
%|           openVibs is part of the GYPSILAB toolbox for Matlab          |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal, Marc Bakry (c) 2017-2019.                 |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             marc.bakry@polytechnique.edu                               |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : vibsBlockOperator.m                           |
%|    #    |   VERSION    : 0.55                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & Marc Bakry                  |
%|  ( # )  |   CREATION   : 14.03.2019                                    |
%|  / 0 \  |   LAST MODIF :                                               |
%| ( === ) |   SYNOPSIS   :                                               |
%|  `---'  |                                                              |
%+========================================================================+

% Constants
mu     = rhoS*cT^2;
lambda = rhoS*(cL^2 - 2*cT^2);
w      = 2*pi*f;
k      = w/c0;

% Dimension
n = size(omega.msh.elt,2)-1;

% Green kernel function
if (n == 2)
    Gxy         = @(X,Y) femGreenKernel(X,Y,'[H0(kr)]',k);
    gradyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]1',k);
    gradyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]2',k);
    gradyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[H0(kr)]3',k);
    G0          = '[log(r)]';
    gradyG0     = 'grady[log(r)]';
    cteGxy      = 1i/4;
    cteG0       = -1/(2*pi);

elseif (n == 3)
    Gxy         = @(X,Y) femGreenKernel(X,Y,'[exp(ikr)/r]',k);
    gradyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]1',k);
    gradyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]2',k);
    gradyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]3',k);
    G0          = '[1/r]';
    gradyG0     = 'grady[1/r]';
    cteGxy      =  1/(4*pi);
    cteG0       =  1/(4*pi);

else
    error('vibsNeumannBW.m : unavailable case.')
end

% Coupling coeff for Brackage-Werner simulation
beta = 1i*k;

%%%%%%%%%%%%%%%%%%%%%%%%%%% ELASTO-ELASTO (A11) %%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialization
A = cell(n,n);

% Static part
GG = integral(omega,grad(U),grad(U));
for i = 1:n
    for j = 1:n
        % Operator div(U):div(U)
        DD = integral(omega,grad(U,i),grad(U,j));

        % Operator e(U):e(U)
        EE = integral(omega,grad(U,j),grad(U,i));
        if (i==j)
            EE = EE + GG;
        end

        % Summation
        A{i,j} = lambda.*DD + mu.*EE;
    end
end

% Dynamic part
if (f ~= 0)
    Id = integral(omega,U,U);
    for i = 1:n
       A{i,i} = A{i,i} - (rhoS*w^2) .* Id;
    end
end

% Final form (sparse)
A = cell2mat(A);

%%%%%%%%%%%%%%%%%%%%%%%%%% ELASTO-ACOUSTIC (A12) %%%%%%%%%%%%%%%%%%%%%%%%%%

% Coupling to FEM
B = cell(n,1);
for i = 1:n
    % Collocation mass operator
    Id = integral(sigma,ntimes(U,i),u);

    % Collocation boundary operator
    S = cteGxy .* integral(sigma,sigma,ntimes(U,i),Gxy,u) + ...
        cteG0  .* regularize(sigma,sigma,ntimes(U,i),G0,u);

    % Collocation boundary operator
    D = cteGxy .* integral(sigma,sigma,ntimes(U,i),gradyGxy,ntimes(u)) + ...
        cteG0  .* regularize(sigma,sigma,ntimes(U,i),gradyG0,ntimes(u));

    % Final operator Brackage-Werner : [1i*k*beta*S - (Id/2 + D)]
    B{i} = beta.*S - (0.5*Id + D);
end

% Final form (full matrix)
B = cell2mat(B);

%%%%%%%%%%%%%%%%%%%%%%%%%% ELASTO-ACOUSTIC (A21) %%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialization
C = cell(1,n);

% Coupling FEM
for i = 1:n
    C{i} = (rho0*w^2) .* integral(sigma,u,ntimes(U,i));
end

% Final format (sparse)
C = cell2mat(C);

%%%%%%%%%%%%%%%%%%%%%%%%%% ACOUSTIC-ACOUSTIC (A22) %%%%%%%%%%%%%%%%%%%%%%%%

% Finite element mass matrix
Id = integral(sigma,u,u);

% Finite element boundary operator
H  = cteGxy .* (k^2 * integral(sigma,sigma,ntimes(u),Gxy,ntimes(u)) ...
    - integral(sigma,sigma,nxgrad(u),Gxy,nxgrad(u)));
Hr = cteG0  .* (k^2 * regularize(sigma,sigma,ntimes(u),G0,ntimes(u)) ...
    - regularize(sigma,sigma,nxgrad(u),G0,nxgrad(u)));

% Finite element boundary operator
D  = cteGxy .* integral(sigma,sigma,u,gradyGxy,ntimes(u));
Dr = cteG0  .* regularize(sigma,sigma,u,gradyG0,ntimes(u));

% Final operator Brackage-Werner : - [1i*k*beta*(-Id/2 + Dt) - H]
D = - (beta.*(-0.5*Id + (D+Dr).') - (H+Hr));
end


function [ a_lu, pivot, rcond, z ] = r8ge_co ( n, a )

%*****************************************************************************80
%
%% R8GE_CO factors a R8GE matrix and estimates its condition number.
%
%  Discussion:
%
%    The R8GE storage format is used for a general M by N matrix.  A storage
%    space is made for each logical entry.  The two dimensional logical
%    array is mapped to a vector, in which storage is by columns.
%
%    For the system A * X = B, relative perturbations in A and B
%    of size EPSILON may cause relative perturbations in X of size
%    EPSILON/RCOND.
%
%    If RCOND is so small that the logical expression
%      1.0E+00 + rcond == 1.0E+00
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate
%    underflows.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 March 2004
%
%  Author:
%
%    Original FORTRAN77 version by Dongarra, Bunch, Moler, Stewart.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Bunch, Moler, Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979
%
%  Parameters:
%
%    Input, integer N, the order of the matrix A.
%
%    Input, real A(N,N), a matrix to be factored.
%
%    Output, real A_LU(N,N), the LU factorization of the matrix.
%
%    Output, integer PIVOT(N), the pivot indices.
%
%    Output, real RCOND, an estimate of the reciprocal condition number of A.
%
%    Output, real Z(N), a work vector whose contents are usually unimportant.
%    If A is close to a singular matrix, then Z is an approximate null vector
%    in the sense that
%      norm ( A * Z ) = RCOND * norm ( A ) * norm ( Z ).
%

%
%  Compute the L1 norm of A.
%
  anorm = 0.0E+00;
  for j = 1 : n
    anorm = max ( anorm, sum ( abs ( a(1:n,j) ) ) );
  end
%
%  Compute the LU factorization.
%
  [ a, pivot, info ] = r8ge_fa ( n, a );
%
%  RCOND = 1 / ( norm(A) * (estimate of norm(inverse(A))) )
%
%  estimate of norm(inverse(A)) = norm(Z) / norm(Y)
%
%  where
%    A * Z = Y
%  and
%    A' * Y = E
%
%  The components of E are chosen to cause maximum local growth in the
%  elements of W, where U'*W = E.  The vectors are frequently rescaled
%  to avoid overflow.
%
%  Solve U' * W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  for k = 1 : n

    if ( z(k) ~= 0.0 )
      ek = - r8_sign ( z(k) ) * abs ( ek );
    end

    if ( abs ( a(k,k) ) < abs ( ek - z(k) ) )
      s = abs ( a(k,k) ) / abs ( ek - z(k) );
      z(1:n) = s * z(1:n);
      ek = s * ek;
    end

    wk = ek - z(k);
    wkm = -ek - z(k);
    s = abs ( wk );
    sm = abs ( wkm );

    if ( a(k,k) ~= 0.0 )
      wk = wk / a(k,k);
      wkm = wkm / a(k,k);
    else
      wk = 1.0;
      wkm = 1.0;
    end

    if ( k + 1 <= n )

      for j = k + 1 : n
        sm = sm + abs ( z(j) + wkm * a(k,j) );
        z(j) = z(j) + wk * a(k,j);
        s = s + abs ( z(j) );
      end

      if ( s < sm )
        t = wkm - wk;
        wk = wkm;
        z(k+1:n) = z(k+1:n) + t * a(k,k+1:n);
      end

    end

    z(k) = wk;

  end

  t = sum ( abs ( z(1:n) ) );
  z(1:n) = z(1:n) / t;
%
%  Solve L' * Y = W
%
  for k = n : -1 : 1

    z(k) = z(k) + a(k+1:n,k)' * z(k+1:n)';

    t = abs ( z(k) );

    if ( 1.0E+00 < t )
      z(1:n) = z(1:n) / t;
    end

    l = pivot(k);

    t = z(l);
    z(l) = z(k);
    z(k) = t;

  end

  z(1:n) = z(1:n) / sum ( abs ( z(1:n) ) );

  ynorm = 1.0E+00;
%
%  Solve L * V = Y.
%
  for k = 1 : n

    l = pivot(k);

    t = z(l);
    z(l) = z(k);
    z(k) = t;

    z(k+1:n) = z(k+1:n) + t * a(k+1:n,k)';

    if ( 1.0E+00 < abs ( z(k) ) )
      ynorm = ynorm / abs ( z(k) );
      z(1:n) = z(1:n) / abs ( z(k) );
    end

  end

  s = sum ( abs ( z(1:n) ) );
  z(1:n) = z(1:n) / s;
  ynorm = ynorm / s;
%
%  Solve U * Z = V.
%
  for k = n : -1 : 1

    if ( abs ( a(k,k) ) < abs ( z(k) ) )
      s = abs ( a(k,k) ) / abs ( z(k) );
      z(1:n) = s * z(1:n);
      ynorm = s * ynorm;
    end

    if ( a(k,k) ~= 0.0E+00 )
      z(k) = z(k) / a(k,k);
    else
      z(k) = 1.0E+00;
    end

    z(1:k-1) = z(1:k-1) - z(k) * a(1:k-1,k)';

  end
%
%  Normalize Z in the L1 norm.
%
  s = 1.0E+00 / sum ( abs ( z(1:n) ) );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;

  if ( anorm ~= 0.0E+00 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0E+00;
  end

  return
end


classdef SOP_F23 < PROBLEM
% <single> <real> <expensive/none>
% Shekel's family

%------------------------------- Reference --------------------------------
% X. Yao, Y. Liu, and G. Lin, Evolutionary programming made faster, IEEE
% Transactions on Evolutionary Computation, 1999, 3(2): 82-102.
%------------------------------- 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
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 1;
            obj.D = 4;
            obj.lower    = zeros(1,obj.D);
            obj.upper    = zeros(1,obj.D) + 10;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            a = [4 4 4 4;1 1 1 1;8 8 8 8;6 6 6 6;3 7 3 7;2 9 2 9;5 5 3 3;8 1 8 1;6 2 6 2;7 3.6 7 3.6];
            c = [0.1;0.2;0.2;0.4;0.4;0.6;0.3;0.7;0.5;0.5];
            PopObj = zeros(size(PopDec,1),1);
            for i = 1 : size(PopDec,1)
                PopObj(i) = -sum(1./(sum((repmat(PopDec(i,:),10,1)-a).^2,2)+c));
            end
        end
        %% Generate the minimum objective value
        function R = GetOptimum(obj,N)
            R = -10.54;
        end
    end
end


function [ts_smooth] = circ_smoothTS(varargin)
% USAGE
% [ts_smooth] = circ_smoothTS(varargin)
%
% Given a timeseries input of circular data, this function smooths over the
% desired number of bins and returns a circularly smooth timeseries
%
% INPUTS
%
%   ts         a time series of circular data
%   nBins      integer number of bins you would like to smooth over
%   method     string argument that determines the smoothing method,
%              options are 'median', 'mean' [default: 'median']
%   exclude    vector of values in ts to exclude from smoothing (can be
%              used to exclude 0 values)
%
% OUTPUTS
%
%   ts_smooth  a time series vector of smoothed circular data
%
% HELP
%
% Written by David Tingley, 2017
% TODO error handling when nBins > 1/2 ts

p = inputParser;
addRequired(p,'ts',@isvector);
addRequired(p,'nBins',@isnumeric);
addParameter(p,'method','median',@isstr)
addParameter(p,'exclude',[],@isvector);

parse(p,varargin{:});

ts = p.Results.ts;
if size(ts,1) == 1;
    ts = ts';
end

nBins = p.Results.nBins;
method = p.Results.method;
exclude = p.Results.exclude;

if length(exclude) == length(ts)
   ts_smooth = ts;
   return
end
if nBins == 1
    ts_smooth = ts;
    return
end

if ~isempty(exclude)
    list = find(ts==exclude);
    ts(list) = nan;
end

exclude = find(isnan(ts));
keep = find(~isnan(ts));

f = find(diff(keep)<nBins);
ff = find(diff(keep)>=nBins);
if ~isempty(ff)
    ff(end+1) = length(keep);  % include last ts
end
if length(keep) == 0
    ts(isnan(ts))=0;
    ts_smooth = ts;
    return
end

ts_smooth = zeros(length(ts),1);

for i =1:length(ff)  % populate list with single spikes that occur sparsely
    if keep(ff(i))>ceil(nBins/2) & keep(ff(i))+ceil(nBins/2) < length(ts) % prevents negative indices from being added
        ts_smooth(keep(ff(i))-ceil(nBins/2):keep(ff(i))+ceil(nBins/2)) = ts(keep(ff(i)));
    elseif keep(ff(i))+ceil(nBins/2) < length(ts)
        ts_smooth(1:keep(ff(i))+ceil(nBins/2)) = ts(keep(ff(i)));
    elseif keep(ff(i))>ceil(nBins/2)
        ts_smooth(keep(ff(i))-ceil(nBins/2):end) = ts(keep(ff(i)));
    end
end

for i=1:length(f) % populate list with spikes that occur within smoothing window
    ind = (keep(f(i))-ceil(nBins/2):keep(f(i))+ceil(nBins/2));
    ind(ind<1) = [];
    ind(ind>length(ts)) = [];
    keep = [keep; ind'];
%     if keep(f(i)) > ceil(nBins/2) & keep(f(i)+1)+ceil(nBins/2) < length(ts) % prevents negative indices from being added
%         keep = [keep; [keep(f(i))-ceil(nBins/2):keep(f(i)+1)+ceil(nBins/2)]'];
%     elseif ~(keep(f(i)) > ceil(nBins/2)) &  keep(f(i)+1)+ceil(nBins/2) < length(ts)
%         keep = [keep; [ceil(nBins/2) + 1:keep(f(i)+1)+ceil(nBins/2)]'];
%     elseif keep(f(i)) > ceil(nBins/2) & ~(keep(f(i)+1)+ceil(nBins/2) < length(ts))
%         keep = [keep; [keep(f(i))-ceil(nBins/2):length(ts)- ceil(nBins/2)]'];
%     end
end

keep = sort(unique(keep));
% keep(keep>0)=[];

%% start smoothing
for ii = 1:length(keep)
    i = keep(ii);
    ind = (i-ceil(nBins/2):i+ceil(nBins/2));
    ind(ind<1) = [];
    ind(ind>length(ts)) = [];
    [loc] = ~ismember(ind,exclude);

%     if ~isempty(loc) & sum(loc) > 0

        if strcmp(method,'median')
                ts_smooth(i) = circ_median(ts(ind(loc)));
        elseif strcmp(method,'mean')
                ts_smooth(i) = circ_mean(ts(ind(loc)));
        elseif strcmp(method,'gaussian')
                g = gauss(length(ind),1)';
                ts_smooth(i) = circ_mean(ts(ind(loc)),g(loc));
%                 ts_smooth(i) = circ_mean(ts(ind(loc)));
        elseif strcmp(method,'interp')
                error('interp not implented yet...')
        else
                error('couldnt find smoothing method')
        end
%     else
%         ts_smooth(ind) = ts(i);
%     end

end

ts_smooth(isnan(ts_smooth)) = 0;
% there's a lot of indexing going on, so let's double check the returned
% time series didn't change length...
if length(ts_smooth) ~= length(ts)
   error('output TS is the wrong length!')
end


% SP_L2_ERROR: Evaluate the error in L^2 norm.
%
%   errl2 = sp_l2_error (space, msh, u, uex)
%
% INPUT:
%
%   space: object defining the space of discrete functions (see sp_vector)
%   msh:   object defining the domain partition and the quadrature rule (see msh_cartesian)
%   u:     vector of dof weights
%   uex:   function handle to evaluate the exact solution
%
% OUTPUT:
%
%     errl2:  error in L^2 norm
%
% Copyright (C) 2010 Carlo de Falco
% Copyright (C) 2011, 2015 Rafael Vazquez
%
% 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 Octave; see the file COPYING.  If not, see
% <http://www.gnu.org/licenses/>.

function errl2 = sp_l2_error (space, msh, u, uex)

  if (numel(u) ~= space.ndof)
    error ('Wrong size of the vector of degrees of freedom')
  end

  errl2 = 0;

  for iel = 1:msh.nel_dir(1)
    msh_col = msh_evaluate_col (msh, iel);
    sp_col  = sp_evaluate_col (space, msh_col, 'value', true, 'gradient', false);

    errl2 = errl2 + (sp_l2_error (sp_col, msh_col, u, uex)).^2;
  end

  errl2 = sqrt (errl2);

end


function pde = LinearFun2HM(am,ap,bm,bp,kappa,r,x0,y0,z0)
%% USAGE: polynomial solution for Poisson equation
%  Last Modified: 02/21/2020 by Xu Zhang

%% PDE Structure
pde = struct('intf',@intf,...
    'exactu1',@exactu1,'exactu2',@exactu2,'exactu3',@exactu3,...
    'um1',@um1,'um2',@um2,'um3',@um3,'up1',@up1,'up2',@up2,'up3',@up3,...
    'Dxu',@Dxu,'Dxum',@Dxum,'Dxup',@Dxup,'Dyu',@Dyu,...
    'Dyum',@Dyum,'Dyup',@Dyup,'Dzu',@Dzu,'Dzum',@Dzum,'Dzup',@Dzup,...
    'f1',@f1,'f2',@f2,'f3',@f3,...
    'fm1',@fm1,'fm2',@fm2,'fm3',@fm3,...
    'fp1',@fp1,'fp2',@fp2,'fp3',@fp3,...
    'A',@A,'Am',@Am,'Ap',@Ap,'one',@one,...
    'B',@B,'Bm',@Bm,'Bp',@Bp);

pde.bm = bm;
pde.bp = bp;
pde.am = am;
pde.ap = ap;
pde.kappa = kappa;
%% interface function
    function u = intf(x,y,z)
        u = ((x-x0).^2 + (y-y0).^2 + (z-z0).^2).^(1/2)/r-1;
    end

%% exact solution
    function u = exactu1(x,y,z)
        u = um1(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up1(x(id),y(id),z(id));
    end
    function u = exactu2(x,y,z)
        u = um2(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up2(x(id),y(id),z(id));
    end
    function u = exactu3(x,y,z)
        u = um3(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up3(x(id),y(id),z(id));
    end
    function u = um1(x,y,z)
        u = 2*(x-x0)/bm;
    end
    function u = um2(x,y,z)
        u = 2*(y-y0)/bm;
    end
    function u = um3(x,y,z)
        u = 2*(z-z0)/bm;
    end
    function u = up1(x,y,z)
        u = 2*(x-x0)/bp;
    end
    function u = up2(x,y,z)
        u = 2*(y-y0)/bp;
    end
    function u = up3(x,y,z)
        u = 2*(z-z0)/bp;
    end

%% Boundary Function
    function u = gD1(x,y,z)
        u = exactu1(x,y,z);
    end
    function u = gD2(x,y,z)
        u = exactu2(x,y,z);
    end
    function u = gD3(x,y,z)
        u = exactu3(x,y,z);
    end
%% Derivative of the exact solution
    function u = Dxu(x,y,z)
        u = Dxum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dxup(x(id),y(id),z(id));
    end
    function u = Dyu(x,y,z)
        u = Dyum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dyup(x(id),y(id),z(id));
    end
    function u = Dzu(x,y,z)
        u = Dzum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dzup(x(id),y(id),z(id));
    end
    function u = Dxum(x,y,z)
        u = zeros(size(x));
    end
    function u = Dyum(x,y,z)
        u = zeros(size(x));
    end
    function u = Dzum(x,y,z)
        u = zeros(size(x));
    end
    function u = Dxup(x,y,z)
        u = zeros(size(x));
    end
    function u = Dyup(x,y,z)
        u = zeros(size(x));
    end
    function u = Dzup(x,y,z)
        u = zeros(size(x));
    end

    function u = Duker(x,y,z)
        u = zeros(size(x));
    end

%% right hand side function
    function u = f1(x,y,z)
        u = fm1(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp1(x(id),y(id),z(id));
    end
    function u = f2(x,y,z)
        u = fm2(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp2(x(id),y(id),z(id));
    end
    function u = f3(x,y,z)
        u = fm3(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp3(x(id),y(id),z(id));
    end

    function u = fm1(x,y,z)
        u = -2*(x-x0)*kappa;
    end
    function u = fm2(x,y,z)
        u = -2*(y-y0)*kappa;
    end
    function u = fm3(x,y,z)
        u = -2*(z-z0)*kappa;
    end
    function u = fp1(x,y,z)
        u = -2*(x-x0)*kappa;
    end
    function u = fp2(x,y,z)
        u = -2*(y-y0)*kappa;
    end
    function u = fp3(x,y,z)
        u = -2*(z-z0)*kappa;
    end

%% Diffusion coefficient function
    function u = A(x,y,z)
        u = Am(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Ap(x(id),y(id),z(id));
    end
    function u = Am(x,y,z)
        u = am*ones(size(x));
    end
    function u = Ap(x,y,z)
        u = ap*ones(size(x));
    end
%% Mass coefficient function
    function u = B(x,y,z)
        u = Bm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Bp(x(id),y(id),z(id));
    end
    function u = Bm(x,y,z)
        u = bm*ones(size(x));
    end
    function u = Bp(x,y,z)
        u = bp*ones(size(x));
    end

%% Other function
    function u = one(x,y,z)
        u = ones(size(x));
    end
end


function [X, info] = IRirn(A, b, varargin)
% IRirn Least squares solver with 1-norm penalization term
%
% options  = IRirn('defaults')
% [X,info] = IRirn(A,b)
% [X,info] = IRirn(A,b,K)
% [X,info] = IRirn(A,b,options)
% [X,info] = IRirn(A,b,K,options)
%
% Iteratively reweighted norm algorithm for computing a 1-norm penalized
% solution.
%
% IRirn is a simplified driver for IRrestart, which uses an inner-outer
% iteration scheme. Semi-convergent or hybrid iterative solvers are used
% in the inner iterations, using one of the iterative methods in IRtools
% (e.g., IRhybrid_lsqr). In the case of IRirn, the 1-norm penalization
% is updated at each outer iteration.
%
% The regularization parameter and number of inner iterations influence
% the behavior and convergence of the outer iterations.
%
% With 'defaults' as input returns 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 handle
%  b : right-hand side vector
%  K : (optional) integer vector that specifies which (total) 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)
%      x0            - initial guess for the iterations; default = zero vector
%                      [ array | {'none'} ]
%      MaxIterIn     - maximum number of inner iterations
%      MaxIterOut    - maximum number of outer iterations
%      x_true        - true solution; allows us to returns error norms with
%                      respect to x_true at each iteration
%                      [ array | {'none'} ]
%      RegParam      - a value or a method to find the regularization used
%                      in the inner iterations:
%                      [ non-negative scalar | {'gcv'} | 'discrep' ]
%                      This also determines which stopping rule is used for
%                      the inner iterations.
%                      If 'gcv' is chosen, the inner iteration is stopped
%                        when the GCV function minimum stabilizes or increases
%                        within a certain window of iterations (see 'stopGCV',
%                        'FlatTol' and 'MinTol').
%                      If 'discrep' is chosen, and NoiseLevel is provided,
%                        then the discrepancy principle is used as stopping
%                        criterion (see 'NoiseLevel' and 'eta').
%      stopGCV       - stopping criterion for the inner iterations when
%                      GCV is used
%                      [ GCVvalues | {'resflat'} ]
%      FlatTol       - tolerance for detecting flatness (stabilization)
%                      in the GCV function as a stopping criterion for the
%                      inner iterations
%                      [ {10^-6} | non-negative scalar ]
%      MinTol        - window of iterations: if the GCV minimum continues
%                      to increase over this window, then the inner
%                      iterations are stopped
%                      [ {3} | positive integer ]
%      RegMatrix     - priorconditioner for the inner iterations
%                      [ {'identity'} | square nonsingular matrix |
%                        function handle ]
%      NoiseLevel    - norm of noise in rhs divided by norm of rhs (must be
%                      assigned if RegParam is 'discrep')
%                      [ {'none'} | nonnegative scalar ]
%      eta           - safety factor for the discrepancy principle
%                      [ {1.01} | scalar greater than (and close to) 1 ]
%      RegParam0     - first regularization parameter, used only on the
%                      very first iteration (needed if RegParam is 'discrep')
%                      [ {1} | positive scalar ]
%      stopOut       - stopping criterion for the outer iterations;
%                      [ {'xstab'} | 'Lxstab' | 'regPstab' ]
%      inSolver      - solver to be employed during the inner iterations
%                      [ 'gmres' | {'lsqr'} | 'fgmres' | 'cgls']
%      adaptConstr   - approximate constraint or regularization to be
%                      incorporated
%                      [ {'sp'} | 'spnn' | 'none' ]
%      nonnegativity - may be used to also impose nonnegativity
%                      (similarly to 'spnn')
%                      [ 'on' | {'off'} ]
%      IterBar       - shows the progress of the outer iterations
%                      [ {'on'} | 'off' ]
%      NoStopIn      - specifies whether the inner iterations should
%                      proceed after a stopping criterion has been satisfied
%                      [ 'on' | {'off'} ]
%      NoStopOut     - specifies whether the outer iterations should
%                      proceed after a stopping criterion has been satisfied
%                      [ 'on' | {'off'} ]
%      verbosity     - switch on or off the "verbosity" of the function
%                      [ {'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_in  - string that describes the inner stopping condition:
%                       * Stopping criterion of the inner iterations is
%                         never satisfied
%                       * Stopping criterion is satisfied at least once
%                         during the inner iterations
%      StopFlag_out - string that describes the outer stopping condition;
%                     depending on the inputs it can be one of the following:
%                       * Outer stopping criterion is never satisfied
%                       * Diagonal weighting matrix is numerically zero
%                       * Solution stabilizes
%                       * Transformed solution stabilizes
%                       * Tegularization parameter stabilizes
%      Rnrm     - relative residual norms at each iteration
%      Xnrm     - solution norms at each iteration
%      Enrm     - relative error norms (requires x_true) at each iteration
%      Xout     - approximate solutions at the end of each inner cycle,
%                 stored column-wise
%      itsInOut - 3-column matrix whose the columns store
%                   1. outer iteration count
%                   2. inner iteration count (i.e., for each cycle)
%                   3. total iteration count
%      StopReg  - struct containing information about the solution that
%                 satisfies the stopping criterion.  Fields:
%                   It   : iteration where the stopping criterion is satisfied
%                   X    : solution satisfying the stopping criterion
%                   Enrm : the corresponding relative error (requires x_true)
%      BestReg  - struct containing information about the solution that
%                 minimizes Enrm (requires x_true). Fields:
%                   It   : iteration where the minimum is attained
%                   X    : best solution
%                   Enrm : best relative error
%
% See also: IRell1, IRhtv, IRhybrid_fgmres, IRrestart, 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', 'MaxIterIn', 30 , 'MaxIterOut', 20 , ...
    'RegParam', 'gcv', 'stopGCV', 'resflat', ...
    'resflatTol', 0.05, 'GCVflatTol', 10^-6, 'GCVminTol', 3,...
    'x_true', 'none', 'IterBar', 'on', 'NoStop', 'off', 'NoStopIn', 'off',...
    'NoStopOut', 'off', 'stopOut', 'xstab', 'stabOut', 1e-6, 'thr0', 1e-10, ...
    'NoiseLevel', 'none', 'eta', 1.01, 'RegParam0', 1, 'inSolver', 'lsqr', ...
    'adaptConstr', 'sp', 'nonnegativity', 'off', 'verbosity', 'off',...
    'SparsityTrans', 'none', 'wname', 'db1', 'wlevels', 2, 'warmrestart', 'on');

% 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);
nn       = IRget(options, 'nonnegativity', [], 'fast');
inSolver = IRget(options, 'inSolver',      [], 'fast');

if strcmp(nn, 'on')
    nn = 1;
    options.adaptConstr = 'spnn';
else
    nn = 0;
end

if strcmp(inSolver, 'fgmres') && ~nn
    warning(['With options.inSolver = ''fgmres'' and options.nonnegativity = ''off'' ',...
        'it is more appropriate to use IRhybrid_fgmeres than to use IRirn.'])
end

% Call IRrestart with the specified options.
% Note that nonnegativity is specified via options.adaptConstr.
options = rmfield(options, 'nonnegativity');
[X, info] = IRrestart(A, b, K, options);