function [Pj closest] = P_r2j(Pr, Pj, closest)
%P_R2J  Change reference frame of projection matrices
%
%   [Pk I] = P_r2j(Pr, Pj, closest)
%
% Tranforms the coordinate frame of a set of projection matrices such that
% a reference projection matrix becomes the identity matrix eye(3, 4). Can
% also select input matrices based on camera centre proximity to the
% reference frame.
%
%IN:
%   Pr - 3x4 projection matrix of the reference frame.
%   Pj - 3x4xN array of input projection matrices.
%   closest - 1xP indices of matrices of Pj to output, once arranged in
%             order of proximity of camera centre to Pr. Default: 1:N.
%
%OUT:
%   Pk - 3x4x(min(N,P)) array of transformed projection matrices.
%   I - 1x(min(N,P)) indices of matrices of Pj that make up Pk, such
%       that Pk = T(Pj(:,:,I)), T() being the coordinate transformation.

% $Id: P_r2j.m,v 1.2 2007/12/10 10:58:31 ojw Exp $

if nargin > 2
    % Determine which Pj matrices are required based on distance of optical
    % centres from that of Pr.
    T = zeros(3, size(Pj, 3));
    for a = 1:size(Pj, 3)
        T(:,a) = -Pj(:,1:3,a) \ Pj(:,4,a);
    end
    order = ojw_bsxfun(@minus, T, -Pr(:,1:3) \ Pr(:,4));
    [T order] = sort(sum(order .^ 2));
    closest = order(closest);
    Pj = Pj(:,:,closest);
end

% Calculate Pj relative to Pr
% Assume projection matrices make 0,0 the top left corner of the top left
% pixel. Convert to Matlab form, i.e. 0.5,0.5 is the top left corner of the
% top left pixel.
T = [1 0 0.5; 0 1 0.5; 0 0 1];
Pr = inv([T * Pr; 0 0 0 1]);
for a = 1:size(Pj, 3)
    Pj(:,:,a) = T * Pj(:,:,a) * Pr;
end
return


function [xdot] = nldetect_anex1_v1_eom(t,x); % ,junk
% Note:  Include ',junk' before eom if this is to be called as an m-file,
%   i.e. [T,Y] = ode45('nldetec_anex1_v1_eom',...)
% Exlude ',junk' if this is to be called as:
%   sol = ode45(@nldetect_anex1_v1_eom, ...)

% State Space representation:
% x = [q; qdot], xdot = [qdot; qdoubledot]
global eom % more efficient to treat as global than to pass in every time.

% Define nonlinear force:
if strcmp(eom.NLType,'bang');
    % Bang (Contact Nonlinearity)
    % Assumes that stiffness of contacted parts is k4mult times higher.
    % Damping is also assumed higher by a factor of c4mult
    del4 = (x(eom.at_ns(1))-x(eom.at_ns(2)));
    if del4 < eom.delcont % No Contact Occurs
        Fnl = eom.kat*del4 ...
            + eom.cfactk*eom.kat*(x(eom.at_ns(1)+eom.Ntot)-x(eom.at_ns(2)+eom.Ntot)); % Add damping terms
    else                % Contact Occurs
        Fnl = eom.kat*eom.k4mult*del4 ...
            + eom.c4mult*eom.cfactk*eom.kat*(x(eom.at_ns(1)+eom.Ntot)-x(eom.at_ns(2)+eom.Ntot)); % Add damping terms
    end
elseif strcmp(eom.NLType,'cubic');
    % Cubic Spring
    del4 = (x(eom.at_ns(1))-x(eom.at_ns(2)));
    Fnl = eom.kat*del4*(1 + eom.katnl*del4^2) ...
        + eom.cfactk*eom.kat*(x(eom.at_ns(1)+eom.Ntot)-x(eom.at_ns(2)+eom.Ntot)); % Add damping terms
else
    error('NLType not recognized');
end

% Define input force - normalized to have unit area (energy) * Afnl
% This is applied to mass 5 below
if t <= eom.tfp
    fext = eom.Afnl*(2*eom.tfp/pi)*sin((pi/eom.tfp)*t);
else
    fext = 0;
end

% Equations of Motion
xdot(eom.dnodes,1) = x(eom.vnodes);
xdot(eom.vnodes,1) = (-eom.Ctot*x(eom.vnodes) - eom.Ktot*x(eom.dnodes) + fext*eom.fext_vec + ...
    Fnl*eom.fnl_vec)./diag(eom.Mtot);


 function [cost, fit, reg] = pwls_cost(xs, A, W, yi, R, mask)
%function [cost, fit, reg] = pwls_cost(xs, A, W, yi, R, mask)
%|
%| compute PWLS cost for each column of xs
%|
%| in
%|	xs	[np niter]	iterates
%|	A	[nd np]		system matrix
%|	W	[nd nd]		data weighting matrix, usually diag_sp(wi)
%|	yi	[nd 1]		data
%|	R			penalty object (see Reg1.m or Robject.m)
%|				with penalty cost method: R.penal(R, x)
%|				or just *sparse* C matrix (quadratic only)
%|	mask	[nx ny ...]	optional mask, iff xs is [nx ny ... niter]
%|
%| out
%|	cost	[niter 1]	cost
%|	fit	[niter 1]	(y-A*x)'W(y-Ax)/2
%|	reg	[niter 1]	R(x)
%|
%| Copyright 2002-2-12, Jeff Fessler, University of Michigan

if nargin < 4, ir_usage, end

if ~isvar('R'), R = []; end

if isvar('mask') && ~isempty(mask)
	xs = reshapee(xs, numel(mask), []);	% [(*N) niter]
	xs = xs(mask(:), :);			% [np niter]
end

niter = size(xs,2);
reg = zeros(niter,1);

if isempty(R)
	warning 'empty R means no penalty'

elseif issparse(R) || isa(R, 'Fatrix')
	C = R; % trick!
	if size(C,2) == size(C,1)
		warning 'square C is quite unusual!?'
	end
	for ii=1:niter
		reg(ii) = sum(abs(C * xs(:,ii)).^2)/2;
	end

elseif isstruct(R) || isa(R, 'strum')
	for ii=1:niter
		reg(ii) = R.penal(R, xs(:,ii));
	end

else
	keyboard
	error 'bad R'
end

fit = zeros(niter,1);
for ii=1:niter
	resid = yi - A * xs(:,ii); % predicted measurements
	fit(ii) = resid' * (W * resid) / 2;
end

cost = fit + reg;
cost = reale(cost, 'warn'); % trick: x'*W*x is not always real for complex values

if ~nargout
	pr fit
	pr reg
	pr cost
end


%SerialLink.coriolis Coriolis matrix
%
% C = R.coriolis(Q, QD) is the Coriolis/centripetal matrix (NxN) for
% the robot in configuration Q and velocity QD, where N is the number of
% joints.  The product C*QD is the vector of joint force/torque due to velocity
% coupling.  The diagonal elements are due to centripetal effects and the
% off-diagonal elements are due to Coriolis effects.  This matrix is also
% known as the velocity coupling matrix, since it describes the disturbance forces
% on any joint due to velocity of all other joints.
%
% If Q and QD are matrices (KxN), each row is interpretted as a joint state
% vector, and the result (NxNxK) is a 3d-matrix where each plane corresponds
% to a row of Q and QD.
%
% C = R.coriolis( QQD) as above but the matrix QQD (1x2N) is [Q QD].
%
% Notes::
% - Joint viscous friction is also a joint force proportional to velocity but it is
%   eliminated in the computation of this value.
% - Computationally slow, involves N^2/2 invocations of RNE.
%
% See also SerialLink.rne.

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function C = coriolis(robot, q, qd)

    n = robot.n;

    if nargin == 2
        % coriolis( [q qd] )
        if numcols(q) ~= 2*n
            error('RTB:coriolis:badarg', 'arg must have %d columns', 2*n);
        end
        qd = q(:,n+1:end);
        q = q(:,1:n);
    else
        if numcols(q) ~= n
            error('RTB:coriolis:badarg', 'Cq must have %d columns', n);
        end
        if numcols(qd) ~= n
            error('RTB:coriolis:badarg', 'qd must have %d columns', n);
        end
    end

    % we need to create a clone robot with no friciton, since friction
    % is also proportional to joint velocity
    robot2 = robot.nofriction('all');

    if numrows(q) > 1
        if numrows(q) ~= numrows(qd)
            error('RTB:coriolis:badarg', 'for trajectory q and qd must have same number of rows');
        end
        C = [];
        for i=1:numrows(q)
            C = cat(3, C, robot2.coriolis(q(i,:), qd(i,:)));
        end
        return
    end

    N = robot2.n;

    if isa(q, 'sym')
        C(N,N) = sym();
        Csq(N,N) = sym();
    else

        C = zeros(N,N);
        Csq = zeros(N,N);
    end

    % find the torques that depend on a single finite joint speed,
    % these are due to the squared (centripetal) terms
    %
    %  set QD = [1 0 0 ...] then resulting torque is due to qd_1^2
    for j=1:N
        QD = zeros(1,N);
        QD(j) = 1;
        tau = robot2.rne(q, QD, zeros(size(q)), [0 0 0]');
        Csq(:,j) = Csq(:,j) + tau.';
    end

    % find the torques that depend on a pair of finite joint speeds,
    % these are due to the product (Coridolis) terms
    %  set QD = [1 1 0 ...] then resulting torque is due to
    %    qd_1 qd_2 + qd_1^2 + qd_2^2
    for j=1:N
        for k=j+1:N
            % find a product term  qd_j * qd_k
            QD = zeros(1,N);
            QD(j) = 1;
            QD(k) = 1;
            tau = robot2.rne(q, QD, zeros(size(q)), [0 0 0]');
            C(:,k) = C(:,k) + (tau.' - Csq(:,k) - Csq(:,j)) * qd(j)/2;
            C(:,j) = C(:,j) + (tau.' - Csq(:,k) - Csq(:,j)) * qd(k)/2;

        end
    end

    C = C + Csq * diag(qd);

    if isa(q, 'sym')
        C = simplify(C);
    end


function f = constructorTurbo(f, op, pref)
%   F = CONSTRUCTORTURBO(F, OP, PREF) takes the CHEBTECH F that was constructed
%   from the function handle OP using the preferences in PREF and tries to use
%   contour integrals to compute some additional higher-order coefficients to
%   high accuracy.  These coefficients are returned in the appropriate
%   positions in F.COEFFS.
%
%   This function is only meant to be called by the CHEBTECH constructor.
%
% References:
%
%   [1] Bornemann, F.  Accuracy and stability of computing high-order
%         derivatives of analytic functions by Cauchy integrals.  Found.
%         Comput. Math. 11 (2011), pp. 1-63.
%
%   [2] Wang, H. and Huybrechs, D.  Fast and accurate computation of Jacobi
%         expansion coefficients of analytic functions.  Technical Report
%         TW-645, K.U. Leuven, 2015.

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

% How many coefficients do we want to compute?
if ( ~isnan(pref.fixedLength) )
    n = pref.fixedLength;
else
    n = 2*length(f);
end

% Pick an ellipse over which to integrate.
rhoCheb = exp(abs(log(eps())) / length(f));
rho = rhoCheb^(2/3);

% Do the contour integrals.
c = chebCoeffsTurbo(op, rho, n);

% Assign the new coefficients.
if ( isreal(f) )
    f.coeffs = real(c);
elseif ( isreal(1i*f) )
    f.coeffs = imag(c);
else
    f.coeffs = c;
end

end

function c = chebCoeffsTurbo(f, rho, n)
%CHEBCOEFFSTURBO   Compute Chebyshev coefficients using contour integrals.
%   C = CHEBCOEFFSTURBO(F, RHO, N) computes the first N Chebyshev coefficients
%   of the holomorphic function represented by the function handle F using
%   Cauchy integrals over the Bernstein ellipse of parameter RHO.  F must be
%   vectorized and able to accept complex inputs.

K = 4*n;                                    % Number of quadrature nodes.
g = @(z) f((rho*z + 1./(rho*z))/2);         % Remap ellipse to unit circle.
z = exp(2*pi*1i*(0:1:(K - 1)).'/K);         % Roots of unity.

% Compute integrals with trap. rule and rescale to get coefficients.
c = bsxfun(@rdivide, fft(g(z))/K, rho.^(0:1:(K - 1)).');
c = [c(1,:) ; 2*c(2:n,:)];

end


function [ x, w ] = rule01 ( n )

%*****************************************************************************80
%
%% RULE01 returns the rule of degree 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer N, the number of nodes.
%
%    Output, real X(2,N), the coordinates of the nodes.
%
%    Output, real W(N), the weights.
%
  xs = [ ...
       0.00000000000000000 ];
  ys = [ ...
       0.00000000000000000 ];
  ws = [ ...
       0.2828427124746189E+01 ];

  x(1,1:n) = xs(1:n);
  x(2,1:n) = ys(1:n);
  w(1:n) = ws(1:n);

  return
end


function [sos,P] = sospolymatrixvar(sos,ZSym,n,matrixstr)
% SOSMATRIXVAR --- Declare a polynomial matrix variable P in the sos program
% SOS of
%
% [SOSP,P] = sosmatrixvar(SOSP,ZSym,n,matrixstr)
%
% SOSP is the sum of squares program.
% P is the new polynomial matrix.
% n is the dimension of the matrix P: n(1) x n(2)
% ZSym is the vector of monomials contained in VAR. Decision
% variables corresponding to those monomials will be assigned
% automatically by SOSPOLYVAR.
% matrixstr is a char string with the option 'symmetric' when required

% This file is part of SOSTOOLS - Sum of Squares Toolbox ver 3.00.
%
% Copyright (C)2002, 2004, 2013  A. Papachristodoulou (1), J. Anderson (1),
%                                G. Valmorbida (1), S. Prajna (2),
%                                P. Seiler (3), P. A. Parrilo (4)
% (1) Department of Engineering Science, University of Oxford, Oxford, U.K.
% (2) Control and Dynamical Systems - California Institute of Technology,
%     Pasadena, CA 91125, USA.
% (3) Aerospace and Engineering Mechanics Department, University of
%     Minnesota, Minneapolis, MN 55455-0153, USA.
% (4) Laboratory for Information and Decision Systems, M.I.T.,
%     Massachusetts, MA 02139-4307
%
% Send bug reports and feedback to: sostools@cds.caltech.edu
%
% 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, see <http://www.gnu.org/licenses/>.

% JA&GV - 6/13/2013

% Original Code
if nargin == 4
    if matrixstr=='symmetric'
        if n(1)==n(2)
            if isfield(sos,'symvartable')
                P = sym(zeros(n(1),n(2)));
            else
                % Code for multipoly: PJS 9/9/2013
                P = polynomial(zeros(n(1),n(2)));
            end
            for i = 1:n(1)
                for j = i:n(1)
                    [sos,var] = sospolyvar(sos,ZSym);
                    P(i,j) = var;
                    P(j,i) = var;
                    clear var;
                end
            end
        else
            disp(['''symmetric''' ' option used, matrix must be square.']);
            P = [];
            return
        end
    else
        disp(['Matrix structure ' matrixstr ' is not defined.' ]);
        P = [];
        return

    end
else
    if isfield(sos,'symvartable')
        P = sym(zeros(n(1),n(2)));
    else
        % Code for multipoly: PJS 9/9/2013
        P = polynomial(zeros(n(1),n(2)));
    end
    for i = 1:n(1)
        for j = 1:n(2)
            [sos,var] = sospolyvar(sos,ZSym);
            P(i,j) = var;
            clear var;
        end
    end

end


function [ a, ipvt, rcond ] = zgeco ( a, lda, n )

%*****************************************************************************80
%
%% ZGECO factors a complex matrix and estimates its condition.
%
%  Discussion:
%
%    If RCOND is not needed, ZGEFA is slightly faster.
%
%    To solve A*X = B, follow ZGECO by ZGESL.
%
%    To compute inverse(A)*C, follow ZGECO by ZGESL.
%
%    To compute determinant(A), follow ZGECO by ZGEDI.
%
%    To compute inverse(A), follow ZGECO by ZGEDI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complexA(LDA,N), the matrix to be factored.
%
%    Input, integer LDA, the leading dimension of A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, complex A(LDA,N), an upper triangular matrix and the multipliers
%    which were used to obtain it.  The factorization can be written A = L*U
%    where L is a product of permutation and unit lower triangular matrices
%    and U is upper triangular.
%
%    Output, integer IPVT(N), the pivot indices.
%
%    Output, real RCOND, an estimate of the reciprocal condition of A.
%    For the system A*X = B, relative perturbations in A and B of size
%    EPSILON may cause relative perturbations in X of size (EPSILON/RCOND).
%    If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate
%    underflows.
%

%
%  Compute the 1-norm of A.
%
  anorm = 0.0;
  for j = 1 : n
    anorm = max ( anorm, dzasum ( n, a(1:n,j), 1 ) );
  end
%
%  Factor.
%
  [ a, ipvt, info ] = zgefa ( a, lda, n );
%
%  RCOND = 1/(norm(A)*(estimate of norm(inverse(A)))).
%
%  Estimate = norm(Z)/norm(Y) where A*Z = Y and hermitian(A)*Y = E.
%
%  Hermitian(A) is the conjugate transpose of A.
%
%  The components of E are chosen to cause maximum local
%  growth in the elements of W where hermitian(U)*W = E.
%
%  The vectors are frequently rescaled to avoid overflow.
%
%  Solve hermitian(U)*W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  for k = 1 : n

    if ( zabs1 ( z(k) ) ~= 0.0 )
      ek = zsign1 ( ek, -z(k) );
    end

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

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

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

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

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

    z(k) = wk;

  end

  s = 1.0 / dzasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
%
%  Solve hermitian(L) * Y = W.
%
  for k = n : -1 : 1

    if ( k < n )
      z(k) = z(k) + z(k+1:n) * conj ( a(k+1:n,k) );
    end

    if ( 1.0 < zabs1 ( z(k) ) )
      s = 1.0 / zabs1 ( z(k) );
      z(1:n) = z(1:n) * s;
    end

    l = ipvt(k);

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

  end

  s = 1.0 / dzasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = 1.0;
%
%  Solve L * V = Y.
%
  for k = 1 : n

    l = ipvt(k);

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

    if ( k < n )
      z(k+1:n) = z(k+1:n) + t * transpose ( a(k+1:n,k) );
    end

    if ( 1.0 < zabs1 ( z(k) ) )
      s = 1.0 / zabs1 ( z(k) );
      z(1:n) = z(1:n) * s;
      ynorm = s * ynorm;
    end

  end

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

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

    if ( zabs1 ( a(k,k) ) ~= 0.0 )
      z(k) = z(k) / a(k,k);
    else
      z(k) = 1.0;
    end

    t = -z(k);
    z(1:k-1) = z(1:k-1) + t * transpose ( a(1:k-1,k) );

  end
%
%  Make ZNORM = 1.
%
  s = 1.0 / dzasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = s * ynorm;

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

  return
end


% QUATERNION CONJUGATE
function [qCon] = qConjugate(q)
% This function defines the conjugate of the given quaternion.
% Associated block:
% "Quaternion Conjugate"
qCon = zeros(4,1);
qCon(2) = -q(2);
qCon(3) = -q(3);
qCon(4) = -q(4);
end


function [HF,psi,x,y] = feed_dist(F,z,TF,PF)

parameters_dist
c = length(z); % c includes all species
options = optimset('Display','iter','Diagnostics','on','Algorithm','interior-point');

% Bubble point calculation
xB = z'; % Bubble point requirement (together with PF)
yB = xB; % Initial guess
TB = TF; % Initial guess
x0 = [yB; TB];
r  = fmincon('1',x0,[],[],[],[],[],[],@gB,options,xB,PF,c);
yB = r(1:c);
TB = r(end);

% Dew point calculation
yD = z'; % Dew point requirement (together with PF)
xD = yD; % Initial guess
TD = TF; % Initial guess
x0 = [xD; TD];
r  = fmincon('1',x0,[],[],[],[],[],[],@gD,options,yD,PF,c);
xD = r(1:c);
TD = r(end);

if TF <= TB
    psi = 0;
    x   = xB;
    y   = yB;
    [~,~,~,~,~,HF,~,~] = prsrk(x,PF*1e5,TF+273.15,pc,Tc,w,k,cpig,DHf,DGf,'L',Thermo);
elseif TF >= TD
    psi = 1;
    x   = xD;
    y   = yD;
    [~,~,~,~,~,HF,~,~] = prsrk(y,PF*1e5,TF+273.15,pc,Tc,w,k,cpig,DHf,DGf,'V',Thermo);
else
    % Two-phase calculation
    x  = z';    % Initial guess
    y  = x;     % Initial guess
    V  = 0.5*F; % Initial guess
    L  = V;     % Initial guess
    x0 = [x; y; V; L]; d = [F; TF; PF; z'];
    r = fmincon('1',x0,[],[],[],[],[],[],@gF,options,d,c);
    x = r(1:c); y = r(c+1:2*c);
    [~,~,~,~,~,HL,~,~] = prsrk(x,PF*1e5,TF+273.15,pc,Tc,w,k,cpig,DHf,DGf,'L',Thermo);
    [~,~,~,~,~,HV,~,~] = prsrk(y,PF*1e5,TF+273.15,pc,Tc,w,k,cpig,DHf,DGf,'V',Thermo);
    V = r(end-1); L = r(end);
    HF = 1/F*(HV*V + HL*L); psi = r(end-1)/F;
end

function [c,ceq] = gB(x0,x,PF,c)

parameters_dist
y = x0(1:c);
T = x0(end);

[~,~,phiL,~,~,~,~,~] = prsrk(x,PF*1e5,T+273.15,pc,Tc,w,k,cpig,DHf,DGf,'L',Thermo);
[~,~,phiV,~,~,~,~,~] = prsrk(y,PF*1e5,T+273.15,pc,Tc,w,k,cpig,DHf,DGf,'V',Thermo);
K = phiL./phiV;

ceq = [y - K.*x; sum(y) - 1];
c   = [];

function [c,ceq] = gD(x0,y,PF,c)

parameters_dist
x = x0(1:c);
T = x0(end);

[~,~,phiL,~,~,~,~,~] = prsrk(x,PF*1e5,T+273.15,pc,Tc,w,k,cpig,DHf,DGf,'L',Thermo);
[~,~,phiV,~,~,~,~,~] = prsrk(y,PF*1e5,T+273.15,pc,Tc,w,k,cpig,DHf,DGf,'V',Thermo);
K = phiL./phiV;

ceq = [y - K.*x; sum(x) - 1];
c   = [];

function [c,ceq] = gF(x0,d,c)

parameters_dist
x = x0(1:c);
y = x0(c+1:2*c);
V = x0(end-1);
L = x0(end);

F  = d(1);
TF = d(2);
PF = d(3);
z  = d(4:end);

[~,~,phiL,~,~,~,~,~] = prsrk(x,PF*1e5,TF+273.15,pc,Tc,w,k,cpig,DHf,DGf,'L',Thermo);
[~,~,phiV,~,~,~,~,~] = prsrk(y,PF*1e5,TF+273.15,pc,Tc,w,k,cpig,DHf,DGf,'V',Thermo);
K = phiL./phiV;

ceq = [y - K.*x; F*z - V*y - L*x; sum(x) - 1; sum(y) - 1];
c   = [];


function c = ttv(a,v,dims)
%TTV Tensor times vector for ktensor.
%
%   Y = TTV(X,A,N) computes the product of Kruskal tensor X with a
%   (column) vector A.  The integer N specifies the dimension in X
%   along which A is multiplied.  If size(A) = [I,1], then X must have
%   size(X,N) = I.  Note that ndims(Y) = ndims(X) - 1 because the N-th
%   dimension is removed.
%
%   Y = TTV(X,{A1,A2,...}) computes the product of tensor X with a
%   sequence of vectors in the cell array.  The products are computed
%   sequentially along all dimensions (or modes) of X. The cell array
%   contains ndims(X) vectors.
%
%   Y = TTV(X,{A1,A2,...},DIMS) computes the sequence tensor-vector
%   products along the dimensions specified by DIMS.
%
%   See also TENSOR/TTV, KTENSOR, KTENSOR/TTM.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%%%%%%%%%%%%%%%%%%%%%%
%%% ERROR CHECKING %%%
%%%%%%%%%%%%%%%%%%%%%%

% Check the number of arguments
if (nargin < 2)
    error('TTV requires at least two arguments.');
end

% Check for 3rd argument
if ~exist('dims','var')
    dims = [];
end

% Check that 2nd argument is cell array. If not, recall with v as a
% cell array with one element.
if ~iscell(v)
    c = ttv(a,{v},dims);
    return;
end

% Get sorted dims and index for multiplicands
[dims,vidx] = tt_dimscheck(dims,ndims(a),numel(v));

% Check that each multiplicand is the right size.
for i = 1:numel(dims)
    if ~isequal(size(v{vidx(i)}),[size(a,dims(i)) 1])
        error('Multiplicand is wrong size');
    end
end

% Figure out which dimensions will be left when we're done
remdims = setdiff(1:ndims(a),dims);

% Collapse dimensions that are being multiplied out
newlambda = a.lambda;
for i = 1:numel(dims)
    newlambda = newlambda .* ( a.u{dims(i)}' * v{vidx(i)} );
end

% Create final result
if isempty(remdims)
    c = sum(newlambda);
else
    c = ktensor(newlambda,a.u{remdims});
end


function [ x, w ] = rule01 ( n )

%*****************************************************************************80
%
%% RULE01 returns the rule of degree 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer N, the number of nodes.
%
%    Output, real X(2,N), the coordinates of the nodes.
%
%    Output, real W(N), the weights.
%
  xs = [ ...
       0.00000000000000000E+00 ];
  ys = [ ...
       0.00000000000000000E+00 ];
  ws = [ ...
       0.28284271247461904E+01 ];

  x(1,1:n) = xs(1:n);
  x(2,1:n) = ys(1:n);
  w(1:n) = ws(1:n);

  return
end


function [kN] = dyn2kN(dyn)
% Convert force from dyne to kilonewtons.
% Chad A. Greene 2012
kN = dyn*1e-8;
%
% C'mon, help me with this conversion--I'm dyne over here!


% Fixed budget quantized kernel least-mean-square algorithm
%
% S. Zhao, B. Chen, P. Zhu, J. C. Principe, "Fixed budget quantized kernel
% least-mean-square algorithm", Signal Processing, Volume 93, Issue 9,
% September 2013, Pages 2759-2770,
% http://dx.doi.org/10.1016/j.sigpro.2013.02.012.
%
% Remark: significance calculation only implemented for Gaussian kernel.
%
% This file is part of the Kernel Adaptive Filtering Toolbox for Matlab.
% https://github.com/steven2358/kafbox/

classdef qklms_fb < kernel_adaptive_filter

    properties (GetAccess = 'public', SetAccess = 'private')
        eta = .9; % learning rate
        epsu = .1; % quantization threshold
        beta = .95; % forgetting factor for influence
        M = 500; % dictionary size (K in publication)
        kerneltype = 'gauss'; % kernel type
        kernelpar = 1; % kernel parameter
    end

    properties (GetAccess = 'public', SetAccess = 'private')
        dict = []; % codebook
        alpha = []; % expansion coefficients
        E = []; % significance vector (~ importance)
        lambda = []; % influence vector (~ how many samples are quantized
        % to each centre)
    end

    methods
        function kaf = qklms_fb(parameters) % constructor
            if (nargin > 0) % copy valid parameters
                for fn = fieldnames(parameters)'
                    if ismember(fn,fieldnames(kaf))
                        kaf.(fn{1}) = parameters.(fn{1});
                    end
                end
            end
        end

        function y_est = evaluate(kaf,x) % evaluate the algorithm
            if size(kaf.dict,1)>0
                k = kernel(kaf.dict,x,kaf.kerneltype,kaf.kernelpar);
                y_est = k'*kaf.alpha;
            else
                y_est = zeros(size(x,1),1);
            end
        end

        function train(kaf,x,y) % train the algorithm
            y_est = kaf.evaluate(x);
            err = y - y_est;

            m = size(kaf.dict,1);
            if m==0
                d2 = kaf.epsu^2 + 1; % force dictionary growth
            else
                [d2,j] = min(sum((kaf.dict - repmat(x,m,1)).^2,2));
            end

            c1 = pi*kaf.kernelpar^2/2; % pre-calculate

            if d2 <= kaf.epsu^2 % new basis under quantization threshold
                kaf.alpha(j) = kaf.alpha(j) + kaf.eta*err;

                % update significance for quantizing y to j-th centre (16)
                if m>1
                    inds = 1:m;
                    inds(j) = [];
                    kaf.E(inds) = kaf.beta*kaf.E(inds) + c1*abs(kaf.alpha(inds)).* ...
                        kernel(kaf.dict(inds,:),kaf.dict(j,:),kaf.kerneltype,kaf.kernelpar);
                end
                kaf.E(j) = abs(1 + kaf.eta*err/kaf.alpha(j)) * ...
                    kaf.beta*kaf.E(j) + ...
                    abs(kaf.alpha(j) + kaf.eta*err) * ...
                    c1*kernel(kaf.dict(j,:),kaf.dict(j,:),kaf.kerneltype,kaf.kernelpar);

                % update influence
                kaf.lambda = kaf.beta*kaf.lambda;
                kaf.lambda(j) = kaf.lambda(j) + 1;

            else % new basis not under quantization threshold
                if m < kaf.M % still room for extra centres
                    kaf.dict = [kaf.dict; x]; % add to codebook
                    kaf.alpha = [kaf.alpha; kaf.eta*err];

                    % update significance for addition of m+1-th centre (15)
                    kaf.E(m+1,1) = 0;
                    kaf.E = kaf.beta*kaf.E + abs(kaf.alpha(m+1)) * ...
                        kernel(kaf.dict,kaf.dict(m+1,:),kaf.kerneltype,kaf.kernelpar);

                    % update influence
                    kaf.lambda = kaf.beta*kaf.lambda;
                    % initial influence
                    kaf.lambda(m+1) = 1;

                else % no room for extra centres
                    [~,L] = min(kaf.E); % centre with lowest significance

                    % update significance for removing L-th centre (17)
                    kaf.E = kaf.E - kaf.lambda(L) * c1*abs(kaf.alpha) .* ...
                        kernel(kaf.dict,kaf.dict(L,:),kaf.kerneltype,kaf.kernelpar);

                    % prune
                    kaf.dict(L,:) = [];
                    kaf.alpha(L) = [];
                    kaf.E(L) = [];
                    kaf.lambda(L) = [];

                    % re-calculate error
                    y_est = kaf.evaluate(x);
                    err = y - y_est;

                    % grow
                    kaf.dict = [kaf.dict; x];
                    kaf.alpha = [kaf.alpha; kaf.eta*err];

                    % update significance for addition of m-th centre (15)
                    kaf.E(m,1) = 0;
                    kaf.E = kaf.beta*kaf.E + abs(kaf.alpha(m)) * ...
                        kernel(kaf.dict,kaf.dict(m,:),kaf.kerneltype,kaf.kernelpar);

                    % update influence
                    kaf.lambda = kaf.beta*kaf.lambda;
                    % initial influence
                    kaf.lambda(m) = 1;
                end
            end
        end
    end
end


% REJKURT  - calculation of kutosis of a 1D, 2D or 3D array and
%              rejection of outliers values of the input data array
%              using the discrete kutosis of the values in that dimension.
%
% Usage:
%   >>  [kurtosis rej] = rejkurt( signal, threshold, kurtosis, normalize);
%
% Inputs:
%   signal     - one dimensional column vector of data values, two
%                dimensional column vector of values of size
%                sweeps x frames or three dimensional array of size
%                component x sweeps x frames. If three dimensional,
%                all components are treated independently.
%   threshold  - Absolute threshold. If normalization is used then the
%                threshold is expressed in standard deviation of the
%                mean. 0 means no threshold.
%   kurtosis   - pre-computed kurtosis (only perform thresholding). Default
%                is the empty array [].
%   normalize  - 0 = do not not normalize kurtosis. 1 = normalize kurtosis.
%                2 is 20% trimming (10% low and 10% high) kurtosis before
%                normalizing. Default is 0.
%
% Outputs:
%   kurtosis    - normalized joint probability  of the single trials
%                (same size as signal without the last dimension)
%   rej         - rejected matrix (0 and 1, size: 1 x sweeps)
%
% Remarks:
%   The exact values of kurtosis depend on the size of a time
%   step and thus cannot be considered as absolute.
%   This function uses the kurtosis function from the statistival
%   matlab toolbox. If the statistical toolbox is not installed,
%   it uses the 'kurt' function of the ICA/EEG toolbox.
%
% See also: KURT, KURTOSIS

% Copyright (C) 2001 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function [kurto, rej] = rejkurt( signal, threshold, oldkurtosis, normalize);

if nargin < 1
	help rejkurt;
	return;
end;
if nargin < 2
	threshold = 0;
end;
if nargin < 4
	normalize = 0;
end;
if nargin < 3
	oldkurtosis = [];
end;

if size(signal,2) == 1 % transpose if necessary
	signal = signal';
end

nbchan = size(signal,1);
pnts = size(signal,2);
sweeps = size(signal,3);
kurto = zeros(nbchan,sweeps);

if ~isempty( oldkurtosis ) % speed up the computation
	kurto = oldkurtosis;
else
	for rc = 1:nbchan
		% compute all kurtosis
		% --------------------
		for index=1:sweeps
			try
			    kurto(rc, index) = kurtosis(signal(rc,:,index));
			catch
				kurto(rc, index) = kurt(signal(rc,:,index));
			end;
		end
	end

	% normalize the last dimension
	% ----------------------------
	if normalize
        tmpkurt = kurto;
        if normalize == 2,
            tmpkurt = sort(tmpkurt);
            minind  = max(round(length(tmpkurt)*0.1),1);
            maxind  = round(length(tmpkurt)-round(length(tmpkurt)*0.1));
            if size(tmpkurt,2) == 1
                 tmpkurt = tmpkurt(minind:maxind);
            else tmpkurt = tmpkurt(:,minind:maxind);
            end
        end
	    switch ndims( signal )
	    	case 2,	kurto = (kurto-mean(tmpkurt)) / std(tmpkurt);
	    	case 3,	kurto = (kurto-mean(tmpkurt,2)*ones(1,size(kurto,2)))./ ...
				        (std(tmpkurt,0,2)*ones(1,size(kurto,2)));
		end
	end
end

% reject
% ------
if threshold(1) ~= 0
    if length(threshold) > 1
    	rej = (threshold(1) > kurto) | (kurto > threshold(2));
    else
    	rej = abs(kurto) > threshold;
    end
else
	rej = zeros(size(kurto));
end;

return;


function value = sdot ( n, dx, incx, dy, incy )

%*****************************************************************************80
%
%% SDOT forms the dot product of two vectors.
%
%  Discussion:
%
%    This routine uses unrolled loops for increments equal to one.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2006
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vectors.
%
%    Input, real DX(*), the first vector.
%
%    Input, integer INCX, the increment between successive entries in DX.
%
%    Input, real DY(*), the second vector.
%
%    Input, integer INCY, the increment between successive entries in DY.
%
%    Output, real VALUE, the sum of the product of the
%    corresponding entries of DX and DY.
%
  value = 0.0;

  if ( n <= 0 )
    return
  end
%
%  Code for unequal increments or equal increments
%  not equal to 1.
%
  if ( incx ~= 1 | incy ~= 1 )

    if ( 0 <= incx )
      ix = 1;
    else
      ix = ( - n + 1 ) * incx + 1;
    end

    if ( 0 <= incy )
      iy = 1;
    else
      iy = ( - n + 1 ) * incy + 1;
    end

    for i = 1 : n
      value = value + dx(ix) * dy(iy);
      ix = ix + incx;
      iy = iy + incy;
    end
%
%  Code for both increments equal to 1.
%
  else

    m = mod ( n, 5 );

    for i = 1 : m
      value = value + dx(i) * dy(i);
    end

    for i = m+1 : 5 : n

      value = value + dx(i  ) * dy(i  ) ...
                    + dx(i+1) * dy(i+1) ...
                    + dx(i+2) * dy(i+2) ...
                    + dx(i+3) * dy(i+3) ...
                    + dx(i+4) * dy(i+4);
    end

  end

  return
end


function[] = hindawi()
%
% Simulation code for [1],
%
% [1] Paul Rodriguez "Review: Total Variation Regularization Algorithms for
%  		      Images Corrupted With Different Noise Models"
%
%
%
% Legal:
%
% Authors
%   Paul Rodriguez    prodrig@pucp.edu.pe
%

l2TV         = 1;
l2TV_nqp     = 2;
l1TV         = 3;
l1TV_adapt   = 4;
poiTV_denoi  = 5;
poiTV_deconv = 6;
gammaTV      = 7;
mixTV        = 8;
nTV          = mixTV;

% Test Images (for each case)

l2Imgs = {'goldhl', 'Clena'};           % To be corrupted with Gaussian noise
l1Imgs = {'Cbarb', 'boats'};            % To be corrupted with Salt & Pepper noise
poiImgs = {'cman512', 'cpeppers'};     % To be corrupted with Poisson noise
gammaImgs = {'tank', 'Clena'};          % To be corrupted with Gamma noise
mixImgs = {'cman512'};                  % To be corrupted with mixed Gaussian + Impulse noise

Tests = cell(nTV-1, 1);

Tests{l2TV} = l2Imgs;
Tests{l2TV_nqp} = l2Imgs;
Tests{l1TV} = l1Imgs;
Tests{l1TV_adapt} = l1Imgs;
Tests{poiTV_denoi} = poiImgs;
Tests{poiTV_deconv} = poiImgs;

%  Tests{gammaTV} = gammaImgs;
%  Tests{mixTV} = mixImgs;

% kernel

  kernel = fspecial('disk',3.2);

  K = @(x) imfilter(x, kernel, 'symmetric','conv');

  KT = @(x) K(x);
  KC = {K, KT};

% Noise level (define the noise level to corrupt images)

l2Noise = [0.1 0.2];
l1Noise = [0.3 0.5 0.8];
randomNoise = [0.1 0.2 0.3];
poiNoise = [5 30 100 255];

Noise = cell(nTV-1, 1);

Noise{l2TV} = l2Noise;
Noise{l2TV_nqp} = l2Noise;
Noise{l1TV} = l1Noise;
Noise{l1TV_adapt} = l1Noise;
Noise{poiTV_denoi} = poiNoise;
Noise{poiTV_deconv} = poiNoise;

% TV Noise Model (to corrupt imput images)

NoiseModel = cell(nTV-1, 1);

NoiseModel{l2TV} = @(Img, sigma) imnoise(K(Img),'gaussian', 0, (sigma^2) );
NoiseModel{l2TV_nqp} = @(Img, sigma) imnoise(K(Img),'gaussian', 0, (sigma^2) );
NoiseModel{l1TV} = @(Img, spnoise) imnoise(Img, 'salt & pepper', spnoise);
NoiseModel{l1TV_adapt} = @(Img, spnoise) imnoise(Img, 'salt & pepper', spnoise);

NoiseModel{poiTV_denoi} = @(Img, M) poissrnd( M*NormalizeMax(Img) );
NoiseModel{poiTV_deconv} = @(Img, M) poissrnd( M*NormalizeMax(K(Img)) );

% ----------------------
% Define TV parameters
% ----------------------

% Loops

TVLoops = zeros(nTV-1, 1);

TVLoops(l2TV)           = 5;
TVLoops(l2TV_nqp)       = 5;
TVLoops(l1TV)           = 4;
TVLoops(l1TV_adapt)     = 4;

TVLoops(poiTV_denoi)    = 6;
TVLoops(poiTV_deconv)   = 6;

% regularization parameters

TVLambda = zeros(nTV-1, 5);

TVLambda(l2TV,1) = 0.05; TVLambda(l2TV,2) = 0.1;
TVLambda(l2TV_nqp,1) = 0.05; TVLambda(l2TV_nqp,2) = 0.1;

TVLambda(l1TV,1) = 1.2; TVLambda(l1TV,2) = 1.4; TVLambda(l1TV,3) = 1.5;
TVLambda(l1TV_adapt,1) = 1.0; TVLambda(l1TV_adapt,2) = 1.0; TVLambda(l1TV_adapt,3) = 1.0;

TVLambda(poiTV_denoi,1:4) = 2./[0.35, 0.15, 0.075, 0.025 ];

%  Wrappers to TV functions)

RestoreTV = cell(nTV-1, 1);

RestoreTV{l2TV} = @(b, lambda, loops, KC) runl2TV(b, lambda, loops, KC);
RestoreTV{l2TV_nqp} = @(b, lambda, loops, KC) runl2TV_nqp(b, lambda, loops, KC);
RestoreTV{l1TV} = @(b, lambda, loops, dummy) runl1TV(b, lambda, loops, []);
RestoreTV{l1TV_adapt} = @(b, lambda, loops, dummy) runl1TV_Adapt(b, lambda, loops);

RestoreTV{poiTV_denoi} = @(b, lambda, loops, dummy) run_poiTV(b, lambda, loops, {});
RestoreTV{poiTV_deconv} = @(b, lambda, loops, KC) run_poiTV(b, lambda, loops, KC);

% additional vars

t1 = zeros(nTV-1, 2, 2);
t2 = zeros(nTV-1, 2, 2);

nmpdef;

%%%%%%%%%%%%%%%%%%%%%%%%
%        Setup         %
%%%%%%%%%%%%%%%%%%%%%%%%

SSIM_CODE = exist('ssim_index');
if( SSIM_CODE == 0 )
  disp('NOTE:');
  disp('  The function ssim_index (code for [SSIM]) is not in your path...');
  disp('  You may download it from:');
  disp('  http://www.ece.uwaterloo.ca/~z70wang/research/ssim/');
  disp('  ');
  disp('  Disabling SSIM reports');
  disp('  ');
  disp('  [SSIM]  Z. Wang, A. Bovik, H. Sheikh and E. Simoncelli, ');
  disp('         "Image quality assessment: From error visibility to');
  disp('         structural similarity " ');
  disp('         IEEE Transactions on Image Processing, 2004, 13:4(600-612).');

end

t=5;

str_all = sprintf(' \n');

while( iscell(Tests{t}) )

  str = sprintf(' \n'); str_all = [ str_all str ];
  str = sprintf(' Img \t\t Noise \t\t SNR (db) \t\t Time (s) \t\t SSIM \n');
  str_all = [ str_all str ];

  NImgs = length( Tests{t} );

  for k = 1: NImgs,

  switch lower( Tests{t}{k} )

    case{'lena'}
      I = double( imread('gray_imgs/lena_gray_512.png') ) / 255;
      SSIM_COLOR = 1;

    case{'peppers'}
      I = double( imread('gray_imgs/peppers_gray.png') ) / 255;
      SSIM_COLOR = 1;

    case{'boats'}
      I = double( imread('gray_imgs/boats_gray.png') ) / 255;
      SSIM_COLOR = 1;

    case{'goldhl'}
      I = double( imread('gray_imgs/goldhill_gray.png') ) / 255;
      SSIM_COLOR = 1;

    case{'cman'}
      I = double( imread('gray_imgs/cameraman_256x256.tiff') ) / 255;
      SSIM_COLOR = 1;

    case{'cman512'}
      I = double( imread('gray_imgs/cameraman_512x512.tiff') ) / 255;
      SSIM_COLOR = 1;

    case{'clena'}
      I = double( imread('color_imgs/lena_color_512.png') ) / 255;
      SSIM_COLOR = 0;

    case{'cbarb'}
      I = double( imread('color_imgs/barbara_color.png') ) / 255;
      SSIM_COLOR = 0;

    case{'cpeppers'}
      I = double( imread('color_imgs/peppers_color.png') ) / 255;
      SSIM_COLOR = 0;

    case{'cmandrill'}
      I = double( imread('color_imgs/mandrill_color.png') ) / 255;
      SSIM_COLOR = 0;

  end % --- END (switch) ---

  for n = 1:length(Noise{t})

    % add noise
    b = NoiseModel{t}( I, Noise{t}(n) );
    figure; imagesc( Normalize(b) );  colormap gray; axis image; axis off;

    % Restore via TV
    [u t1(t,k,n) t2(t,k,n)] = RestoreTV{t}(b, TVLambda(t,n), TVLoops(t), KC );

    % Restore performance
    [snrRec(t,k,n), ssimRec(t,k,n)] = computePerf(I, u, SSIM_CODE*SSIM_COLOR, 0);

    if(n==1)
      str = sprintf('%s \t\t %1.2f \t\t %2.2f \t\t %2.2f (%2.2f) \t\t %1.3f \n', ...
                    lower( Tests{t}{k} ), Noise{t}(n), snrRec(t,k,n), t1(t,k,n), t2(t,k,n), ssimRec(t,k,n) );
    else
      str = sprintf('     \t\t %1.2f \t\t %2.2f \t\t %2.2f (%2.2f) \t\t %1.3f \n', ...
                     Noise{t}(n), snrRec(t,k,n), t1(t,k,n), t2(t,k,n), ssimRec(t,k,n) );
    end
    str_all = [ str_all str ];

  end % --- END (FOR(n)) ---

  end % --- END (FOR(k)) ---

  disp(str_all);

 t = t + 1;

 if(t==6) break; end

end

end

% ============================================

%%%%%%%%%%%%%%%%%%%%%%%%
%     Normalize        %
%%%%%%%%%%%%%%%%%%%%%%%%

function[u] = Normalize(x)

u = (x - min(x(:)))/(max(x(:)) - min(x(:)));

end

% ============================================

function[u] = NormalizeMax(x)

u = x/max(abs(x(:)));

end

% ============================================

function[u, timePerf, t2] = runl2TV(b, lambda, loops, KC)

    pars = irntvInputPars('l2tv');

    pars.pcgtol_ini = 1e-4;
    pars.loops      = loops;
%      pars.U0         = b;

    pars.adapt_epsR   = 1;
    pars.epsR_cutoff  = 0.01;
    pars.adapt_epsF   = 1;
    pars.epsF_cutoff  = 0.05;

    tic;
    u = irntv(b, KC, lambda, pars);
    timePerf = toc;

    t2 = 0;

end

% ============================================

function[u, timePerf, t2] = runl2TV_nqp(b, lambda, loops, KC)

    pars = irntvInputPars('l2tv_nqp');

    pars.pcgtol_ini = 1e-4;
    pars.loops      = loops;
%      pars.U0         = b;

    pars.adapt_epsR   = 1;
    pars.epsR_cutoff  = 0.01;
    pars.adapt_epsF   = 1;
    pars.epsF_cutoff  = 0.1;

    pars.loops_NQP    = 15;
    pars.gamma_NQP    = 0.5e-3;
    pars.alpha_NQP    = 5e-1;
    pars.vmax_NQP     = 1+1;            % maximum value of output image

    tic;
    u = irntv(b+1, KC, lambda, pars)-1;
    timePerf = toc;

    t2 = 0;

end

% ============================================

function[u, timePerf, t2] = runl1TV(b, lambda, loops, KC)

    if(nargin < 6)
      flagTitle = 0;
    end

    pars = irntvInputPars('l1tv');

    pars.pcgtol_ini = 1e-4;
    pars.epsF       = 1e-2;
    pars.epsR       = 1e-4;
    pars.loops      = loops;

    tic;
    u = irntv(b, KC, lambda, pars);
    timePerf = toc;

    t2 = 0;

end

% ============================================

function[u, timePerf, t2] = runl1TV_Adapt(b, lambda, loops)

tic;

S0 = adaptMedian(b);

t2 = toc;

InputDims = size(S0);

if(length(InputDims) == 2)

    lambda_adapt  = 1e-6*(S0 == 0) + lambda*5*(S0 ~= 0);

else

    for d=1:3,
      lambda_adapt(:,:,d)  = 1e-6*(S0(:,:,d) == 0) + lambda*5.0*(S0(:,:,d) ~= 0);
    end

end

% ---------- Setup for IRN_adapt  ------------------

pars_irn_adapt = irntvInputPars('l1tv_adapt');

pars_irn_adapt.adapt_epsR   = 1;
pars_irn_adapt.epsR_cutoff  = 0.01;
pars_irn_adapt.adapt_epsF   = 1;
pars_irn_adapt.epsF_cutoff  = 0.1;

pars_irn_adapt.pcgtol_ini = 1e-4;

pars_irn_adapt.loops      = 1;

pars_irn_adapt.U0      = b;   % necessary the for adapt case

  tic;
  u = irntv(b, [], lambda_adapt, pars_irn_adapt);

  for m = 2:loops

    pars_irn_adapt.U0 = u;
    lambda_adapt = lambda_adapt*1.2;

    u = irntv(b, [], lambda_adapt, pars_irn_adapt);

  end

  timePerf = toc;

end

% ============================================

% ============================================

function[u, timePerf, t2] = run_poiTV(b, lambda, loops, KC)

    pars = irntvInputPars('tv_poisson_adapt');

    pars.pcgtol_ini = 1e-4;
    pars.loops      = 1;
%      pars.U0         = b;

    pars.adapt_epsR   = 1;
    pars.epsR_cutoff  = 0.1;
    pars.adapt_epsF   = 1;
    pars.epsF_cutoff  = 0.15;

    pars.loops_NQP    = 35;
    pars.gamma_NQP    = 5e-3;
    pars.alpha_NQP    = 5e-1;
    pars.vmax_NQP     = 1+1;            % maximum value of output image

    T = lambda;

    tic;
    u = irntv(b+0.01, KC, lambda, pars)-0.01;

    for ln=2:loops,

      [sig1, cv1, mu1, siginv1, cvinv1, S1, lmu1] = estNoise(u, 2*3+1, 100, 1);
      [sig2, cv2, mu2, siginv2, cvinv2, S2, lmu2] = estNoise(lmu1, 2*3+1, 100, 1);

      pars.U0 = u;
      vmin = min( pars.U0(:) );
      if(vmin < 0) pars.U0 = pars.U0 - vmin + 0.01; end

      alpha = 1.0;
      [Nrows Ncols Ndims] = size(u);

      mask = zeros(size(S2));

      for d = 1:Ndims,
        maskTmp = zeros(Nrows*Ncols,1);

        vS1 = S2(:,:,d); vS1 = vS1(:);

        tau = unimodal(S1(:,:,d));
        p2 = find( vS1 < alpha*tau );
        maskTmp(p2) = (1 - Normalize( vS1(p2) ) )*(1-0.85) + 0.85;

        p3 = find(vS1 >= alpha*tau);
        maskTmp(p3) = 1.0;

        mask(:,:,d) = reshape(maskTmp, [Nrows Ncols]);

      end % _END_ FOR(d)

      T = T.*mask;

      u = irntv(b+0.1, KC, T, pars)-0.1;

    end

    % ====================================
    timePerf = toc;

    t2 = 0;

end

% ============================================

function[snrRec, ssimRec] = computePerf(Img, u, flagSSIM, flagTitle)

    snrRec = snr(Img, u);
    if(flagSSIM)
      ssimRec = ssim_index(255*Normalize(Img), 255*Normalize(u));
    else
      ssimRec = NaN;
    end

    figure; imagesc( Normalize(u) );  colormap gray; axis image; axis off;
    if(flagTitle)
      title(sprintf('Restored Image\n SNR: %4.1fdB. SSIM: %1.2f ', ...
               snrRec, ssimRec));
    end

end


function line_enhanced = EnhanceLine(band)
%ENHANCELINE enhance NDBI to light urban/built-up areas and dark other
%bright surface, such as desert, rock. ref Guindon et. RSE 2004

% also can see the details at
% https://homepages.inf.ed.ac.uk/rbf/HIPR2/linedet.htm.
%     band=data_toabt.BandGreen;
%
%     band=ndbi;

%% line with a length of three pixels
%     template1=[-1 1 0;
%                -1 1 0;
%                -1 1 0;];
%     template2=[0 1 -1;
%                0 1 -1;
%                0 1 -1;];
%     line_enhanced1 = imfilter(band,template1);
%     line_enhanced2 = imfilter(band,template2);
%     line_enhanced=max(line_enhanced1,line_enhanced2);
%
%     template1=[-1 -1 -1;
%                 1  1  1;
%                 0  0  0;];
%     template2=[0  0  0;
%                1  1  1;
%               -1 -1 -1;];
%     line_enhanced1 = imfilter(band,template1);
%     line_enhanced2 = imfilter(band,template2);
%     line_enhanced=max(line_enhanced1,line_enhanced);
%     line_enhanced=max(line_enhanced2,line_enhanced);
%
%     template1=[1 -1 -1;
%                0  1 -1;
%                0  0  1;];
%     template2=[1  0  0;
%               -1  1  0;
%               -1 -1  1;];
%     line_enhanced1 = imfilter(band,template1);
%     line_enhanced2 = imfilter(band,template2);
%     line_enhanced=max(line_enhanced1,line_enhanced);
%     line_enhanced=max(line_enhanced2,line_enhanced);
%
%     template1=[-1 -1  1;
%                -1  1  0;
%                 1  0  0;];
%     template2=[0  0  1;
%                0  1 -1;
%                1 -1 -1;];
%
%     line_enhanced1 = imfilter(band,template1);
%     line_enhanced2 = imfilter(band,template2);
%     line_enhanced=max(line_enhanced1,line_enhanced);
%     line_enhanced=max(line_enhanced2,line_enhanced);
% 	line_enhanced = line_enhanced./3;

    template=[-1 2 -1;
              -1 2 -1;
              -1 2 -1;];
	template = template./6;
    line_enhanced = imfilter(band,template);

    template=[-1 -1 -1;
               2  2  2;
              -1 -1 -1;];
	template = template./6;
    line_enhanced_new = imfilter(band,template);
    line_enhanced=max(line_enhanced_new,line_enhanced);

    template =[2 -1 -1;
               -1  2 -1;
               -1  -1  2;];
	template = template./6;
    line_enhanced_new = imfilter(band,template);
    line_enhanced=max(line_enhanced_new,line_enhanced);

    template =[-1  -1  2;
               -1  2 -1;
               2 -1 -1;];
	template = template./6;
    line_enhanced_new = imfilter(band,template);
    line_enhanced=max(line_enhanced_new,line_enhanced);
end


classdef anisotropicdiffusion < TTeMPS_op_laplace
    % Class for anisotropic diffusion operator with tridiagonal diffusion matrix
    %
    %       [ 1 a 0     ...0 ]
    %       [ a 1 a 0    ..0 ]
    %   D = [ 0 a 1 a 0  ..0 ]
    %       [  ..   .. . .   ]
    %       [ 0 ... .. 0 a 1 ]
    %
    %

    %   TTeMPS Toolbox.
    %   Michael Steinlechner, 2013-2016
    %   Questions and contact: michael.steinlechner@epfl.ch
    %   BSD 2-clause license, see LICENSE.txt

    properties
        L
        D
        % precomputed spectral decomp of 1D Laplace:

    end

    methods

        function A = update_properties( A );

            A.rank = [1,  3*ones(1, length(A.U)-1), 1];  % the TT rank is always three for such Laplace-like tensors
            size_col_ = cellfun( @(y) size(y,1), A.U);
            A.size_col = size_col_ ./ (A.rank(1:end-1).*A.rank(2:end));
            A.size_row = cellfun( @(y) size(y,2), A.U);
            A.order = length( A.size_row );
        end
    end

    methods( Access = public )

        function A = anisotropicdiffusion( n, d, alpha )

            if ~exist('alpha', 'var')
                alpha = 0.25;
            end

            one = ones(n,1);
            q = linspace( -10, 10, n)';
            h = abs(q(2) - q(1));
            L = -spdiags( [one, -2*one, one], [-1 0 1], n, n) / (h^2);

            % superclass constructor
            A = A@TTeMPS_op_laplace( L, d );
            % precompute eigenvalue information and exponential for use in local
            A = initialize_precond( A );
            % preconditioner:
            A.L = L;

			[A.V_L, A.E_L] = eig(full(A.L));
            A.E_L = diag(A.E_L);

            A.D = spdiags( [-one,one], [-1,1], n, n ) / (2*h);
            I = speye( n, n );

            e1 = sparse( 1, 1, 1, 3, 1 );
            e2 = sparse( 2, 1, 1, 3, 1 );
            e3 = sparse( 3, 1, 1, 3, 1 );

            l_mid = sparse( 3, 1, 1, 9, 1 );                % e_3
            b_mid = sparse( 6, 1, 1, 9, 1 );                % e_6
            m_mid = sparse( [1;9], [1;1], [1;1], 9, 1 );    % e_1 + e_9
            c_mid = sparse( 2, 1, 1, 9, 1 );                % e_2

            A.U = cell( 1, d );
            A.U{1} = kron( A.L, e1 ) + kron( 2*alpha*A.D, e2 ) + kron( I, e3);
            A_mid = kron( A.L, l_mid ) + kron( 2*alpha*A.D, b_mid ) + kron( I, m_mid) + kron( A.D, c_mid );
            for i=2:d-1
                A.U{i} = A_mid;
            end
            A.U{d} = kron( I, e1 ) + kron( A.D, e2 ) + kron( A.L, e3);

            A = update_properties( A );

        end

        function expB = constr_precond_inner( A, X, mu )

            n = size(A.L, 1);
            sz = [X.rank(mu), X.size(mu), X.rank(mu+1)]

            B1 = zeros( X.rank(mu) );
            % calculate B1 part:
            for i = 1:mu-1
                % apply L to the i'th core
                tmp = X;
                Xi = matricize( tmp.U{i}, 2 );
                Xi = A.L*Xi;
                tmp.U{i} = tensorize( Xi, 2, [X.rank(i), n, X.rank(i+1)] );
                B1 = B1 + innerprod( X, tmp, 'LR', mu-1);
            end

            B3 = zeros( X.rank(mu+1) );
            % calculate B3 part:
            for i = mu+1:A.order
                tmp = X;
                Xi = matricize( tmp.U{i}, 2 );
                Xi = A.L*Xi;
                tmp.U{i} = tensorize( Xi, 2, [X.rank(i), n, X.rank(i+1)] );
                B3 = B3 + innerprod( X, tmp, 'RL', mu+1);
            end

            [V1,e1] = eig(B1);
            e1 = diag(e1);
            [V3,e3] = eig(B3);
            e3 = diag(e3);

            lmin = min(e1) + min(A.E_L) + min(e3);
            lmax = max(e1) + max(A.E_L) + max(e3);

            R = lmax/lmin

            [omega, alpha] = load_coefficients( R );

            k = 3;
            omega = omega/lmin;
            alpha = alpha/lmin;

            expB = cell(3,k);

            for i = 1:k
                expB{1,i} = omega(i) * V1*diag( exp( -alpha(i)*e1 ))*V1';    % include omega in first part
                expB{2,i} = A.V_L*diag( exp( -alpha(i)*A.E_L ))*A.V_L';
                expB{3,i} = V3*diag( exp( -alpha(i)*e3 ))*V3';
            end
        end

        function expB = constr_precond_outer( A, X, mu1, mu2 )

            n = size(A.L, 1);

            B1 = zeros( X.rank(mu1) );
            % calculate B1 part:
            for i = 1:mu1-1
                % apply L to the i'th core
                tmp = X;
                Xi = matricize( tmp.U{i}, 2 );
                Xi = A.L*Xi;
                tmp.U{i} = tensorize( Xi, 2, [X.rank(i), n, X.rank(i+1)] );
                B1 = B1 + innerprod( X, tmp, 'LR', mu1-1);
            end

            B3 = zeros( X.rank(mu2+1) );
            % calculate B3 part:
            for i = mu2+1:A.order
                tmp = X;
                Xi = matricize( tmp.U{i}, 2 );
                Xi = A.L*Xi;
                tmp.U{i} = tensorize( Xi, 2, [X.rank(i), n, X.rank(i+1)] );
                B3 = B3 + innerprod( X, tmp, 'RL', mu2+1);
            end

            [V1,e1] = eig(B1);
            e1 = diag(e1);
            [V3,e3] = eig(B3);
            e3 = diag(e3);

            lmin = min(e1) + 2*min(A.E_L) + min(e3);
            lmax = max(e1) + 2*max(A.E_L) + max(e3);

            R = lmax/lmin

            [omega, alpha] = load_coefficients( R );

            k = 3;
            omega = omega/lmin;
            alpha = alpha/lmin;

            expB = cell(4,k);

            for i = 1:k
                expB{1,i} = omega(i) * V1*diag( exp( -alpha(i)*e1 ))*V1';    % include omega in first part
                expB{2,i} = A.V_L*diag( exp( -alpha(i)*A.E_L ))*A.V_L';
                expB{3,i} = A.V_L*diag( exp( -alpha(i)*A.E_L ))*A.V_L';
                expB{4,i} = V3*diag( exp( -alpha(i)*e3 ))*V3';
            end
        end

        function P = constr_precond( A, k )

            d = A.order;

            lmin = d*min(A.E_L);
            lmax = d*max(A.E_L);

            R = lmax/lmin

            %  http://www.mis.mpg.de/scicomp/EXP_SUM/1_x/1_xk07_2E2
            %  0.0133615547183825570028305575534521842940   {omega[1]}
            %  0.0429728469424360175410925952177443321034   {omega[2]}
            %  0.1143029399081515586560726591147663100401   {omega[3]}
            %  0.2838881266934189482611071431161775535656   {omega[4]}
            %  0.6622322841999484042811198458711174907876   {omega[5]}
            %  1.4847175320092703810050463464342840325116   {omega[6]}
            %  3.4859753729916252771962870138366952232900   {omega[7]}
            %  0.0050213411684266507485648978019454613531   {alpha[1]}
            %  0.0312546410994290844202411500801774835168   {alpha[2]}
            %  0.1045970270084145620410366606112262388706   {alpha[3]}
            %  0.2920522758702768403556507270657505159761   {alpha[4]}
            %  0.7407504784499061527671195936939341208927   {alpha[5]}
            %  1.7609744335543204401530945069076494746696   {alpha[6]}
            %  4.0759036969145123916954953635638503328664   {alpha[7]}

            if k == 3
                [omega, alpha] = load_coefficients( R );

            elseif k == 7
                omega = [0.0133615547183825570028305575534521842940 0.0429728469424360175410925952177443321034 0.1143029399081515586560726591147663100401,...
                         0.2838881266934189482611071431161775535656 0.6622322841999484042811198458711174907876 1.4847175320092703810050463464342840325116,...
                         3.4859753729916252771962870138366952232900];
                alpha = [0.0050213411684266507485648978019454613531 0.0312546410994290844202411500801774835168 0.1045970270084145620410366606112262388706,...
                         0.2920522758702768403556507270657505159761 0.7407504784499061527671195936939341208927 1.7609744335543204401530945069076494746696,...
                         4.0759036969145123916954953635638503328664];
            else
                error('Unknown rank specified. Choose either k=3 or k=7');
            end

            omega = omega/lmin;
            alpha = alpha/lmin;

            % careful: all cores assumed to be of same size
            E = reshape( expm( -alpha(1) * A.L), [1, A.size_row(2), A.size_col(2), 1]);
            P = omega(1)*TTeMPS_op( repmat({E},1,d) );
            for i = 2:k
                E = reshape( expm( -alpha(i) * A.L), [1, A.size_row(2), A.size_col(2), 1]);
                P = P + omega(i)*TTeMPS_op( repmat({E},1,d) );
            end

        end

    end

end


%==========================================================================
%
%   Active contour with Chen-Vese Method
%   for image segementation
%
%   Implemented by Yue Wu (yue.wu@tufts.edu)
%   Tufts University
%   Feb 2009
%   http://sites.google.com/site/rexstribeofimageprocessing/
%
%   all rights reserved
%   Last update 02/26/2009
%--------------------------------------------------------------------------
%   Usage of varibles:
%   input:
%       I           = any gray/double/RGB input image
%       mask        = initial mask, either customerlized or built-in
%       num_iter    = total number of iterations
%       mu          = weight of length term
%       method      = submethods pick from ('chen','vector','multiphase')
%
%   Types of built-in mask functions
%       'small'     = create a small circular mask
%       'medium'    = create a medium circular mask
%       'large'     = create a large circular mask
%       'whole'     = create a mask with holes around
%       'whole+small' = create a two layer mask with one layer small
%                       circular mask and the other layer with holes around
%                       (only work for method 'multiphase')
%   Types of methods
%       'chen'      = general CV method
%       'vector'    = CV method for vector image
%       'multiphase'= CV method for multiphase (2 phases applied here)
%
%   output:
%       phi0        = updated level set function
%
%--------------------------------------------------------------------------
%
% Description: This code implements the paper: "Active Contours Without
% Edges" by Chan and Vese for method 'chen', the paper:"Active Contours Without
% Edges for vector image" by Chan and Vese for method 'vector', and the paper
% "A Multiphase Level Set Framework for Image Segmentation Using the
% Mumford and Shah Model" by Chan and Vese.
%
%--------------------------------------------------------------------------
% Deomo: Please see HELP file for details
%==========================================================================

function [seg, phi0]= chenvese(I,mask,num_iter,mu,method)

%%
%-- Default settings
%   length term mu = 0.2 and default method = 'chan'
if(~exist('mu','var'))
    mu=0.2;
end

if(~exist('method','var'))
    method = 'chan';
end

%-- End default settings

%%
%-- Initializations on input image I and mask
%  resize original image
row=size(I,1);
col=size(I,2);
s = 200./min(size(I,1),size(I,2)); % resize scale
if s<1
    I = imresize(I,s);
end

%   auto mask settings
if ischar(mask)
    switch lower (mask)
        case 'small'
            mask = maskcircle2(I,'small');
        case 'medium'
            mask = maskcircle2(I,'medium');
        case 'large'
            mask = maskcircle2(I,'large');
        case 'whole'
            mask = maskcircle2(I,'whole');
            %mask = init_mask(I,30);
        case 'whole+small'
            m1 = maskcircle2(I,'whole');
            m2 = maskcircle2(I,'small');
            mask = zeros(size(I,1),size(I,2),2);
            mask(:,:,1) = m1(:,:,1);
            mask(:,:,2) = m2(:,:,2);
        otherwise
            error('unrecognized mask shape name (MASK).');
    end
else
    if s<1
        mask = imresize(mask,s);
    end
    if size(mask,1)>size(I,1) || size(mask,2)>size(I,2)
        error('dimensions of mask unmathch those of the image.')
    end
    switch lower(method)
        case 'multiphase'
            if  (size(mask,3) == 1)
                error('multiphase requires two masks but only gets one.')
            end
    end

end

switch lower(method)
    case 'chan'
        if size(I,3)== 3
            P = rgb2gray(uint8(I));
            P = double(P);
        elseif size(I,3) == 2
            P = 0.5.*(double(I(:,:,1))+double(I(:,:,2)));
        else
            P = double(I);
        end
        layer = 1;

    case 'vector'
        s = 200./min(size(I,1),size(I,2)); % resize scale
        I = imresize(I,s);
        mask = imresize(mask,s);
        layer = size(I,3);
        if layer == 1
            display('only one image component for vector image')
        end
        P = double(I);

    case 'multiphase'
        layer = size(I,3);
        if size(I,1)*size(I,2)>200^2
            s = 200./min(size(I,1),size(I,2)); % resize scale
            I = imresize(I,s);
            mask = imresize(mask,s);
        end

        P = double(I);  %P store the original image
    otherwise
        error('!invalid method')
end
%-- End Initializations on input image I and mask

%%
%--   Core function
switch lower(method)
    case {'chan','vector'}
        %-- SDF
        %   Get the distance map of the initial mask

        mask = mask(:,:,1);
        phi0 = bwdist(mask)-bwdist(1-mask)+im2double(mask)-.5;
        %   initial force, set to eps to avoid division by zeros
        force = eps;
        %-- End Initialization

        %-- Display settings
        figure();
        subplot(2,2,1); imshow(I); title('Input Image');
        subplot(2,2,2); contour(flipud(phi0), [0 0], 'r','LineWidth',1); title('initial contour');
        subplot(2,2,3); title('Segmentation');
        %-- End Display original image and mask

        %-- Main loop
        for n=1:num_iter
            inidx = find(phi0>=0); % frontground index
            outidx = find(phi0<0); % background index
            force_image = 0; % initial image force for each layer
            for i=1:layer
                L = im2double(P(:,:,i)); % get one image component
                c1 = sum(sum(L.*Heaviside(phi0)))/(length(inidx)+eps); % average inside of Phi0
                c2 = sum(sum(L.*(1-Heaviside(phi0))))/(length(outidx)+eps); % verage outside of Phi0
                force_image=-(L-c1).^2+(L-c2).^2+force_image;
                % sum Image Force on all components (used for vector image)
                % if 'chan' is applied, this loop become one sigle code as a
                % result of layer = 1
            end

            % calculate the external force of the image
            force = mu*kappa(phi0)./max(max(abs(kappa(phi0))))+1/layer.*force_image;

            % normalized the force
            force = force./(max(max(abs(force))));

            % get stepsize dt
            dt=0.5;

            % get parameters for checking whether to stop
            old = phi0;
            phi0 = phi0+dt.*force;
            new = phi0;
            indicator = checkstop(old,new,dt);
%             if n==1
%                 figure;
%                 showphi(I,phi0,n);
%             end
%

            % intermediate output

            if(mod(n,20) == 0)
                showphi(I,phi0,n);
            end;
            if indicator % decide to stop or continue
                showphi(I,phi0,n);

                %make mask from SDF
                seg = phi0<=0; %-- Get mask from levelset
                if s<1
                    seg = imresize(seg,[row col]);
                end

                subplot(2,2,4); imshow(seg); title('Global Region-Based Segmentation');

                return;
            end
        end;
        showphi(I,phi0,n);

        %make mask from SDF
        seg = phi0<=0; %-- Get mask from levelset

        subplot(2,2,4); imshow(seg); title('Global Region-Based Segmentation');
    case 'multiphase'
        %-- Initializations
        %   Get the distance map of the initial masks
        mask1 = mask(:,:,1);
        mask2 = mask(:,:,2);
        phi1=bwdist(mask1)-bwdist(1-mask1)+im2double(mask1)-.5;%Get phi1 from the initial mask 1
        phi2=bwdist(mask2)-bwdist(1-mask2)+im2double(mask2)-.5;%Get phi1 from the initial mask 2

        %-- Display settings
        figure();
        subplot(2,2,1);
        if layer ~= 1
            imshow(I); title('Input Image');
        else
            imagesc(P); axis image; colormap(gray);title('Input Image');
        end
        subplot(2,2,2);
        hold on
        contour(flipud(mask1),[0,0],'r','LineWidth',2.5);
        contour(flipud(mask1),[0,0],'x','LineWidth',1);
        contour(flipud(mask2),[0,0],'g','LineWidth',2.5);
        contour(flipud(mask2),[0,0],'x','LineWidth',1);
        title('initial contour');
        hold off
        subplot(2,2,3); title('Segmentation');
        %-- End display settings

        %Main loop
        for n=1:num_iter
            %-- Narrow band for each phase
            nb1 = find(phi1<1.2 & phi1>=-1.2); %narrow band of phi1
            inidx1 = find(phi1>=0); %phi1 frontground index
            outidx1 = find(phi1<0); %phi1 background index

            nb2 = find(phi2<1.2 & phi2>=-1.2); %narrow band of phi2
            inidx2 = find(phi2>=0); %phi2 frontground index
            outidx2 = find(phi2<0); %phi2 background index
            %-- End initiliazaions on narrow band

            %-- Mean calculations for different partitions
            %c11 = mean (phi1>0 & phi2>0)
            %c12 = mean (phi1>0 & phi2<0)
            %c21 = mean (phi1<0 & phi2>0)
            %c22 = mean (phi1<0 & phi2<0)

            cc11 = intersect(inidx1,inidx2); %index belong to (phi1>0 & phi2>0)
            cc12 = intersect(inidx1,outidx2); %index belong to (phi1>0 & phi2<0)
            cc21 = intersect(outidx1,inidx2); %index belong to (phi1<0 & phi2>0)
            cc22 = intersect(outidx1,outidx2); %index belong to (phi1<0 & phi2<0)

            f_image11 = 0;
            f_image12 = 0;
            f_image21 = 0;
            f_image22 = 0; % initial image force for each layer

            for i=1:layer
                L = im2double(P(:,:,i)); % get one image component

                if isempty(cc11)
                    c11 = eps;
                else
                    c11 = mean(L(cc11));
                end

                if isempty(cc12)
                    c12 = eps;
                else
                    c12 = mean(L(cc12));
                end

                if isempty(cc21)
                    c21 = eps;
                else
                    c21 = mean(L(cc21));
                end

                if isempty(cc22)
                    c22 = eps;
                else
                    c22 = mean(L(cc22));
                end

                %-- End mean calculation

                %-- Force calculation and normalization
                % force on each partition

                f_image11=(L-c11).^2.*Heaviside(phi1).*Heaviside(phi2)+f_image11;
                f_image12=(L-c12).^2.*Heaviside(phi1).*(1-Heaviside(phi2))+f_image12;
                f_image21=(L-c21).^2.*(1-Heaviside(phi1)).*Heaviside(phi2)+f_image21;
                f_image22=(L-c22).^2.*(1-Heaviside(phi1)).*(1-Heaviside(phi2))+f_image22;
            end

            % sum Image Force on all components (used for vector image)
            % if 'chan' is applied, this loop become one sigle code as a
            % result of layer = 1

            % calculate the external force of the image

            % curvature on phi1
            curvature1 = mu*kappa(phi1);
            curvature1 = curvature1(nb1);
            % image force on phi1
            fim1 = 1/layer.*(-f_image11(nb1)+f_image21(nb1)-f_image12(nb1)+f_image22(nb1));
            fim1 = fim1./max(abs(fim1)+eps);

            % curvature on phi2
            curvature2 = mu*kappa(phi2);
            curvature2 = curvature2(nb2);
            % image force on phi2
            fim2 = 1/layer.*(-f_image11(nb2)+f_image12(nb2)-f_image21(nb2)+f_image22(nb2));
            fim2 = fim2./max(abs(fim2)+eps);

            % force on phi1 and phi2
            force1 = curvature1+fim1;
            force2 = curvature2+fim2;
            %-- End force calculation

            % detal t
            dt = 1.5;

            old(:,:,1) = phi1;
            old(:,:,2) = phi2;

            %update of phi1 and phi2
            phi1(nb1) = phi1(nb1)+dt.*force1;
            phi2(nb2) = phi2(nb2)+dt.*force2;

            new(:,:,1) = phi1;
            new(:,:,2) = phi2;

            indicator = checkstop(old,new,dt);

            if indicator
                showphi(I, new, n);
                %make mask from SDF
                seg11 = (phi1>0 & phi2>0); %-- Get mask from levelset
                seg12 = (phi1>0 & phi2<0);
                seg21 = (phi1<0 & phi2>0);
                seg22 = (phi1<0 & phi2<0);

                se = strel('disk',1);
                aa1 = imerode(seg11,se);
                aa2 = imerode(seg12,se);
                aa3 = imerode(seg21,se);
                aa4 = imerode(seg22,se);
                seg = aa1+2*aa2+3*aa3+4*aa4;
                if s<1
                    seg = imresize(seg,[row col]);
                end
                subplot(2,2,4); imagesc(seg);axis image;title('Global Region-Based Segmentation');

                return
            end
            % re-initializations
            phi1 = reinitialization(phi1, 0.6);%sussman(phi1, 0.6);%
            phi2 = reinitialization(phi2, 0.6);%sussman(phi2,0.6);

            %intermediate output
            if(mod(n,20) == 0)
                phi(:,:,1) = phi1;
                phi(:,:,2) = phi2;
                showphi(I, phi, n);
            end;
        end;
        phi(:,:,1) = phi1;
        phi(:,:,2) = phi2;
        showphi(I, phi, n);
        %make mask from SDF
        seg11 = (phi1>0 & phi2>0); %-- Get mask from levelset
        seg12 = (phi1>0 & phi2<0);
        seg21 = (phi1<0 & phi2>0);
        seg22 = (phi1<0 & phi2<0);

        se = strel('disk',1);
        aa1 = imerode(seg11,se);
        aa2 = imerode(seg12,se);
        aa3 = imerode(seg21,se);
        aa4 = imerode(seg22,se);
        seg = aa1+2*aa2+3*aa3+4*aa4;
        %seg = bwlabel(seg);
        subplot(2,2,4); imagesc(seg);axis image;title('Global Region-Based Segmentation');

end
if s<1
    seg = imresize(seg,[row col]);
end
end


function rVectECEF = getrVectEcefFromLatLongAlt(lat, long, alt, bodyInfo)
    r = bodyInfo.radius + alt;

    x = r.*cos(lat).*cos(long);
    y = r.*cos(lat).*sin(long);
    z = r.*sin(lat);

    rVectECEF = [x(:)';y(:)';z(:)'];
end