function [Z,L,obj,err,iter] = latlrr(X,lambda,opts)

% Solve the Latent Low-Rank Representation by M-ADMM
%
% min_{Z,L,E} ||Z||_*+||L||_*+lambda*loss(E),
% s.t., XZ+LX-X=E.
% loss(E) = ||E||_1 or 0.5*||E||_F^2 or ||E||_{2,1}
% ---------------------------------------------
% Input:
%       X       -    d*n matrix
%       lambda  -    >0, parameter
%       opts    -    Structure value in Matlab. The fields are
%           opts.loss       -   'l1' (default): loss(E) = ||E||_1
%                               'l2': loss(E) = 0.5*||E||_F^2
%                               'l21': loss(E) = ||E||_{2,1}
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       Z       -    n*n matrix
%       L       -    d*d matrix
%       E       -    d*n matrix
%       obj     -    objective function value
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 19/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;
loss = 'l1';

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'loss');        loss = opts.loss;            end
if isfield(opts, 'tol');         tol = opts.tol;              end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

eta1 = 1.02*2*norm(X,2)^2; % for Z
eta2 = eta1; % for L
eta3 = 1.02*2; % for E

[d,n] = size(X);
E = zeros(d,n);
Z = zeros(n,n);
L = zeros(d,d);
Y = E;

XtX = X'*X;
XXt = X*X';

iter = 0;
for iter = 1 : max_iter
    Lk = L;
    Ek = E;
    Zk = Z;
    % first super block {Z}
    [Z,nuclearnormZ] = prox_nuclear(Zk-(X'*(Y/mu+L*X-X-E)+XtX*Z)/eta1,1/(mu*eta1));
    % second super block {L,E}
    temp = Lk-((Y/mu+X*Z-Ek)*X'+Lk*XXt-XXt)/eta2;
    [L,nuclearnormL] = prox_nuclear(temp,1/(mu*eta2));
    if strcmp(loss,'l1')
        E = prox_l1(Ek+(Y/mu+X*Z+Lk*X-X-Ek)/eta3,lambda/(mu*eta3));
    elseif strcmp(loss,'l21')
        E = prox_l21(Ek+(Y/mu+X*Z+Lk*X-X-Ek)/eta3,lambda/(mu*eta3));
    elseif strcmp(loss,'l2')
        E = (Y+mu*(X*Z+Lk*X-X+(eta3-1)*Ek))/(lambda+mu*eta3);
    else
        error('not supported loss function');
    end

    dY = X*Z+L*X-X-E;
    chgL = max(max(abs(Lk-L)));
    chgE = max(max(abs(Ek-E)));
    chgZ = max(max(abs(Zk-Z)));
    chg = max([chgL chgE chgZ max(abs(dY(:)))]);
    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            obj = nuclearnormZ+nuclearnormL+lambda*comp_loss(E,loss);
            err = norm(dY,'fro')^2;
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', obj=' num2str(obj) ', err=' num2str(err)]);
        end
    end

    if chg < tol
        break;
    end
    Y = Y + mu*dY;
    mu = min(rho*mu,max_mu);
end
obj = nuclearnormZ+nuclearnormZ+lambda*comp_loss(E,loss);
err = norm(dY,'fro')^2;

function out = comp_loss(E,loss)

switch loss
    case 'l1'
        out = norm(E(:),1);
    case 'l21'
        out = 0;
        for i = 1 : size(E,2)
            out = out + norm(E(:,i));
        end
    case 'l2'
        out = 0.5*norm(E,'fro')^2;
end


function [ o, x, w ] = en_r2_07_2 ( n )

%*****************************************************************************80
%
%% EN_R2_07_2 implements the Stroud rule 7.2 for region EN_R2.
%
%  Discussion:
%
%    The rule has order O = 2^(N+1) + 4 * N^2.
%
%    The rule has precision P = 7.
%
%    EN_R2 is the entire N-dimensional space with weight function
%
%      w(x) = exp ( - x1^2 - x2^2 ... - xn^2 )
%
%    The rule requires 3 <= N.
%
%    The reference has a typographical error in the description of this rule.
%    The formula:
%
%      (t,t,t,...,t)FS
%
%    should read
%
%      (t,t,0,...,0)FS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971,
%    ISBN: 0130438936,
%    LC: QA311.S85.
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%    3 <= N.
%
%    Output, integer O, the order.
%
%    Output, real X(N,O), the abscissas.
%
%    Output, real W(O), the weights.
%
  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'EN_R2_07_2 - Fatal error!\n' );
    fprintf ( 1, '  3 <= N is required.\n' );
    error ( 'EN_R2_07_2 - Fatal error!' );
  end

  o = 2^(n+1) + 4 * n^2;
  volume = sqrt ( pi^n );

  rho1 = sqrt ( ( n + 2 - sqrt ( 2 * ( n + 2 ) ) ) / 2 );
  rho2 = sqrt ( ( n + 2 + sqrt ( 2 * ( n + 2 ) ) ) / 2 );
  a1 = ( n + 2 + sqrt ( 2 * ( n + 2 ) ) ) / 2 / ( n + 2 );
  a2 = ( n + 2 - sqrt ( 2 * ( n + 2 ) ) ) / 2 / ( n + 2 );

  r = 1.0;
  s = sqrt ( 1 / n );
  t = sqrt ( 1 / 2 );
  b = ( 8 - n ) * volume / n / ( n + 2 ) / ( n + 4 );
  c = n^3 * volume / 2^n / n / ( n + 2 ) / ( n + 4 );
  d = 4 * volume / n / ( n + 2 ) / ( n + 4 );

  x = zeros ( n, o );
  w = zeros ( o, 1 );

  k = 0;
%
%  2 * 2 * N points.
%
  for i = 1 : n
    k = k + 1;
    x(i,k) = - rho1 * r;
    w(k) = a1 * b;
    k = k + 1;
    x(i,k) = - rho2 * r;
    w(k) = a2 * b;
    k = k + 1;
    x(i,k) = + rho1 * r;
    w(k) = a1 * b;
    k = k + 1;
    x(i,k) = + rho2 * r;
    w(k) = a2 * b;
  end
%
%  2 * 2^N points.
%
  k = k + 1;
  x(1:n,k) = - rho1 * s;
  w(k) = a1 * c;
  k = k + 1;
  x(1:n,k) = - rho2 * s;
  w(k) = a2 * c;
  more = 1;
  while ( more )
    more = 0;
    for i = n : -1 : 1
      if ( x(i,k) < 0.0 )
        k = k + 1;
        x(1:n,k) =     x(1:n,k-2);
        x(i,k)     =   abs ( x(i,k) );
        x(i+1:n,k) = - abs ( x(i+1:n,k) );
        w(k) = a1 * c;
        k = k + 1;
        x(1:n,k) =     x(1:n,k-2);
        x(i,k)     =   abs ( x(i,k) );
        x(i+1:n,k) = - abs ( x(i+1:n,k) );
        w(k) = a2 * c;
        more = 1;
        break;
      end
    end
  end
%
%  2 * 4 * ( N * ( N - 1 ) / 2 ) points.
%
  for i = 1 : n - 1
    for j = i + 1 : n
      k = k + 1;
      x(i,k) = - rho1 * t;
      x(j,k) = - rho1 * t;
      w(k) = a1 * d;
      k = k + 1;
      x(i,k) = - rho1 * t;
      x(j,k) = + rho1 * t;
      w(k) = a1 * d;
      k = k + 1;
      x(i,k) = + rho1 * t;
      x(j,k) = - rho1 * t;
      w(k) = a1 * d;
      k = k + 1;
      x(i,k) = + rho1 * t;
      x(j,k) = + rho1 * t;
      w(k) = a1 * d;
      k = k + 1;
      x(i,k) = - rho2 * t;
      x(j,k) = - rho2 * t;
      w(k) = a2 * d;
      k = k + 1;
      x(i,k) = - rho2 * t;
      x(j,k) = + rho2 * t;
      w(k) = a2 * d;
      k = k + 1;
      x(i,k) = + rho2 * t;
      x(j,k) = - rho2 * t;
      w(k) = a2 * d;
      k = k + 1;
      x(i,k) = + rho2 * t;
      x(j,k) = + rho2 * t;
      w(k) = a2 * d;
    end
  end

  return
end


%% Mean-Shift Video Tracking
% by Sylvain Bernhardt
% July 2008
%% Description
% Measures the similarity between two
% density estimations q and p done with
% a kernel which profile is k.
% q is the estimation of a reference patch
% and p the estimation of a candidate one 'T2'
% which size is H,W.
% The outputs are the similarity value f
% and the weight mask w for the gradient ascent
% in the extended Mean-Shift algorithm.
%
% [f,w] = Simil_func(q,p,T2,k,H,W)

function [f,w] = Simil_func(q,p,T2,k,H,W)

w = zeros(H,W);
f = 0;
for i=1:H
    for j=1:W
        w(i,j) = sqrt(q(T2(i,j)+1)/p(T2(i,j)+1));
        f = f+w(i,j)*k(i,j);
    end
end
% Normalization of f
f = f/(H*W);


function [varargout] = slick(coords)
    % turned into MATLAB from C by Celso G Reyes
    % accepts input of [DipDir, Dip, Rake]
    % returns different items, based on varargout.

    % answer is CLOSE to original, but does not match.  Sig Figs? Technique?

    TORADS = 57.29577951;

    % COORDINATES ARE EAST,NORTH,UP
    % this version does no statistics
    % and therefore makes no plot

    assert(size(coords,2) == 3); % [ ddir, dip, rake ] as ENU

    % ddir = zeros(MAXDATA,1);  % dip direction for data
    % dip = zeros(MAXDATA,1);   % dip of data
    % rake = zeros(MAXDATA,1);  % rake of data
    % amat = zeros(MAX3,5);  % coefficient matrix for normal equation
    % stress = zeros(6,1);  % stress tensor in vector form, element order is:
    % xx,xy,xz,yy,yz,zz
    % slick_vec_el_vec  slickenside vector elements vector
    % norm  % storage of n1,n2,n3
    char name(20);      % output file name
    %FILE *fpin;   % input file pointer
    %FILE *fpout;  % output file pointer
    %FILE *fplot;  % plot file pointer
    % sigma = 0;  % for use with leasq subr
    % a2i = zeros(5,5);  % to get covariance mtrix
    line='';  % character line
    t = zeros(3,1);  % shear stress vector
    % iso = 0;  % isotropic stress mag
    %angavg = 0; angstd = 0;  % average and standard deviation of fit angle
    %isoavg = 0; isostd = 0;  % same for isotropic stress size
    magavg = 0; magstd = 0;  % same for tangential stress size
    % tf = zeros(3,1), tnorm;  % full traction vector
    % and normal traction

    % get file pointers
    %{
    -- argc;
    ++argv;
    if argc == 0
        printf("usage: slick data_file\n");
        return;
    end
    fpin = fopen(argv,"r");
    if fpin==NULL
        printf("unable to open %s.\n",argv);
        return;
    end
    fprintf(name,"%s.oput",argv);
    %}
    % read and write comment line from data file to output file
    % fgets(line,80,fpin);
    line = 'Inversion data';
    fpout = string(line);
    ddir = coords(:,1);
    dip = coords(:,2);
    rake = coords(:,3);
    % loop to get data and make up equation
    for i=1:size(coords,1)
        %ddir = coords(i,1); dip = coords(i,2); rake = coords(i,3);
        j = 3*i;%?

        z = ddir(i)/TORADS;
        z2 = dip(i)/TORADS;
        z3 = rake(i)/TORADS;

        %n1 to n3 are normal vector elements
        n1 = sin(z)*sin(z2);  % normal vector to fault plane
        n2 = cos(z)*sin(z2);
        n3 = cos(z2);

        norm(i,1:3) = [n1 n2 n3];

        % slickenside vector calculation
        slick_vec_el_vec(j,1)= -cos(z3)*cos(z) - sin(z3)*sin(z)*cos(z2);
        slick_vec_el_vec(j+1,1)= cos(z3)*sin(z) - sin(z3)*cos(z)*cos(z2);
        slick_vec_el_vec(j+2,1)= sin(z3)*sin(z2);

        % find the matrix elements
        amat(j:j+2, 1:5) = [...
            n1-n1*n1*n1+n1*n3*n3,  n2-2.*n1*n1*n2, n3-2.*n1*n1*n3, -n1*n2*n2+n1*n3*n3,    -2.*n1*n2*n3 ;...
            -n2*n1*n1+n2*n3*n3,    n1-2.*n1*n2*n2, -2.*n1*n2*n3,   n2-n2*n2*n2+n2*n3*n3,  n3-2.*n2*n2*n3;...
            -n3*n1*n1-n3+n3*n3*n3, -2.*n1*n2*n3,   n1-2.*n1*n3*n3, -n3*n2*n2-n3+n3*n3*n3, n2-2.*n2*n3*n3];

        % check to see if all possible data has been read
    end  % end of data read loop

	% solve equations via linear least squares
	[stress, sigma]=leasq(amat,slick_vec_el_vec);
	% correct zz element by using trace = 0
	stress(6)= -(stress(1)+stress(4));

	% put stress tensor into tensor form
    strten = [  stress(1),    stress(2),   stress(3) ;
                stress(2),    stress(4),   stress(5) ;
                stress(3),    stress(5),   stress(6)];

    %fpout(end+1)=sprintf("\nCOORDINATES ARE EAST,NORTH,UP.");
    %fpout(end+1)=sprintf("stress tensor is:");
    for i=1:3
    %    fpout(end+1)=sprintf("%g  %g  %g  ",strten(i,:));
    end

	% find  eigenvalues and eigenvectors
    [vecs,lam] = eig(strten,"vector"); %LAM is eigenvalues, VECS is eigenvectors
    % eigen(strten,lam,vecs);
    %fpout(end+1)=sprintf("eigenvalue   vector: E,N,UP,direction,plunge");
    for i = 3:-1:1
        [v_direction(i), v_plunge(i)] = dirplg(vecs(1,i),vecs(2,i),vecs(3,i));
        %[z, z2] = dirplg(vecs(1,i),vecs(2,i),vecs(3,i));
        %fpout(end+1)=sprintf("%g  ",lam(i)) +...
        %    sprintf("%g  %g  %g  ",vecs(:,i)) +...
        %    sprintf("%f  %f",z,z2);
    end
    %fpout(end+1)=sprintf("variance= %g",sigma);

	% order eigenvalues and compute phi
    %lam=sort(lam,'descend');

    if lam(1) ~= lam(3)
        phi = (lam(2)-lam(3)) / (lam(1)-lam(3));
        %fpout(end+1)=sprintf("phi value= %g",phi);
    else
        phi=nan;
    end
	% output data and fit angle

	angavg = 0.;
	angstd = 0.;
	isoavg = 0.;
	isostd = 0.;
	iso = 0.;
	%fpout(end+1)=sprintf("\ndip direction, dip, rake, fit angle, mag tau");
    nobs=size(coords,1);
    for i=1:nobs %from 0

        for j= 1 : 3 %from 0  % compute shear traction
            t(j)=0;
            tf(j)=0;
            myt(j) = sum(amat(3*(i-1)+j) * stress(1:5));
            for k=1:5
                t(j) = t(j)+ amat(3*(i-1)+j,k) * stress(k);
            end
            for k=1:3
                tf(j) = tf(j) + strten(j,k) * norm(i,k);
            end
        end
        tnorm = 0;
        for k=1:3
            tnorm = tnorm + tf(k) * norm(i,k);
        end
        % find angle between t and slickenside
        z = 0.;
        for j=1:3
            z = z + t(j)*slick_vec_el_vec(3*(i-1)+j);
        end
        z2 = 0.;
        for j=1:3
            z2 = z2 + t(j)*t(j);
        end
        z2 = sqrt(z2);
        z3 = 0.;
        for j=1:3
            z3 = z3 + slick_vec_el_vec(3*(i-1)+j)*slick_vec_el_vec(3*(i-1)+j);
        end
        z3 = sqrt(z3);
        z = z/(z2*z3);
        z = acos(z)*TORADS;
        angavg = angavg + z;
        angstd = angstd + z*z;
        z3= (z2/(-0.8)) - tnorm;
        iso = iso + abs(tnorm);
        isoavg = isoavg +z3;
        isostd = isostd + z3*z3;
        magavg = magavg +z2;
        magstd = magstd + z2*z2;
        %fpout(end+1)=sprintf("%7.1f  %7.1f  %7.1f  %7.1f %7.2f", ddir(i),dip(i),rake(i),z,z2);
    end
    z3 = nobs-1;
    angstd = angstd-(angavg*angavg/nobs);
    angstd = angstd/z3;
    angstd = sqrt(angstd);
    angavg = angavg/nobs;

    isostd = isostd-(isoavg*isoavg/nobs);
    isostd = isostd/z3;
    isostd = sqrt(isostd);
    isoavg = isoavg/nobs;
    iso = iso / nobs;
    isoavg = isoavg / iso;
    isostd = isostd / iso;

    magstd = magstd-(magavg*magavg/nobs);
    magstd = magstd/z3;
    magstd = sqrt(magstd);
    magavg = magavg/nobs;

    %fpout(end+1)=sprintf("fit angle mean= %f standard deviation= %f",angavg,angstd);
    %fpout(end+1)=sprintf("for f=0.8 I= %f , std. dev.= %f D norm= %f", isoavg,isostd,iso);
    %fpout(end+1)=sprintf("avg tau= %f , std. dev.= %f",magavg,magstd);

    if nargout==1
        fpout(end+1)=sprintf("fit angle mean= %f standard deviation= %f",angavg,angstd);
        fpout(end+1)=sprintf("for f=0.8 I= %f , std. dev.= %f D norm= %f", isoavg,isostd,iso);
        fpout(end+1)=sprintf("avg tau= %f , std. dev.= %f",magavg,magstd);

    varargout = {strjoin(fpout,newline)};
    elseif nargout == 5
        varargout={angavg, angstd, magstd/magavg, magavg, magstd}; %[fBeta2, fStdBeta2, fTauFit2, fAvgTau2, fStdTau2]
    elseif nargout == 9
        varargout=fastoutput();
    end

    function output = fastoutput()
        %%line 1
        % variance
        output = {... line 1 of output file
            sigma,... variance
            stress,...      stress tensor upper triangle
            ... line 2 of output file
            phi,...
            round(v_direction(1),1),...
            round(v_plunge(1),1),...
            round(v_direction(2),1),...
            round(v_plunge(2),1),...
            round(v_direction(3),1),...
            round(v_plunge(3),1)...
            };
    end

end

function [pdir, pplg]  = dirplg(e,n,u)
    % dirplb to find direction and plunge of a vector
    % double e,n,u; /* the vector in east,north,up coordinates
    % double *pdir,*pplg are pointers to the direction in east of north
    % and the plunge down the direction

    TORADS = 57.29577951;

    z=e*e+n*n;
    z=sqrt(z);
    pplg=atan2(-u,z) * TORADS;
    if pplg<0
        pplg= -pplg;
        e= -e;
        n= -n;
    end
    pdir=atan2(e,n) * TORADS;
end

function [x, psis] = leasq(a,b)
% /* finds the least squares solution of ax=b */
%{
double a[]; /* the coefficients matrix with n rows and m columns */
double x[]; /* the solution vector of length m */
double b[]; /* the constant vector of length n */
double a2[]; /* a square matrix of size m for internal use */
double c[]; /* vector of length m for internal use */
double *psis; /*pointer to the variance */

/* steps 1 a2= a transpose a */
/*       2 c= a transpose b */
/*       3 solve a2x=c by gaussian elimination */
%}

	% a2=atransa(a,a2); % computes b=a transpose*a
    a2 = a' * a;
    c = a' * b;
    x = a2 \ c;
	% x = gaus(a2, c); % solves ax=b for x by gaussian elimination
	psis = sigsq(a,x,b);
end
%{
function x = gaus(a,b)
    % /* solves ax=b for x by gaussian elimination */

%double a[];  /* a square matrix of size m */
%double b[];  /* a vector of length m */
%double x[];  /* a vector of length m */

    m=length(a);
    %  take care of special cases */
    if m<2
        x=0;
        if m==1
            x=b/a;
        end
        return
    end

	for i=1:m % (i=0;i<m;++i)   %  /* loop for each pivot */
		for i2=i+1:m %  /* loop for each row below a pivot */

			% /* see if element below pivot is 0 */
			if(a(i2,i)==0.)
                continue
            end

			% /* if element below pivot > pivot flop rows  */
			if(abs(a(i2,i))>abs(a(i,i)))
				hold=b[i];
				b[i]=b[i2];
				b[i2]=hold;
				for(i3=i;i3<m;++i3)
					hold=a(i,i3);
					a(i,i3)=a(i2,i3);
					a(i2,i3)=hold;
                end
            end

			% /* do the elimination */
			fact=a(i2,i)/a(i,i);
			a(i2,i)=0.;
			for(i3=i+1;i3<m;++i3)
                a(i2,i3)=a(i2,i3)-fact*a(i,i3);
            end
			b[i2]=b[i2]-fact*b[i];

        end
    end

	/* solve the equations */
	x[m-1]=b[m-1]/a[m*m-1];
	for(i=m-2;i> -1;--i){
		d=b[i];
		for(i2=i+1;i2<m;++i2)
            d=d-x[i2]*a(i,i2);
        end
		x[i]=d/a(i,i);
	}
	return;
}

end
%}

function psis = sigsq(a,x,b)
% SIGSQ  computes the variance of a single observation */
% double a[];    /* matrix of n rows and m columns */%
% double b[];    /* data vector length n*/
% double x[];    /* solution vector length m */
% double *psis;  /* where to put answer */
% short m,n;     /* see above */

	% double y,z,z2;     /* sum variables */

    [n,m] = size(a);
    allY = a * x; % nx1
    z=sum(b-allY).^2;

	if n ~= m
		z2 = n - m;
		z = z/z2;
    end
	psis=z;
end


function q=roteu2qr(m,t)
%ROTEU2QR converts a sequence of euler angles to a real unit quaternion
% Inputs:
%
%     M(1,n)   a string of n characters from the set {'x','y','z'}
%              or, equivalently, a vector whose elements are 1, 2, or 3
%     T(1,n)   n rotation angles
%
% Outputs:
%
%     Q(1,4)   output quaternion
%
% The string M specifies the axes about which the rotations are performed.
% You cannot have the same axis in adjacent positions and so there are 12
% possibilities. Common ones are "ZXZ" and "ZYX". A positive rotation is clockwise
% if looking along the axis away from the origin.

% Suggestions:
%   (1) Should allow 1,2,3 as well as x,y,z to specify the axes

%
%      Copyright (C) Mike Brookes 2007
%      Version: $Id: roteu2qr.m,v 1.2 2007/11/21 12:42:36 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y=[2 4 1 3 1 3 2 4; 3 2 1 4 1 4 3 2; 3 4 2 1 1 2 4 3];
% m consists of a sequence of axes e.g. 'zxy'
% and t gives the rotation angles in radians
q=[1 0 0 0]';
if ischar(m)
    m=lower(m)-'w';
end
if any(abs(m-2)>1), error('Euler axis must be x,y or z'); end
for i=1:length(m)
    x=y(m(i),:);
    b=0.5*t(i);
    c=cos(b);
    s=sin(b);
    r=zeros(4,1);
    r(x(1:2))=q(x(3:4));
    r(x(5:6))=-q(x(7:8));
    q=c*q+s*r;
end
f=find(q~=0);
if (q(f(1))<0), q=-q; end


function [xopt,fval,exitflag,output] = spcgsearch(z, xbox, options)
% SPCGSEARCH  Optimizes the sparse grid interpolant using the CG
%    method.
%    X = SPCGSEARCH(Z) Starts search at the best available
%    sparse grid point and attempts to find a local minimizer of the
%    sparse grid interpolant Z. The entire range of the sparse
%    grid interpolant is searched.
%
%    X = SPCGSEARCH(Z,XBOX)  Uses the search box XBOX = [a1,
%    b1; a2, b2; ...]. The size of search box XBOX must be smaller
%    than or equal to the range of the interpolant.
%
%    X = SPCGSEARCH(Z,XBOX,OPTIONS)  Minimizes with the default
%    optimization parameters replaced by values in the structure
%    OPTIONS, created with the SPOPTIMSET function.  See SPOPTIMSET
%    for details.
%
%    [X,FVAL] = SPCGSEARCH(...)  Returns the value of the
%    sparse grid interpolant at X.
%
%    [X,FVAL,EXITFLAG] = SPCGSEARCH(...)  Returns an EXITFLAG
%    that describes the exit condition of SPCGSEARCH. Possible
%    values of EXITFLAG and the corresponding exit conditions are
%
%    1  SPCGSEARCH converged to a solution X.
%    0  Maximum number of function evaluations or iterations
%       reached.
%
%    [X,FVAL,EXITFLAG,OUTPUT] = SPCGSEARCH(...) Returns a
%    structure OUTPUT with the number of function evaluations in
%    OUTPUT.nFEvals, the number of gradients in .nGradEvals,
%    and the computing time in .time.
%
%    Example: (minimizing the three-hump camel-back function)
%      f = inline('12*x.^2-6.3*x.^4+x.^6+6*y*(y-x)');
%      range = [-3 3; -3 3];
%      options = spset('keepFunctionValues','on', ...
%                      'GridType', 'Chebyshev', ...
%                      'DimensionAdaptive', 'on', ...
%                      'DimAdaptDegree', 1, ...
%                      'MinPoints', 10);
%      z = spvals(f, 2, range, options)
%      [xopt, fval] = spcgsearch(z)
%
%    See also SPOPTIMSET.

% Author : Andreas Klimke
% Version: 1.0
% Date   : September 1, 2007

% Change log:
% V1.0   : September 1, 2007
%          Initial version.

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

t0 = clock;

if nargin < 2, xbox = []; end
if nargin < 3, options = []; end

d = z.d;

% In case that no range has been provided to spvals -> set it to
% [0,1]^d.
range = z.range;
if isempty(range)
	range = [zeros(d,1) ones(d,1)];
end
if isempty(xbox)
	xbox = range;
end

% Break if maximize is set; not yet supported.
maximize = spoptimget(options, 'Maximize', 'off');
minimize = spoptimget(options, 'Minimize', 'on');
if strcmpi(maximize, 'on')
  warning('MATLAB:spinterp:unsupported',['spcgsearch ' ...
	  'does currently not support searching for maxima. ' ...
		'Search for local maximum is skipped.']);
	options = spoptimset(options, 'Maximize', 'off');
  if strcmpi(minimize, 'off')
    xopt = NaN;
	  fval = NaN;
	  exitflag = 0;
	  if nargout == 4
		  output.nFEvals = 0;
		  output.nGradEvals = 0;
		  output.time = etime(clock, t0);
	  end
	  return;
  end
end

% Determine optimization start point
[x, fval] = spgetstartpoint(z, xbox, options);
fprev = fval;

maxiter = spoptimget(options, 'MaxIter', 100);
tolfun  = spoptimget(options, 'TolFun', 1e-6);
dispopt = spoptimget(options, 'Display', 'off');
[isdispiter, iterstr] = initoptidisp(dispopt);

if isfield(z,'selectOutput')
	numout = z.selectOutout;
else
	numout = 1;
end
abstol = (z.fevalRange(numout,2) - z.fevalRange(numout,1)).*100*eps;

% Default tolx is computed from range times floating point accuracy.
tolxvec = eps * (range(:,2) - range(:,1));

% Define step size variable; initial value will be computed by
% spminbracket.
stepsize = [];

% Do a maximum of 50 inner iterations
brentopt = spoptimset('MaxIter',50,'TolFun',tolfun);

[dummy, gf] = spsurfun(x,z);

nfevals = 1; ngradevals = 1;
if isdispiter, disp(sprintf(iterstr, 0, 1, 1, fprev, 'start point')); end
neggf  = -gf;
xi = neggf;
h  = neggf;

exitflag = 0;
for k = 1:maxiter
	% Check if gradient is zero
	gg  = dot(neggf,neggf);
	if gg == 0.0
		exitflag = 1;
		break;
	end
	% Check if new search direction is all-zero vector
	if dot(xi,xi) == 0.0
		exitflag = 1;
		break;
	end
	[xbrac,fxbrac,bflag,p,fp,gfp,addfevals] = ...
	  spminbracket(z,x,fval,-neggf,xi,xbox,[],stepsize);
	nfevals = nfevals + addfevals;

	if bflag == 0 || bflag == 2
		% Compute tolx along the search line
		brentopt.TolX = abs(dot(xi / norm(xi), tolxvec));
		brentopt.gf = -neggf;
		[u,fnext,flag,tempoutput] = spbrent(z,x,xi,xbrac,fxbrac,brentopt);

		% Next line intentionally commented out (AK)
		% if 2.0*(fnext-fprev) <= tolfun * (abs(fnext)+abs(fprev)+eps);

		% Security check that fval has not increased beyond
		% allowed tolerance for break condition
		fval = fnext;
		stepsize = norm(xi * u);
		x = x + xi * u;

		% Next line intentionally commented out (AK)
		% end

		nfevals = nfevals + tempoutput.nFEvals;
		if isdispiter, disp(sprintf(iterstr, k, nfevals, ...
												ngradevals, fval, 'line search')); end
	else
		if isdispiter, disp(sprintf(iterstr, k, nfevals, ...
												ngradevals, fval, 'boundary hit')); end
		stepsize = norm(x-p);
		x = p;
		fval = fp;
		xi = gfp;
		ngradevals = ngradevals + 1;
	end

	if 2.0*abs(fval-fprev) <= max(tolfun * (abs(fval)+abs(fprev)),abstol)
	  exitflag = 1;
	  break;
	end

	fprev = fval;
	if bflag == 0 || bflag == 2
	  [dummy, xi] = spsurfun(x,z);
		nfevals = nfevals + 1;
		ngradevals = ngradevals + 1;
	end
	dgg = dot((xi + neggf),xi);
	neggf = -xi;
	xi = neggf + dgg / gg * h;
	% Adjust search direction for boundary
	for l = 1:d
		if x(l) <= xbox(l,1) + 100*eps*(range(l,2)-range(l,1))
			if neggf(l) < 0
			  xi(l) = 0;
			elseif sign(xi(l)) < 0
			  xi(l) = neggf(l);
		  end
	  end
		if x(l) >= xbox(l,2) - 100*eps*(range(l,2)-range(l,2))
		  if neggf(l) > 0
			  xi(l) = 0;
		  elseif sign(xi(l)) > 0
			  xi(l) = neggf(l);
			end
		end
	end
	h = xi;
end

xopt = x;

% Return stats
if nargout == 4
  output.nFEvals = nfevals;
  output.nGradEvals = ngradevals;
	output.time = etime(clock, t0);
end

if strcmpi(maximize, 'on')
  xopt = [xopt NaN.*ones(size(xopt))];
	fval = [fval NaN];
	exitflag = [exitflag 0];
end


function [] = showSuperquadrics(x, varargin)

R = eul2rotm(x(6 : 8));
t = x(9 : 11);

% with tapering or not
taper = false;
color = 'r';
ViewAxis = [0 0];
CamRoll = 0;
ShowAxis = 0;
arclength = 0.02;
FaceAlpha = 1;
FaceLighting = 'flat';
lighting = false;

for k = 1 : size(varargin, 2)
    if strcmp(varargin{k}, 'Taper')
        taper = varargin{k + 1};
    end
    if strcmp(varargin{k}, 'Color')
        color = varargin{k + 1};
    end
    if strcmp(varargin{k}, 'ViewAxis')
        ViewAxis = varargin{k + 1};
    end
    if strcmp(varargin{k}, 'CamRoll')
        CamRoll = varargin{k + 1};
    end
    if strcmp(varargin{k}, 'ShowAxis')
        ShowAxis= varargin{k + 1};
    end
    if strcmp(varargin{k}, 'Arclength')
        arclength= varargin{k + 1};
    end
    if strcmp(varargin{k}, 'FaceAlpha')
        FaceAlpha= varargin{k + 1};
    end
    if strcmp(varargin{k}, 'FaceLighting')
        FaceLighting= varargin{k + 1};
    end
    if strcmp(varargin{k}, 'Light')
        lighting= varargin{k + 1};
    end
end

% validate dimensionality
if taper == true
    if size(x, 2) ~= 13
        error('Input parameters should have dimension (:, 13) for taperred SQ.')
    end
else
    if size(x, 2) ~= 11
        error('Input parameters should have dimension (:, 11) for taperred SQ.')
    end
end

% avoiding numerical instability of points sampling on superquadrics
if x(1) < 0.01 %0.007
    x(1) = 0.01;
end
if x(2) < 0.01
    x(2) = 0.01;
end

[point_eta] = uniformSampledSuperellipse(x(1), [1, x(5)], arclength);
[point_omega] = uniformSampledSuperellipse(x(2), [x(3), x(4)], arclength);

x_mesh = ones(size(point_omega, 2), size(point_eta, 2));
y_mesh = ones(size(point_omega, 2), size(point_eta, 2));
z_mesh = ones(size(point_omega, 2), size(point_eta, 2));

for m = 1 : size(point_omega, 2)
    for n = 1 : size(point_eta, 2)
        point_temp = [point_omega(:, m) * point_eta(1, n); point_eta(2, n)];

        if taper == true
            fx = x(12) * point_temp(3) / x(5) + 1;
            fy = x(13) * point_temp(3) / x(5) + 1;
            fz = 1;

            point_temp(1) = point_temp(1) * fx;
            point_temp(2) = point_temp(2) * fy;
            point_temp(3) = point_temp(3) * fz;
        end

        point_temp = R * point_temp + t';

        x_mesh(m, n) = point_temp(1);
        y_mesh(m, n) = point_temp(2);
        z_mesh(m, n) = point_temp(3);
    end
end

mesh(x_mesh, y_mesh, z_mesh, 'FaceAlpha', FaceAlpha, 'facecolor', color, ...
    'LineStyle', 'none', 'FaceLighting', FaceLighting)
if lighting == 1
    light
    material dull
end

axis equal
view(ViewAxis)
camroll(CamRoll)

if ShowAxis == 0
    axis off
end

hold off
% ---------------------------------utility functions ----------------------
    function [point, theta] = uniformSampledSuperellipse(epsilon, scale, arclength)
        threshold = 1e-2;
        num_limit = 10000;
        theta = zeros(1, num_limit);
        theta(1) = 0;

        for i = 2 : num_limit
            dt = dtheta(theta(i - 1), arclength, threshold, scale, epsilon);
            theta_temp = theta(i - 1) + dt;

            if theta_temp > pi/4
                break
            else
                if i < num_limit
                    theta(i) = theta_temp;
                else
                    error(['The number of the sampled points exceeds the limit of ', ...
                        num2str(num_limit * 4),...
                        '. Please increase the arclength or raise the limit'])
                end
            end
        end
        critical = i;

        for j = critical + 1 : num_limit
            dt = dtheta(theta(j - 1), arclength, threshold, flip(scale), epsilon);
            theta_temp = theta(j - 1) + dt;

            if theta_temp > pi/4
                break
            else
                if j < num_limit
                    theta(j) = theta_temp;
                else
                    error(['The number of the sampled points exceeds the limit of ', ...
                        num2str(num_limit * 4),...
                        '. Please increase the arclength or raise the limit'])
                end
            end
        end

        num_pt = j - 1;
        theta = theta(1 : num_pt);

        points_fw = angle2points(theta(1 : critical - 1), scale, epsilon);
        points_bw = flip(angle2points(theta(critical : end), flip(scale), epsilon), 2);
        point = [points_fw, [points_bw(2, :); points_bw(1, :)]];

        point = [point, flip([-point(1, 1 : num_pt - 1); point(2, 1 : num_pt - 1)], 2), ...
            [-point(1, 2 : end); -point(2, 2 : end)], flip([point(1, 1 : num_pt - 1); ...
            -point(2, 1 : num_pt - 1)], 2)];

    end

    function [dt] = dtheta(theta, arclength, threshold, scale, sigma)
        if theta < threshold
            dt = abs((arclength / scale(2) + (theta)^(sigma))^(1 / sigma) ...
                - (theta));
        else
            dt = arclength / sigma * ((cos(theta) ^ 2 * sin(theta) ^ 2) / ...
                (scale(1) ^ 2 * cos(theta) ^ (2 * sigma) * sin(theta) ^ 4 + ...
                scale(2) ^ 2 * sin(theta) ^ (2 * sigma) * cos(theta) ^ 4))^(1 / 2);
        end
    end

    function [point] = angle2points(theta, scale, sigma)
        point = zeros(2, size(theta, 2));
        point(1, :) = scale(1) .* sign(cos(theta)) .* abs(cos(theta)).^sigma;
        point(2, :) = scale(2) .* sign(sin(theta)) .* abs(sin(theta)).^sigma;
    end

end


function tparameters=scalar_vt_vech_transform(parameters,p,o,q,kappa)
% SCALAR_VT_VECH(P,Q) parameter transformation.  Used to map parameters
% from a scalar MVGARCH process to the real line. Used in the estimation of SCALAR_VT_VECH.
%
% USAGE:
%   [TPARAMETERS]=scalar_vt_vech_transform(PARAMETERS,P,O,Q,KAPPA)
%
% INPUTS:
%   PARAMETERS       - Column parameter vector
%   P                - Positive, scalar integer representing the number of symmetric innovations
%   Q                - Non-negative, scalar integer representing the number of lags of conditional variance
%
% OUTPUTS:
%   TPARAMETERS      - A 1+p+q column vector of transformed parameters corresponding to
%                      [alpha(1),...,alpha(p), beta1 ... beta(q)]'
%
% COMMENTS:
%   Input parameters must satisfy:
%    (1) alpha(i) >= 0 for i = 1,2,...,p
%    (2) beta(i)  >= 0 for i = 1,2,...,q
%    (3) sum(alpha) + sum(beta) < 1
%
% See also SCALAR_VT_VECH

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 9/1/2005

if size(parameters,2)>size(parameters,1)
   parameters = parameters';
end
%Upper bound to keep it a bit away from 1
UB=.999998;

alpha=parameters(1:p);
gamma = parameters(p+1:p+o);
beta=parameters(p+o+1:p+o+q);
%Check that the parameters satisfy the necessary constraints
if  any(alpha<0) || any(beta<0) || any(gamma<0) || (sum(alpha)+sum(gamma)/kappa+sum(beta))>=UB
    error('These do not conform to the necessary set of restrictions to be transformed.')
end

%Alpha, beta cannot be exactly zero or there will be problems with log()
alpha(alpha<1e-8)=1e-8;
gamma(gamma<1e-8)=1e-8;
beta(beta<1e-8)=1e-8;
%Up the upper bound a small amount to make sure it is satisfied
UB=UB+1e-8*(p+o+q);

%Set the scale
scale=UB;
%Initialize the transformed parameters
parameters=[alpha;gamma;beta];
tparameters=[alpha;gamma;beta];
for i=1:(p)
    %Scale the parameters
    tparameters(i)=tparameters(i)/scale;
    %Use an inverse logistic
    tparameters(i)=log(tparameters(i)/(1-tparameters(i)));
    %Update the scale
    scale=scale-parameters(i);
end
for i=p+1:p+o
    %Scale the parameters
    tparameters(i)=tparameters(i)/scale/kappa;
    %Use an inverse logistic
    tparameters(i)=log(tparameters(i)/(1-tparameters(i)));
    %Update the scale
    scale=scale-parameters(i)/kappa;
end

for i=p+o+1:p+o+q
    %Scale the parameters
    tparameters(i)=tparameters(i)/scale;
    %Use an inverse logistic
    tparameters(i)=log(tparameters(i)/(1-tparameters(i)));
    %Update the scale
    scale=scale-parameters(i);
end


function fem2d_pack_test16 ( )

%*****************************************************************************80
%
%% TEST16 tests REFERENCE_TO_PHYSICAL_T6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  n = 16;

  ref = [ ...
    0.00, 0.00; ...
    1.00, 0.00; ...
    0.00, 1.00; ...
    0.50, 0.00; ...
    0.50, 0.50; ...
    0.00, 0.50; ...
    0.25, 0.75; ...
    0.75, 0.25; ...
    0.40, 0.10; ...
    0.30, 0.20; ...
    0.20, 0.30; ...
    0.10, 0.40; ...
    0.10, 0.10; ...
    0.20, 0.20; ...
    0.30, 0.30; ...
    0.40, 0.40 ]';
  t = [ ...
    0.0, 0.0; ...
    2.0, 0.0; ...
    0.0, 4.0; ...
    1.0, 0.0; ...
    1.0, 1.0; ...
    0.0, 2.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST16\n' );
  fprintf ( 1, '  For an order 6 triangle,\n' );
  fprintf ( 1, '  REFERENCE_TO_PHYSICAL_T6 maps a reference point to\n' );
  fprintf ( 1, '    a physical point.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      XSI     ETA  ==>  X	Y\n' );
  fprintf ( 1, '\n' );

  phy = reference_to_physical_t6 ( t, n, ref );

  for j = 1 : n
    fprintf ( 1, '  %8f  %8f  %8f  %8f\n', ref(1:2,j), phy(1:2,j) );
  end

  return
end


function denoise_PRL_2010_main
%
% This is a demo program of the paper J. Tian, W. Yu, and L. Ma, "AntShrink: Ant
% colony optimization for image shrinkage," Pattern Recognition Letters,
% Vol. 31, Oct. 2010, pp. 1751-1758.
%
% Contact: eejtian@gmail.com
%
% Note that the PSNR values could be slightly different with that reported
% in paper due to the random number generator used to generate noisy image.
%
% The input image should have a square size.
%
% Acknowledgement: This program needs Wavelet transform toolbox, which is
% downloaded from http://www-stat.stanford.edu/~wavelab/.

clear all; close all; clc;

% Load the ground truth image
img_truth = double(imread('barbara_truth.bmp'));
[nRow, nColumn] = size(img_truth);

% Use the ground truth image to generate the noisy image
noise_sig_truth = 10; % sigma_n used in the paper. This parameter is adjusted by the user.
noise_mu = 0;
img_noisy = img_truth + randn(size(img_truth)) .* noise_sig_truth + noise_mu;

% wavelet parameters
wbase = 'Daubechies';
mom = 8;
dwt_level = 5; %note that here, dwt_scale means the decomposition level of the DWT
[n,J] = func_quadlength(img_truth);
L = J-dwt_level;%here, L means the size of the coarsest level, 2^L

win_size = 2;
img_denoised = zeros(size(img_noisy));

% Since this is time consuming approach, divide the image into four parts, then
% conduct denoising for each part
for ii=1:4
    win_size=2;

    switch ii
        case 1
            img_denoised(1:end/2,1:end/2) = func_ACOShrink(img_noisy(1:end/2,1:end/2), wbase, mom, dwt_level, win_size);
        case 2
            img_denoised(end/2+1:end,1:end/2) = func_ACOShrink(img_noisy(end/2+1:end,1:end/2), wbase, mom, dwt_level, win_size);
        case 3
            img_denoised(1:end/2,end/2+1:end) = func_ACOShrink(img_noisy(1:end/2,end/2+1:end), wbase, mom, dwt_level, win_size);
        case 4
            img_denoised(end/2+1:end,end/2+1:end) = func_ACOShrink(img_noisy(end/2+1:end,end/2+1:end), wbase, mom, dwt_level, win_size);
    end

end

% Calculate the PSNR performance
fprintf('PSNR=%.2fdB\n', func_psnr_gray(img_truth, img_denoised));

% Write the output image
imwrite(uint8(img_denoised), 'barbara_denoised.bmp','bmp');

%-------------------------------------------------------------------------
%------------------------------Inner Function ----------------------------
%-------------------------------------------------------------------------
% Main algorithm of proposed AntShrink algorithm
function x_out= func_ACOShrink(x_in, wbase, mom, dwt_level, win_size)

[nrow, ncol] = size(x_in);
L = log2(size(x_in,2))-dwt_level;

% Estimate the noise_sigma from the noisy signal
qmf = func_MakeONFilter(wbase,mom);
[temp, coef] = func_NormNoise_2d(x_in, qmf);
noise_sigma = 1/coef;

wx  = func_FWT2_PO(x_in, L, qmf);
[n,J] = func_dyadlength(wx);
ws = wx;

rr = (meshgrid(1:nrow))'; cc = meshgrid(1:ncol);

ant_search_range_row_min = rr;
ant_search_range_row_max = rr;
ant_search_range_col_min = cc;
ant_search_range_col_max = cc;

for j=(J-1):-1:L
    [t1,t2] = func_dyad2HH(j);
    ant_search_range_row_min(t1,t2) = min2(t1);
    ant_search_range_row_max(t1,t2) = max2(t1);
    ant_search_range_col_min(t1,t2) = min2(t2);
    ant_search_range_col_max(t1,t2) = max2(t2);
    [t1,t2] = func_dyad2HL(j);
    ant_search_range_row_min(t1,t2) = min2(t1);
    ant_search_range_row_max(t1,t2) = max2(t1);
    ant_search_range_col_min(t1,t2) = min2(t2);
    ant_search_range_col_max(t1,t2) = max2(t2);
    [t1,t2] = func_dyad2LH(j);
    ant_search_range_row_min(t1,t2) = min2(t1);
    ant_search_range_row_max(t1,t2) = max2(t1);
    ant_search_range_col_min(t1,t2) = min2(t2);
    ant_search_range_col_max(t1,t2) = max2(t2);
end

data_var = func_signal_variance_estimation(wx, noise_sigma, win_size,ant_search_range_row_min,ant_search_range_row_max,ant_search_range_col_min,ant_search_range_col_max,J,L);

for j=(J-1):-1:L
    [t1,t2] = func_dyad2HH(j);
    ws(t1,t2) = wx(t1,t2) .* data_var(t1,t2) ./ (data_var(t1,t2) + noise_sigma.^2);
    [t1,t2] = func_dyad2HL(j);
    ws(t1,t2) = wx(t1,t2) .* data_var(t1,t2) ./ (data_var(t1,t2) + noise_sigma.^2);
    [t1,t2] = func_dyad2LH(j);
    ws(t1,t2) = wx(t1,t2) .* data_var(t1,t2) ./ (data_var(t1,t2) + noise_sigma.^2);
end

x_out  = func_IWT2_PO(ws, L, qmf);

%-------------------------------------------------------------------------
%------------------------------Inner Function ----------------------------
%-------------------------------------------------------------------------
% Estimate the signal variance value, which will be used for shrinkage
function result = func_signal_variance_estimation(wx, noise_sigma, win_size,ant_search_range_row_min,ant_search_range_row_max,ant_search_range_col_min,ant_search_range_col_max,J,L)

% System setup
ant_move_step_within_iteration = 5; % the numbe of iterations?
total_iteration_num = 5;
search_clique_mode = 8;

wx = abs(wx);
[nrow, ncol] = size(wx);
wx_1D = func_2D_LexicoOrder(wx);

% initialization
h = zeros(size(wx));
h_1D = func_2D_LexicoOrder(h);
p = 1./(wx.*(wx~=0)+1.*(wx==0)).*(wx~=0)+(wx==0);
p_1D = func_2D_LexicoOrder(p);

%paramete setting
alpha = 1;
beta = 2;
rho = 0.1;
w = 0.6;
A = 5000;
B = 10;

%use one ant for each pixel position
ant_total_num = nrow*ncol;
ant_current_row = zeros(ant_total_num, 1); % record the location of ant
ant_current_col = zeros(ant_total_num, 1); % record the location of ant
ant_current_val = zeros(ant_total_num, 1); % record the location of ant
rr = (meshgrid(1:nrow))'; cc = meshgrid(1:ncol);
ant_current_row = rr(:);
ant_current_col = cc(:);
ant_current_val = wx_1D((ant_current_row-1).*ncol+ant_current_col);

ant_search_range_row_min = ant_search_range_row_min(:);
ant_search_range_row_min = padarray(ant_search_range_row_min, [0 search_clique_mode-1],'replicate','post');
ant_search_range_row_max = ant_search_range_row_max(:);
ant_search_range_row_max = padarray(ant_search_range_row_max, [0 search_clique_mode-1],'replicate','post');
ant_search_range_col_min = ant_search_range_col_min(:);
ant_search_range_col_min = padarray(ant_search_range_col_min, [0 search_clique_mode-1],'replicate','post');
ant_search_range_col_max = ant_search_range_col_max(:);
ant_search_range_col_max = padarray(ant_search_range_col_max, [0 search_clique_mode-1],'replicate','post');

for nIteration = 1: total_iteration_num

        ant_current_path_val_mean = zeros(ant_total_num,1);
        ant_current_path_val_mean = ant_current_val;

        for nMoveStep = 1: ant_move_step_within_iteration-1

            if search_clique_mode == 4
                ant_search_range_row = [ant_current_row-1, ant_current_row, ant_current_row+1, ant_current_row];
                ant_search_range_col = [ant_current_col, ant_current_col+1, ant_current_col, ant_current_col-1];
            elseif search_clique_mode == 8
                ant_search_range_row = [ant_current_row-1, ant_current_row-1, ant_current_row-1, ant_current_row, ant_current_row,ant_current_row+1, ant_current_row+1, ant_current_row+1];
                ant_search_range_col = [ant_current_col-1, ant_current_col, ant_current_col+1, ant_current_col-1, ant_current_col+1, ant_current_col-1, ant_current_col, ant_current_col+1];
            end

            ant_current_row_extend = padarray(ant_current_row, [0 search_clique_mode-1],'replicate','post');
            ant_current_col_extend = padarray(ant_current_col, [0 search_clique_mode-1],'replicate','post');
            ant_search_range_val = zeros(ant_total_num,search_clique_mode);

            %replace the positions our of the image's range
            temp = (ant_search_range_row>=ant_search_range_row_min) & (ant_search_range_row<=ant_search_range_row_max) & (ant_search_range_col>=ant_search_range_col_min) & (ant_search_range_col<=ant_search_range_col_max);
            ant_search_range_row = temp.*ant_search_range_row + (~temp).*ant_current_row_extend;
            ant_search_range_col = temp.*ant_search_range_col + (~temp).*ant_current_col_extend;

            ant_search_range_transit_prob_h = zeros(size(ant_search_range_val));
            ant_search_range_transit_prob_p = zeros(size(ant_search_range_val));

            for ii=1:search_clique_mode
                ant_search_range_val(:,ii) = wx_1D((ant_search_range_row(:,ii)-1).*ncol+ant_search_range_col(:,ii));

                temp = ant_search_range_val(:,ii);
                temp = abs(temp - ant_current_path_val_mean);
                temp = 1./(temp.*(temp~=0)+1.*(temp==0)) .* (temp~=0)+(temp==0);

                ant_search_range_transit_prob_h(:,ii) = temp;
                ant_search_range_transit_prob_p(:,ii) = p_1D((ant_search_range_row(:,ii)-1).*ncol+ant_search_range_col(:,ii));

            end
            temp = (ant_search_range_transit_prob_h.^alpha) .* (ant_search_range_transit_prob_p.^beta);

            temp_sum = sum(temp,2);
            temp_sum = padarray(temp_sum, [0 search_clique_mode-1],'replicate','post');

            ant_search_range_transit_prob = temp ./ temp_sum;

            % generate a random number to determine the next position.
            rand('state', sum(100*clock));
            temp = rand(ant_total_num,1);
            temp = padarray(temp, [0 search_clique_mode-1],'replicate','post');
            temp = cumsum(ant_search_range_transit_prob,2)>=temp;
            temp = padarray(temp, [0 1],'pre');
            temp = (diff(temp,1,2)==1);

            temp_row = (ant_search_range_row .* temp)';
            [ii, jj, vv] = find(temp_row);
            ant_next_row = vv;
            temp_col = (ant_search_range_col .* temp)';
            [ii, jj, vv] = find(temp_col);
            ant_next_col = vv;

            ant_current_row = ant_next_row;
            ant_current_col = ant_next_col;
            ant_current_val = wx_1D((ant_current_row-1).*ncol+ant_current_col);
            ant_current_path_val_mean = (ant_current_path_val_mean.*nMoveStep + ant_current_val)/(nMoveStep+1);

            %update p;
            rr = ant_current_row;
            cc = ant_current_col;
            p_1D((rr-1).*ncol+cc,1) = w*p_1D((rr-1).*ncol+cc,1) + 1./(A+B.*ant_current_path_val_mean);
            p = func_LexicoOrder_2D(p_1D, nrow, ncol);

        end % end of nMoveStep

        p = (1-rho).*p;
        p_1D = func_2D_LexicoOrder(p);

end % end of nIteration

clear h h_1D temp
clear ant_current_row ant_current_col ant_current_val
clear ant_current_path_val_mean
clear ant_search_range_row search_range_path_col ant_search_range_val
clear ant_search_range_transit_prob_h ant_search_range_transit_prob_v ant_search_range_transit_prob
clear ant_search_range_row_min ant_search_range_row_max ant_search_range_col_min ant_search_range_col_max

result = wx.^2;

for j=(J-1):-1:L
    [t1,t2] = func_dyad2HH(j);
    result(t1,t2) = func_determine_class_fcm(wx(t1,t2), p(t1,t2), win_size, noise_sigma);
    [t1,t2] = func_dyad2HL(j);
    result(t1,t2) = func_determine_class_fcm(wx(t1,t2), p(t1,t2), win_size, noise_sigma);
    [t1,t2] = func_dyad2LH(j);
    result(t1,t2) = func_determine_class_fcm(wx(t1,t2), p(t1,t2), win_size, noise_sigma);
end

% ******************************************************************************
% **************************Inner Function *************************************
% ******************************************************************************
% Bi-class classification algorithm using Fuzzy C-means
function result = func_determine_class_fcm(wx, p, win_size, noise_sigma)

p_1D = func_2D_LexicoOrder(p);
wx_1D = func_2D_LexicoOrder(wx);
[nrow, ncol] = size(wx);

nFeature = p_1D(:)./(max(max(p_1D)));
[center,U,obj_fcn] = fcm(nFeature, 2,[2.0 100 1e-5 0]);

idx = zeros(nrow:ncol,1);
if sum(sum(wx_1D(U(1,:) >= U(2,:)))) >= sum(sum(wx_1D(U(1,:) < U(2,:))))
    idx = U(1,:) >= U(2,:);
else
    idx = U(1,:) < U(2,:);
end
idx = func_LexicoOrder_2D(idx, nrow, ncol);

center_class = idx(:);
padnum = win_size;
A = padarray(wx, [padnum padnum], 'replicate', 'bot');
B = padarray(idx, [padnum padnum], 'replicate', 'bot');

A = im2col(A, [win_size*2+1 win_size*2+1],'sliding');
A = A.^2;
B = im2col(B, [win_size*2+1 win_size*2+1],'sliding');

C = padarray(center_class', [size(A,1)-1, 0], 'replicate', 'post');

result = sum(A,1) ./ (win_size*2+1)./(win_size*2+1);
result = reshape(result, [nrow, ncol]);
result = (result>=noise_sigma^2).*(result-noise_sigma^2);

%-------------------------------------------------------------------------
%------------------------------Inner Function ----------------------------
%-------------------------------------------------------------------------
% Convert data from 2D format to 1D format
function result = func_2D_LexicoOrder(x)
temp = x';
result = temp(:);

%-------------------------------------------------------------------------
%------------------------------Inner Function ----------------------------
%-------------------------------------------------------------------------
% Convert data from 1D format to 2D format
function result = func_LexicoOrder_2D(x, nRow, nColumn)
result = reshape(x, nColumn, nRow)';

%-------------------------------------------------------------------------
%------------------------------Inner Function ----------------------------
%-------------------------------------------------------------------------
function result=min2(f)
%calculate minimum of 2D matrix
result=min(min(f));

%-------------------------------------------------------------------------
%------------------------------Inner Function ----------------------------
%-------------------------------------------------------------------------
function result=max2(f)
%calculate maximum of 2D matrix
result=max(max(f));

%-------------------------------------------------------------------------
%------------------------------Inner Function ----------------------------
%-------------------------------------------------------------------------
% Calculate the PSNR performance to two images
function result = func_psnr_gray(f, g)

f = double(f);
g = double(g);
Q=255; MSE=0;
[M,N]=size(f);
h = f - g;
MSE = sum(sum(h.*h));
MSE=MSE/M/N;
result=10*log10(Q*Q/MSE);


function im=mat2im(mat,cmap,limits)
% mat2im - convert to rgb image
%
% function im=mat2im(mat,cmap,maxVal)
%
% PURPOSE
% Uses vectorized code to convert matrix "mat" to an m-by-n-by-3
% image matrix which can be handled by the Mathworks image-processing
% functions. The the image is created using a specified color-map
% and, optionally, a specified maximum value. Note that it discards
% negative values!
%
% INPUTS
% mat     - an m-by-n matrix
% cmap    - an m-by-3 color-map matrix. e.g. hot(100). If the colormap has
%           few rows (e.g. less than 20 or so) then the image will appear
%           contour-like.
% limits  - by default the image is normalised to it's max and min values
%           so as to use the full dynamic range of the
%           colormap. Alternatively, it may be normalised to between
%           limits(1) and limits(2). Nan values in limits are ignored. So
%           to clip the max alone you would do, for example, [nan, 2]
%
%
% OUTPUTS
% im - an m-by-n-by-3 image matrix
%
%
% Example 1 - combine multiple color maps on one figure
% clf, colormap jet, r=rand(40);
% subplot(1,3,1),imagesc(r), axis equal off , title('jet')
% subplot(1,3,2),imshow(mat2im(r,hot(100))) , title('hot')
% subplot(1,3,3),imshow(mat2im(r,summer(100))), title('summer')
% colormap winter %changes colormap in only the first panel
%
% Example 2 - clipping
% p=peaks(128); J=jet(100);
% subplot(2,2,1), imshow(mat2im(p,J)); title('Unclipped')
% subplot(2,2,2), imshow(mat2im(p,J,[0,nan])); title('Remove pixels <0')
% subplot(2,2,3), imshow(mat2im(p,J,[nan,0])); title('Remove pixels >0')
% subplot(2,2,4), imshow(mat2im(p,J,[-1,3])); title('Plot narrow pixel range')
%
% Rob Campbell - April 2009
%
% See Also: ind2rgb, imadjust

%Check input arguments
error(nargchk(2,3,nargin));

if ~isa(mat, 'double')
    mat = double(mat)+1;    % Switch to one based indexing
    limits = limits + 1;
end

if ~isnumeric(cmap)
    error('cmap must be a colormap, such as jet(100)')
end

%Clip if desired
L=length(cmap);
if nargin==3 && length(limits)==1
    warning('limits should be vector of length of 2. Assuming a max value was specified.')
    limits=[nan,limits];
end

if nargin==3
    minVal=limits(1);
    if isnan(minVal), minVal=min(mat(:)); end
    mat(mat<minVal)=minVal;

    maxVal=limits(2);
    if isnan(maxVal), maxVal=max(mat(:)); end
    mat(mat>maxVal)=maxVal;
else
minVal=min(mat(:));
maxVal=max(mat(:));
end

%Normalise
mat=mat-minVal;
mat=(mat/(maxVal-minVal))*(L-1);
mat=mat+1;

%convert to indecies
mat=round(mat);

%Vectorised way of making the image matrix
im=reshape(cmap(mat(:),:),[size(mat),3]);


%This Matlab script can be used to reproduce Figure 7.20 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%% Define parameters
M = 64; %Number of antennas
Mprime = 30; %Number of
beta = [0,10^(-5/10),1];
IT = 10000;

%Derived parameters
betad = (M-beta*Mprime)/(M-Mprime);

%% Simulate channels
correlationAA = zeros(numel(beta),IT);
correlationAB = zeros(numel(beta),IT);

%Go through all beta values
for b = 1:numel(beta)
    R_A = diag([beta(b)*ones(1,Mprime), betad(b)*ones(1,M-Mprime)]);
    R_B = diag([betad(b)*ones(1,M-Mprime), beta(b)*ones(1,Mprime)]);
    R_A2 = sqrt(R_A);
    R_B2 = sqrt(R_B);

    %Compute UE correlation metric in (7.22)
    for it = 1:IT

        hA1 = R_A2*1/sqrt(2)*(randn(M,1) + 1i*randn(M,1));
        hA2 = R_A2*1/sqrt(2)*(randn(M,1) + 1i*randn(M,1));
        hB1 = R_B2*1/sqrt(2)*(randn(M,1) + 1i*randn(M,1));
        correlationAA(b,it) = 10*log10((abs(hA1'*hA2)/(norm(hA1)*norm(hA2)))^2);
        correlationAB(b,it) = 10*log10((abs(hA1'*hB1)/(norm(hA1)*norm(hB1)))^2);

    end
end

%% Plot the simulation results
figure;
hold on; box on;
nth = 1000;
Colors = {'r','b'};
Markers = {'square', 'o'};

plot(-100,-100,'Color', 'k', 'LineStyle', '-');
plot(-100,-100,'Color', 'k', 'LineStyle', '--');
plot(-100,-100,'Color','k', 'LineStyle', 'none', 'Marker', '*');
plot(-100,-100,'Color', Colors{2}, 'LineStyle', 'none', 'Marker', Markers{2});
plot(-100,-100,'Color', Colors{1}, 'LineStyle', 'none', 'Marker', Markers{1});

[y,x] = ecdf(correlationAA(end,:));
plot(x,y,'Color','k', 'LineWidth', 1);
plot(x(1:nth:end),y(1:nth:end),'Color','k', 'LineStyle', 'none', 'Marker', '*');

for b = 1:numel(beta)-1
    [y,x] = ecdf(correlationAA(b,:));
    plot(x,y,'Color', Colors{b}, 'LineWidth', 1);
end

for b = 1:numel(beta)-1
    [y,x] = ecdf(correlationAB(b,:));
    plot(x,y,'--','Color', Colors{b}, 'LineWidth', 1)
end

for b = 1:numel(beta)-1
    [y,x] = ecdf(correlationAA(b,:));
    plot(x(1:nth:end),y(1:nth:end),'LineStyle', 'none','Color', Colors{b}, 'LineWidth', 1, 'Marker', Markers{b});
end

for b = 1:numel(beta)-1
    [y,x] = ecdf(correlationAB(b,:));
    plot(x(1:nth:end),y(1:nth:end),'LineStyle', 'none','Color', Colors{b}, 'LineWidth', 1, 'Marker', Markers{b})
end

xlim([-40,-10])
legend('Same region', 'Different regions','\beta=1', '\beta=-5 dB', '\beta=0','Location','NorthWest')
xlabel('Average UE correlation [dB]')
ylabel('CDF')


function [out] = evap_1(S,Ep,dt)
%evap_1

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Evaporation at the potential rate
% Constraints:  f <= S/dt
% @(Inputs):    S    - current storage [mm]
%               Ep   - potential evaporation rate [mm/d]
%               dt   - time step size

out = min(S/dt,Ep);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inverse dynamics for the 1dof planar robot
%
%   tau = inversedynamics_1dofplanar(robot, q, qd, qdd, fext)
%
%   Where robot stores the kinematic and dynamic parameters for this robot.
%   q: joint positions.
%   qd: joint velocities.
%   qdd: joint accelerations.
%   fext: vector of external forces. Defined in the last reference system.
%
%   This function just executes the inverse dynamic model for this robot.
%   The equations to compute this dynamic model can be found in:
%   "ROBOT ANALYSIS. The mechanics of Serial and Parallel
%        manipulators". Lung Weng Tsai. John Wiley and Sons, inc. ISBN:
%        0-471-32593-7. page 405.
%
%
%   Author: Arturo Gil Aparicio arturo.gil@umh.es
%   Date: 23/11/2016
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.
function tau = exercise_inv_dynamics_1dofplanar(robot, q, qd, qdd, g, fext)
a = eval(robot.DH.a);
a1=a(1);

g=abs(g);
m=robot.dynamics.masses(1);

%In this case we must define the Inertia with respect to the rotating axis.
J = (1/3)*m*a1^2;
tau = J*qdd + m*g*a1*cos(q(1))/2;


% pet_transmission_example.m
% Example of reconstructing a PET attenuation map via penalized-likelihood
% estimation from real 2D PET transmission data.
%
% Copyright 2002-12-19, Jeff Fessler, University of Michigan

% read raw data
if ~isvar('yi'), printm 'raw data'
    yi = ir_get_data(fullfile('pet_trans_2d_sino','phan_trans.mat'));
    bi = ir_get_data(fullfile('pet_trans_2d_sino','phan_blank.mat'));
	im plc 2 2, im(1, yi, 'yi: transmission scan')
	im(2, bi, 'bi: blank scan')

	% system model for ecat921 PET scanner geometry
	ig = image_geom('nx', 128, 'dx', 0.421875, 'dy', 0.421875); % cm
%		'center_x', 0.5, 'center_y', -0.5, ...
	sg = sino_geom('par', 'nb', size(yi,1), 'na', size(yi,2), ...
		'dr', 0.3375, 'offset_r', 0.5, 'strip_width', 'dr', ...
		'orbit_start', -15);
prompt
end

% FBP image
if ~isvar('xfbp'), printm 'do FBP'
	kernel = gaussian_kernel(3);
%	kernel = [1];
	xfbp = tr_fbp(sg, ig, max(yi,1), bi, 0*bi, 'kernel', kernel);
% fix: correct for backproject scaling problem; probably incorrect!
%	xfbp = xfbp * na/pi * f.pixel_size / f.ray_spacing;
	xfbp = max(xfbp,0);
	im(3, xfbp, 'fbp'), cbar

	ig.mask = ig.circ(26, 21, 0, 3) > 0;
	im(4, ig.mask+6*xfbp, 'mask check')
prompt
end

% strip integral system matrix with mask for iterative reconstruction.
if ~isvar('A2'), printm 'A2'
	if 0 && has_mex_jf
		A2 = Gtomo2_wtmex(sg, ig); % preferable for speed
	else
		A2 = Gtomo2_strip(sg, ig, 'strip_width', sg.dr); % slower but universal
	end
end

if ~isvar('Ab'), printm 'make Ab' % block system object for ordered-subsets
	f.nblock = 5;
	Ab = Gblock(A2, f.nblock);
end

if ~isvar('R'), printm 'make R' % regularizer object
	f.l2b = 10.5;
	f.delta = 0.03;
%	f.pot = 'huber';
	f.pot = 'hyper3';
	R = Reg1(ig.mask, 'type_denom', 'matlab', ...
		'beta', 2^f.l2b, 'pot_arg', {f.pot, f.delta});
end

% matlab iterations
if ~isvar('xmat'), printm 'matlab T-PL-OS-SPS'
	f.niter = 8+1;
	f.niter = 16+1;
	f.pixmax = 0.4;
	xinit = max(xfbp,0);

	xmat = tpl_os_sps(xinit(ig.mask), Ab, yi, bi, [], R, ...
		f.niter, f.pixmax, 'pc');
	xmat = ig.embed(xmat);

	im clf, im(xmat, 'T-PL-OSPS iterations (0th is FBP)')
prompt
end

% nice figure for book
if 1
	im clf
	im(211, yi, 'sinogram yi')
	clim = [0 0.2];
	im(223, xfbp, 'FBP', clim), cbar
	im(224, xmat(:,:,end), 'Statistical', clim), cbar
end


function [R, G, B] = Lab2RGB(L, a, b)
%LAB2RGB Convert an image from CIELAB to RGB
%
% function [R, G, B] = Lab2RGB(L, a, b)
% function [R, G, B] = Lab2RGB(I)
% function I = Lab2RGB(...)
%
% Lab2RGB takes L, a, and b double matrices, or an M x N x 3 double
% image, and returns an image in the RGB color space.  Values for L are in
% the range [0,100] while a* and b* are roughly in the range [-110,110].
% If 3 outputs are specified, the values will be returned as doubles in the
% range [0,1], otherwise the values will be uint8s in the range [0,255].
%
% This transform is based on ITU-R Recommendation BT.709 using the D65
% white point reference. The error in transforming RGB -> Lab -> RGB is
% approximately 10^-5.
%
% See also RGB2LAB.

% By Mark Ruzon from C code by Yossi Rubner, 23 September 1997.
% Updated for MATLAB 5 28 January 1998.
% Fixed a bug in conversion back to uint8 9 September 1999.
% Updated for MATLAB 7 30 March 2009.

if nargin == 1
  b = L(:,:,3);
  a = L(:,:,2);
  L = L(:,:,1);
end

if max(max(L)) < 1.1 || max(max(a)) < 1.1 || max(max(b)) < 1.1
  L = double(L) * 100;
  a = double(a) * 220;
  a = a - 110;
  b = double(b) * 220;
  b = b - 110;
end

% Thresholds
T1 = 0.008856;
T2 = 0.206893;

[M, N] = size(L);
s = M * N;
L = reshape(L, 1, s);
a = reshape(a, 1, s);
b = reshape(b, 1, s);

% Compute Y
fY = ((L + 16) / 116) .^ 3;
YT = fY > T1;
fY = (~YT) .* (L / 903.3) + YT .* fY;
Y = fY;

% Alter fY slightly for further calculations
fY = YT .* (fY .^ (1/3)) + (~YT) .* (7.787 .* fY + 16/116);

% Compute X
fX = a / 500 + fY;
XT = fX > T2;
X = (XT .* (fX .^ 3) + (~XT) .* ((fX - 16/116) / 7.787));

% Compute Z
fZ = fY - b / 200;
ZT = fZ > T2;
Z = (ZT .* (fZ .^ 3) + (~ZT) .* ((fZ - 16/116) / 7.787));

% Normalize for D65 white point
X = X * 0.950456;
Z = Z * 1.088754;

% XYZ to RGB
MAT = [ 3.240479 -1.537150 -0.498535;
       -0.969256  1.875992  0.041556;
        0.055648 -0.204043  1.057311];

RGB = max(min(MAT * [X; Y; Z], 1), 0);

R = reshape(RGB(1,:), M, N);
G = reshape(RGB(2,:), M, N);
B = reshape(RGB(3,:), M, N);

if nargout < 2
  R = uint8(round(cat(3,R,G,B) * 255));
end


function UNew = diffusionNeumann2D(varargin);
% diffusionNeumann2D: solve diffusion registraion in 2D with Neumann
%        boundary conditions
%
%
% author: Nathan D. Cahill
% email: nathan.cahill@rit.edu
% affiliation: Rochester Institute of Technology
% date: January 2014
% licence: GNU GPL v3
%
% Copyright Nathan D. Cahill
% Code available from https://github.com/tomdoel/npReg
%
%

% parse input arguments
[U,F,PixSize,NumPix,RegularizerFactor] = parse_inputs(varargin{:});

% divide by regularizer factor
Fnew = F/RegularizerFactor;

% compute dct of new force field
FnewF1 = real(fft(real(fft(Fnew(:,:,1),2*NumPix(1)-2,1)),2*NumPix(2)-2,2));
FnewF2 = real(fft(real(fft(Fnew(:,:,2),2*NumPix(1)-2,1)),2*NumPix(2)-2,2));
FnewF1 = FnewF1(1:NumPix(1),1:NumPix(2));
FnewF2 = FnewF2(1:NumPix(1),1:NumPix(2));

% construct images of coordinates scaled by pi/(N or M)
[alpha,beta] = ndgrid(pi*(0:(NumPix(1)-1))/(NumPix(1)-1),pi*(0:(NumPix(2)-1))/(NumPix(2)-1));

% construct LHS factor
LHSfactor = 2*cos(alpha) + 2*cos(beta) - 4;

% set origin term to 1, as DC term does not matter
LHSfactor(1,1) = 1;

% solve for FFT of U
UF1 = FnewF1./LHSfactor;
UF2 = FnewF2./LHSfactor;

% if gamma is zero, set DC term to 0
UF1(1,1) = 0;
UF2(1,1) = 0;

% perform inverse dct
U1 = real(ifft(real(ifft(UF1,2*NumPix(1)-2,1)),2*NumPix(2)-2,2));
U2 = real(ifft(real(ifft(UF2,2*NumPix(1)-2,1)),2*NumPix(2)-2,2));

% crop and concatenate
UNew = cat(3,U1(1:NumPix(1),1:NumPix(2)),U2(1:NumPix(1),1:NumPix(2)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [U,F,PixSize,NumPix,RegularizerFactor] = parse_inputs(varargin);

% get displacement field and check size
U = varargin{1};
F = varargin{2};
PixSize = varargin{4}(1:2);
NumPix = [varargin{5} varargin{6}];
RegularizerFactor = varargin{10};
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [CC, rhs, bb, gg, Px, Py, xsplit, ysplit] = discretize(N, f, m, n, flag)
%DISCRETIZE   Given a CHEBOP2, this function converts the problem to one of the form
%
%  sum_i  kron(A_i,B_i)
%
% and computes the discretisation as a cell array:
%
%  {{
%
%       A_1 , B_1
%       A_2 , B_2
%        .  ,  .
%        .  ,  .
%        .  ,  .
%       A_k , B_k
%
%                   }}
%
%    INPUTS:
%      N = PDE (CHEBOP2).
%      f = forcing term (CHEBFUN2).
%      m = discretization size in 1st variable.
%      n = discretization size in 2nd variable.
%      flag = 0 (default) means assigned boundary conditions, flag = 1
%      means do not assign boundary conditions.
%
%    OUTPUTS:
%      CC = cell array of matrices storing the terms in the matrix
%      equation.
%      rhs = matrix discretizing forcing term.
%      bb = cell array storing the discretized linear constraints.
%      gg = cell array storing the discretized nonhomogeneous part of the
%      constraints.
%      Px, Py = store permutation matrix to ensure bcs are linear
%      dependent.
%      XSPLIT, YSPLIT = 0 if subproblems cannot be formed. XSPLIT = 1 if
%      even and odd modes decouple in 1st variable. YSPLIT = 1 if even and
%      odd modes decouple in 2nd variable.
%
% Returns RHS with degrees of freedom removed and bb which stores
% elminated boundary conditions, gg eliminated boundary rows.

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

% Parse inputs.
if nargin < 5
    flag = 0;
end

%%
% Get information of PDE and pref.
A = N.coeffs;
rect = N.domain;
pref = chebfunpref();
tol = pref.cheb2Prefs.chebfun2eps;
xorder = N.xorder;
yorder = N.yorder;

%%
% Check if the PDO was given as a variable coefficient PDO with the notation
% @(x,y,u), but is actually a constant coefficient PDO.
if ( iscell( A ) )
    doesNotDependOnXorY = all(all(cellfun(@isnumeric, N.coeffs)));
    if ( doesNotDependOnXorY )
        A = cell2mat( A ).';
    end
end

%%
% Convert matrix of coefficients to a discretization for the PDE using the
% singular value decomposition.  We find the rank of the PDE operator and
% then use the optimal low rank expansion of the operator as a way to
% discretise the PDE.
if (  isempty(N.V) || isempty(N.U) )
    if ( iscell(A) )
        counter = 1;
        U = cell(size(A, 1), 1);
        V = cell(size(A, 2), 1);
        for jj = 1:size(A, 1)
            for kk = 1:size(A, 2)
                a = A{jj,kk};
                if ( isa(a, 'chebfun2') )
                    if ( abs(vscale(a)) > tol )
                        [C, D, R] = cdr(a);
                        for col = 1:size(C, 2)
                            U{jj,counter} = C(:,col)*D(col,col);
                            V{kk,counter} = R(:,col);
                            counter = counter + 1;
                        end
                    end
                elseif ( isa(a,'double') && ( isempty(a) || abs(a) > tol ) )
                    U{jj,counter} = a;
                    V{kk,counter} = 1;
                    counter = counter + 1;
                end
            end
        end
        rk = size(U, 2);
        S = diag(ones(rk, 1));
        na = size(U, 1);
        nb = size(V, 1);
    else
        % Compute the SVD of the coefficient matrix.
        [U, S, V] = svd(A.');
        % Find the rank of A, which is also the rank of the PDE operator and
        % construct the low rank expansion for A.
        rk = find(diag(S) > tol, 1, 'last');
        U = U(:,1:rk);
        S = S(1:rk,1:rk);
        V = V(:,1:rk);
        [na, nb] = size(A.');
    end
else
    rk = size(N.S, 2);
    U = N.U;
    V = N.V;
    S = N.S;
    na = size(U, 1);
    nb = size(V, 1);
end
% LEFT = zeros(m); RIGHT = zeros(n);

% Construct the discretisation in matrix equation form.
CC = cell(rk,2);

for jj = 1 : rk

    RIGHT = unconstrainedMatrixEquation(V, jj, n, xorder, rect(1:2)); % jjth term on the right.
    LEFT = unconstrainedMatrixEquation(U, jj, m, yorder, rect(3:4)); % jjth term on the left.

    % Balance out the scaling from the singular value. This does slightly
    % improve the accuracy.
    singvalue = sqrt(S(jj,jj));
    CC{jj,2} = singvalue * RIGHT;
    CC{jj,1} = singvalue * LEFT;

end

%%
% Test to see if we can solve subproblems. This checks if the PDE operator
% contains differential terms of the same parity.
ysplit = 0; xsplit=0;
if ~iscell(U) && ~iscell(V)
    emask = 1:2:na;
    omask = 2:2:na;
    if ( min( norm(U(emask,:)), norm(U(omask,:)) ) < 10*tol )
        ysplit = 1;
    end
    emask = 1:2:nb;
    omask = 2:2:nb;
    if ( min( norm(V(emask,:)), norm(V(omask,:)) ) < 10*tol )
        xsplit = 1;
    end
end

%%
% We have a discretisation for the PDE operator, now let's find a
% discretisation for the boundary conditions.

% If no boundary conditions is prescribed then make it empty.
bcLeft = [];
leftVal = [];
bcRight = [];
rightVal = [];
bcUp = [];
upVal = [];
bcDown = [];
downVal = [];

if ( ~isempty(N.lbc) ) % Left boundary conditions.
    [bcLeft, leftVal] = chebop2.constructBC(N.lbc, -1, m, n, rect(3:4), rect(1:2), xorder);
end
if ( ~isempty(N.rbc) ) % Right boundary conditions.
    [bcRight, rightVal] = chebop2.constructBC(N.rbc, 1, m, n, rect(3:4), rect(1:2), xorder);
end
if ( ~isempty(N.ubc) ) % Top boundary conditions.
    [bcUp, upVal] = chebop2.constructBC(N.ubc, 1, n, m, rect(1:2), rect(3:4), yorder);
end
if ( ~isempty(N.dbc) ) % Bottom boundary conditions.
    [bcDown, downVal] = chebop2.constructBC(N.dbc, -1, n, m, rect(1:2), rect(3:4), yorder);
end

%%

% For the down and up BCs we have B^TX = g^T.
By = [ bcUp.'; bcDown.' ];
Gy = [ upVal.'; downVal.' ];
[By, Gy, Py] = canonicalBC(By, Gy);

% For the left and right BCs we have X*B = g. We do the LU to B^T.
Bx = [ bcLeft.'; bcRight.' ];
Gx = [ leftVal.'; rightVal.' ];
[Bx, Gx, Px] = canonicalBC(Bx, Gx);
Bx = Bx.';
Gx = Gx.'; % Now transpose so that X*B = g;

%%
% Construct the RHS of the Sylvester matrix equation.

% Complete the RHS (part of the RHS could have been in the operator):
if ( ~isempty(N.rhs) && ( isa(N.rhs, 'chebfun2') || isa(N.rhs, 'double') ) )
    f = f + N.rhs;
end
E = zeros(m, n);
[n2, n1] = length(f);
F = chebcoeffs2(f);

% Map the RHS to the right ultraspherical space.
lmap = ultraS.convertmat(n1, 0, yorder-1);
rmap = ultraS.convertmat(n2, 0, xorder-1);
F = lmap * F * rmap.';

% Place those coefficients of the forcing function onto the RHS.
n1 = min(n1, m);
n2 = min(n2, n);
E(1:n1,1:n2) = F(1:n1,1:n2);

if ( ~flag ) % Impose boundary conditions.

    % Use the eliminated boundary condition to place zeros in the columns of
    % the matrix equation discretization. There are rk columns to zero out.

    for jj = 1:rk  % For term in the matrix equation.
        [C, E] = zeroDOF(CC{jj,1}, CC{jj,2}, E, By, Gy);
        CC{jj,1} = C;
        [C, E] = zeroDOF(CC{jj,2}, CC{jj,1}, E.', Bx.', Gx.');
        CC{jj,2} = C;
        E = E.';
    end

    % Remove degrees of freedom.
    nn = n - max(xorder, yorder);
    mm = m - max(xorder, yorder);
    df1 = max(0, xorder - yorder);
    df2 = max(0, yorder - xorder);
    for jj = 1:rk
        CC{jj,1} = CC{jj,1}(1:mm, yorder+1:m-df1);
        CC{jj,2} = CC{jj,2}(1:nn, xorder+1:n-df2);
    end
    % Truncation of righthand side.
    rhs = E(1:mm, 1:nn);

else
    rhs = E;
end

% Pass back the eliminated boundary conditions.
bb = {bcLeft bcRight bcUp bcDown};
gg = {leftVal rightVal upVal downVal};

%%
% Check boundary continunity conditions.

% Check BCs at corners:
allbc = 0;
if ( ~isempty(bcUp) && ~isempty(upVal) && ~isempty(bcRight) && ~isempty(rightVal) )
    if ( norm(rightVal(end-4:end),inf) < sqrt(tol) && norm(upVal(end-4:end),inf) < sqrt(tol) )
        allbc = allbc + norm(upVal.'*bcRight - bcUp.'*rightVal);
    end
end
if ( ~isempty(bcUp) && ~isempty(upVal) && ~isempty(bcLeft) && ~isempty(leftVal) )
    if ( norm(leftVal(end-4:end),inf) < sqrt(tol) && norm(upVal(end-4:end),inf) < sqrt(tol) )
        allbc = allbc + norm(upVal.'*bcLeft - bcUp.'*leftVal);
    end
end
if ( ~isempty(bcDown) && ~isempty(downVal) && ~isempty(bcRight) && ~isempty(rightVal) )
    if ( norm(rightVal(end-4:end),inf) < sqrt(tol) && norm(downVal(end-4:end),inf) < sqrt(tol) )
        allbc = allbc + norm(downVal.'*bcRight - bcDown.'*rightVal);
    end
end
if ( ~isempty(bcDown) && ~isempty(downVal) && ~isempty(bcLeft) && ~isempty(leftVal) )
    if ( norm(leftVal(end-4:end),inf)<sqrt(tol) && norm(downVal(end-4:end),inf)<sqrt(tol) )
        allbc = allbc + norm(downVal.'*bcLeft - bcDown.'*leftVal);
    end
end

% [TODO]: Should there be an error if the compatibility conditions do not
% match?
%
% if allbc >= 100*sqrt(tol)
%     s = sprintf('Boundary conditions differ by %1.4f', allbc');
%     warning('CHEBFUN:CHEBOP2:discretize:BCs', s)
% end

end

function B = unconstrainedMatrixEquation(ODE, jj, n, order, dom)
% Construct the unconstrained Matix Equation. Adding in the constraints later.

B = spalloc(n, n, 3*n);
for kk = 1:size(ODE, 1)

    % Get conversion and differentiation matrices:
    S = ultraS.convertmat(n, kk-1, order-1);
    D = ((2./diff(dom))^(kk-1)) * ultraS.diffmat(n, kk-1);

    if ( iscell(ODE(kk,jj)) && isa(ODE{kk,jj}, 'chebfun') )
        % Variable coefficient term:
        c = ODE{kk,jj}.coeffs;
        M = ultraS.multmat(n, c, kk-1);
        A = S * M * D;

    elseif ( iscell(ODE(kk,jj)) && ~isempty(ODE{kk,jj}) )
        % Constant coefficient term in a variable coefficient ODE:
        A = ODE{kk,jj}.* S * D;

    elseif ( isa(ODE(kk,jj),'double') )
        % Constant coefficient term in a constant coefficient ODE:
        A = ODE(kk,jj).* S * D;

    else
        % Empty cell in array, so no term:
        A = zeros(n);

    end

    % Form the ODE operator:
    B = B + A;

end

end

function [B, G, P] = canonicalBC(B, G)
%CANONICALBC   Form a linear combintation of the boundary conditions
%so that they can be used for imposing on the PDE.

P = nonsingularPermute(B);
B = B*P;
[L, B] = lu(B);
G = L \ G;

% Scale so that B is unit upper triangular.
if ( min(size(B)) > 1 )
    D = diag(1./diag(B));
elseif ( ~isempty(B) )
    D = 1./B(1,1);
else
    D = []; % No boundary conditions.
end
B = D*B;
G = D*G;

end

function P = nonsingularPermute(B)
%NONSINGULARPERMUTE   Permute the columns of B to ensure that the principal
%m*m submatrix of B is nonsingular, where m = size(B, 1).
%
% Note: This is needed for solving the matrix equations with linear
% constraints, see DPhil thesis of Alex Townsend (section 6.5).

m = size(B, 1);
k = 1;

% [TODO]: improve this check.
% Try each mxm block in a linear fashion:
while ( rank(B(:,k:m+k-1)) < m )
    k = k+1;
    if ( m+k > size(B, 2) )
        error('CHEBFUN:CHEBOP2:discretize:nonsingularPermute:BCs', ...
            'Boundary conditions are linearly dependent.');
    end
end

P = speye(size(B, 2));
P = P(:,[k:m+k-1, 1:k-1, m+k:end]);

end

function [C1, E] = zeroDOF(C1, C2, E, B, G)
%ZERODOF   Eliminate so degrees of freedom in the matrix equation can be
%removed.

for ii = 1:size(B, 1) % For each boundary condition, zero a column.
    for kk = 1:size(C1, 1)
        if ( abs(C1(kk,ii)) > 10*eps )
            c = C1(kk, ii); % Constant required to zero entry out.
            C1(kk,:) = C1(kk,:) - c*B(ii,:);
            E(kk,:) = E(kk,:) - c*G(ii,:)*C2.';
        end
    end
end

end


function [ xmin, xmax ] = r8_gaml ( )

%*****************************************************************************80
%
%% R8_GAML evaluates bounds for an R8 argument of the gamma function.
%
%  Discussion:
%
%    This function calculates the minimum and maximum legal bounds
%    for X in the evaluation of GAMMA ( X ).
%
%    XMIN and XMAX are not the only bounds, but they are the only
%    non-trivial ones to calculate.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Output, real XMIN, XMAX, the bounds.
%
  alnsml = log ( r8_mach ( 1 ) );
  xmin = - alnsml;

  for i = 1 : 10

    xold = xmin;
    xln = log ( xmin );
    xmin = xmin - xmin * ( ( xmin + 0.5 ) * xln - xmin ...
      - 0.2258 + alnsml ) / ( xmin * xln + 0.5 );

    if ( abs ( xmin - xold ) < 0.005 )

      xmin = - xmin + 0.01;

      alnbig = log ( r8_mach ( 2 ) );
      xmax = alnbig;

      for j = 1 : 10

        xold = xmax;
        xln = log ( xmax );
        xmax = xmax - xmax * ( ( xmax - 0.5 ) * xln - xmax ...
          + 0.9189 - alnbig ) / ( xmax * xln - 0.5 );

        if ( abs ( xmax - xold ) < 0.005 )
          xmax = xmax - 0.01;
          xmin = max ( xmin, - xmax + 1.0 );
          return
        end

      end

      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8_GAML - Fatal error!\n' );
      fprintf ( 1, '  Unable to find XMAX.\n' );
      error ( 'R8_GAML - Fatal error!' )

    end

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8_GAML - Fatal error!\n' );
  fprintf ( 1, '  Unable to find XMIN.\n' );

  error ( 'R8_GAML - Fatal error!' )
end


  function sino = fbp2_sino_weight(sg, sino, varargin)
%|function sino = fbp2_sino_weight(sg, sino, varargin)
%|
%| Apply sinogram weighting for first step of 2D fan-beam FBP.
%| This matlab version is the backup alternative for users lacking mex routine.
%|
%| in
%|	sg			sino_geom()
%|	ig			image_geom()
%|	sino	[nb,na,(L)]	fan-beam sinogram(s) (line integrals)
%| out
%|	sino	[nb,na,(L)]	weighted sinogram
%|
%| Copyright 2006-4-19 by Jeff Fessler, University of Michigan

if nargin == 1 && streq(sg, 'test'), fbp2_sino_weight_test, return, end
if nargin < 2, ir_usage, end

arg.chat = 0;
arg = vararg_pair(arg, varargin);

idim = size(sino);
sino = reshape(sino, idim(1), idim(2), []); % [nb,na,*L]
sino = fbp2_sino_weight_do(sino, sg);
sino = reshape(sino, idim); % [nb,na,(L)]

%
% fbp2_sino_weight_do()
%
function sino = fbp2_sino_weight_do(sino, sg)

if isinf(sg.dfs)
	sino = fbp2_sino_weight_flat(sino, ...
		sg.s, sg.dsd, sg.dso, sg.source_offset);
elseif sg.dfs == 0
	sino = fbp2_sino_weight_arc(sino, ...
		sg.s, sg.dsd, sg.dso, sg.source_offset);
else
	error 'only flat and arc done'
end

%
% fbp2_sino_weight_arc()
%
function sino = fbp2_sino_weight_arc(sino, ss, dsd, dso, source_offset);
na = size(sino,2);
nz = size(sino,3);
gam = ss / dsd;
w1 = abs(dso * cos(gam) - source_offset * sin(gam)) / dsd; % 1D weighting
sino = sino .* repmat(w1, [1 na nz]);

%
% fbp2_sino_weight_flat()
%
function sino = fbp2_sino_weight_flat(sino, ss, dsd, dso, source_offset);
na = size(sino,2);
nz = size(sino,3);
gam = atan(ss / dsd);
w1 = abs(dso * cos(gam) - source_offset * sin(gam)) / dsd; % 1D weighting
sino = sino .* repmat(w1, [1 na nz]);

%
% fbp2_sino_weight_test()
%
function fbp2_sino_weight_test
sg = sino_geom('ge1', 'down', 4);
sino = sg.ones;
s1 = fbp2_sino_weight(sg, sino);

im pl 1 2
im(1, s1), cbar
%max_percent_diff(s1,s2)


function pos2 = aberat (pos1, ve, tlight)

% this function corrects position vector for aberration of light.
% algorithm includes relativistic terms.  adapted from murray (1981)
% mon. notices royal ast. society 195, 639-648.

% input

%  pos1   = position vector of observed object, with reespect to
%           origin at observer (or the geocenter), components in au

%  ve     = velocity vector of observer (or the geocenter),
%           with respect to origin at solar system barycenter,
%           components in au/day (in)

%  tlight = light time from body to observer (or the geocenter) in days

% output

%  pos2 = position vector of observed object, with respect to
%         origin at observer (or the geocenter), corrected
%         for aberration, components in au

% ported from NOVAS 3.1

%%%%%%%%%%%%%%%%%%%%%%%

% light-time for one astronomical unit in seconds, from de-405

ausec = 499.0047838061d0;

% speed of light in au/day

c = 86400.0d0 / ausec;

tl = tlight;

p1mag = tl * c;

if (tl == 0.0d0)

    p1mag = sqrt(pos1(1)^2 + pos1(2)^2 + pos1(3)^2);

    tl = p1mag / c;
end

vemag = sqrt(ve(1)^2 + ve(2)^2 + ve(3)^2);

beta = vemag / c;

rdotv = pos1(1) * ve(1) + pos1(2) * ve(2) + pos1(3) * ve(3);

cosd = rdotv / (p1mag * vemag);

gammai = sqrt(1.0d0 - beta^2);

p = beta * cosd;

q = (1.0d0 + p / (1.0d0 + gammai)) * tl;

r = 1.0d0 + p;

for j = 1:1:3

    pos2(j) = (gammai * pos1(j) + q * ve(j)) / r;

end


%IMM_FILTER  Interacting Multiple Model (IMM) Filter prediction and update steps
%
% Syntax:
%   [X_i,P_i,MU,X,P] = IMM_FILTER(X_ip,P_ip,MU_ip,p_ij,ind,dims,A,Q,Y,H,R)
%
% In:
%   X_ip  - Cell array containing N^j x 1 mean state estimate vector for
%           each model j after update step of previous time step
%   P_ip  - Cell array containing N^j x N^j state covariance matrix for
%           each model j after update step of previous time step
%   MU_ip - Vector containing the model probabilities at previous time step
%   p_ij  - Model transition matrix
%   ind   - Indices of state components for each model as a cell array
%   dims  - Total number of different state components in the combined system
%   A     - State transition matrices for each model as a cell array.
%   Q     - Process noise matrices for each model as a cell array.
%   Y    - Dx1 measurement vector.
%   H    - Measurement matrices for each model as a cell array.
%   R    - Measurement noise covariances for each model as a cell array.
%
%
% Out:
%   X_p  - Updated state mean for each model as a cell array
%   P_p  - Updated state covariance for each model as a cell array
%   MU   - Model probabilities as vector
%   X    - Combined state mean estimate
%   P    - Combined state covariance estimate
%
% Description:
%   IMM filter prediction and update steps. Use this instead
%   of separate prediction and update functions, if you don't need
%   the prediction estimates.
%
% See also:
%   IMM_UPDATE, IMM_SMOOTH, IMM_FILTER

% History:
%   01.11.2007 JH The first official version.
%
% Copyright (C) 2007 Jouni Hartikainen
%
% $Id: imm_update.m 111 2007-11-01 12:09:23Z jmjharti $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

function [X_i,P_i,MU,X,P] = imm_filter(X_ip,P_ip,MU_ip,p_ij,ind,dims,A,Q,Y,H,R)
    % Number of models
    m = length(X_ip);

    % Default values for state mean and covariance
    MM_def = zeros(dims,1);
    PP_def = diag(20*ones(dims,1));

    % Normalizing factors for mixing probabilities
    c_j = zeros(1,m);
    for j = 1:m
        for i = 1:m
            c_j(j) = c_j(j) + p_ij(i,j).*MU_ip(i);
        end
    end

    % Mixing probabilities
    MU_ij = zeros(m,m);
    for i = 1:m
        for j = 1:m
            MU_ij(i,j) = p_ij(i,j) * MU_ip(i) / c_j(j);
        end
    end

    % Calculate the mixed state mean for each filter
    X_0j = cell(1,m);
    for j = 1:m
        X_0j{j} = zeros(dims,1);
        for i = 1:m
            X_0j{j}(ind{i}) = X_0j{j}(ind{i}) + X_ip{i}*MU_ij(i,j);
        end
    end

    % Calculate the mixed state covariance for each filter
    P_0j = cell(1,m);
    for j = 1:m
        P_0j{j} = zeros(dims,dims);
        for i = 1:m
            P_0j{j}(ind{i},ind{i}) = P_0j{j}(ind{i},ind{i}) + MU_ij(i,j)*(P_ip{i} + (X_ip{i}-X_0j{j}(ind{i}))*(X_ip{i}-X_0j{j}(ind{i}))');
        end
    end

    % Space for estimates
    X_p = cell(1,m);
    P_p = cell(1,m);
    X_i = cell(1,m);
    P_i = cell(1,m);
    lambda = zeros(1,m);

    % Filter the estimates for each model
    for i = 1:m
        % Predict the estimates
        [X_p{i}, P_p{i}] = kf_predict(X_0j{i}(ind{i}),P_0j{i}(ind{i},ind{i}),A{i},Q{i});
        % Update the estimates
        [X_i{i}, P_i{i}, K, IM, IS] = kf_update(X_p{i},P_p{i},Y,H{i},R{i});

        % Calculate likelihoods
        lambda(i) = kf_lhood(X_p{i},P_p{i},Y,H{i},R{i});
    end

     % Calculate the model probabilities
    MU = zeros(1,m);
    c = sum(lambda.*c_j);
    MU = c_j.*lambda/c;

    % Output the combined updated state mean and covariance, if wanted.
    if nargout > 3
        % Space for estimates
        X = zeros(dims,1);
        P = zeros(dims,dims);
        % Updated state mean
        for i = 1:m
            X(ind{i}) = X(ind{i}) + MU(i)*X_i{i};
        end
        % Updated state covariance
        for i = 1:m
            P(ind{i},ind{i}) = P(ind{i},ind{i}) + MU(i)*(P_i{i} + (X_i{i}-X(ind{i}))*(X_i{i}-X(ind{i}))');
        end
    end


function dx = dynamics(x,u,p)
% dx = dynamics(x,u,p)
%
% Computes the dynamics for the simple pendulum
%

q = x(1,:);
dq = x(2,:);

k = p.k;    c = p.c;
ddq = -c*dq - k*sin(q) + u;
dx = [dq;ddq];

end


function [r,u,t,p] = macroproperties1d(n,j_x,E,nx,nv,theta)
%% Recover Macroscopic Properties
% compute back, fugacity, macroscopic velocities, temperature and pressure.
    % Computing first velocite,s from the momentum:
    u = j_x./n;

% to compute fugacity, temperature and pressure, we need to rely on the
% distribution fucntion that we where are using: MB, FD, BE.

switch theta
    case{-1} % BE
    % If BE: we apply bisection method to the approx BE distribution Eq.
        r_a = 0.001; r_b = 0.99; tol = 1e-7;
        for i = 1:nx
        psi = @(r_x) 2*E(i)- BE(r_x,1.5)*(n(i)/BE(r_x,0.5))^3/(2*pi) ...
        - n(i)*(u(i)^2);
        r_p = bisection(psi,r_a,r_b,tol);
        r(i) = r_p;
        t(i) = n(i)^2/(pi*(BE(r_p,0.5))^2);
        p(i) = E(i) - 1/2*n(i)*(u(i)^2);
        end

    case{1} % FD
    % if FD: we apply bisection method to the approx FD distribution Eq.
        r_a = 0.001; r_b = 0.99; tol = 1e-7;
        for i = 1:nx
        psi = @(r_x) 2*E(i)- FD(r_x,1.5)*(n(i)/FD(r_x,0.5))^3/(2*pi) ...
        - n(i)*(u(i)^2);
        r_p = bisection(psi,r_a,r_b,tol);
        r(i) = r_p;
        t(i) = n(i)^2/(pi*(FD(r_p,0.5))^2);
        p(i) = E(i) - 1/2*n(i)*(u(i)^2);
        end

    case{0} % MB
    % IF MB: the task is much simple.
        t = 4*E./n - 2*u.^2;
        r = n./sqrt(pi.*t);
        p = E - 1/2.*n.*u.^2;
    otherwise
        error('theta can only be: -1, 0, +1 ');
end

% Using Discrete Ordinate Method:
%     r = repmat(r,nv,1);     u = repmat(u,nv,1);
%     t = repmat(t,nv,1);     p = repmat(p,nv,1);
%     n = repmat(n,nv,1);


function loglik = compute_log_lik(P, S_bar, V, E_z, E_zz, RO, Tr, sigma_sq)

[K, T] = size(E_z);
J = size(S_bar, 2);

M_t = zeros(2*J, K);

loglik = - 0.5*T * (2*J*log(sigma_sq));
for t = 1:T,
   R_t = RO{t};

   Sdef = S_bar;
   for kk = 1:K,
      Sdef = Sdef + E_z(kk,t)*V((kk-1)*3+[1:3],:);

      M_t(1:J, kk) = (R_t(1,:)*V((kk-1)*3+[1:3],:))';
      M_t(J+1:end, kk) = (R_t(2,:)*V((kk-1)*3+[1:3], :))';
   end;

   invSigmaSq_p = eye(2*J)./sigma_sq;

   f_bar_t = R_t(1:2,:)*S_bar;
   f_bar_t = [f_bar_t(1,:) f_bar_t(2,:)]';

   f_t = [P(t, :) P(t+T, :)]';
   t_vect_t = [Tr(t,1)*ones(J,1); Tr(t,2)*ones(J,1)];

   covZ_t = E_zz((t-1)*K+1:t*K,:) - E_z(:,t)*E_z(:,t)';
   loglik = loglik - 0.5*(((f_t-f_bar_t-t_vect_t)./sigma_sq)'*(f_t-f_bar_t-t_vect_t)) + (((f_t-f_bar_t-t_vect_t)'./sigma_sq)*M_t*E_z(:,t)) ...
      - 0.5*trace(((M_t./sigma_sq)'*M_t) * E_zz((t-1)*K+1:t*K,:)) - 0.5*log(det(covZ_t));
end


% Data file PILZ1
% Forced damped vibrations of a mechanical
% system with one degree of freedom, contained
% a reverse pendulum.
  Ek   = '3.6666*qt^2'; % Kinetic energy
  N    = '(10*sin(p*t)-c*q-k*qt+19.62*sin(10*q))*qt'; % power
  Tend = 10;    % upper bound of integration
  q0   = '0.1'; % initial coordinate
  qt0  = '0';   % initial velocity
  eps  = 1e-10; % desirable accuracy
  np   = 3;     % number of parameters
  P{1} = 'c';   % spring stiffness
  P{2} = 'k';   % coefficient of damping
  P{3} = 'p';   % disturbance frequency


function [relaxRxnBool, solutionRelax] = minCardinalityConservationRelaxationVector(S, param, printLevel)
% DC programming for solving the cardinality optimization problem
%
% .. math::
%
%    min  ~& \lambda ||x||_0 \\
%    s.t. ~& x + S^T z = 0 \\
%         ~& -\infty \leq x \leq \infty, \\
%         ~& 1 \leq z \leq 1 / \epsilon
%
% USAGE:
%
%    [relaxRxnBool, solutionRelax] = minCardinalityConservationRelaxationVector(S, param, printLevel)
%
% INPUT:
%    S:                `m` x `n` stoichiometric matrix
%
% OPTIONAL INPUTS:
%    param:           structure with:
%
%                        * param.epsilon - (getCobraSolverParams('LP', 'feasTol')*100) 1/epsilon is the largest flux expected
%                        * param.eta - (`feasTol` * 100), cutoff for mass leak/siphon
%                        * param.nonRelaxBool - (false(n, 1)), `n` x 1 boolean vector for reactions not to relax
%    printLevel:       verbose level
%
% OUTPUTS:
%    relaxRxnBool:     `n` x 1 boolean vector where true correspond to relaxation
%    solutionRelax:    structure with:
%
%                        * solutionRelax.stat - solution status
%                        * solutionRelax.x - `n` x 1 vector where nonzeros>eta correspond to relaxations
%                        * solutionRelax.z - `m` x 1 vector where positives correspond to molecular mass

[mlt,nlt]=size(S');

if ~exist('param','var') || isempty(param)
    param.epsilon=getCobraSolverParams('LP', 'feasTol')*100;
    feasTol = getCobraSolverParams('LP', 'feasTol');
    param.eta=feasTol*100;
    param.nonRelaxBool=false(mlt,1);
    param.checkConsistency=0;
else
    if ~isfield(param,'epsilon')
        param.epsilon=getCobraSolverParams('LP', 'feasTol')*100;
    end
    if ~isfield(param,'eta')
        feasTol = getCobraSolverParams('LP', 'feasTol');
        param.eta=feasTol*100;
    end
    if ~isfield(param,'nonRelaxBool')
        param.nonRelaxBool=false(mlt,1);
    end
    if ~isfield(param,'checkConsistency')
        param.checkConsistency=1;
    end
end

if ~exist('printLevel','var')
    printLevel =0;
end

if param.checkConsistency
    % Check the stoichiometric consistency of the network without relaxation by
    % solving the following linear problem
    %       min sum(m_i)
    %           s.t     S'*m = 0
    %                   m >= 1
    % where l  is is a  mx1 vector of the molecular mass of m molecular species
    cardProblem.A=S';
    cardProblem.b=zeros(size(cardProblem.A,1),1);
    cardProblem.lb=ones(size(cardProblem.A,2),1);
    cardProblem.ub=inf*ones(size(cardProblem.A,2),1);
    cardProblem.c=1*ones(size(cardProblem.A,2),1);
    cardProblem.osense=1;
    cardProblem.csense(1:size(cardProblem.A,1),1)='E';

    solutionRelax = solveCobraLP(cardProblem,'printLevel',printLevel);
    if solutionRelax.stat==1
        solutionRelax.z = solutionRelax.full;
        solutionRelax.x = S'*solutionRelax.z;
    end
    done=1;
else
    done=0;
end

%relaxation problem
if done==0
    cardProblem.p=mlt;
    cardProblem.q=0;
    cardProblem.r=nlt;
    cardProblem.c=zeros(nlt+mlt,1);
    if 1
        cardProblem.A=[speye(mlt,mlt),S'];
    else
        cardProblem.A=[sparse(mlt,mlt),S'];
    end
    cardProblem.b=zeros(mlt,1);
    cardProblem.lb=[-inf*ones(mlt,1);ones(nlt,1)];
    %cardProblem.lb=[zeros(mlt,1);epsilon*ones(nlt,1)];
    cardProblem.ub=[inf*ones(nlt,1);(1/param.epsilon)*ones(mlt,1)];
    %omits flux from this reaction - perhaps not a good way to do it.
    if any(param.nonRelaxBool)
        %prevent relaxation of specified reactions
        cardProblem.lb([param.nonRelaxBool;false(mlt,1)])=0;
        cardProblem.ub([param.nonRelaxBool;false(mlt,1)])=0;
    end
    cardProblem.csense(1:mlt,1)='E';
    cardProblem.lambda0=1;
    cardProblem.lambda1=getCobraSolverParams('LP', 'feasTol')*100;% sensitive to this value, 1e-4 works for Recon3Model.
    cardProblem.delta=0;
    solutionRelax = optimizeCardinality(cardProblem,param);
    %  problem                  Structure containing the following fields describing the problem
    %       p                   size of vector x
    %       q                   size of vector y
    %       r                   size of vector z
    %       c                   (p+q+r) x 1 linear objective function vector
    %       lambda              trade-off parameter of ||x||_0
    %       delta               trade-off parameter of ||y||_0
    %       A                   s x (p+q+r) LHS matrix
    %       b                   s x 1 RHS vector
    %       csense              s x 1 Constraint senses, a string containting the constraint sense for
    %                           each row in A ('E', equality, 'G' greater than, 'L' less than).
    %       lb                  (p+q+r) x 1 Lower bound vector
    %       ub                  (p+q+r) x 1 Upper bound vector
    %
    % OPTIONAL INPUTS
    % param                    parameters structure
    %       nbMaxIteration      stopping criteria - number maximal of iteration (Defaut value = 1000)
    %       epsilon             stopping criteria - (Defaut value = 10e-6)
    %       theta               parameter of the approximation (Defaut value = 2)
    %
    % OUTPUT
    % solution                  Structure containing the following fields
    %       x                   p x 1 solution vector
    %       y                   q x 1 solution vector
    %       z                   r x 1 solution vector
    %       stat                status
    %                           1 =  Solution found
    %                           2 =  Unbounded
    %                           0 =  Infeasible
    %                           -1=  Invalid input
end

%check optimality
if printLevel>2
    fprintf('%g%s\n',norm(solutionRelax.x + S'*solutionRelax.z),' = ||x + S''*z||')
    fprintf('%g%s\n',min(solutionRelax.z),' = min(z_i)')
    fprintf('%g%s\n',max(solutionRelax.z),' = min(z_i)')
    fprintf('%g%s\n',min(solutionRelax.x),' = min(x_i)')
    fprintf('%g%s\n',max(solutionRelax.x),' = max(x_i)')
end

if solutionRelax.stat==1
    %conserved if relaxation is below epsilon
    relaxRxnBool=abs(solutionRelax.x)>=param.eta;
    if printLevel>1
        fprintf('%g%s\n',norm(S(:,~relaxRxnBool)'*solutionRelax.z),' = ||N''*z|| (should be zero)')
    end
    if printLevel>1
        fprintf('%s\n',[int2str(nnz(relaxRxnBool)) '/' int2str(length(relaxRxnBool)) ' reactions relaxed.'])
    end
else
    disp(solutionRelax)
    error('solve for minimum cardinality of conservation relaxation vector failed')
end


function [N,E] = rentian_scaling(A,XYZ,n)
%RENTIAN_SCALING    Physical Rentian scaling
%
% [N E] = rentian_scaling(A,XYZ,n)
%
% Physical Rentian scaling (or more simply Rentian scaling) is a property
% of systems that are cost-efficiently embedded into physical space. It is
% what is called a "topo-physical" property because it combines information
% regarding the topological organization of the graph with information
% about the physical placement of connections. Rentian scaling is present
% in very large scale integrated circuits, the C. elegans neuronal network,
% and morphometric and diffusion-based graphs of human anatomical networks.
% Rentian scaling is determined by partitioning the system into cubes,
% counting the number of nodes inside of each cube (N), and the number of
% edges traversing the boundary of each cube (E). If the system displays
% Rentian scaling, these two variables N and E will scale with one another
% in loglog space. The Rent's exponent is given by the slope of log10(E)
% vs. log10(N), and can be reported alone or can be compared to the
% theoretical minimum Rent's exponent to determine how cost efficiently the
% network has been embedded into physical space. Note: if a system displays
% Rentian scaling, it does not automatically mean that the system is
% cost-efficiently embedded (although it does suggest that). Validation
% occurs when comparing to the theoretical minimum Rent's exponent for that
% system.
%
% Inputs:
% 	A       MxM adjacency matrix
%           must be unweighted, binary, and symmetric.
% 	XYZ     Vector of node placement coordinates
%           must be Mx3 matrix, where M is the number of nodes.
% 	n       Number of partitions to compute. Each partition is a data
%           point. You want a large enough number to adequately estimate
%           the Rent's exponent.
%
% Outputs:
%	N       nx1 vector of the number of nodes in each of the n partitions.
%	E       nx1 vector of the number of edges crossing the boundary of each
%           partition.
%
% Subsequent Analysis:
%   Rentian scaling plots are then created by: figure; loglog(E,N,'*');
%
%	To determine the Rent's exponent, p, it is important not to use
%	partitions which may be affected by boundary conditions. In Bassett et
%	al. 2010 PLoS CB, only partitions with N<M/2 were used in the
%	estimation of the Rent's exponent. Thus, we can define N_prime =
%	N(find(N<M/2)) and E_prime = E(find(N<M/2)). Next we need to determine
%	the slope of Eprime vs. Nprime in loglog space, which is the Rent's
%	exponent. There are many ways of doing this with more or less
%	statistical rigor. Robustfit in MATLAB is one such option:
%       [b,stats] = robustfit(log10(N_prime),log10(E_prime))
%
%   Then the Rent's exponent is b(1,2) and the standard error of the
%   estimation is given by stats.se(1,2).
%
% Note: n=5000 was used in Bassett et al. 2010 in PLoS CB.
%
%
% Reference:
%   Danielle S. Bassett, Daniel L. Greenfield, Andreas Meyer-Lindenberg,
%   Daniel R. Weinberger, Simon W. Moore, Edward T. Bullmore. Efficient
%   physical embedding of topologically complex information processing
%   networks in brains and computer circuits. PLoS Comput Biol, 2010,
%   6(4):e1000748.
%
%
% Danielle Bassett, UCSB, 2010

% determine the number of nodes in the system
M = numel(XYZ(:,1));
% rescale coordinates so that they are all greater than unity
XYZn = XYZ-repmat(min(XYZ)-1,M,1);
% find the absolute minimum and maximum over all directions
nmax = max(max(XYZn));
nmin = min(min(XYZn));

% initialize variables
count = 0;
N = zeros(n,1);
E = zeros(n,1);

% create partitions, and count the number of nodes inside the partition (N) and the number of % edges traversing the boundary of the partition  (E)
while count<(n+1);
    % define cube end points
    randx = sort((1+nmax-nmin).*rand(2,1),'ascend');
    % find nodes in cube
    L = find(XYZn(:,1)>randx(1) & XYZn(:,1)<randx(2) & XYZn(:,2)>randx(1) & XYZn(:,2)<randx(2) & XYZn(:,3)>randx(1) & XYZn(:,3)<randx(2));
    if ~isempty(L)
        count = count+1;
        % count edges crossing the boundary of the cube
        E(count,1) = sum(sum(A(L,setdiff(1:M,L))));
        % count nodes inside of the cube
        N(count,1) = numel(L);
    end
end