function out=atan2(y,x)

if isa(x,'mp')
 precision=x(1).precision;
 if ~isa(y,'mp')
  y=mp(y,precision);
 end
else
 precision=y(1).precision;
 if ~isa(x,'mp')
  x=mp(x,precision);
 end
end

if any(~isreal(x)) | any(~isreal(y))
 warning('atan2 for mp objects currently ignores imaginary parts')
 x=real(x);
 y=real(y);
end

ex=numel(x); ey=numel(y);
if ex==1
 x=ones(size(y))*x;
elseif ey==1
 y=ones(size(x))*y;
end

out=atan(y./x);
mpPi=mppi(precision);
for ii=1:numel(x)
 if x(ii)<0
  if y(ii)>0
   out(ii)=out(ii)+mpPi;
  else
   out(ii)=out(ii)-mpPi;
  end
 end
end

out=real(out);


function [x,y,info] = sedumi(A,b,c,K,pars)
%      [x,y,info] = sedumi(A,b,c,K,pars)
%
% SEDUMI  Self-Dual-Minimization/ Optimization over self-dual homogeneous
%         cones.
%
% >  X = SEDUMI(A,b,c) yields an optimal solution to the linear program
%      MINIMIZE c'*x SUCH THAT A*x = b, x >= 0
%      x is a vector of decision variables.
%      If size(A,2)==length(b), then it solves the linear program
%      MINIMIZE c'*x SUCH THAT A'*x = b, x >= 0
%
% >  [X,Y,INFO] = SEDUMI(A,b,c) also yields a vector of dual multipliers Y,
%      and a structure INFO, with the fields INFO.pinf, INFO.dinf and
%      INFO.numerr.
%
%    (1) INFO.pinf=INFO.dinf=0: x is an optimal solution (as above)
%      and y certifies optimality, viz.\ b'*y = c'*x and c - A'*y >= 0.
%      Stated otherwise, y is an optimal solution to
%      MAXIMIZE b'*y SUCH THAT c-A'*y >= 0.
%      If size(A,2)==length(b), then y solves the linear program
%      MAXIMIZE b'*y SUCH THAT c-A*y >= 0.
%
%    (2) INFO.pinf=1: there cannot be x>=0 with A*x=b, and this is certified
%      by y, viz. b'*y > 0 and A'*y <= 0. Thus y is a Farkas solution.
%
%    (3) INFO.dinf=1: there cannot be y such that c-A'*y >= 0, and this is
%      certified by x, viz. c'*x <0, A*x = 0, x >= 0. Thus x is a Farkas
%      solution.
%
%    (I)   INFO.numerr = 0: desired accuracy achieved (see PARS.eps).
%    (II)  INFO.numerr = 1: numerical problems warning. Results are accurate
%          merely to the level of PARS.bigeps.
%    (III) INFO.numerr = 2: complete failure due to numerical problems.
%
%    INFO.feasratio is the final value of the feasibility indicator. This
%    indicator converges to 1 for problems with a complementary solution, and
%    to -1 for strongly infeasible problems. If feasratio in somewhere in
%    between, the problem may be nasty (e.g. the optimum is not attained),
%    if the problem is NOT purely linear (see below). Otherwise, the reason
%    must lie in numerical problems: try to rescale the problem.
%
% >  [X,Y,INFO] = SEDUMI(A,b,0) or SEDUMI(A,b) solves the feasibility problem
%    FIND x>=0 such that A*x = b
%
% >  [X,Y,INFO] = SEDUMI(A,0,c) or SEDUMI(A,c) solves the feasibility problem
%    FIND y such that A'*y <= c
%
% >  [X,Y,INFO] = SEDUMI(A,b,c,K) instead of the constraint "x>=0", this
%      restricts x to a self-dual homogeneous cone that you describe in the
%      structure K. Up to 5 fields can be used, called K.f, K.l, K.q, K.r and
%      K.s, for Free, Linear, Quadratic, Rotated quadratic and Semi-definite.
%      In addition, there are fields K.xcomplex, K.scomplex and K.ycomplex
%      for complex-variables.
%
%    (1) K.f is the number of FREE, i.e. UNRESTRICTED primal components.
%      The dual components are restricted to be zero. E.g. if
%      K.f = 2 then x(1:2) is unrestricted, and z(1:2)=0.
%      These are ALWAYS the first components in x.
%
%    (2) K.l is the number of NONNEGATIVE components. E.g. if K.f=2, K.l=8
%      then x(3:10) >=0.
%
%    (3) K.q lists the dimensions of LORENTZ (quadratic, second-order cone)
%      constraints. E.g. if K.l=10 and K.q = [3 7] then
%          x(11) >= norm(x(12:13)),
%          x(14) >= norm(x(15:20)).
%      These components ALWAYS immediately follow the K.l nonnegative ones.
%      If the entries in A and/or c are COMPLEX, then the x-components in
%      "norm(x(#,#))" take complex-values, whenever that is beneficial.
%       Use K.ycomplex to impose constraints on the imaginary part of A*x.
%
%    (4) K.r lists the dimensions of Rotated LORENTZ
%      constraints. E.g. if K.l=10, K.q = [3 7] and K.r = [4 6], then
%          2*x(21)x(22) >= norm(x(23:24))^2,
%          2*x(25)x(26) >= norm(x(27:30))^2.
%      These components ALWAYS immediately follow the K.q ones.
%      Just as for the K.q-variables, the variables in "norm(x(#,#))" are
%      allowed to be complex, if you provide complex data. Use K.ycomplex
%      to impose constraints on the imaginary part of A*x.
%
%    (5) K.s lists the dimensions of POSITIVE SEMI-DEFINITE (PSD) constraints
%      E.g. if K.l=10, K.q = [3 7] and K.s = [4 3], then
%          mat( x(21:36),4 ) is PSD,
%          mat( x(37:45),3 ) is PSD.
%      These components are ALWAYS the last entries in x.
%
%    (a) K.xcomplex lists the components in f,l,q,r blocks that are allowed
%     to have nonzero imaginary part in the primal. For f,l blocks, these
%    (b) K.scomplex lists the PSD blocks that are Hermitian rather than
%      real symmetric.
%    (c) Use K.ycomplex to impose constraints on the imaginary part of A*x.
%
%    The dual multipliers y have analogous meaning as in the "x>=0" case,
%    except that instead of "c-A'*y>=0" resp. "-A'*y>=0", one should read that
%    c-A'*y resp. -A'*y are in the cone that is described by K.l, K.q and K.s.
%    In the above example, if z = c-A'*y and mat(z(21:36),4) is not symmetric/
%    Hermitian, then positive semi-definiteness reflects the symmetric/
%    Hermitian parts, i.e. Z + Z' is PSD.
%
%    If the model contains COMPLEX data, then you may provide a list
%    K.ycomplex, with the following meaning:
%      y(i) is complex if ismember(i,K.ycomplex)
%      y(i) is real otherwise
%    The equality constraints in the primal are then as follows:
%          A(i,:)*x = b(i)      if imag(b(i)) ~= 0 or ismember(i,K.ycomplex)
%          real(A(i,:)*x) = b(i)  otherwise.
%    Thus, equality constraints on both the real and imaginary part
%    of A(i,:)*x should be listed in the field K.ycomplex.
%
%    You may use EIGK(x,K) and EIGK(c-A'*y,K) to check that x and c-A'*y
%    are in the cone K.
%
% >  [X,Y,INFO] = SEDUMI(A,b,c,K,pars) allows you to override the default
%      parameter settings, using fields in the structure `pars'.
%
%    (1) pars.fid     By default, fid=1. If fid=0, then SeDuMi runs quietly,
%      i.e. no screen output. In general, output is written to a device or
%      file whose handle is fid. Use fopen to assign a fid to a file.
%
%    (2) pars.alg     By default, alg=2. If alg=0, then a first-order wide
%      region algorithm is used, not recommended. If alg=1, then SeDuMi uses
%      the centering-predictor-corrector algorithm with v-linearization.
%      If alg=2, then xz-linearization is used in the corrector, similar
%      to Mehrotra's algorithm. The wide-region centering-predictor-corrector
%      algorithm was proposed in Chapter 7 of
%        J.F. Sturm, Primal-Dual Interior Point Approach to Semidefinite Pro-
%        gramming, TIR 156, Thesis Publishers Amsterdam, 1997.
%
%    (3) pars.theta, pars.beta   By default, theta=0.25 and beta=0.5. These
%      are the wide region and neighborhood parameters. Valid choices are
%      0 < theta <= 1 and 0 < beta < 1. Setting theta=1 restricts the iterates
%      to follow the central path in an N_2(beta)-neighborhood.
%
%    (4) pars.stepdif, pars.w. By default, stepdif = 2 and w=[1 1].
%       This implements an adaptive heuristic to control ste-differentiation.
%       You can enable primal/dual step length differentiation by setting stepdif=1 or 0.
%      If so, it weights the rel. primal, dual and gap residuals as
%      w(1):w(2):1 in order to find the optimal step differentiation.
%
%    (5) pars.eps     The desired accuracy. Setting pars.eps=0 lets SeDuMi run
%      as long as it can make progress. By default eps=1e-8.
%
%    (6) pars.bigeps  In case the desired accuracy pars.eps cannot be achieved,
%     the solution is tagged as info.numerr=1 if it is accurate to pars.bigeps,
%     otherwise it yields info.numerr=2.
%
%    (7) pars.maxiter Maximum number of iterations, before termination.
%
%    (8) pars.stopat  SeDuMi enters debugging mode at the iterations specified in this vector.
%
%    (9) pars.cg      Various parameters for controling the Preconditioned conjugate
%     gradient method (CG), which is only used if results from Cholesky are inaccurate.
%    (a) cg.maxiter   Maximum number of CG-iterates (per solve). Theoretically needed
%          is |add|+2*|skip|, the number of added and skipped pivots in Cholesky.
%          (Default 49.)
%    (b) cg.restol    Terminates if residual is a "cg.restol" fraction of duality gap.
%          Should be smaller than 1 in order to make progress (default 5E-3).
%    (c) cg.refine    Number of refinement loops that are allowed. The maximum number
%          of actual CG-steps will thus be 1+(1+cg.refine)*cg.maxiter. (default 1)
%    (d) cg.stagtol  Terminates if relative function progress less than stagtol (5E-14).
%    (e) cg.qprec    Stores cg-iterates in quadruple precision if qprec=1 (default 0).
%
%    (10) pars.chol   Various parameters for controling the Cholesky solve.
%     Subfields of the structure pars.chol are:
%    (a) chol.canceltol: Rel. tolerance for detecting cancelation during Cholesky (1E-12)
%    (b) chol.maxu:   Adds to diagonal if max(abs(L(:,j))) > chol.maxu otherwise (5E5).
%    (c) chol.abstol: Skips pivots falling below abstol (1e-20).
%    (d) chol.maxuden: pivots in dense-column factorization so that these factors
%      satisfy max(abs(Lk)) <= maxuden (default 5E2).
%
%    (11) pars.vplot  If this field is 1, then SeDuMi produces a fancy
%      v-plot, for research purposes. Default: vplot = 0.
%
%    (12) pars.errors  If this field is 1 then SeDuMi outputs some error
%    measures as defined in the Seventh DIMACS Challenge. For more details
%    see the User Guide.
%
% Bug reports can be submitted at http://sedumi.mcmaster.ca.
%
% See also mat, vec, cellK, eyeK, eigK

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

% J.F. Sturm, "Using SeDuMi 1.02, a MATLAB toolbox for optimization over
% symmetric cones," Optimization Methods and Software 11-12 (1999) 625-653.
% http://sedumi.mcmaster.ca

cputime0=cputime;
% ************************************************************
% INITIALIZATION
% ************************************************************
% ----------------------------------------
% Check input
% ----------------------------------------
if (nargin < 5)
    pars.fid = 1;
    if nargin < 3
        if nargin < 2
            error('Should have at least (A,b) or (A,c) arguments')
        end
        if length(b) == max(size(A))
            c = b; b = 0;           % given (A,c): LP feasibility problem
        else
            c = 0;                  % given (A,b): LP feasibility problem
        end
    end
    if isstruct(c)
        if nargin == 4
            pars = K;      % given (A,b,K,pars) or (A,c,K,pars)
        end
        K = c;          % given (A,b,K) or (A,c,K): cone feasibility problem
        if length(b) == max(size(A))
            c = b; b = 0;
        else
            c = 0;
        end
    elseif nargin < 4
        K.l = max(size(A));    % given (A,b,c): default to LP problem.
    end
end
% ----------------------------------------
% Bring data in (strict) internal SeDuMi form, check parameters,
% and print welcome.
% ----------------------------------------
[A,b,c,K,prep,origcoeff] = pretransfo(A,b,c,K,pars);
[N,m]=size(A);
if issparse(A)
    if sprank([A;b'])>sprank(A)
        info.pinf=1;
        x=[];
        %A dual improving direction could be computed easily
        y=[A;b']\[zeros(N,1);1];

        warning('SeDuMi:PrimalInfeasible','The problem is primal infeasible. there is no x such that Ax=b.')
        return
    elseif sprank(A)<m
        error('The coefficient matrix is not full row rank.')
    end
else
    if rank([A;b'])>rank(A)
        info.pinf=1;
        x=[];
        y=[A;b']\[zeros(N,1);1];
        warning('SeDuMi:PrimalInfeasible', 'The problem is primal infeasible, there is no x such that Ax=b.')
        return
    elseif rank(A)<m
        error('The coefficient matrix is not full row rank.')
    end
end
if prep.cpx.dim>0
    origcoeff=[];      % No error measures for complex problems.
end
lponly = (K.l == length(c));
pars = checkpars(pars,lponly);
% ----------------------------------------
% Print welcome and statistics of cone-problem
% ----------------------------------------
my_fprintf(pars.fid,'SeDuMi 1.21 by AdvOL, 2005-2008 and Jos F. Sturm, 1998-2003.\n');
switch pars.alg
    case 0
        my_fprintf(pars.fid,'Alg = 0: No corrector, ');
    case  1
        my_fprintf(pars.fid,'Alg = 1: v-corrector, ');
    case 2
        my_fprintf(pars.fid,'Alg = 2: xz-corrector, ');
end
switch pars.stepdif
    case 0
    case 1
        my_fprintf(pars.fid,'Step-Differentiation, ');
    case 2
        my_fprintf(pars.fid,'Adaptive Step-Differentiation, ');
end
my_fprintf(pars.fid,'theta = %5.3f, beta = %5.3f\n',pars.theta,pars.beta);
% --------------------------------------------------
% Print preprocessing information
% --------------------------------------------------
if pars.prep==1
    if isfield(prep,'sdp')
        blockcount=0;
        varcount=0;
        for sdpind=1:length(prep.sdp)
            if prep.sdp{sdpind}(1)==1
                blockcount=blockcount+1;
                varcount=varcount+prep.sdp{sdpind}(2);
            end
        end
        if blockcount>0
            my_fprintf(pars.fid,'Detected %i diagonal SDP block(s) with %i linear variables\n',blockcount,varcount);
        end
    end
    if isfield(prep,'freeblock1') && ~isempty(prep.freeblock1)
        my_fprintf(pars.fid,'Detected %i free variables in the linear part\n',length(prep.freeblock1));
    end
    if isfield(prep,'Kf') && prep.Kf>0
        switch pars.free
            case 0
                my_fprintf(pars.fid,'Split %i free variables\n',prep.Kf);
            case 1
                my_fprintf(pars.fid,'Put %i free variables in a quadratic cone\n',prep.Kf);
        end
    end
end
% --------------------------------------------------
% Remove dense columns (if any)
% --------------------------------------------------
Ablkjc = partitA(A,K.mainblks);
[dense,DAt.denq] = getdense(A,Ablkjc,K,pars);
if ~isempty(dense.cols)
    dense.A = A(dense.cols,:)';
    A(dense.cols,:) = 0.0;
    Ablkjc = partitA(A,K.mainblks);
else
    dense.A = sparse(length(b),0);
end
% ----------------------------------------
% Order constraints from sparse to dense, and find corresponding
% incremental nonzero pattern "Aord.dz" of At*dy in PSD part.
% ----------------------------------------
Aord.lqperm = sortnnz(A,[],Ablkjc(:,3));        % Sparse LP+Lorentz
DAt.q = findblks(A,Ablkjc,2,3,K.qblkstart);     % Lorentz ddotA-part
if ~isempty(DAt.q)
    DAt.q(dense.q,:) = 0.0;
    DAt.q = DAt.q + spones(extractA(A,Ablkjc,1,2,K.mainblks(1),K.mainblks(2)));
    Aord.qperm = sortnnz(DAt.q,[],[]);
else
    Aord.qperm = (1:length(b))';
end
[Aord.sperm, Aord.dz] = incorder(A,Ablkjc(:,3),K.mainblks(3));  % PSD
% ----------------------------------------
% Get nz-pattern of ADA.
% ----------------------------------------
ADA = getsymbada(A,Ablkjc,DAt,K.sblkstart);
% ----------------------------------------
% Ordering and symbolic factorization of ADA.
% ----------------------------------------
L = symbchol(ADA);
% --------------------------------------------------
% Symbolic fwsolve dense cols: L\[dense.A, dense.blkq],
% sparse ordering for dense column factorization
% --------------------------------------------------
symLden = symbcholden(L,dense,DAt);
% ----------------------------------------
% Initial solution
% ----------------------------------------
[d, v,vfrm, y,y0, R] = sdinit(A,b,c,dense,K,pars);
n = length(vfrm.lab);                         % order of K
merit = (sum(R.w) + max(R.sd,0))^2 * y0 / R.b0;  % Merit function
my_fprintf(pars.fid,'eqs m = %g, order n = %g, dim = %g, blocks = %g\n',...
    length(b),n,length(c),1 + length(K.q) + length(K.s));
my_fprintf(pars.fid,'nnz(A) = %d + %d, nnz(ADA) = %d, nnz(L) = %d\n',nnz(A),nnz(dense.A), nnz(ADA), nnz(L.L));
if ~isempty(dense.cols)
    my_fprintf(pars.fid,'Handling %d + %d dense columns.\n',...
        length(dense.cols),length(dense.q));
end
my_fprintf(pars.fid,' it :     b*y       gap    delta  rate   t/tP*  t/tD*   feas cg cg  prec\n');
my_fprintf(pars.fid,'  0 :            %8.2E %5.3f\n',merit,0);
% ----------------------------------------
% Initialize iterative statistics
% ----------------------------------------
STOP = 0;
err.maxb = 0.0;                 % Estimates the need to recompute residuals
iter = 0;
if pars.vplot == 1
    vlist = [vfrm.lab];
    ratelist = [];
end
wr.delta = 0.0;
wr.desc = 1;
%Seems unnecessary
%rate = 1.0;
feasratio = 0.0;
cputime1 = cputime;
% ************************************************************
% MAIN PREDICTOR-CORRECTOR LOOP
% ************************************************************
while STOP == 0
    iter = iter+1;
    if any(iter == pars.stopat)
        keyboard
    end

    if pars.stepdif==2 && ...
            (iter>20 || (iter>1 && (err.kcg + Lsd.kcg>3)) || ...
            (iter>5 && abs(1-feasratio)<0.05) )
        pars.stepdif=1;
    end
    % --------------------------------------------------
    % Compute ADA
    % --------------------------------------------------
    DAt = getDAtm(A, Ablkjc, dense, DAt.denq, d, K);
    ADA = getada1(ADA, A, Ablkjc(:,3), Aord.lqperm, d, K.qblkstart);
    ADA = getada2(ADA, DAt, Aord, K);
    [ADA,absd] = getada3(ADA, A, Ablkjc(:,3), Aord, invcholfac(d.u, K, d.perm), K);
    % ------------------------------------------------------------
    % Block Sparse Cholesky: ADA(L.perm,L.perm) = L.L*diag(L.d)*L.L'
    % ------------------------------------------------------------
    [L.L,L.d,L.skip,L.add] = blkchol(L,ADA,pars.chol,absd);
    % ------------------------------------------------------------
    % Factor dense columns
    % ------------------------------------------------------------
    [Lden, L.d] = deninfac(symLden, L,dense,DAt,d,absd, K.qblkstart,pars.chol);
    % ----------------------------------------
    % FACTORIZATION of self-dual embedding
    % ----------------------------------------
    Lsd = sdfactor(L,Lden, dense,DAt, d,v,y, A,c,K,R, y0,pars);
    % ------------------------------------------------------------
    % Compute and take IPM-step
    % from (v,y,v, y0) --> (xscl,y,zscl,y0)
    % ------------------------------------------------------------
    y0Old = y0;
    [xscl,yNxt,zscl,y0Nxt, w,relt, dxmdz,err, wr] = ...
        wregion(L,Lden,Lsd,...
        d,v,vfrm,A,DAt,dense, R,K,y,y0,b, pars, wr);
    % ------------------------------------------------------------
    % Evaluate the computed step.
    % ------------------------------------------------------------
    if y0Nxt > 0
        R.b = R.b + err.b / y0Nxt;
        R.sd = R.sd + err.g / y0Nxt;
        R.b0 = R.b0 + err.db0 / y0Nxt;
        y0 = y0Nxt;
    else
        R.b = (y0Nxt * R.b + err.b) / y0Old;      % In fact, we should have y0=0.
        R.sd = (y0Nxt * R.sd + err.g) / y0Old;
        R.b0 = (y0Nxt * R.b0 + err.db0) / y0Old;
        R.w(2) = abs(y0Nxt/y0Old) * R.w(2);        %=0: dual feasible
        R.c = (y0Nxt/y0Old) * R.c;
        R.maxRc = norm(R.c,inf);
        y0 = y0Old;
    end
    R.maxRb = norm(R.b,inf);                % Primal residual
    R.w(1) = 2 * pars.w(1) * R.maxRb / (1+R.maxb);
    meritOld = merit;
    merit = (sum(R.w) + max(R.sd,0))^2 * y0 / R.b0;
    rate = merit / meritOld;
    if (rate >= 0.9999) && (wr.desc == 1)
        % ------------------------------------------------------------
        % STOP = -1  --> Stop due to numerical problems
        % ------------------------------------------------------------
        STOP = -1;                  % insuf. progress in descent direction.
        iter = iter - 1;
        y0 = y0Old; %#ok
        my_fprintf(pars.fid,'Run into numerical problems.\n');
        break
    end
    feasratio = dxmdz(1) / v(1);            % (deltax0/x0) - (deltaz0/z0)
    % --------------------------------------------------
    % Primal-Dual transformation
    % --------------------------------------------------
    y = yNxt;
    by = full(sum(b.*y));
    [d,vfrm] = updtransfo(xscl,zscl,w,d,K);
    v = frameit(vfrm.lab,vfrm.q,vfrm.s,K);
    x0 = sqrt(d.l(1)) * v(1);
    % ----------------------------------------
    % SHOW ITERATION STATISTICS
    % ----------------------------------------
    my_fprintf(pars.fid,' %2.0f : %10.2E %8.2E %5.3f %6.4f %6.4f %6.4f %6.2f %2d %2d  ',...
        iter,by/x0,merit,wr.delta,rate,relt.p,relt.d,feasratio,err.kcg, Lsd.kcg);
    if pars.vplot == 1
        vlist = [vlist vfrm.lab/sqrt((R.b0*y0)/n)];
        ratelist = [ratelist rate];
    end
    % ----------------------------------------
    % If we get in superlinear region of LP,
    % try to guess optimal solution:
    % ----------------------------------------
    if lponly && (rate < 0.05)
        [xsol,ysol] = optstep(A,b,c, y0,y,d,v,dxmdz, ...
            K,L,symLden,dense, Ablkjc,Aord,ADA,DAt, feasratio, R,pars);
        if ~isempty(xsol)
            STOP = 2;                   % Means that we guessed right !!
            feasratio = 1 - 2*(xsol(1)==0);
            break
        end
    elseif (by > 0) && (abs(1+feasratio) < 0.05) && (R.b0*y0 < 0.5)
        if max(eigK(full(qreshape(Amul(A,dense,y,1),1,K)),K)) <= pars.eps * by
            STOP = 3;                   % Means Farkas solution found !
            break
        end
    end
    % --------------------------------------------------
    % OPTIMALITY CHECK: stop if y0*resid < eps * (x0+z0).
    % For feas. probs, we should divide the residual by x0, otherwise by z0.
    % Before stopping, recompute R.norm, since it may have changed due to
    % residual updates (the change should be small though).
    % --------------------------------------------------
    r0 = sum(R.w);
    cx = by + y0*R.sd - x0 / d.l(1);
    rgap = max(cx-by,0) / max([abs(cx),abs(by),1e-3 * x0]);
    precision1=y0*r0/(1+x0);
    precision2=(y0 * r0 + rgap)/x0;
    my_fprintf(pars.fid,'%1.1E\n',max(precision1,precision2));
    if precision1 < pars.eps       % P/D residuals small
        if precision2 < pars.eps    %Approx feasible and optimal
            STOP = 1;
            break
        elseif y0 * R.maxRb + x0 * R.maxb < -pars.eps * cx   % Approx Farkas
            STOP = 1;
            break
        elseif y0 * R.maxRc + x0 * R.maxc < pars.eps * by    % Approx Farkas
            STOP = 1;
            break;
        end
    end
    if iter >= pars.maxiter
        my_fprintf(pars.fid,'Maximum number of iterations reached.\n');
        STOP = -1;
    end
end % while STOP == 0.
my_fprintf(pars.fid,'\n');
clear ADA
nnzLadd=nnz(L.add);
nnzLskip=nnz(L.skip);
normLL=full(max(max(abs(L.L))));
clear L
% ************************************************************
% FINAL TASKS:
% ************************************************************
cputime2=cputime;
info.iter = iter;
info.feasratio = feasratio;
info.pinf = 0; info.dinf = 0;
info.numerr = 0;
% ------------------------------------------------------------
% Create x = D*v.
% ------------------------------------------------------------
if STOP == 2                % Exact optimal solution found (LP)
    x = xsol;
    y = ysol;
elseif STOP == 3            % Farkas solution y found (in early stage)
    x = zeros(length(c),1);
else
    x = [sqrt(d.l).*v(1:K.l); asmDxq(d,v,K); psdscale(d,v,K,1)];
end
% --------------------------------------------------
% Compute cx, Ax, etc.
% --------------------------------------------------
x0 = x(1);
cx = full(sum(c.*x));
abscx = sum(abs(c).*abs(x));
by = full(sum(b.*y));
Ax = Amul(A,dense,x,0);
Ay = full(Amul(A,dense,y,1));      % "full" since y may be scalar.
normy = norm(y);
normx = norm(x(2:end));
clear A

% ------------------------------------------------------------
% Determine infeasibility
% ------------------------------------------------------------

pinf = norm(x0*b-Ax);
z = qreshape(Ay-x0*c,1,K);
dinf = max(eigK(z,K));
if x0 > 0
    relinf = max(pinf / (1+R.maxb), dinf / (1+R.maxc)) / x0;
    % ------------------------------------------------------------
    % If infeasibility larger than epsilon, evaluate Farkas-infeasibility
    % ------------------------------------------------------------
    if relinf > pars.eps
        pdirinf = norm(Ax);
        ddirinf = max(eigK(qreshape(Ay,1,K),K));
        if cx < 0.0
            reldirinf = pdirinf / (-cx);
        else
            reldirinf = inf;
        end
        if by > 0.0
            reldirinf = min(reldirinf, ddirinf / by);
        end
        % ------------------------------------------------------------
        % If the quality of the Farkas solution is good and better than
        % the approx. feasible soln, set x0=0: Farkas solution found.
        % ------------------------------------------------------------
        if (reldirinf < pars.eps) || (relinf > max(pars.bigeps, reldirinf))
            x0 = 0.0;
            pinf = pdirinf;
            dinf = ddirinf;
        end
    end % relinf too large
end % x0 > 0
% ------------------------------------------------------------
% Interpret the solution as feasible:
% ------------------------------------------------------------
info.r0 = Inf;
if x0 > 0
    x = x / x0;
    y = y / x0;
    pinf = pinf /x0;
    dinf = dinf / x0;
    cx = cx/ x0;
    by = by / x0;
    normx = normx / x0;
    normy = normy / x0;
    if cx <= by                % zero or negative duality gap
        r0 = 0;
    elseif cx == 0.0           % Dual feasibility problem
        r0 = -by/(R.maxb*normy +1E-10 * x0);
    elseif by == 0.0           % Primal feasibility problem
        r0 = cx / (R.maxc*normx +1E-10 * x0);
    else                       % Optimization problem
        r0 = (cx-by)/(abs(by) + 1E-5 * (x0+abscx));
    end
    if r0 == 0,
        sigdig = Inf;
    else
        sigdig = -log10(r0);
    end
    my_fprintf(pars.fid,...
        'iter seconds digits       c*x               b*y\n');
    my_fprintf(pars.fid,'%3d %8.1f %5.1f %- 17.10e %- 17.10e\n',...
        iter,cputime2-cputime1,sigdig,cx,by);
    my_fprintf(pars.fid,'|Ax-b| = %9.1e, [Ay-c]_+ = %9.1E, |x|=%9.1e, |y|=%9.1e\n',...
        pinf,dinf,normx,normy);
    % ------------------------------------------------------------
    % Determine level of numerical problems with x0>0 (feasible)
    % ------------------------------------------------------------
    info.r0 = max([r0 ; pinf;dinf] ./ [1; 1+R.maxb+(1E-3)*R.maxRb;...
            1+R.maxc+(1E-3)*R.maxRc]);
    if STOP == -1
        if info.r0 > pars.bigeps
            my_fprintf(pars.fid, 'No sensible solution found.\n');
            info.numerr = 2;                          % serious numerical error
        elseif info.r0 > pars.eps
            info.numerr = 1;                          % moderate numerical error
        else
            info.numerr = 0;                          % achieved desired accuracy
        end
    else
        info.r0 = min( info.r0, pars.eps );
    end
else  % (if x0>0)
    % --------------------------------------------------
    % Infeasible problems: pinf==norm(Ax), dinf==max(eigK(At*y,K)).
    % --------------------------------------------------
    if pinf < -pars.bigeps * cx
        info.r0 = abs(pinf/cx);
        info.dinf = 1;
        abscx = -cx;
        pinf = pinf / abscx;
        normx = normx / abscx;
        x = x / abscx;
        my_fprintf(pars.fid, 'Dual infeasible, primal improving direction found.\n');
    end
    if dinf < pars.bigeps * by
        info.r0 = abs(dinf/by);
        info.pinf = 1;
        dinf = dinf / by;
        normy = normy / by;
        y = y / by;
        my_fprintf(pars.fid, 'Primal infeasible, dual improving direction found.\n');
    end
    my_fprintf(pars.fid,'iter seconds  |Ax|    [Ay]_+     |x|       |y|\n');
    my_fprintf(pars.fid,'%3d %8.1f %9.1e %9.1e %9.1e %9.1e\n',...
        iter,cputime2-cputime1,pinf,dinf,normx,normy);
    % --------------------------------------------------
    % Guess infeasible, but stopped due to numerical problems
    % --------------------------------------------------
    if info.pinf + info.dinf == 0
        my_fprintf(pars.fid, 'Failed: no sensible solution/direction found.\n');
        info.numerr = 2;
    elseif STOP == -1
        if (pinf > -pars.eps * cx) && (dinf > pars.eps * by)
            info.numerr = 1;
        else
            info.numerr = 0;
        end
    end
end
% ----------------------------------------
% - Bring xsol into the complex format of original (At,c),
% - transform q-variables into r-variables (Lorentz),
% - bring ysol into complex format, indicated by K.ycomplex.
% - at 0's in ysol where rows where removed.
% ----------------------------------------'
[x,y,K] = posttransfo(x,y,prep,K,pars); %#ok
% Detailed timing
%Preprocessing+IPM+Postprocessing
info.timing=[cputime1-cputime0 cputime2-cputime1 cputime-cputime2];
% Total time (for backward compatibility)
info.cpusec=sum(info.timing);
my_fprintf(pars.fid,'\nDetailed timing (sec)\n')
my_fprintf(pars.fid,'   Pre          IPM          Post\n')
my_fprintf(pars.fid,'%1.3E    ',info.timing)
my_fprintf(pars.fid,'\n')

% ----------------------------------------
% Make a fancy v-plot if desired
% ----------------------------------------
if pars.vplot == 1
    subplot(2,1,1)
    plot(0:iter,vlist,'o',[0 iter],[1 1],'b',...
        [0 iter],[pars.theta pars.theta],'g')
    title('Wide region v-plot')
    xlabel('iterations')
    ylabel('normalized v-values')
    subplot(2,1,2)
    plot(1:iter,ratelist)
    axis([0 iter 0 1])
    title('Reduction rates')
    xlabel('iterations')
    ylabel('reduction rate')
end
my_fprintf(pars.fid,'Max-norms: ||b||=%d, ||c|| = %d,\n',R.maxb,R.maxc);
my_fprintf(pars.fid,'Cholesky |add|=%d, |skip| = %d, ||L.L|| = %g.\n',...
    nnzLadd, nnzLskip, normLL);

% ----------------------------------------
% Compute error measures if needed
% ----------------------------------------
if ~isempty(origcoeff)
    %Reload the original coefficients
    s=(origcoeff.c)-(origcoeff.At)*sparse(y);       %To make s sparse
    cx=sum((origcoeff.c).*x);        %faster than c'*x
    by=sum((origcoeff.b).*y);
    xs=sum(x.*s);
    normb=norm(origcoeff.b,1);
    normc=norm(origcoeff.c,1);
    info.err=zeros(1,6);
    %     Error measures.
    %     Primal infeasibility
    info.err(1)=norm(x'*(origcoeff.At)-(origcoeff.b)',2)/(1+normb);
    %Let us get rid of the K.f part, since the free variables don't make
    %any difference in the cone infeasibility.
    %origcoeff.K.f=0;
    %     Primal cone infeasibility
    info.err(2)=max(0,-min(eigK(full(x(origcoeff.K.f+1:end)),origcoeff.K))/(1+normb));
    %     Dual infeasibility
    %info.err(3)=0.0; %s is not maintained explicitely
    %     Dual cone infeasibility
    info.err(4)=max(0,-min(eigK(full(s(origcoeff.K.f+1:end)),origcoeff.K))/(1+normc));
    %     Relative duality gap
    info.err(5)=(cx-by)/(1+abs(cx)+abs(by));
    %     Relative complementarity
    info.err(6)=xs/(1+abs(cx)+abs(by));

    my_fprintf(pars.fid,'\nDIMACS error measures\n')
    my_fprintf(pars.fid,'  PInf     PConInf     DInf    DConInf    RelGap    RelComp\n')
    my_fprintf(pars.fid,'%2.2E  ',info.err)
    my_fprintf(pars.fid,'\n')
end


function [sol, info] = gsp_proj_filterbank(x, ~ , G, W, y, param)
%GSP_PROJ_FILTERBANK Projection onto the synthesis coefficients
%   Usage:  sol = gsp_proj_filterbank(x, 0, G, W, y, param);
%           sol = gsp_proj_filterbank(x, 0, G, W, y);
%           [sol, info] = gsp_proj_filterbank(...)
%
%   Input parameters:
%         x     : Input signal
%         G     : Graph structure
%         W     : Filterbank (cell array of functions)
%         y     : Measurements
%         param : Structure of optional parameters
%   Output parameters
%         sol   : Solution.
%         info  : Structure summarizing informations at convergence
%
%   `gsp_proj_filterbank(x, gamma, G, W, param)` can solves:
%
%   .. sol = argmin_{z} 0.5*||x - z||_2^2  such that W^* x =y
%
%   .. math::  sol = \min_{z} \frac{1}{2} \|x - z\|_2^2 \text{ s. t. }  W^* x = y
%
%   Where $W$ is the linear analysis operator associated with the
%   filterbank.
%
%   The function can use different techniques
%
%   * 'exact' : if the Fourier basis is computed, go for this one
%   * 'cheby' : use the pseudo-inverse filters of the filterbank with
%     chebyshev approximation. It works well for well-conditionned
%     filterbanks.
%   * 'lanczos' : use the pseudo-inverse filters of the filterbank with
%     lanczos approximation. It works well for well-conditionned
%     filterbanks.
%   * 'proj_b2': scallable and robust way to do it. However, the
%     convergence maybe slow and might require a lot of filtering
%     operations.
%
%   param is a Matlab structure containing the following fields:
%
%   * *param.verbose* : 0 no log, 1 a summary at convergence, 2 print main
%     steps (default: 1)
%   * *param.eps* : tolerance for the pseudo inverse method
%   * *param.proj_method*: selected method
%
%   info is a Matlab structure containing the following fields:
%
%   * *info.algo* : Algorithm used
%   * *info.iter* : Number of iteration
%   * *info.time* : Time of exectution of the function in sec.
%   * *info.final_eval* : Final evaluation of the function
%   * *info.crit* : Stopping critterion used
%
%
%   See also:  gsp_solve_l1 gsp_proj_b2_filterbank
%

% Author: Nathanael Perraudin
% Date: 25 March 2014
% Testing: test_gsp_proj_filerbank

if nargin < 5
    error('GSP_PROJ_FILTERBANK: Not enought input arguments');
end

if nargin < 6, param=struct; end

if ~isfield(param, 'eps'), param.eps = 1e-8; end
if ~isfield(param, 'proj_method'),
    if gsp_check_fourier(G)
        param.proj_method = 'exact';
    else
        [A,B] = gsp_filterbank_bounds(G,W);
        if B/A > 20
            warning('Filterbank ill-conditioned, going for a slow method');
            param.proj_method = 'primal_dual';
        else
            param.proj_method = 'cheby';
        end
    end
end

t = tic;

if size(y,2)==1 && size(x,2)>1
    y = repmat(y,1,size(x,2));
end
if isfield(param,'method')
    param2 = rmfield(param,'method');
else
    param2 = param;
end
if strcmp(param.proj_method, 'proj_b2') || strcmp(param.proj_method, 'primal_dual')
    [~,B] = gsp_filterbank_bounds(G,W);
    if ~isfield(param,'paramproj'), param.paramproj = struct; end
    paramproj = param.paramproj;
    paramproj.A = @(x) gsp_filter_synthesis(G,W,x,param2);
    paramproj.At = @(x) gsp_filter_analysis(G,W,x,param2);
    paramproj.nu = B^2;
    paramproj.epsilon = sqrt(G.N)*param.eps;
    paramproj.y = y;
    paramproj.method  = param.proj_method;

    [sol,info] = proj_linear_eq(x,0,paramproj);

else
    if strcmp(param.proj_method, 'cheby')  || strcmp(param.proj_method, 'lanczos')
        [A,B] = gsp_filterbank_bounds(G,W);
        if B/A > 20
            warning('Filterbank ill-conditioned, check your solution!');
        end
    end
    Wd = gsp_design_can_dual(W,param.eps);
    sol =  x - gsp_filter_analysis(G,Wd,(gsp_filter_synthesis(G,W,x,param2)-y),param2);

    info.iter = 1;
    info.final_eval = 0;
    info.crit = 'Direct computation';
end

    info.time = toc(t);
    info.algo = param.proj_method;

end


function [Jul1,Jul2]=GPS2TT(Jul1,Jul2)
%GPS2TT  Convert from the timescale used by the Global Positioning System
%           (GPS)  given as a two-part Julian date to terrestrial time
%           (TT), represented as a two-part Julian date.
%
%INPUTS:    Jul1, Jul2  Two parts of a Julian date given in GPS time. The
%                       units of the date are days. The full date is the
%                       sum of both terms. The date is broken into two
%                       parts to provide more bits of precision. It does
%                       not matter how the date is split.
%
%OUTPUTS:   Jul1, Jul2  The time as a Julian date in TT.
%
%This function just calls GPS2TAI and TAI2TT.
%
%Many temporal coordinate systems standards are compared in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report, Naval
%    Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016, 173
%    pages.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

[Jul1,Jul2]=GPS2TAI(Jul1,Jul2);
[Jul1,Jul2]=TAI2TT(Jul1,Jul2);
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.


% POWER_BOUNDED computes the power cells about the points (x,y) inside
% the bounding box (must be a rectangle or a square) crs.  If crs is not supplied, an
% axis-aligned box containing (x,y) is used.
% It is optimised to work fast on large number of sites (e.g. 10000 sites or more)
% Input:
%   * x, y: coordinate of the Voronoi point (numPoints x 1)
%   * wts: weights of each point (numPoints x 1)
%   * crs: vortices of the bounding box in cw order (numVert x 2)
% Output:
%   * V: x,y-coordinate of vertices of the power cells
%   * C: indices of the Voronoi cells from V
% See Matlab's voronoin for more information about the output
% Made by: Aaron Becker, atbecker@uh.edu, and Muhammad Kasim, muhammad.kasim@wolfson.ox.ac.uk

function [V,C] = power_bounded(x,y, wts, crs)
    bnd=[min(x) max(x) min(y) max(y)]; %data bounds
    if nargin < 3
        crs=double([bnd(1) bnd(4);bnd(2) bnd(4);bnd(2) bnd(3);bnd(1) bnd(3);bnd(1) bnd(4)]);
    end

    rgx = max(crs(:,1))-min(crs(:,1));
    rgy = max(crs(:,2))-min(crs(:,2));
    rg = max(rgx,rgy);
    midx = (max(crs(:,1))+min(crs(:,1)))/2;
    midy = (max(crs(:,2))+min(crs(:,2)))/2;

    % add 4 additional edges
    xA = [x; midx + [0;0;-5*rg;+5*rg]];
    yA = [y; midy + [-5*rg;+5*rg;0;0]];

    if (all(wts == 0))
        [vi,ci] = voronoin([xA,yA]);
    else
        [vi,ci] = powerDiagram2([xA,yA], [wts;zeros(4,1)]);
    end

    % remove the last 4 cells
    C = ci(1:end-4);
    V = vi;
    % use Polybool to crop the cells
    %Polybool for restriction of polygons to domain.

    maxX = max(crs(:,1)); minX = min(crs(:,1));
    maxY = max(crs(:,2)); minY = min(crs(:,2));
    for ij=1:length(C)
        % thanks to http://www.mathworks.com/matlabcentral/fileexchange/34428-voronoilimit
        Cij = C{ij};
        if (length(Cij) == 0) continue; end;

        % first convert the contour coordinate to clockwise order:
        pts = V(Cij,:);
        K = convhull(pts);
        K = K(end-1:-1:1);
        C{ij} = Cij(K);
        X2 = pts(K,1);
        Y2 = pts(K,2);

        % if all points are inside the bounding box, then skip it
        if (all((X2 <= maxX) & (X2 >= minX) & (Y2 <= maxY) & (Y2 >= minY))) continue; end;

        [xb, yb] = clip_polygons(crs(:,1),crs(:,2),X2,Y2);
        % xb = xb'; yb = yb';
        ix=nan(1,length(xb));
        for il=1:length(xb)
            if any(V(:,1)==xb(il)) && any(V(:,2)==yb(il))
                ix1=find(V(:,1)==xb(il));
                ix2=find(V(:,2)==yb(il));
                for ib=1:length(ix1)
                    if any(ix1(ib)==ix2)
                        ix(il)=ix1(ib);
                    end
                end
                if isnan(ix(il))==1
                    lv=length(V);
                    V(lv+1,1)=xb(il);
                    V(lv+1,2)=yb(il);
                    ix(il)=lv+1;
                end
            else
                lv=length(V);
                V(lv+1,1)=xb(il);
                V(lv+1,2)=yb(il);
                ix(il)=lv+1;
            end
        end
        C{ij} = ix;
    end
end


function [registered]=nonrigidICP(target,source,Ft,Fs,iterations)

% INPUT
% -target: vertices of target mesh; n*3 array of xyz coordinates
% -source: vertices of source mesh; n*3 array of xyz coordinates
% -Ft: faces of target mesh; n*3 array
% -Fs: faces of source mesh; n*3 array
% -iterations: number of iterations; usually > 100
%
% OUTPUT
% -registered: registered source vertices on target mesh. Faces are not affected and remain the same is before the registration (Fs).

%EXAMPLE

% load EXAMPLE
% [registered]=nonrigidICP(target,source,Ft,Fs,200);

tic
clf
%initial allignment and scaling
[error1,source,transform]=rigidICP(target,source,0);

%plot of the meshes
h=trisurf(Fs,source(:,1),source(:,2),source(:,3),0.3,'Edgecolor','none');
hold
light
lighting phong;
set(gca, 'visible', 'off')
set(gcf,'Color',[1 1 0.88])
view(90,90)
set(gca,'DataAspectRatio',[1 1 1],'PlotBoxAspectRatio',[1 1 1]);
trisurf(Ft,target(:,1),target(:,2),target(:,3),0.75,'Edgecolor','none');
alpha(0.6)

for i =1:iterations
    % find index of max deviating point
    [distancemax,I,error,Reallignedsource]=ICPmanu_allign2(target,source);
     distancemax=distancemax
     % define measure for decreasing surface area to be transformed
     areafactor=0.5+24*i/iterations;

    for j=1:4
        areafactor=areafactor*j;
        r=areafactor/25;

        if r>1
            r=1;
        end
        %decrease sample size for large surfaces

        [nfs,nvs] = reducepatch(Fs,source,r);
        [nft,nvt] = reducepatch(Ft,target,r);
        %define index of max deviating point on reduced surface
        Inieuw=knnsearch(nvs,source(I,:));
        % define order of neighbouring vertices
        [distancemap]=surfacemap(nvs,nfs,Inieuw);
        d2=distancemap(:,3);
        %use distance to define specific surface area of interest
        d2=d2.^areafactor;
        [error,Reallignedsource,transform]=ICPmanu2weigthed(nvt,nvs,d2);
        [distancemap]=surfacemap(source,Fs,I);
        d2=distancemap(:,3);
        d2=d2.^areafactor;
        correction=1-d2;
        Reallignedsource=transform.b*source*transform.T+repmat(transform.c(1,1:3),length(source(:,1)),1);
        source=horzcat(source(:,1).*correction,source(:,2).*correction,source(:,3).*correction)+horzcat(Reallignedsource(:,1).*d2,Reallignedsource(:,2).*d2,Reallignedsource(:,3).*d2);
        [error,source,transform]=rigidICP(target,source,1);
            delete(h)
            h=trisurf(Fs,source(:,1),source(:,2),source(:,3),d2,'Edgecolor','none');
            pause (0.1)
    end
end
registered=source;
toc


function fr = comp_insdgfb(c,g,shift,Ls,dual)
%COMP_INSDGFB  Non-stationary Gabor filterbank synthesis
%   Usage: fr = comp_insdgfb(c,g,shift,Ls,dual)
%          fr = comp_insdgfb(c,g,shift,Ls)
%          fr = comp_insdgfb(c,g,shift)
%
%   Input parameters:
%         c         : Transform coefficients (matrix or cell array)
%         g         : Cell array of Fourier transforms of the analysis
%                     windows
%         shift     : Vector of frequency shifts
%         Ls        : Original signal length (in samples)
%         dual      : Synthesize with the dual frame
%   Output parameters:
%         fr        : Synthesized signal (Channels are stored in the
%                     columns)
%
%   Given the cell array *c* of non-stationary Gabor coefficients, and a
%   set of filters *g* and frequency shifts *shift* this function computes
%   the corresponding non-stationary Gabor filterbank synthesis.
%
%   If *dual* is set to 1 (default), an attempt is made to compute the
%   canonical dual frame for the system given by *g*, *shift* and the size
%   of the vectors in *c*. This provides perfect reconstruction in the
%   painless case, see the references for more information.
%
%   See also:  cqt, icqt, erblett, ierblett
%
%   References:  ltfatnote018 dogrhove12

% Author: Nicki Holighaus
% Date: 10.04.13

%% Check input arguments
if nargin < 5
    dual = 1;
    if nargin < 3
        error('Not enough input arguments');
    end
end

if iscell(c) == 0 % If matrix format coefficients were used, convert to
    % cell
    [M,N,CH] = size(c);
    c = reshape(c,N*M,CH);
    c = mat2cell(c,M*ones(N,1),CH);
else
    N = length(c);
    CH = size(c{1},2);
    M = cellfun(@(x) size(x,1),c);
end

timepos = cumsum(shift);        % Calculate positions from shift vector
NN = timepos(end);              % Reconstruction length before truncation
timepos = timepos-shift(1);     % Adjust positions

fr = zeros(NN,CH,assert_classname(c{1},g{1})); % Initialize output

if nargin < 4
    Ls = NN; % If original signal length is not given do not truncate
end

if dual == 1 % Attempt to compute canonical dual frame
    g = nsgabdual(g,shift,M,Ls);
end

%% The overlap-add procedure including multiplication with the synthesis
% windows

if numel(M) == 1
    M = M*ones(N,1);
end

for ii = 1:N
    Lg = length(g{ii});

    win_range = mod(timepos(ii)+(-floor(Lg/2):ceil(Lg/2)-1),NN)+1;

    temp = fft(c{ii})*M(ii);
    temp = temp(mod([end-floor(Lg/2)+1:end,1:ceil(Lg/2)]-1,M(ii))+1,:);

    fr(win_range,:) = fr(win_range,:) + ...
        bsxfun(@times,temp,g{ii}([Lg-floor(Lg/2)+1:Lg,1:ceil(Lg/2)]));
end

fr = ifft(fr);
fr = fr(1:Ls,:); % Truncate the signal to original length (if given)


function Offspring = Operator(Problem,Particle,Pbest,Gbest,Parameter)
% Particle swarm optimization in MPSO/D
% c1   ---   2 --- Parameter in updating particle's velocity
% c2   ---   2 --- Parameter in updating particle's velocity
% CR   --- 0.5 --- Parameter CR in differental evolution
% F    --- 0.5 --- Parameter F in differental evolution
% proM ---   1 --- The expectation of number of bits doing mutation
% disM ---  20 --- The distribution index of polynomial mutation

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

    %% Parameter setting
    if nargin > 4
        [c1,c2,CR,F,proM,disM] = deal(Parameter{:});
    else
        [c1,c2,CR,F,proM,disM] = deal(2,2,0.5,0.5,1,20);
    end
    ParticleDec = Particle.decs;
    PbestDec    = Pbest.decs;
    GbestDec    = Gbest.decs;
    [N,D]       = size(ParticleDec);
    ParticleVel = Particle.adds(zeros(N,D));

    %% Particle swarm optimization
    Lower = repmat(Problem.lower,N,1);
    Upper = repmat(Problem.upper,N,1);
    DoPSO = repmat(rand(N,1)<0.5,1,D);
    W     = 0.9 - Problem.FE./Problem.maxFE*0.8;
    r1    = repmat(rand(N,1),1,D);
    r2    = repmat(rand(N,1),1,D);
    OffVel        = ParticleVel;
    OffDec        = ParticleDec;
    OffVel(DoPSO) = W.*ParticleVel(DoPSO) + c1.*r1(DoPSO).*(PbestDec(DoPSO)-ParticleDec(DoPSO)) + c2.*r2(DoPSO).*(GbestDec(DoPSO)-ParticleDec(DoPSO));
    OffDec(DoPSO) = ParticleDec(DoPSO) + OffVel(DoPSO);
    % Set the infeasible decision variables to the value of their parents
    Invalid         = OffDec < Lower | OffDec > Upper;
    OffDec(Invalid) = ParticleDec(Invalid);

    %% DE
    Site = ~DoPSO & rand(N,D)<CR;
    OffDec(Site) = ParticleDec(Site) + F.*(GbestDec(Site)-PbestDec(Site));
    % Set the infeasible decision variables to boundary values
    OffDec = max(min(OffDec,Upper),Lower);

    %% Polynomial mutation
    Site  = rand(N,D) < proM/D;
    mu    = rand(N,D);
    temp  = Site & mu<=0.5;
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*((2.*mu(temp)+(1-2.*mu(temp)).*...
                   (1-(OffDec(temp)-Lower(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1))-1);
    temp = Site & mu>0.5;
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*(1-(2.*(1-mu(temp))+2.*(mu(temp)-0.5).*...
                   (1-(Upper(temp)-OffDec(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1)));
    Offspring = Problem.Evaluation(OffDec,OffVel);
end


function [Set_PSM, Set_TDS, OperatingTime] = ORCD (CT, OLF_Min, OLF_Max, LC, FC, C1, C2, TOP_Desired, IDMT_Saturation, Tolerance_Dn, Discrimination_Time, MinPSM, MaxPSM, StepPSM, MinTDS, MaxTDS, StepTDS)

[PSM_Max, PSM_Min] = PSM(CT, OLF_Max, OLF_Min, LC, MaxPSM, MinPSM, StepPSM);

TOP_Desired_For_Present_Relay=TOP_Desired+Discrimination_Time;
n=round((PSM_Max-PSM_Min)/StepPSM);
PSM_Temp=PSM_Min;
D=zeros(1,4);
    for i=1:n+1
        M=(FC/(PSM_Temp*CT));
        if (M>=IDMT_Saturation)
            M=IDMT_Saturation;
        end

        K=C1/((M^C2)-1);

        TDS_Desired=TOP_Desired_For_Present_Relay/K;

        TDS_Rounded_Up=ceil(TDS_Desired/StepTDS)*StepTDS;
        Difference_Up=abs(TDS_Rounded_Up-TDS_Desired);

        TDS_Rounded_Dn=floor(TDS_Desired/StepTDS)*StepTDS;
        Difference_Dn=abs(TDS_Rounded_Dn-TDS_Desired);

        Difference=min(Difference_Up,Difference_Dn);

        D(i,1)=PSM_Temp;
        D(i,2)=K;
        D(i,3)=TDS_Desired;
        D(i,4)=Difference;

        PSM_Temp=PSM_Temp+StepPSM;
        if (i~=n+1)
            D=[D;zeros(1,4)];
        end
    end
    D;
    [~,I]=min(D(:,4));
    Set_PSM=D(I,1);
    Set_TDS_Up=ceil(D(I,3)/StepTDS)*StepTDS;
    Set_TDS_Dn=floor(D(I,3)/StepTDS)*StepTDS;
    OperatingTime_Up=D(I,2)*Set_TDS_Up;
    OperatingTime_Dn=D(I,2)*Set_TDS_Dn;
    TOP_Error_Up=abs(TOP_Desired_For_Present_Relay-OperatingTime_Up);
    TOP_Error_Dn=abs(TOP_Desired_For_Present_Relay-OperatingTime_Dn);
    TOP_Error=min(TOP_Error_Up,TOP_Error_Dn);
    if (TOP_Error==TOP_Error_Dn && TOP_Error<=Tolerance_Dn)
        OperatingTime=OperatingTime_Dn;
        Set_TDS=Set_TDS_Dn;
    else
        OperatingTime=OperatingTime_Up;
        Set_TDS=Set_TDS_Up;
    end


%% Demo: DISTATIS
%
% The data used here is available from http://cosmomvpa.org/datadb.zip
%
% It is based on the following work:
% * Connolly et al (2012), Representation of biological classes in the human
%   brain. Journal of Neuroscience, doi 10.1523/JNEUROSCI.5547-11.2012
%
% Six categories (monkey, lemur, mallard, warbler, ladybug, lunamoth)
% during ten runs in an fMRI study.
%
% This example shows the application of DISTATIS, which tries to find an
% optimal 'compromise' dissimilarity matrix across a set of observations
% (participants)
%
% Reference:
%   - Abdi, H., Valentin, D., O?Toole, A. J., & Edelman, B. (2005).
%     DISTATIS: The analysis of multiple distance matrices. In
%     Proceedings of the IEEE Computer Society: International conference
%     on computer vision and pattern recognition, San Diego, CA, USA
%     (pp. 42?47).
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

%% Set data paths
% The function cosmo_config() returns a struct containing paths to tutorial
% data. (Alternatively the paths can be set manually without using
% cosmo_config.)
config=cosmo_config();
study_path=fullfile(config.tutorial_data_path,'ak6');
output_path=config.output_data_path;

readme_fn=fullfile(study_path,'README');
cosmo_type(readme_fn);

% reset citation list
cosmo_check_external('-tic');

%% Preprocessing for DISTATIS: RSM analysis
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subject_ids={'s01','s02','s03','s04','s05','s06','s07','s08'};
nsubjects=numel(subject_ids);

mask_label='vt_mask';

ds_rsms=cell(nsubjects,1); % allocate space for output
for subject_num=1:nsubjects
    subject_id=subject_ids{subject_num};

    % Code from here is pretty much identical to that above >>>

    % set path for this subject
    data_path=fullfile(study_path,subject_id);

    % Define data locations and load data from even and odd runs
    mask_fn=fullfile(data_path, [mask_label '.nii']); % vt mask

    % Use odd runs only
    data_fn=fullfile(data_path,'glm_T_stats_odd.nii');
    ds=cosmo_fmri_dataset(data_fn,'mask',mask_fn,...
                            'targets',1:6,'chunks',1);

    ds_rsm=cosmo_dissimilarity_matrix_measure(ds);

    % set chunks (one chunk per subject)
    ds_rsm.sa.chunks=subject_num*ones(size(ds_rsm.samples,1),1);
    ds_rsms{subject_num}=ds_rsm;
end

% combine data from all subjects
all_ds=cosmo_stack(ds_rsms);

%% Run DISTATIS
distatis=cosmo_distatis(all_ds);

%% show comprimise distance matrix
[compromise_matrix,dim_labels,values]=cosmo_unflatten(distatis,1);

labels={'monkey', 'lemur', 'mallard', 'warbler', 'ladybug', 'lunamoth'};
n_labels=numel(labels);
figure();
imagesc(compromise_matrix)
title('DSM');
set(gca,'YTick',1:n_labels,'YTickLabel',labels);
set(gca,'XTick',1:n_labels,'XTickLabel',labels);
ylabel(dim_labels{1});
xlabel(dim_labels{2});
colorbar

% skip if stats toolbox is not present
if cosmo_check_external('@stats',false)
    figure();
    hclus = linkage(compromise_matrix);
    dendrogram(hclus,'labels',labels,'orientation','left');
    title('dendrogram');

    figure();
    F = cmdscale(squareform(compromise_matrix));
    text(F(:,1), F(:,2), labels);
    title('2D MDS plot');
    mx = max(abs(F(:)));
    xlim([-mx mx]); ylim([-mx mx]);
end

%% show citation information
cosmo_check_external('-cite');


function rtk=udpos(rtk,tt)

global glc
VAR_POS=30^2; VAR_VEL=10^2; VAR_ACC=10^2; var=0;

% initialize position for first epoch
if norm(rtk.x(1:3))<=0
    for i=1:3,rtk=initx(rtk,rtk.sol.pos(i),VAR_POS,i);end
    if rtk.opt.dynamics==1
        for i=1:3,rtk=initx(rtk,rtk.sol.vel(i),VAR_VEL,i+3);end
        for i=1:3,rtk=initx(rtk,1e-6,VAR_ACC,i+6);end
    end
    return;
end

% static mode
if rtk.opt.mode==glc.PMODE_STATIC,return;end

% kinmatic mode without dynamics
if ~rtk.opt.dynamics
    for i=1:3,rtk=initx(rtk,rtk.sol.pos(i),VAR_POS,i);end
    return;
end

for i=1:3
    var=var+rtk.P(i,i);
end
var=var/3;

if var>VAR_POS
    for i=1:3,rtk=initx(rtk,rtk.sol.pos(i),VAR_POS,i);end
    for i=4:6,rtk=initx(rtk,rtk.sol.vel(i-3),VAR_VEL,i);end
    for i=7:9,rtk=initx(rtk,1e-6,VAR_ACC,i);end
    return;
end

nx=0;ix=zeros(rtk.nx,1);
for i=1:rtk.nx
    if rtk.x(i)~=0&&rtk.P(i,i)>0
        ix(nx+1)=i;
        nx=nx+1;
    end
end
ix(nx+1:end)=[];

if nx<9,return;end

% state transition of position/velocity/acceleration
F=eye(nx); x=zeros(nx,1); P=zeros(nx,nx);
for i=1:6,F(i,i+3)=tt;end
for i=1:3,F(i,i+6)=tt^2*0.5;end

for i=1:nx
    x(i)=rtk.x(ix(i));
    for j=1:nx
        P(i,j)=rtk.P(ix(i),ix(j));
    end
end

xp=F*x; P=F*P*F';

for i=1:nx
    rtk.x(ix(i))=xp(i);
    for j=1:nx
        rtk.P(ix(i),ix(j))=P(i,j);
    end
end

% process noise added to only acceleration
Q(1,1)=rtk.opt.prn(4)^2*abs(tt);
Q(2,2)=rtk.opt.prn(4)^2*abs(tt);
Q(3,3)=rtk.opt.prn(5)^2*abs(tt);
[~,Cne]=xyz2blh(x(1:3));
Qv=Cne'*Q*Cne;

for i=1:3
    for j=1:3
        rtk.P(i+6,j+6)=rtk.P(i+6,j+6)+Qv(i,j);
    end
end

return


function r = times(a,b)
%TIMES        Taylor multiplication  a .* b
%

% written  05/21/09     S.M. Rump
% modified 08/26/12     S.M. Rump  global variables removed
%

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

  K1 = getappdata(0,'INTLAB_TAYLOR_ORDER') + 1;

  if ~isa(a,'taylor')           % non-taylor times taylor
    r = b;
    if isa(a,'intval')
      r.t = intval(r.t);
    end
    m = prod(size(a));
    if m==1                     % non-taylor scalar .* taylor
      r.t = a*r.t;
    else                        % non-taylor array .* taylor
      n = prod(b.size);
      if n==1                   % non-taylor array .* taylor scalar
        r.size = size(a);
        r.t = repmat(a(:).',K1,1) .* repmat(b.t,1,m);
      else                      % non-taylor array .* taylor array
        if ~isequal(size(a),b.size)
          error('Taylor multiplication : dimensions not compatible')
        end
        r.t = repmat(a(:).',K1,1) .* b.t;
      end
    end
  elseif ~isa(b,'taylor')       % taylor times non-taylor
    r = a;
    if isa(b,'intval')
      r.t = intval(r.t);
    end
    m = prod(size(b));
    if m==1                     % taylor scalar .* non-taylor
      r.t = r.t*b;
    else                        % taylor array .* non-taylor
      n = prod(a.size);
      if n==1                   % taylor array .* non-taylor scalar
        r.t = repmat(a.t,1,m) .* repmat(b(:).',K1,1);
      else                      % non-taylor array .* taylor array
        if ~isequal(size(b),a.size)
          error('Taylor multiplication : dimensions not compatible')
        end
        r.t = a.t .* repmat(b(:).',K1,1);
      end
    end
  else                          % both factors taylor
    m = prod(a.size);
    n = prod(b.size);
    if m==1                     % taylor scalar .* taylor
      r = b;
      if isa(a,'intval')
        r.t = intval(r.t);
      end
      for j=1:K1
        r.t(j,:) = sum(repmat(a.t(1:j),1,n).*b.t(j:-1:1,:),1);
      end
    else                        % taylor array .* taylor
      r = a;
      if isa(b.t,'intval')
        r.t = intval(r.t);
      end
      if n==1                   % taylor array .* taylor scalar
        for j=1:K1
          r.t(j,:) = sum(a.t(1:j,:).*repmat(b.t(j:-1:1),1,m),1);
        end
      else                      % taylor array .* taylor array
        if ~isequal(a.size,b.size)
          error('Taylor multiplication : dimensions not compatible')
        end
        for j=1:K1
          r.t(j,:) = sum(a.t(1:j,:).*b.t(j:-1:1,:),1);
        end
      end
    end
  end

  if rndold
    setround(rndold)
  end


function qfsum = eiqf ( nt, t, mlt, wts, nwts, ndx, key, f )

%*****************************************************************************80
%
%% EIQF evaluates an interpolatory quadrature formula.
%
%  Discussion:
%
%   The knots, weights and integrand are supplied.
%
%   All knots with nonzero NDX are used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 January 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer NT, the number of knots.
%
%    Input, real T(NT), the knots.
%
%    Input, integer MLT(NT), the multiplicity of the knots.
%
%    Input, real WTS(NWTS), the weights.
%
%    Input, integer NWTS, the number of weights.
%
%    Input, integer NDX(NT), used to index the array WTS.
%    If KEY = 1, then NDX need not be preset.  For more details see the
%    comments in CAWIQ.
%
%    Input, integer KEY, indicates the structure of the WTS
%    array.  It will normally be set to 1.  This will cause the weights to be
%    packed sequentially in array WTS.  For more details see the comments
%    in CAWIQ.
%
%    Input, function F, the name of a routine which
%    evaluates the function and some of its derivatives.  The routine
%    must have the form
%      function value = f ( x, i )
%    and return in VALUE the value of the I-th derivative of the function
%    at X.  The highest value of I will be the maximum value in MLT minus
%    one.  The value X will always be a knot.
%
%    Output, real QFSUM, the value of the quadrature formula
%    applied to F.
%
  l = abs ( key );

  if ( l < 1 || 4 < l )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'EIQF - Fatal error!\n' );
    fprintf ( 1, '  Magnitude of KEY must be between 1 and 4.\n' );
    error ( 'EIQF - Fatal error!' );
  end

  qfsum = 0.0;
  for j = 1 : nt
    l = abs ( ndx(j) );
    if ( l ~= 0 )
      p = 1.0;
      for i = 1 : mlt(j)
        qfsum = qfsum + wts(l+i-1) * f ( t(j), i - 1 ) / p;
        if ( key <= 0 )
          p = p * i;
        end
      end
    end
  end

  return
end


function [ceq,ceqJac] = autoGen_cst_steplength(q1m,q2m,q4m,q5m,l1,l2,l4,l5,stepLength)
%AUTOGEN_CST_STEPLENGTH
%    [CEQ,CEQJAC] = AUTOGEN_CST_STEPLENGTH(Q1M,Q2M,Q4M,Q5M,L1,L2,L4,L5,STEPLENGTH)

%    This function was generated by the Symbolic Math Toolbox version 6.3.
%    25-Oct-2015 18:36:52

t2 = cos(q1m);
t3 = l1.*t2;
t4 = cos(q2m);
t5 = l2.*t4;
t6 = cos(q4m);
t7 = cos(q5m);
t8 = sin(q1m);
t9 = sin(q2m);
t10 = sin(q4m);
t11 = l4.*t10;
t12 = sin(q5m);
t13 = l5.*t12;
ceq = [-stepLength+t11+t13-l1.*t8-l2.*t9;t3+t5-l4.*t6-l5.*t7];
if nargout > 1
    ceqJac = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-t3,-l1.*t8,-t5,-l2.*t9,0.0,0.0,l4.*t6,t11,l5.*t7,t13,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2,32]);
end


function [t,f_new,g_new,funEvals,H] = WolfeLineSearch(...
    x,t,d,f,g,gtd,c1,c2,LS,maxLS,tolX,debug,doPlot,saveHessianComp,funObj,varargin)
%
% Bracketing Line Search to Satisfy Wolfe Conditions
%
% Inputs:
%   x: starting location
%   t: initial step size
%   d: descent direction
%   f: function value at starting location
%   g: gradient at starting location
%   gtd: directional derivative at starting location
%   c1: sufficient decrease parameter
%   c2: curvature parameter
%   debug: display debugging information
%   LS: type of interpolation
%   maxLS: maximum number of iterations
%   tolX: minimum allowable step length
%   doPlot: do a graphical display of interpolation
%   funObj: objective function
%   varargin: parameters of objective function
%
% Outputs:
%   t: step length
%   f_new: function value at x+t*d
%   g_new: gradient value at x+t*d
%   funEvals: number function evaluations performed by line search
%   H: Hessian at initial guess (only computed if requested

% Evaluate the Objective and Gradient at the Initial Step
if nargout == 5
    [f_new,g_new,H] = funObj(x + t*d,varargin{:});
else
    [f_new,g_new] = funObj(x+t*d,varargin{:});
end
funEvals = 1;
gtd_new = g_new'*d;

% Bracket an Interval containing a point satisfying the
% Wolfe criteria

LSiter = 0;
t_prev = 0;
f_prev = f;
g_prev = g;
gtd_prev = gtd;
done = 0;

while LSiter < maxLS

    %% Bracketing Phase
    if ~isLegal(f_new) || ~isLegal(g_new)
        if 0
            if debug
                fprintf('Extrapolated into illegal region, Bisecting\n');
            end
            t = (t + t_prev)/2;
            if ~saveHessianComp && nargout == 5
                [f_new,g_new,H] = funObj(x + t*d,varargin{:});
            else
                [f_new,g_new] = funObj(x + t*d,varargin{:});
            end
            funEvals = funEvals + 1;
            gtd_new = g_new'*d;
            LSiter = LSiter+1;
            continue;
        else
            if debug
                fprintf('Extrapolated into illegal region, switching to Armijo line-search\n');
            end
            t = (t + t_prev)/2;
            % Do Armijo
            if nargout == 5
                [t,x_new,f_new,g_new,armijoFunEvals,H] = ArmijoBacktrack(...
                  x,t,d,f,f,g,gtd,c1,max(0,min(LS-2,2)),tolX,debug,doPlot,saveHessianComp,...
                  funObj,varargin{:});
            else
                [t,x_new,f_new,g_new,armijoFunEvals] = ArmijoBacktrack(...
                  x,t,d,f,f,g,gtd,c1,max(0,min(LS-2,2)),tolX,debug,doPlot,saveHessianComp,...
                  funObj,varargin{:});
            end
            funEvals = funEvals + armijoFunEvals;
            return;
        end
    end

    if f_new > f + c1*t*gtd || (LSiter > 1 && f_new >= f_prev)
        bracket = [t_prev t];
        bracketFval = [f_prev f_new];
        bracketGval = [g_prev g_new];
        break;
    elseif abs(gtd_new) <= -c2*gtd
        bracket = t;
        bracketFval = f_new;
        bracketGval = g_new;
        done = 1;
        break;
    elseif gtd_new >= 0
        bracket = [t_prev t];
        bracketFval = [f_prev f_new];
        bracketGval = [g_prev g_new];
        break;
    end
    temp = t_prev;
    t_prev = t;
    minStep = t + 0.01*(t-temp);
    maxStep = t*10;
    if LS == 3
        if debug
            fprintf('Extending Braket\n');
        end
        t = maxStep;
    elseif LS ==4
        if debug
            fprintf('Cubic Extrapolation\n');
        end
        t = polyinterp([temp f_prev gtd_prev; t f_new gtd_new],doPlot,minStep,maxStep);
    else
        t = mixedExtrap(temp,f_prev,gtd_prev,t,f_new,gtd_new,minStep,maxStep,debug,doPlot);
    end

    f_prev = f_new;
    g_prev = g_new;
    gtd_prev = gtd_new;
    if ~saveHessianComp && nargout == 5
        [f_new,g_new,H] = funObj(x + t*d,varargin{:});
    else
        [f_new,g_new] = funObj(x + t*d,varargin{:});
    end
    funEvals = funEvals + 1;
    gtd_new = g_new'*d;
    LSiter = LSiter+1;
end

if LSiter == maxLS
    bracket = [0 t];
    bracketFval = [f f_new];
    bracketGval = [g g_new];
end

%% Zoom Phase

% We now either have a point satisfying the criteria, or a bracket
% surrounding a point satisfying the criteria
% Refine the bracket until we find a point satisfying the criteria
insufProgress = 0;
Tpos = 2;
LOposRemoved = 0;
while ~done && LSiter < maxLS

    % Find High and Low Points in bracket
    [f_LO LOpos] = min(bracketFval);
    HIpos = -LOpos + 3;

    % Compute new trial value
    if LS == 3 || ~isLegal(bracketFval) || ~isLegal(bracketGval)
        if debug
            fprintf('Bisecting\n');
        end
        t = mean(bracket);
    elseif LS == 4
        if debug
            fprintf('Grad-Cubic Interpolation\n');
        end
        t = polyinterp([bracket(1) bracketFval(1) bracketGval(:,1)'*d
            bracket(2) bracketFval(2) bracketGval(:,2)'*d],doPlot);
    else
        % Mixed Case %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        nonTpos = -Tpos+3;
        if LOposRemoved == 0
            oldLOval = bracket(nonTpos);
            oldLOFval = bracketFval(nonTpos);
            oldLOGval = bracketGval(:,nonTpos);
        end
        t = mixedInterp(bracket,bracketFval,bracketGval,d,Tpos,oldLOval,oldLOFval,oldLOGval,debug,doPlot);
    end

    % Test that we are making sufficient progress
    if min(max(bracket)-t,t-min(bracket))/(max(bracket)-min(bracket)) < 0.1
        if debug
            fprintf('Interpolation close to boundary');
        end
        if insufProgress || t>=max(bracket) || t <= min(bracket)
            if debug
                fprintf(', Evaluating at 0.1 away from boundary\n');
            end
            if abs(t-max(bracket)) < abs(t-min(bracket))
                t = max(bracket)-0.1*(max(bracket)-min(bracket));
            else
                t = min(bracket)+0.1*(max(bracket)-min(bracket));
            end
            insufProgress = 0;
        else
            if debug
                fprintf('\n');
            end
            insufProgress = 1;
        end
    else
        insufProgress = 0;
    end

    % Evaluate new point
    if ~saveHessianComp && nargout == 5
        [f_new,g_new,H] = funObj(x + t*d,varargin{:});
    else
        [f_new,g_new] = funObj(x + t*d,varargin{:});
    end
    funEvals = funEvals + 1;
    gtd_new = g_new'*d;
    LSiter = LSiter+1;

    if f_new > f + c1*t*gtd || f_new >= f_LO
        % Armijo condition not satisfied or not lower than lowest
        % point
        bracket(HIpos) = t;
        bracketFval(HIpos) = f_new;
        bracketGval(:,HIpos) = g_new;
        Tpos = HIpos;
    else
        if abs(gtd_new) <= - c2*gtd
            % Wolfe conditions satisfied
            done = 1;
        elseif gtd_new*(bracket(HIpos)-bracket(LOpos)) >= 0
            % Old HI becomes new LO
            bracket(HIpos) = bracket(LOpos);
            bracketFval(HIpos) = bracketFval(LOpos);
            bracketGval(:,HIpos) = bracketGval(:,LOpos);
            if LS == 5
                if debug
                    fprintf('LO Pos is being removed!\n');
                end
                LOposRemoved = 1;
                oldLOval = bracket(LOpos);
                oldLOFval = bracketFval(LOpos);
                oldLOGval = bracketGval(:,LOpos);
            end
        end
        % New point becomes new LO
        bracket(LOpos) = t;
        bracketFval(LOpos) = f_new;
        bracketGval(:,LOpos) = g_new;
        Tpos = LOpos;
    end

    if sum(abs(bracket(1)-bracket(2))*gtd_new) < tolX
        if debug
            fprintf('Line Search can not make further progress\n');
        end
        break;
    end

end

%%
if LSiter == maxLS
    if debug
        fprintf('Line Search Exceeded Maximum Line Search Iterations\n');
    end
end

[f_LO LOpos] = min(bracketFval);
t = bracket(LOpos);
f_new = bracketFval(LOpos);
g_new = bracketGval(:,LOpos);

% Evaluate Hessian at new point
if nargout == 5 && funEvals > 1 && saveHessianComp
    [f_new,g_new,H] = funObj(x + t*d,varargin{:});
    funEvals = funEvals + 1;
end

end

%%
function [t] = mixedExtrap(x0,f0,g0,x1,f1,g1,minStep,maxStep,debug,doPlot);
alpha_c = polyinterp([x0 f0 g0; x1 f1 g1],doPlot,minStep,maxStep);
alpha_s = polyinterp([x0 f0 g0; x1 sqrt(-1) g1],doPlot,minStep,maxStep);
if alpha_c > minStep && abs(alpha_c - x1) < abs(alpha_s - x1)
    if debug
        fprintf('Cubic Extrapolation\n');
    end
    t = alpha_c;
else
    if debug
        fprintf('Secant Extrapolation\n');
    end
    t = alpha_s;
end
end

%%
function [t] = mixedInterp(bracket,bracketFval,bracketGval,d,Tpos,oldLOval,oldLOFval,oldLOGval,debug,doPlot);

% Mixed Case %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nonTpos = -Tpos+3;

gtdT = bracketGval(:,Tpos)'*d;
gtdNonT = bracketGval(:,nonTpos)'*d;
oldLOgtd = oldLOGval'*d;
if bracketFval(Tpos) > oldLOFval
    alpha_c = polyinterp([oldLOval oldLOFval oldLOgtd
        bracket(Tpos) bracketFval(Tpos) gtdT],doPlot);
    alpha_q = polyinterp([oldLOval oldLOFval oldLOgtd
        bracket(Tpos) bracketFval(Tpos) sqrt(-1)],doPlot);
    if abs(alpha_c - oldLOval) < abs(alpha_q - oldLOval)
        if debug
            fprintf('Cubic Interpolation\n');
        end
        t = alpha_c;
    else
        if debug
            fprintf('Mixed Quad/Cubic Interpolation\n');
        end
        t = (alpha_q + alpha_c)/2;
    end
elseif gtdT'*oldLOgtd < 0
    alpha_c = polyinterp([oldLOval oldLOFval oldLOgtd
        bracket(Tpos) bracketFval(Tpos) gtdT],doPlot);
    alpha_s = polyinterp([oldLOval oldLOFval oldLOgtd
        bracket(Tpos) sqrt(-1) gtdT],doPlot);
    if abs(alpha_c - bracket(Tpos)) >= abs(alpha_s - bracket(Tpos))
        if debug
            fprintf('Cubic Interpolation\n');
        end
        t = alpha_c;
    else
        if debug
            fprintf('Quad Interpolation\n');
        end
        t = alpha_s;
    end
elseif abs(gtdT) <= abs(oldLOgtd)
    alpha_c = polyinterp([oldLOval oldLOFval oldLOgtd
        bracket(Tpos) bracketFval(Tpos) gtdT],...
        doPlot,min(bracket),max(bracket));
    alpha_s = polyinterp([oldLOval sqrt(-1) oldLOgtd
        bracket(Tpos) bracketFval(Tpos) gtdT],...
        doPlot,min(bracket),max(bracket));
    if alpha_c > min(bracket) && alpha_c < max(bracket)
        if abs(alpha_c - bracket(Tpos)) < abs(alpha_s - bracket(Tpos))
            if debug
                fprintf('Bounded Cubic Extrapolation\n');
            end
            t = alpha_c;
        else
            if debug
                fprintf('Bounded Secant Extrapolation\n');
            end
            t = alpha_s;
        end
    else
        if debug
            fprintf('Bounded Secant Extrapolation\n');
        end
        t = alpha_s;
    end

    if bracket(Tpos) > oldLOval
        t = min(bracket(Tpos) + 0.66*(bracket(nonTpos) - bracket(Tpos)),t);
    else
        t = max(bracket(Tpos) + 0.66*(bracket(nonTpos) - bracket(Tpos)),t);
    end
else
    t = polyinterp([bracket(nonTpos) bracketFval(nonTpos) gtdNonT
        bracket(Tpos) bracketFval(Tpos) gtdT],doPlot);
end
end

%%
function [legal] = isLegal(v)
legal = sum(any(imag(v(:))))==0 & sum(isnan(v(:)))==0 & sum(isinf(v(:)))==0;
end


function [Cyy] = global2localCov(Cxx, X)

% SYNTAX:
%   [Cyy] = global2localCov(Cxx, X);
%
% INPUT:
%   Cxx = input covariance matrices
%   X   = position vectors
%
% OUTPUT:
%   Cyy = output covariance matrices
%
% DESCRIPTION:
%   Covariance propagation from Earth-fixed reference frame to local-level reference frame

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

%initialize new covariance matrices
Cyy = zeros(size(Cxx));

for i = 1 : size(X,2)

    %geodetic coordinates
    [phi, lam] = cart2geod(X(1,i), X(2,i), X(3,i));

    %rotation matrix from global to local reference system
    R = [-sin(lam) cos(lam) 0;
         -sin(phi)*cos(lam) -sin(phi)*sin(lam) cos(phi);
         +cos(phi)*cos(lam) +cos(phi)*sin(lam) sin(phi)];

    %covariance propagation
    Cyy(:,:,i) = R * Cxx(:,:,i) * R';
end

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


%

function [f, df] = sampleEijOpt_6(x)
% optimization code for six wall case

% param
vert = x(6:11);
w_res = x(12);

x0 = x(1); y0 = x(2); % camera center
yc = x(3); % corner
xd = x(4);
yf = x(5);

% assume vertical lines are fixed
a_aob = (vert(2) - vert (1))/w_res*2*pi;
a_boc = (vert(3) - vert (2))/w_res*2*pi;
a_cod = (vert(4) - vert (3))/w_res*2*pi;
a_doe = (vert(5) - vert (4))/w_res*2*pi;
a_eof = (vert(6) - vert (5))/w_res*2*pi;
a_foa = (vert(1) + w_res - vert (6))/w_res*2*pi;

% energy
v_ao  = [x0 y0]; v_bo = [x0-1 y0]; v_co = [x0-1 y0 - yc];
v_do = [x0-xd y0-yc];  v_eo = [x0-xd y0-yf]; v_fo = [x0 y0-yf];

n_v_ao = norm(v_ao); n_v_bo = norm(v_bo); n_v_co = norm(v_co);
n_v_do = norm(v_do); n_v_eo = norm(v_eo); n_v_fo = norm(v_fo);

b_aob = acos(dot(v_ao, v_bo)/n_v_ao/n_v_bo);
if det([v_ao;v_bo]) < 0
    b_aob = 2*pi - b_aob;
end
b_boc = acos(dot(v_bo, v_co)/n_v_bo/n_v_co);
if det([v_bo;v_co]) < 0
    b_boc = 2*pi - b_boc;
end
b_cod = acos(dot(v_co, v_do)/n_v_co/n_v_do);
if det([v_co;v_do]) < 0
    b_cod = 2*pi - b_cod;
end
b_doe = acos(dot(v_do, v_eo)/n_v_do/n_v_eo);
if det([v_do;v_eo]) < 0
    b_doe = 2*pi - b_doe;
end
b_eof = acos(dot(v_eo, v_fo)/n_v_eo/n_v_fo);
if det([v_eo;v_fo]) < 0
    b_eof = 2*pi - b_eof;
end
b_foa = acos(dot(v_fo, v_ao)/n_v_fo/n_v_ao);
if det([v_fo;v_ao]) < 0
    b_foa = 2*pi - b_foa;
end

f = (b_aob - a_aob)^2 + (b_boc - a_boc)^2 + (b_cod - a_cod)^2 + ...
    (b_doe - a_doe)^2 + (b_eof - a_eof)^2 + (b_foa - a_foa)^2;

% gradient
% x0
d_aob_x0 = (2*x0-1)*n_v_ao*n_v_bo + dot(v_ao, v_bo) * (x0*n_v_bo/n_v_ao + (x0-1)*n_v_ao/n_v_bo);
d_aob_x0 = d_aob_x0 * (-1/(sqrt(1-cos(b_aob)*cos(b_aob))+eps))/n_v_ao/n_v_ao/n_v_bo/n_v_bo;
if det([v_ao;v_bo]) < 0
    d_aob_x0 = -d_aob_x0;
end
d_aob_x0 = 2*(b_aob - a_aob) * d_aob_x0;

d_boc_x0 = 2*(x0-1)*n_v_bo*n_v_co + dot(v_bo, v_co) * ((x0-1)*n_v_co/n_v_bo + (x0-1)*n_v_bo/n_v_co);
d_boc_x0 = d_boc_x0 * (-1/(sqrt(1-cos(b_boc)*cos(b_boc))+eps))/n_v_bo/n_v_bo/n_v_co/n_v_co;
if det([v_bo;v_co]) < 0
    d_boc_x0 = -d_boc_x0;
end
d_boc_x0 = 2*(b_boc - a_boc) * d_boc_x0;

d_cod_x0 = (2*x0-xd-1)*n_v_co*n_v_do + dot(v_co, v_do) * ((x0-1)*n_v_do/n_v_co + (x0-xd)*n_v_co/n_v_do);
d_cod_x0 = d_cod_x0 * (-1/(sqrt(1-cos(b_cod)*cos(b_cod))+eps))/n_v_co/n_v_co/n_v_do/n_v_do;
if det([v_co;v_do]) < 0
    d_cod_x0 = -d_cod_x0;
end
d_cod_x0 = 2*(b_cod - a_cod) * d_cod_x0;

d_doe_x0 = 2*(x0-xd)*n_v_do*n_v_eo + dot(v_do, v_eo) * ((x0-xd)*n_v_eo/n_v_do + (x0-xd)*n_v_do/n_v_eo);
d_doe_x0 = d_doe_x0 * (-1/(sqrt(1-cos(b_doe)*cos(b_doe))+eps))/n_v_do/n_v_do/n_v_eo/n_v_eo;
if det([v_do;v_eo]) < 0
    d_doe_x0 = -d_doe_x0;
end
d_doe_x0 = 2*(b_doe - a_doe) * d_doe_x0;

d_eof_x0 = (2*x0-xd)*n_v_eo*n_v_fo + dot(v_eo, v_fo) * ((x0-xd)*n_v_fo/n_v_eo + x0*n_v_eo/n_v_fo);
d_eof_x0 = d_eof_x0 * (-1/(sqrt(1-cos(b_eof)*cos(b_eof))+eps))/n_v_eo/n_v_eo/n_v_fo/n_v_fo;
if det([v_eo;v_fo]) < 0
    d_eof_x0 = -d_eof_x0;
end
d_eof_x0 = 2*(b_eof - a_eof) * d_eof_x0;

d_foa_x0 = 2*x0*n_v_fo*n_v_ao + dot(v_fo, v_ao) * (x0*n_v_ao/n_v_fo + x0*n_v_fo/n_v_ao);
d_foa_x0 = d_foa_x0 * (-1/(sqrt(1-cos(b_foa)*cos(b_foa))+eps))/n_v_fo/n_v_fo/n_v_ao/n_v_ao;
if det([v_fo;v_ao]) < 0
    d_foa_x0 = -d_foa_x0;
end
d_foa_x0 = 2*(b_foa - a_foa) * d_foa_x0;

% y0
d_aob_y0 = 2*y0*n_v_ao*n_v_bo + dot(v_ao, v_bo) * (y0*n_v_bo/n_v_ao + y0*n_v_ao/n_v_bo);
d_aob_y0 = d_aob_y0 * (-1/(sqrt(1-cos(b_aob)*cos(b_aob))+eps))/n_v_ao/n_v_ao/n_v_bo/n_v_bo;
if det([v_ao;v_bo]) < 0
    d_aob_y0 = -d_aob_y0;
end
d_aob_y0 = 2*(b_aob - a_aob) * d_aob_y0;

d_boc_y0 = (2*y0-yc)*n_v_bo*n_v_co + dot(v_bo, v_co) * (y0*n_v_co/n_v_bo + (y0-yc)*n_v_bo/n_v_co );
d_boc_y0 = d_boc_y0 * (-1/(sqrt(1-cos(b_boc)*cos(b_boc))+eps))/n_v_bo/n_v_bo/n_v_co/n_v_co;
if det([v_bo;v_co]) < 0
    d_boc_y0 = -d_boc_y0;
end
d_boc_y0 = 2*(b_boc - a_boc) * d_boc_y0;

d_cod_y0 = 2*(y0-yc)*n_v_co*n_v_do + dot(v_co, v_do) * ((y0-yc)/n_v_co*n_v_do + (y0-yc)/n_v_do*n_v_co);
d_cod_y0 = d_cod_y0 * (-1/(sqrt(1-cos(b_cod)*cos(b_cod))+eps))/n_v_co/n_v_co/n_v_do/n_v_do;
if det([v_co;v_do]) < 0
    d_cod_y0 = -d_cod_y0;
end
d_cod_y0 = 2*(b_cod - a_cod) * d_cod_y0;

d_doe_y0 = (2*y0-yf-yc)*n_v_do*n_v_eo + dot(v_do, v_eo)*((y0-yc)/n_v_do*n_v_eo + (y0-yf)/n_v_eo*n_v_do);
d_doe_y0 = d_doe_y0 * (-1/(sqrt(1-cos(b_doe)*cos(b_doe))+eps))/n_v_do/n_v_do/n_v_eo/n_v_eo;
if det([v_do;v_eo]) < 0
    d_doe_y0 = -d_doe_y0;
end
d_doe_y0 = 2*(b_doe - a_doe) * d_doe_y0;

d_eof_y0 = 2*(y0-yf)*n_v_eo*n_v_fo + dot(v_eo, v_fo)*((y0-yf)/n_v_eo*n_v_fo + (y0-yf)/n_v_fo*n_v_eo);
d_eof_y0 = d_eof_y0 * (-1/(sqrt(1-cos(b_eof)*cos(b_eof))+eps))/n_v_eo/n_v_eo/n_v_fo/n_v_fo;
if det([v_eo;v_fo]) < 0
    d_eof_y0 = -d_eof_y0;
end
d_eof_y0 = 2*(b_eof - a_eof) * d_eof_y0;

d_foa_y0 = (2*y0-yf)*n_v_fo*n_v_ao + dot(v_fo, v_ao)*((y0-yf)/n_v_fo*n_v_ao + y0/n_v_ao*n_v_fo);
d_foa_y0 = d_foa_y0 * (-1/(sqrt(1-cos(b_foa)*cos(b_foa))+eps))/n_v_fo/n_v_fo/n_v_ao/n_v_ao;
if det([v_fo;v_ao]) < 0
    d_foa_y0 = -d_foa_y0;
end
d_foa_y0 = 2*(b_foa - a_foa) * d_foa_y0;

% yc
d_boc_yc = (-y0)*n_v_bo*n_v_co + dot(v_bo, v_co) * n_v_bo* (yc-y0)/n_v_co;
d_boc_yc = d_boc_yc/n_v_bo/n_v_bo/n_v_co/n_v_co;
if det([v_bo;v_co]) < 0
    d_boc_yc = -d_boc_yc;
end
d_boc_yc = 2*(b_boc - a_boc) * d_boc_yc;

d_cod_yc = 2*(yc-y0)*n_v_co*n_v_do + dot(v_co, v_do)*((yc-y0)/n_v_co*n_v_do + (yc-y0)/n_v_do*n_v_co);
d_cod_yc = d_cod_yc * (-1/(sqrt(1-cos(b_cod)*cos(b_cod))+eps))/n_v_co/n_v_co/n_v_do/n_v_do;
if det([v_co;v_do]) < 0
    d_cod_yc = -d_cod_yc;
end
d_cod_yc = 2*(b_cod - a_cod) * d_cod_yc;

d_doe_yc = (yf-y0)*n_v_do*n_v_eo + dot(v_do, v_eo)*(yc-y0)/n_v_do*n_v_eo;
d_doe_yc = d_doe_yc * (-1/(sqrt(1-cos(b_doe)*cos(b_doe))+eps))/n_v_do/n_v_do/n_v_eo/n_v_eo;
if det([v_do;v_eo]) < 0
    d_doe_yc = -d_doe_yc;
end
d_doe_yc = 2*(b_doe - a_doe) * d_doe_yc;

% xd
d_cod_xd = (1-x0)*n_v_co*n_v_do + dot(v_co, v_do)*(xd-x0)*n_v_co/n_v_do;
d_cod_xd = d_cod_xd * (-1/(sqrt(1-cos(b_cod)*cos(b_cod))+eps))/n_v_co/n_v_co/n_v_do/n_v_do;
if det([v_co;v_do]) < 0
    d_cod_xd = -d_cod_xd;
end
d_cod_xd = 2*(b_cod - a_cod) * d_cod_xd;

d_doe_xd = 2*(xd-x0)*n_v_do*n_v_eo + dot(v_do, v_eo)*((xd-x0)/n_v_do*n_v_eo + (xd-x0)/n_v_eo*n_v_do);
d_doe_xd = d_doe_xd * (-1/(sqrt(1-cos(b_doe)*cos(b_doe))+eps))/n_v_do/n_v_do/n_v_eo/n_v_eo;
if det([v_do;v_eo]) < 0
    d_doe_xd = -d_doe_xd;
end
d_doe_xd = 2*(b_doe - a_doe) * d_doe_xd;

d_eof_xd = (-x0)*n_v_eo*n_v_fo + dot(v_eo, v_fo)*(xd-x0)/n_v_eo*n_v_fo;
d_eof_xd = d_eof_xd * (-1/(sqrt(1-cos(b_eof)*cos(b_eof))+eps))/n_v_eo/n_v_eo/n_v_fo/n_v_fo;
if det([v_eo;v_fo]) < 0
    d_eof_xd = -d_eof_xd;
end
d_eof_xd = 2*(b_eof - a_eof) * d_eof_xd;

% yf
d_doe_yf = (yc-y0)*n_v_do*n_v_eo + dot(v_do, v_eo)*(yf-y0)/n_v_eo*n_v_do;
d_doe_yf = d_doe_yf * (-1/(sqrt(1-cos(b_doe)*cos(b_doe))+eps))/n_v_do/n_v_do/n_v_eo/n_v_eo;
if det([v_do;v_eo]) < 0
    d_doe_yf = -d_doe_yf;
end
d_doe_yf = 2*(b_doe - a_doe) * d_doe_yf;

d_eof_yf = 2*(yf-y0)*n_v_eo*n_v_fo + dot(v_eo, v_fo)*((yf-y0)/n_v_eo*n_v_fo + (yf-y0)/n_v_fo*n_v_eo);
d_eof_yf = d_eof_yf * (-1/(sqrt(1-cos(b_eof)*cos(b_eof))+eps))/n_v_eo/n_v_eo/n_v_fo/n_v_fo;
if det([v_eo;v_fo]) < 0
    d_eof_yf = -d_eof_yf;
end
d_eof_yf = 2*(b_eof - a_eof) * d_eof_yf;

d_foa_yf = (-y0)*n_v_fo*n_v_ao + dot(v_fo, v_ao)*(yf-y0)/n_v_fo*n_v_ao;
d_foa_yf = d_foa_yf * (-1/(sqrt(1-cos(b_foa)*cos(b_foa))+eps))/n_v_fo/n_v_fo/n_v_ao/n_v_ao;
if det([v_fo;v_ao]) < 0
    d_foa_yf = -d_foa_yf;
end
d_foa_yf = 2*(b_foa - a_foa) * d_foa_yf;

d_x0 = d_aob_x0 + d_boc_x0 + d_cod_x0 + d_doe_x0 + d_eof_x0 + d_foa_x0;
d_y0 = d_aob_y0 + d_boc_y0 + d_cod_y0 + d_doe_y0 + d_eof_y0 + d_foa_y0;
d_yc = d_boc_yc + d_cod_yc + d_doe_yc;
d_xd = d_cod_xd + d_doe_xd + d_eof_xd;
d_yf = d_doe_yf + d_eof_yf + d_foa_yf;

df = [d_x0; d_y0; d_yc; d_xd; d_yf; 0; 0; 0; 0; 0;0;0];


%   TTeMPS Toolbox.
%   Michael Steinlechner, 2013-2016
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt
function [X, residuum, cost, times] = alsLinsolve_rankOne( L, F, X, opts )

t_start = tic();
% set default opts
if ~exist( 'opts', 'var');       opts = struct();       end
if ~isfield( opts, 'nSweeps');   opts.nSweeps = 4;      end
if ~isfield( opts, 'solver');    opts.solver = 'pcg';   end

d = X.order;
n = X.size;

normF = norm(F);
g = apply(L, X) - F;
cost = cost_function_res( X, g );
residuum = norm( g ) / normF;
times = toc(t_start);

X = orthogonalize(X, 1);
for sweep = 1:opts.nSweeps
    % ====================================================================
    % LEFT-TO-RIGHT SWEEP
    % ====================================================================
    disp( ['STARTING SWEEP ', num2str(sweep), ' from left to right'] )
    disp( '===========================================================')
    for idx = 1:d-1
        disp( ['Current core: ', num2str(idx)] )

        Fi = contract( X, F, idx );
        sz = [X.rank(idx), X.size(idx), X.rank(idx+1)];

        if strcmpi( opts.solver, 'direct' )
            % if system very small
            Li = contract( X, apply(L,X), idx );
            Ui = Li \ Fi(:);
            X.U{idx} = reshape( Ui, sz );

        elseif strcmpi( opts.solver, 'pcg' )

            [left, right] = Afun_prepare( L, X, idx );
            B1 =  prepare_precond( L.A{1}, X, idx );

            Ui = pcg( @(y) Afun( L, y, idx, sz, left, right), ...
                     Fi(:), ...
                     1e-10, 1000, ...
                     @(y) apply_precond( L.A{1}, B1, y, sz ), [],...
                     X.U{idx}(:) );

            X.U{idx} = reshape( Ui, sz );

        elseif strcmpi( opts.solver, 'diag' )
            X.U{idx} = solve_inner( L.L0, X, Fi, idx );

        else
            error( 'Unknown opts.solver type. Use either ''direct'', ''pcg'' (default) or ''diag''.' )
        end

        X = orth_at( X, idx, 'left', true );

        g = apply(L, X) - F;
        residuum = [residuum; norm( g ) / normF];
        cost = [cost; cost_function_res( X, g )];
        times = [times; toc(t_start)];
    end

    % ====================================================================
    % RIGHT-TO-LEFT
    % ====================================================================
    disp( 'Starting right-to-left half-sweep:')
    for idx = d:-1:2
        disp( ['Current core: ', num2str(idx)] )

        Fi = contract( X, F, idx );
        sz = [X.rank(idx), X.size(idx), X.rank(idx+1)];

        if strcmpi( opts.solver, 'direct' )
            % if system very small
            Li = contract( X, apply(L, X), idx );
            Ui = Li \ Fi(:);
            X.U{idx} = reshape( Ui, sz );

        elseif strcmpi( opts.solver, 'pcg' )

            [left, right] = Afun_prepare( L, X, idx );
            B1 =  prepare_precond( L.A{1}, X, idx );

            Ui = pcg( @(y) Afun( L, y, idx, sz, left, right), ...
                     Fi(:), ...
                     1e-10, 1000, ...
                     @(y) apply_precond( L.A{1}, B1, y, sz ), [],...
                     X.U{idx}(:) );

            X.U{idx} = reshape( Ui, sz );

        elseif strcmpi( opts.solver, 'diag' )
            X.U{idx} = solve_inner( L.L0, X, Fi, idx );

        else
            error( 'Unknown opts.solver type. Use either ''direct'', ''pcg'' (default) or ''diag''.' )
        end

        X = orth_at( X, idx, 'right', true );

        g = apply(L, X) - F;
        residuum = [residuum; norm( g ) / normF];
        cost = [cost; cost_function_res( X, g )];
        times = [times; toc(t_start)];
    end

end

end

function res = cost_function( L, X, F )
res = 0.5*innerprod( X, apply(L, X) ) - innerprod( X, F );
end

function res = cost_function_res( X, res )
res = 0.5*innerprod( X, res );
end

function [left, right] = Afun_prepare( A, x, idx )
    y = A.apply(x);
    if idx == 1
        right = innerprod( x, y, 'RL', idx+1 );
        left = [];
    elseif idx == x.order
        left = innerprod( x, y, 'LR', idx-1 );
        right = [];
    else
        left = innerprod( x, y, 'LR', idx-1 );
        right = innerprod( x, y, 'RL', idx+1 );
    end
end

function res = Afun( A, U, idx, sz, left, right )

    V = reshape( U, sz );
    V = A.apply( V, idx );

    if idx == 1
        tmp = tensorprod_ttemps( V, right, 3 );
    elseif idx == A.order
        tmp = tensorprod_ttemps( V, left, 1 );
    else
        tmp = tensorprod_ttemps( V, right, 3);
        tmp = tensorprod_ttemps( tmp, left, 1);
    end

    res = tmp(:);
end

function B1 = prepare_precond( L0, X, idx )

    if idx == 1
        B1 = [];
        return
    end

    n = size(L0, 1);
    r = X.rank;

    X1 = matricize( X.U{1}, 2);
    Y = X;
    Y.U{1} = tensorize( L0*X1, 2, [r(1), n(1), r(2)] );
    B1 = innerprod( X, Y, 'LR', idx-1);
end

function res = apply_precond( L0, B1, rhs, sz )

    n = size(L0, 1);
    rhs = reshape( rhs, sz );
    if isempty(B1) %idx == 1
        res = L0 \ unfold( rhs, 'left' );
        res = reshape( res, sz );
    else
        res = B1 \ unfold(rhs, 'right');
        res = reshape( res, sz );
    end
    res = res(:);
end


function [vec,rotMat]=TEME2GCRS(x,Jul1,Jul2,deltaTTUT1,xpyp,dXdY,LOD)
%%TEME2GCRS Convert from the True Equator Mean Equinox (TEME) of date
%           coordinate system to the Geocentric Celestrial Reference System
%           (GCRS), a type of Earth-Centered Inertial (ECI) coordinate
%           system. The TEME system is non-standard and is generally
%           only used in the Specialized General Perturbations 4 (SGP4)
%           orbit propagation algorithm.
%
%INPUTS: x The NXnumVec collection of vectors in TEME coordinates to
%          convert. N can be 3, or 6. If the vectors are 3D, then they are
%          position. 6D vectors are assumed to be position and velocity,
%          whereby the angular velocity of the Earth's rotation is taken
%          into account using a non-relativistic formula.
% Jul1, Jul2 Two parts of a Julian date given in terrestrial time (TT).
%          The units of the date are days. The full date is the sum of both
%          terms. The date is broken into two parts to provide more bits of
%          precision. It does not matter how the date is split.
% deltaTTUT1 An optional parameter specifying the difference between TT and
%          UT1 in seconds. This information can be obtained from
%          http://www.iers.org/nn_11474/IERS/EN/DataProducts/EarthOrientationData/eop.html?__nnn=true
%          or
%          http://www.usno.navy.mil/USNO/earth-orientation/eo-products
%          If this parameter is omitted or if an empty matrix is passed,
%          then the value provided by the function getEOP will be used
%          instead.
%     xpyp xpyp=[xp;yp] are the polar motion coordinates in radians
%          including the effects of tides and librations. If this parameter
%          is omitted or if an empty matrix is passed, the value from the
%          function getEOP will be used.
%     dXdY dXdY=[dX;dY] are the celestial pole offsets with respect to the
%          IAU 2006/2000A precession/nutation model in radians If this
%          parameter is omitted or if an empty matrix is passed, the value
%          from the function getEOP will be used.
%      LOD The difference between the length of the day using terrestrial
%          time, international atomic time, or UTC without leap seconds and
%          the length of the day in UT1. This is an instantaneous parameter
%          (in seconds) proportional to the rotation rate of the Earth.
%          This is only needed if more than just position components are
%          being converted.
%
%OUTPUTS: vec A 3XN or 6XN matrix of vectors converted from TEME
%             coordinates to GCRS coordinates.
%      rotMat The 3X3 rotation matrix used for the conversion of the
%             positions. That is, vec(1:3)=rotMat*x(1:3)
%
%This function just calls the functions TEME2ITRS and ITRS2GCRS, reusing
%the same Earth orientation parameters across calls.
%
%Different celestial coordinate systems are compared in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report, Naval
%    Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016, 173
%    pages.
%
%January 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%If more EOPs are needed.
if(nargin<7&&size(x,1)==6||nargin<6||isempty(dXdY)||isempty(xpyp)||isempty(deltaTTUT1))
    [JulUTC1,JulUTC2]=TT2UTC(Jul1,Jul2);
    [xpypget,dXdYget,~,deltaTTUT1get,LODget]=getEOP(JulUTC1,JulUTC2);
end

if(nargin<7||isempty(LOD))
   LOD=LODget;
end

if(nargin<6||isempty(dXdY))
    dXdY=dXdYget;
end

if(nargin<5||isempty(xpyp))
    xpyp=xpypget;
end

if(nargin<4||isempty(deltaTTUT1))
    deltaTTUT1=deltaTTUT1get;
end

[x,rotMat1]=TEME2ITRS(x,Jul1,Jul2,deltaTTUT1,xpyp,LOD);
[vec,rotMat2]=ITRS2GCRS(x,Jul1,Jul2,deltaTTUT1,xpyp,dXdY,LOD);

%The combined rotation matrix.
rotMat=rotMat2*rotMat1;
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 [result]=sv_NodeCalcSchuster(params,mCatalog)
% function [result]=sv_NodeCalcSchuster(params,mCatalog)
% ----------------------------------------------------
% Function to calculate Schuster's test and fast Mc (not EMR method)
% estimates; calculates also uncertainties for Schuster's test
%
% Incoming variables:
% mCatalog     : current earthquake catalog
% params       : See sv_calcMc for parameters
%
% Outgoing variable:
% result.fMc_max    : magnitude of completeness using maximum cuvature approach
% result.fMc_90     : magnitude of completeness using GR-law fit at 90% goodness fit level
% result.fMc_95     : magnitude of completeness using GR-law fit at 95% goodness fit level
% result.fMc_com    : magnitude of completeness using GR-law fit at 90% goodness fit level,
%                     GR-law fit at 95% goodness fit level, and maximum cuvature approach
% result.fMcSch      : magnitude of completeness using Schusters method
% result.flogProbSch : log10(Probability) to obtain vector of length >= R from a random walkout
% result.v1Sigma     : 1Sigma (16 and 84 percentiles) for probability to obtain vector of length >= R
%                      from a random walkout from bootstrapping the catalog times
% result.flogv1Sigma : 1Sigma (16 and 84 percentiles) for log10(probability) to obtain vector of length >= R
%                      from a random walkout from bootstrapping the catalog times
% result.v1Srange    : 1Sigma range (84-16 percentile)
% result.flogv1Srange: 1Sigma log10 range (84-16 percentile)
%
% Author: J. Woessner
% woessner@seismo.ifg.ethz.ch
% last update: 17.04.03

% Init variable
result=[];

% Determine Mc by maximum curvature
nCalculateMC = 1;
result.fMc_max = calc_Mc(mCatalog, nCalculateMC);
if isempty(result.fMc_max)
    result.fMc_max = NaN;
end

% Determine Mc by goodness of fit 90%
nCalculateMC = 3;
result.fMc_90 = calc_Mc(mCatalog, nCalculateMC);
if isempty(result.fMc_90)
    result.fMc_90 = NaN;
end

% Determine Mc by goodness of fit 95%
nCalculateMC = 4;
result.fMc_95 = calc_Mc(mCatalog, nCalculateMC);
if isempty(result.fMc_95)
    result.fMc_95 = NaN;
end

% Determine Mc by best combination
nCalculateMC = 5;
result.fMc_com = calc_Mc(mCatalog, nCalculateMC);
if isempty(result.fMc_com)
    result.fMc_com = NaN;
end

% % Mc determination by modelling entire magnitude range using a NORMAL CDF
% [mResult result.fProbMcNorm result.fMcNorm fMu fSigma mDatPredBest vPredBest result.fBvalue] = calc_McCdfnormal(mCatalog, params.fBinning);
%
% if (isempty(result.fProbMcNorm) | isempty(result.fMcNorm))
%     result.fProbMcNorm = NaN;
%     result.fMcNorm = NaN;
% end
% if (isempty(result.fBvalue))
%     result.fBvalue = NaN;
% end

% Mc determination by Schuster
[mResult fMcSch,  fProbability] =  calc_SchusterMc(mCatalog,0.3);
[mWalkout fR95 fProbSch PHI,  R] = calc_Schusterwalk(mCatalog);
try
    result.fMcSch = fMcSch;
    result.flogProbSch = log10(fProbSch);
catch
    result.fMcSch = NaN;
    result.flogProbSch = NaN;
end

% Determine uncertainty of Schuster test
[vPerc v1Sigma,  fStdProb] = calc_BstSchuster(mCatalog,params.fBinning,params.fBstnum);
try
    result.v1Sigma = v1Sigma;
    result.flogv1Sigma = log10(v1Sigma);
    result.v1Srange = (v1Sigma(1,2) - v1Sigma(1,1));
    result.flogv1Srange = log10(result.v1Srange);
catch
    result.fv1Sigma= NaN;
    result.flogv1Sigma = [NaN NaN];
    result.v1Srange = NaN;
    result.flogv1Srange = NaN;
end