function value = base_to_i4 ( s, base )

%*****************************************************************************80
%
%% BASE_TO_I4 returns the value of an integer represented in some base.
%
%  Discussion:
%
%    BASE = 1 is allowed, in which case we allow the digits '1' and '0',
%    and we simply count the '1' digits for the result.
%
%    Negative bases between -16 and -2 are allowed.
%
%    The base -1 is allowed, and essentially does a parity check on
%    a string of 1's.
%
%  Example:
%
%        Input      Output
%    -------------  ------
%         S   BASE       I
%    ------  -----  ------
%      '101'     2       5
%    '-1000'     3     -27
%      '100'     4      16
%   '111111'     2      63
%   '111111'    -2      21
%   '111111'     1       6
%   '111111'    -1       0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string.  The elements of S are
%    blanks, a plus or minus sign, and digits.  Normally, the digits
%    are representations of integers between 0 and |BASE-1|.  In the
%    special case of base 1 or base -1, we allow both 0 and 1 as digits.
%
%    Input, integer BASE, the base in which the representation is given.
%    Normally, 2 <= BASE <= 16.  However, there are two exceptions.
%
%    Output, integer VALUE, the integer.
%
  value = 0;
  s_length = s_len_trim ( s );

  if ( base == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BASE_TO_I4 - Serious error!\n' );
    fprintf ( 1, '  The input base is zero.\n' );
    error ( 'BASE_TO_I4 - Serious error!' )
  end

  if ( 16 < abs ( base ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BASE_TO_I4 - Serious error!\n' );
    fprintf ( 1, '  The input base is greater than 16!\n' );
    error ( 'BASE_TO_I4 - Serious error!' )
  end

  state = 0;
  isgn = 1 ;
  ichr = 1;

  while ( ichr <= s_length )

    ch = s(ichr);
%
%  Blank.
%
    if ( ch == ' ' )

      if ( state == 2 )
        break
      end
%
%  Sign, + or -.
%
    elseif ( ch == '-' )

      if ( state ~= 0 )
        break
      end

      state = 1;
      isgn = -1;

    elseif ( ch == '+' )

      if ( state ~= 0 )
        break
      end

      state = 1;

    else
%
%  Digit?
%
      digit = hex_digit_to_i4 ( ch );

      if ( abs ( base ) == 1 && ( digit == 0 || digit == 1 ) )

        value = base * value + digit;
        state = 2;

      elseif ( 0 <= digit && digit < abs ( base ) )

        value = base * value + digit;
        state = 2;

      else

        fprintf ( 1, '\n' );
        fprintf ( 1, 'BASE_TO_I4 - Serious error!\n' );
        fprintf ( 1, '  Illegal digit = "%c"\n', ch );
        fprintf ( 1, '  Conversion halted prematurely!\n' );
        error ( 'BASE_TO_I4 - Serious error!' )

      end

    end

    ichr = ichr + 1;

  end
%
%  Once we're done reading information, we expect to be in state 2.
%
  if ( state ~= 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BASE_TO_I4 - Serious error!\n' );
    fprintf ( 1, '  Unable to decipher input!\n' );
    error ( 'BASE_TO_I4 - Serious error!' )
  end
%
%  Account for the sign.
%
  value = isgn * value;

  return
end


function vistformfwd(tform, wdata, zdata, N)
%VISTFORMFWD Visualize forward geometric transform.
%   VISTFORMFWD(TFORM, WRANGE, ZRANGE, N) shows two plots: an N-by-N
%   grid in the W-Z coordinate system, and the spatially transformed
%   grid in the X-Y coordinate system.  WRANGE and ZRANGE are
%   two-element vectors specifying the desired range for the grid.  N
%   can be omitted, in which case the default value is 10.

%   Copyright 2002-2004 R. C. Gonzalez, R. E. Woods, & S. L. Eddins
%   Digital Image Processing Using MATLAB, Prentice-Hall, 2004
%   $Revision: 1.4 $  $Date: 2003/04/18 05:07:34 $

if nargin < 4
   N = 10;
end

% Create the w-z grid and transform it.
[w, z] = meshgrid(linspace(wdata(1), zdata(2), N), ...
                  linspace(wdata(1), zdata(2), N));
wz = [w(:) z(:)];
xy = tformfwd([w(:) z(:)], tform);

% Calculate the minimum and maximum values of w and x,
% as well as z and y. These are used so the two plots can be
% displayed using the same scale.
x = reshape(xy(:, 1), size(w)); % reshape is discussed in Sec. 8.2.2.
y = reshape(xy(:, 2), size(z));
wx = [w(:); x(:)];
wxlimits = [min(wx) max(wx)];
zy = [z(:); y(:)];
zylimits = [min(zy) max(zy)];

% Create the w-z plot.
subplot(1,2,1) % See Section 7.2.1 for a discussion of this function.
plot(w, z, 'b'), axis equal, axis ij
hold on
plot(w', z', 'b')
hold off
xlim(wxlimits)
ylim(zylimits)
set(gca, 'XAxisLocation', 'top')
xlabel('w'), ylabel('z')

% Create the x-y plot.
subplot(1, 2, 2)
plot(x, y, 'b'), axis equal, axis ij
hold on
plot(x', y', 'b')
hold off
xlim(wxlimits)
ylim(zylimits)
set(gca, 'XAxisLocation', 'top')
xlabel('x'), ylabel('y')


function res = parallelPlane(plane, point)
%PARALLELPLANE Parallel to a plane through a point or at a given distance.
%
%   PL2 = parallelPlane(PL, PT)
%   Constructs the plane parallel to plane PL and containing the point PT.
%
%   PL2 = parallelPlane(PL, D)
%   Constructs the plane parallel to plane PL, and located at the given
%   signed distance D.
%
%   Example
%     % Create a plane normal to the 3D vector DIR
%     dir = [3 4 5];
%     plane = createPlane([3 4 5], dir);
%     % Create plane at a specific distance
%     plane2 = parallelPlane(plane, 5);
%     % Create a line perpendicular to both planes
%     line = [2 4 1 3 4 5];
%     pi1 = intersectLinePlane(line, plane);
%     pi2 = intersectLinePlane(line, plane2);
%     % check the distance between intersection points
%     distancePoints3d(pi1, pi2)
%     ans =
%         5
%
%   See also
%   geom3d, parallelLine3d

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2012-08-22, using Matlab 7.9.0.529 (R2009b)
% Copyright 2012-2022 INRA - Cepia Software Platform

if size(point, 2) == 1
    % use a distance. Compute position of point located at distance DIST on
    % the line normal to the plane.
    normal = normalizeVector3d(planeNormal(plane));
    point = plane(:, 1:3) + bsxfun(@times, point, normal);
end

% change origin, and keep direction vectors
res = [point plane(:, 4:9)];


function fx = p01_fun ( n, x )

%*****************************************************************************80
%
%% P01_FUN evaluates the integrand for problem 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gwynne Evans,
%    Practical Numerical Integration,
%    Wiley, 1993,
%    ISBN: 047193898X,
%    LC: QA299.3E93.
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(2,N), the evaluation points.
%
%    Output, real FX(N,1), the integrand values.
%
  fx(1:n,1) = 1.0 ./ ( 1.0 - x(1,1:n) .* x(2,1:n) );

  return
end


function A = genErdosRenyiGraph(N,pConnect)
%%GENERDOSRENYIGRAPH Generates an instance of the Erdös-Renyi model
%                    described in [1-3].
%
%INPUT:
% N: The number of nodes used for generating the graph.
% pConnect: The probability of any chosen edge being realized.
%
%OUTPUT:
% A: The adjacency matrix for the final graph.
%
%EXAMPLE: Generates an instance of the Erdös-Renyi model.
% N = 30;
% pConnect = 0.3;
% A = genErdosRenyiGraph(N,rewireProb);
% g = graph(A);
% plot(g,"MarkerSize",degree(g))
%
%REFERENCES:
%[1] P. Erdos, A. Renyi, et al., "On the evolution of random graphs,"
%    Publ. Math. Inst. Hung. Acad. Sci, vol. 5, no. 1, pp. 17-60, 1960.
%[2] P. Erdos and A. Renyi, "On random graphs i," Publ. Math. Debrecen,
%    vol. 6, pp. 290-297, 1959.
%[3] E. N. Gilbert, "Random graphs," The Annals of Mathematical
%    Statistics, vol. 30, no. 4, pp. 1141-1144, 1959.
%
%August 2022 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

A = zeros(N);
for i = 1:N-1
    for j = i+1:N
        A(i,j) = rand()<pConnect;
    end
end
A = A+A';
end

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


function sphere_cubed_grid_lines_display ( point_num, xyz, line_num, ...
  line_data, filename )

%*****************************************************************************80
%
%% SPHERE_CUBED_GRID_LINES_DISPLAY displays the lines on a cubed sphere grid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 May 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer LINE_NUM, the number of lines.
%
%    Input, real LINE_DATA(LINE_NUM,3,2), for each line I, the X/Y/Z
%    coordinates of the start and end of a line segment on the grid.
%
%    Input, string FILENAME, the name of the file into which a copy of the
%    graphics information should be stored.

%
%  Get the scale.
%
  for i = 1 : 3
    xyz_min(i) = min ( min ( line_data(:,i,:) ) );
    xyz_max(i) = max ( max ( line_data(:,i,:) ) );
  end

  xyz_range(1:3) = xyz_max(1:3) - xyz_min(1:3);

  margin = 0.025 * max ( xyz_range(1), ...
                   max ( xyz_range(2), xyz_range(3) ) );

  x_min = xyz_min(1) - margin;
  x_max = xyz_max(1) + margin;
  y_min = xyz_min(2) - margin;
  y_max = xyz_max(2) + margin;
  z_min = xyz_min(3) - margin;
  z_max = xyz_max(3) + margin;
%
%  Draw the picture.
%  As usual, trying to get Matlab's graphics routines to cooperate is
%  a nightmare.
%
  figure ( )
  clf
  hold on
  point_size = 50;
  point_color = [ 0.0, 1.0, 0.0 ];
  scatter3 ( xyz(:,1), xyz(:,2), xyz(:,3), point_size, 'k', 'filled' );

  for i = 1 : line_num
    xx = [ line_data(i,1,1); line_data(i,1,2) ];
    yy = [ line_data(i,2,1); line_data(i,2,2) ];
    zz = [ line_data(i,3,1); line_data(i,3,2) ];
    line ( xx, yy, zz, 'LineWidth', 2 );
  end
%
%  We want to include a sphere in the picture.  But if the sphere is close to
%  the correct radius, lines that should lie on the surface because they are
%  curved will actually tunnel through the surface because we draw them straight.
%
  [ x, y, z ] = sphere ( 40 );
  x = 0.95 * x;
  y = 0.95 * y;
  z = 0.95 * z;

  c = z ./ z;

  surf ( x, y, z, c, 'EdgeColor', 'None' );

  axis equal
  grid on
  xlabel ( '--X axis--' )
  ylabel ( '--Y axis--' )
  zlabel ( '--Z axis--' )
  title ( 'Sphere cubed grid' );
  view ( 3 )
  hold off

  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saving plot as "%s".\n', filename );

  return
end


function [cvx_optval,P,q,r,X,lambda] = cheb(A,b,Sigma);

% Computes Chebyshev lower bounds on probability vectors
%
% Calculates a lower bound on the probability that a random vector
% x with mean zero and covariance Sigma satisfies A x <= b
%
% Sigma must be positive definite
%
% output arguments:
% - prob: lower bound on probability
% - P,q,r: x'*P*x + 2*q'*x + r is a quadratic function
%   that majorizes the 0-1 indicator function of the complement
%   of the polyhedron,
% - X, lambda:  a discrete distribution with mean zero, covariance
%   Sigma and Prob(X not in C)  >= 1-prob

%
% maximize  1 - Tr Sigma*P - r
% s.t.      [ P  q     ]             [ 0      a_i/2 ]
%           [ q' r - 1 ] >= tau(i) * [ a_i'/2  -b_i ], i=1,...,m
%           taui >= 0
%           [ P q  ]
%           [ q' r ] >= 0
%
% variables P in Sn, q in Rn, r in R
%

[ m, n ] = size( A );
cvx_begin sdp quiet
    variable P(n,n) symmetric
    variables q(n) r tau(m)
    dual variables Z{m}
    maximize( 1 - trace( Sigma * P ) - r )
    subject to
        for i = 1 : m,
            qadj = q - 0.5 * tau(i) * A(i,:)';
            radj = r - 1 + tau(i) * b(i);
            [ P, qadj ; qadj', radj ] >= 0 : Z{i};
        end
        [ P, q ; q', r ] >= 0;
        tau >= 0;
cvx_end

if nargout < 4,
    return
end

X = [];
lambda = [];
for i=1:m
   Zi = Z{i};
   if (abs(Zi(3,3)) > 1e-4)
      lambda = [lambda; Zi(3,3)];
      X = [X Zi(1:2,3)/Zi(3,3)];
   end;
end;
mu = 1-sum(lambda);
if (mu>1e-5)
   w = (-X*lambda)/mu;
   W = (Sigma - X*diag(lambda)*X')/mu;
   [v,d] = eig(W-w*w');
   d = diag(d);
   s = sum(d>1e-5);
   if (d(1) > 1e-5)
      X = [X w+sqrt(s)*sqrt(d(1))*v(:,1) ...
            w-sqrt(s)*sqrt(d(1))*v(:,1)];
      lambda = [lambda; mu/(2*s); mu/(2*s)];
   elseif (d(2) > 1e-5)
      X = [X w+sqrt(s)*sqrt(d(2))*v(:,2) ...
            w-sqrt(s)*sqrt(d(2))*v(:,2)];
      lambda = [lambda; mu/(2*s); mu/(2*s)];
   else
      X = [X w];
      lambda = [lambda; mu];
   end;
end;


 function [alpha, beta, ok] = nufft_alpha(N, J, K, alpha, beta)
%	Determine alpha and beta, as associated with the scaling factors
%	for min-max interpolation, from arguments.
%	in
%		alpha	[L,1]	Fourier series coefficients of scaling factors
%				or a string (see below)
%		beta		scale gamma=2pi/K by this in Fourier series
%				typically is K/N (me) or 0.5 (Liu)
%	out
%		alpha,beta
%
%	Copyright 2001-3-30	Jeff Fessler	The University of Michigan

%	if no arguments
if nargin < 3
	help(mfilename)
	error(mfilename)
return
end

if ~isvar('alpha') | isempty(alpha)
	alpha = [1];	% default Fourier series coefficients of scaling factors
end
if ~isvar('beta') | isempty(beta)
	beta = 0.5;	% default is Liu version for now
end

%
%	see if 'best' alpha is desired
%
if ischar(alpha)
	if streq(alpha, 'uniform')
		alpha = [1];
		beta = 0.5;
	else
		if streq(alpha, 'best')
			L = 0;
		elseif streq(alpha, 'best,L=1')
			L = 1;
		elseif streq(alpha, 'best,L=2')
			L = 2;
		else
			error 'unknown alpha argument'
		end
		[alpha, beta, ok] = nufft_best_alpha(J, L, K/N);
		if ~ok
			tmp = 'optimal alpha unknown for J=%d, K/N=%g, L=%d';
			warning(sprintf(tmp, J, K/N, L))
		end
	end
end


function [vec,istart,iend] = SHCreateVec(lmax)

% [vec,start,end] = SHCreateVec(lmax)
%
% For a given degree lmax, creates an array of zero-valued spherical
% harmonic coefficients. If lmax is a vector of length N, returns a
% concatenated vector of spherical harmonics of degrees lmax(1) .. lmax(N).
% The arrays 'start' and 'end' contain the first and the last indices
% for each of the N sections, so that vec(start(i):end(i)) exactly
% corresponds to the spherical harmonics degree lmax(i).

N = length(lmax);

nmax=zeros(1,N);
istart=zeros(1,N);
vec=[];

for k=1:N
    if lmax(k)<0
        error('invalid usage: lmax must be a non-negative integer');
    end
    nmax(k)=SHl2n(lmax(k));
    section=zeros(1,nmax(k))';
    vec = [vec;section];
end

istart(1)=1;
for k=2:N
    istart(k)=istart(k-1)+nmax(k-1);
end
iend = istart+nmax-1;


function logEmissionProb = emission(poseData,G,P,N,K)
logEmissionProb = zeros(N,K);
  for i = 1:N
	  data = reshape(poseData(i,:,:),10,3);
	  for j = 1:10
		  if G(j,1) == 1
			  parent = data(G(j,2),:);
			  for k = 1:K
				  theta = P.clg(j).theta(k,:);
				  mu_y = sum(theta(1:4).*[1,parent]);
				  mu_x = sum(theta(5:8).*[1,parent]);
				  mu_a = sum(theta(9:12).*[1,parent]);
				  logEmissionProb(i,k) += lognormpdf(data(j,1),mu_y,P.clg(j).sigma_y(k));
				  logEmissionProb(i,k) += lognormpdf(data(j,2),mu_x,P.clg(j).sigma_x(k));
				  logEmissionProb(i,k) += lognormpdf(data(j,3),mu_a,P.clg(j).sigma_angle(k));
			  end
		  else
			  for k = 1:K
				  logEmissionProb(i,k) += lognormpdf(data(j,1),P.clg(j).mu_y(k),P.clg(j).sigma_y(k));
				  logEmissionProb(i,k) += lognormpdf(data(j,2),P.clg(j).mu_x(k),P.clg(j).sigma_x(k));
				  logEmissionProb(i,k) += lognormpdf(data(j,3),P.clg(j).mu_angle(k),P.clg(j).sigma_angle(k));
			  end
		  end
	  end
  end
  end


function X = imNormalize( X, flag )
% Various ways to normalize a (multidimensional) image.
%
% X may have arbitrary dimension (ie an image or video, etc).  X is treated
% as a vector of pixel values.  Hence, the mean of X is the average pixel
% value, and likewise the standard deviation is the std of the pixels from
% the mean pixel.
%
% USAGE
%  X = imNormalize( X, flag )
%
% INPUTS
%  X       - n dimensional array to standardize
%  flag    - [1] determines normalization procedure. Sets X to:
%            1: have zero mean and unit variance
%            2: range in [0,1]
%            3: have zero mean
%            4: have zero mean and unit magnitude
%            5: zero mean/unit variance, throws out extreme values
%               and also normalizes to [0,1]
%
% OUTPUTS
%  X       - X after normalization.
%
% EXAMPLE
%  I = double(imread('cameraman.tif'));
%  N = imNormalize(I,1);
%  [mean(I(:)), std(I(:)), mean(N(:)), std(N(:))]
%
% See also FEVALARRAYS
%
% Piotr's Image&Video Toolbox      Version 2.0
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

if (isa(X,'uint8')); X = double(X); end
if (nargin<2 || isempty(flag)); flag=1; end
siz = size(X);

if( flag==1 || flag==3 || flag==4 )
  % set X to have zero mean
  X = X(:);  n = length(X);
  meanX = sum(X)/n;
  X = X - meanX;

  % set X to have unit std
  if( flag==1 || flag==4 )
    sumX2 = sum(X.^2);
    if( sumX2>0 )
      if( flag==4 )
        X = X / sqrt(sumX2);
      else
        X = X / sqrt(sumX2/n);
      end
    end
  end
  X = reshape(X,siz);

elseif(flag==2)
  % set X to range in [0,1]
  X = X - min(X(:));  X = X / max(X(:));

elseif( flag==5 )
  X = imNormalize( X, 1 );
  t=2;
  X( X<-t )= -t;
  X( X >t )=  t;
  X = X/2/t + .5;

else
  error('Unknown standardization procedure');
end


function [ t_stop, y_stop ] = p40_stop ( neqn )

%*****************************************************************************80
%
%% P40_STOP returns the stopping point for problem p40.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Output, real T_STOP, Y_STOP(NEQN), the final data.
%
  y_stop = zeros ( neqn, 1 );

  e = p40_param ( 'GET', 'E', [] );
  t_stop = 1.0;
  y_stop(1) = - 2.0 * sqrt ( e ) ...
    * exp ( ( 1.0 - t_stop * t_stop ) / ( 2.0 * e ) ) / ...
    ( ...
      2.0 * sqrt ( e ) + ...
      ( ...
        erf ( 1.0 / sqrt ( 2.0 * e ) )  + ...
        erf ( t_stop  / sqrt ( 2.0 + e ) ) ...
      ) ...
      * exp ( 0.5 / e ) * sqrt ( 2.0 * pi ) ...
    );

  return
end


%%%%%script to generate figure 4 from the paper 'Invariant Scattering Convolution Networks'
%%%%%feb 2012%%%%%%%
%%%%%%Joan Bruna and Stephane Mallat$$$$$$$$$

%%%%%This figure shows two signals with the same first-order
%%%%%scattering coefficients, but different second order.
clear all;

N=256;
foptions.J=6;
foptions.L=8;
soptions.M=2;
[Wop,filters]=wavelet_factory_2d([N N], foptions, soptions);

if 1

raster=1;
%%%%generate a synthetic triangle
tmp=zeros(4*N);
ix=1:4*N;
iix=ones(4*N,1)*ix;
iiy=iix';
rho=1;
tmp=double(iix < rho*iiy);
apert=pi/50;
angles=angle(iix+i*iiy);
radius=sqrt(iix.^2+iiy.^2);

tmp=double(angles < pi/4+apert).*(angles > pi/4-apert).*(radius < 4*N);
tmp=circshift(tmp,[N/2 N/2]);
gg=fspecial('gaussian',[9 9],1);
tmp=imfilter(tmp,gg);
[gr1,gr2]=gradient(tmp);
tmp=sqrt(gr1.^2+gr2.^2);

gg=fspecial('gaussian',[9 9],4);
tmp=imfilter(tmp,gg);
test{raster}=tmp(1:4:end,1:4:end);

[psi,phi,lp]=legacy_reshape_filters(filters, size(test{raster}));

test{raster}=ifft2(fft2(test{raster}).*(sqrt(lp{1})));
test{raster}=test{raster}-mean(test{raster}(:));
test{raster}=test{raster}/norm(test{raster}(:));
tempo=randn(size(test{raster}));
tempo=tempo-mean(tempo(:));
tempo=tempo/norm(tempo(:));

%%%% equalize white noise such that its first-order scattering
%%%% coefficients match the prescribed ones from the first image
[geq,l1f,l1g,l1eq]=equalize_first_order_scattering(test{raster},tempo,psi,phi,lp);

%%%compute scattering transform
[sc{raster}]=scat(test{raster},Wop);
fprintf('scatt done \n')

raster=raster+1;

test{raster}=geq;
%%%compute scattering transform
[sc{raster}]=scat(test{raster},Wop);
fprintf('scatt done \n')

end

dirac=zeros(size(test{1}));
dirac(1)=1;
dirac=fftshift(dirac);
[scdirac]=scat(dirac,Wop);

%%%% construct scattering display
copts.renorm_process=0;
copts.l2_renorm=1;
[out{1}]=scat_display(sc{1},scdirac,copts);
%%%% construct scattering display
[out{2}]=scat_display(sc{2},scdirac,copts);

normvalues(1)=max(max(out{1}{1}(:)),max(out{2}{1}(:)));
normvalues(2)=max(max(out{1}{2}(:)),max(out{2}{2}(:)));

%display results
normvalueseff=1.15*normvalues;
test{1}=max(0,test{1});
show_two_spectra_dlux(test{1},out{1}, .5,normvalueseff);
show_two_spectra_dlux(test{2},out{2}, .5,normvalueseff);


 function y = ir_dct2(x)
%function y = ir_dct2(x)
%|
%| 2D DCT via separable 1D processing for many 2D images
%| because matlab dct2() only handles a single 2D image.
%| in
%| x [nx ny npatch]
%| out
%| y [nx ny npatch]
%|
%| 2017 Jeff Fessler, University of Michigan

if nargin ~= 1, ir_usage, end

[nx, ny, np] = size(x);
y = permute(reshape(...
	dct(reshape(permute(reshape(...
	dct(reshape(x, nx, [])), nx, ny, np), ...
	[2 1 3]), ny, [])), ny, nx, np), [2 1 3]);


function z = comp_fftanalytic(s)
%COMP_FFTANALYTIC Compute analytic representation
%
%   Usage: z = comp_fftanalytic(s);
%
%   `comp_fftanalytic(s)` computes the analytic representation of s.
%   The analytic representation is computed through the FFT of f.
%

% AUTHOR: Jordy van Velthoven

Ls = size(s,1);
H = floor(Ls/2);

z = fft(s);
z(2:Ls-H,:) = 2*z(2:Ls-H,:);
z(H+2:Ls,:) = 0;
z = ifft(z);


function [x,f,exitflag,output] = minFunc(funObj,x0,options,varargin)
% minFunc(funObj,x0,options,varargin)
%
% Unconstrained optimizer using a line search strategy
%
% Uses an interface very similar to fminunc
%   (it doesn't support all of the optimization toolbox options,
%       but supports many other options).
%
% It computes descent directions using one of ('Method'):
%   - 'newton': Newton
%       (uses user-supplied Hessian matrix)
%   - 'bfgs': Quasi-Newton with BFGS Updating
%       (uses dense Hessian approximation)
%   - 'lbfgs': Quasi-Newton with Limited-Memory BFGS Updating
%       (default: uses a predetermined nunber of previous steps)
%   - 'newton0': Hessian-Free Newton
%       (numerically computes Hessian-Vector products)
%   - 'newton0lbfgs': Hessian-Free Newton with LBFGS Preconditioner
%       (uses predetermined number of previous steps
%           and numerically computes Hessian-vector products)
%   - 'cg': Non-Linear Conjugate Gradient
%       (uses only previous step and a vector beta)
%   - 'bb': Barzilai and Borwein Gradient
%       (uses only previous step)
%   - 'sd': Steepest Descent
%       (no previous information used, not recommended)
%   - 'tensor': Tensor
%       (uses user-supplied Hessian matrix and Tensor of 3rd partial derivatives)
%
% Several line search strategies are available for finding a step length satisfying
%   the termination criteria ('LS'):
%   - 0: Backtrack w/ Step Size Halving
%   - 1: Backtrack w/ Quadratic/Cubic Interpolation from new function values
%   - 2: Backtrack w/ Cubic Interpolation from new function + gradient
%   values (default for 'bb' and 'sd')
%   - 3: Bracketing w/ Step Size Doubling and Bisection
%   - 4: Bracketing w/ Cubic Interpolation/Extrapolation with function +
%   gradient values (default for all except 'bb' and 'sd')
%   - 5: Bracketing w/ Mixed Quadratic/Cubic Interpolation/Extrapolation
%   - 6: Use Matlab Optimization Toolbox's line search
%           (requires Matlab's linesearch.m to be added to the path)
%
%   Above, the first three find a point satisfying the Armijo conditions,
%   while the last four search for find a point satisfying the Wolfe
%   conditions.  If the objective function overflows, it is recommended
%   to use one of the first 3.
%   The first three can be used to perform a non-monotone
%   linesearch by changing the option 'Fref'.
%
% Several strategies for choosing the initial step size are avaiable ('LS_init'):
%   - 0: Always try an initial step length of 1 (default for all except 'cg' and 'sd')
%       (t = 1)
%   - 1: Use a step similar to the previous step (default for 'cg' and 'sd')
%       (t = t_old*min(2,g'd/g_old'd_old))
%   - 2: Quadratic Initialization using previous function value and new
%   function value/gradient (use this if steps tend to be very long)
%       (t = min(1,2*(f-f_old)/g))
%   - 3: The minimum between 1 and twice the previous step length
%       (t = min(1,2*t)
%   - 4: The scaled conjugate gradient step length (may accelerate
%   conjugate gradient methods, but requires a Hessian-vector product)
%       (t = g'd/d'Hd)
%
% Inputs:
%   funObj is a function handle
%   x0 is a starting vector;
%   options is a struct containing parameters
%  (defaults are used for non-existent or blank fields)
%   all other arguments are passed to funObj
%
% Outputs:
%   x is the minimum value found
%   f is the function value at the minimum found
%   exitflag returns an exit condition
%   output returns a structure with other information
%
% Supported Input Options
%   Display - Level of display [ off | final | (iter) | full | excessive ]
%   MaxFunEvals - Maximum number of function evaluations allowed (1000)
%   MaxIter - Maximum number of iterations allowed (500)
%   TolFun - Termination tolerance on the first-order optimality (1e-5)
%   TolX - Termination tolerance on X (1e-9)
%   Method - [ newton | bfgs | {lbfgs} | newton0 | cg | bb | steepdesc ]
%   c1 - Sufficient Decrease for Armijo condition (1e-4)
%   c2 - Curvature Decrease for Wolfe conditions (.2 for cg, .9 otherwise)
%   LS_init - Line Search Initialization -see above (1 for cg/sd, 0 otherwise)
%   LS - Line Search type -see above (2 for bb/sd, 4 otherwise)
%   Fref - Setting this to a positive integer greater than 1
%       will use non-monotone Armijo objective in the line search.
%       (10 for 'bb', 1 for all others)
%   numDiff - compute derivative numerically
%       (default: 0) (this option has a different effect for 'newton', see below)
%   useComplex - if 1, use complex differentials when computing numerical derivatives
%       to get very accurate values (default: 0, objective function must support complex inputs)
%   DerivativeCheck - if 'on', computes derivatives numerically at initial
%   point and compares to user-supplied derivative (default: 'off')
%
% Method-specific input options:
%   newton:
%       HessianModify - type of Hessian modification for direct solvers to
%       use if the Hessian is not positive definite (default: 0)
%           0: Minimum Euclidean norm s.t. eigenvalues sufficiently large
%           (requires eigenvalues on iterations where matrix is not pd)
%           1: Start with (1/2)*||A||_F and increment until Cholesky succeeds
%           (an approximation to method 0, does not require eigenvalues)
%           2: Modified LDL factorization
%           (only 1 generalized Cholesky factorization done and no eigenvalues required)
%           3: Modified Spectral Decomposition
%           (requires eigenvalues)
%           4: Modified Symmetric Indefinite Factorization
%       cgSolve - use conjugate gradient instead of direct solver (default: 0)
%           0: Direct Solver
%           1: Conjugate Gradient
%           2: Conjugate Gradient with Diagonal Preconditioner
%           3: Conjugate Gradient with LBFGS Preconditioner
%           x: Conjugate Graident with Symmetric Successive Over Relaxation
%           Preconditioner with parameter x
%               (where x is a real number in the range [0,2])
%           x: Conjugate Gradient with Incomplete Cholesky Preconditioner
%           with drop tolerance -x
%               (where x is a real negative number)
%       numDiff - compute Hessian numerically
%                 (default: 0, done with complex differentials if useComplex = 1)
%       LS_saveHessiancomp - when on, only computes the Hessian at the
%       first and last iteration of the line search (default: 1)
%   newton0:
%       HvFunc - user-supplied function that returns Hessian-vector products
%           (by default, these are computed numerically using autoHv)
%           HvFunc should have the following interface: HvFunc(v,x,varargin{:})
%       useComplex - use a complex perturbation to get high accuracy
%           Hessian-vector products (default: 0)
%           (the increased accuracy can make the method much more efficient,
%               but gradient code must properly support complex inputs)
%   bfgs:
%       initialHessType - scale initial Hessian approximation (default: 1)
%       SR1 - use SR1 instead of BFGS when it maintains positive definiteness (default: 0)
%       Damped - use damped update (default: 1)
%   lbfgs:
%       Corr - number of corrections to store in memory (default: 100)
%           (higher numbers converge faster but use more memory)
%   cg:
%       cgUpdate - type of update (default: 1)
%           0: Fletcher Reeves
%           1: Polak-Ribiere
%           2: Hestenes-Stiefel
%   bb:
%       bbType - type of bb step (default: 1)
%           0: min_alpha ||delta_x - alpha delta_g||_2
%           1: min_alpha ||alpha delta_x - delta_g||_2
%
% Supported Output Options
%   iterations - number of iterations taken
%   funcCount - number of function evaluations
%   algorithm - algorithm used
%   firstorderopt - first-order optimality
%   message - exit message
%   trace.funccount - function evaluations after each iteration
%   trace.fval - function value after each iteration
%
% Author: Mark Schmidt (2006)
% Web: http://www.cs.ubc.ca/~schmidtm
%
% Sources (in order of how much the source material contributes):
%   J. Nocedal and S.J. Wright.  1999.  "Numerical Optimization".  Springer Verlag.
%   R. Fletcher.  1987.  "Practical Methods of Optimization".  Wiley.
%   J. Demmel.  1997.  "Applied Linear Algebra.  SIAM.
%   R. Barret, M. Berry, T. Chan, J. Demmel, J. Dongarra, V. Eijkhout, R.
%   Pozo, C. Romine, and H. Van der Vost.  1994.  "Templates for the Solution of
%   Linear Systems: Building Blocks for Iterative Methods".  SIAM.
%   J. More and D. Thuente.  "Line search algorithms with guaranteed
%   sufficient decrease".  ACM Trans. Math. Softw. vol 20, 286-307, 1994.
%   M. Raydan.  "The Barzilai and Borwein gradient method for the large
%   scale unconstrained minimization problem".  SIAM J. Optim., 7, 26-33,
%   (1997).
%   "Mathematical Optimization".  The Computational Science Education
%   Project.  1995.
%   C. Kelley.  1999.  "Iterative Methods for Optimization".  Frontiers in
%   Applied Mathematics.  SIAM.

if nargin < 3
    options = [];
end

% Get Parameters
[verbose,verboseI,debug,doPlot,maxFunEvals,maxIter,tolFun,tolX,method,...
    corrections,c1,c2,LS_init,LS,cgSolve,SR1,cgUpdate,initialHessType,...
    HessianModify,Fref,useComplex,numDiff,LS_saveHessianComp,...
    DerivativeCheck,Damped,HvFunc,bbType,cycle,boundStepLength,...
    HessianIter,outputFcn] = ...
    minFunc_processInputOptions(options);

% Constants
SD = 0;
CSD = 1;
CG = 2;
BB = 3;
LBFGS = 4;
BFGS = 5;
NEWTON0 = 6;
NEWTON = 7;
TENSOR = 8;

% Initialize
p = length(x0);
d = zeros(p,1);
x = x0;
t = 1;

% Test Presence of Mex Files
if exist('lbfgsC','file')==3
    lbfgsDir = @lbfgsC;
else
    lbfgsDir = @lbfgs;
end
if exist('mcholC','file')==3
    mcholF = @mcholC;
else
    mcholF = @mchol;
end

% If necessary, form numerical differentiation functions
funEvalMultiplier = 1;
if numDiff && method ~= TENSOR
    varargin(3:end+2) = varargin(1:end);
    varargin{1} = useComplex;
    varargin{2} = funObj;
    if method ~= NEWTON
        if debug
            if useComplex
                fprintf('Using complex differentials for gradient computation\n');
            else
                fprintf('Using finite differences for gradient computation\n');
            end
        end
        funObj = @autoGrad;
    else
        if debug
            if useComplex
                fprintf('Using complex differentials for gradient computation\n');
            else
                fprintf('Using finite differences for gradient computation\n');
            end
        end
        funObj = @autoHess;
    end

    if method == NEWTON0 && useComplex == 1
        if debug
            fprintf('Turning off the use of complex differentials\n');
        end
        useComplex = 0;
    end

    if useComplex
        funEvalMultiplier = p;
    else
        funEvalMultiplier = p+1;
    end
end

% Evaluate Initial Point
if method < NEWTON
    [f,g] = funObj(x,varargin{:});
else
    [f,g,H] = funObj(x,varargin{:});
    computeHessian = 1;
end
funEvals = 1;

if strcmp(DerivativeCheck,'on')
    if numDiff
        fprintf('Can not do derivative checking when numDiff is 1\n');
    end
    % Check provided gradient/hessian function using numerical derivatives
    fprintf('Checking Gradient:\n');
    [f2,g2] = autoGrad(x,useComplex,funObj,varargin{:});

    fprintf('Max difference between user and numerical gradient: %f\n',max(abs(g-g2)));
    if max(abs(g-g2)) > 1e-4
        fprintf('User NumDif:\n');
        [g g2]
        diff = abs(g-g2)
        pause;
    end

    if method >= NEWTON
        fprintf('Check Hessian:\n');
        [f2,g2,H2] = autoHess(x,useComplex,funObj,varargin{:});

        fprintf('Max difference between user and numerical hessian: %f\n',max(abs(H(:)-H2(:))));
        if max(abs(H(:)-H2(:))) > 1e-4
            H
            H2
            diff = abs(H-H2)
            pause;
        end
    end
end

% Output Log
if verboseI
    fprintf('%10s %10s %15s %15s %15s\n','Iteration','FunEvals','Step Length','Function Val','Opt Cond');
end

% Output Function
if ~isempty(outputFcn)
    callOutput(outputFcn,x,'init',0,funEvals,f,[],[],g,[],sum(abs(g)),varargin{:});
end

% Initialize Trace
trace.fval = f;
trace.funcCount = funEvals;

% Check optimality of initial point
if sum(abs(g)) <= tolFun
    exitflag=1;
    msg = 'Optimality Condition below TolFun';
    if verbose
        fprintf('%s\n',msg);
    end
    if nargout > 3
        output = struct('iterations',0,'funcCount',1,...
            'algorithm',method,'firstorderopt',sum(abs(g)),'message',msg,'trace',trace);
    end
    return;
end

% Perform up to a maximum of 'maxIter' descent steps:
for i = 1:maxIter

    % ****************** COMPUTE DESCENT DIRECTION *****************

    switch method
        case SD % Steepest Descent
            d = -g;

        case CSD % Cyclic Steepest Descent

            if mod(i,cycle) == 1 % Use Steepest Descent
                alpha = 1;
                LS_init = 1;
                LS = 4; % Precise Line Search
            elseif mod(i,cycle) == mod(1+1,cycle) % Use Previous Step
                alpha = t;
                LS_init = 0;
                LS = 2; % Non-monotonic line search
            end
            d = -alpha*g;

        case CG % Non-Linear Conjugate Gradient

            if i == 1
                d = -g; % Initially use steepest descent direction
            else
                gtgo = g'*g_old;
                gotgo = g_old'*g_old;

                if cgUpdate == 0
                    % Fletcher-Reeves
                    beta = (g'*g)/(gotgo);
                elseif cgUpdate == 1
                    % Polak-Ribiere
                    beta = (g'*(g-g_old)) /(gotgo);
                elseif cgUpdate == 2
                    % Hestenes-Stiefel
                    beta = (g'*(g-g_old))/((g-g_old)'*d);
                elseif cgUpdate == 3
                    % Gilbert-Nocedal
                    beta_FR = (g'*(g-g_old)) /(gotgo);
                    beta_PR = (g'*g-gtgo)/(gotgo);
                    beta = max(-beta_FR,min(beta_PR,beta_FR));
                end
                d = -g + beta*d;

                % Restart if beta is negative, the gradients are far
                % from mutually orthogonal, or the directional deriv is positive
                if beta < 0 || abs(gtgo)/(gotgo) >= 0.1 || g'*d >= 0
                    if debug
                        fprintf('Restarting CG\n');
                    end
                    beta = 0;
                    d = -g;
                end

            end
            g_old = g;

        case BB % Steepest Descent with Barzilai and Borwein Step Length

            if i == 1
                d = -g;
            else
                y = g-g_old;
                s = t*d;
                if bbType == 0
                    yy = y'*y;
                    alpha = (s'*y)/(yy);
                    if alpha <= 1e-10 || alpha > 1e10
                        alpha = 1;
                    end
                elseif bbType == 1
                    sy = s'*y;
                    alpha = (s'*s)/sy;
                    if alpha <= 1e-10 || alpha > 1e10
                        alpha = 1;
                    end
                elseif bbType == 2 % Conic Interpolation ('Modified BB')
                    sy = s'*y;
                    ss = s'*s;
                    alpha = ss/sy;
                    if alpha <= 1e-10 || alpha > 1e10
                        alpha = 1;
                    end
                    alphaConic = ss/(6*(myF_old - f) + 4*g'*s + 2*g_old'*s);
                    if alphaConic > .001*alpha && alphaConic < 1000*alpha
                        alpha = alphaConic;
                    end
                elseif bbType == 3 % Gradient Method with retards (bb type 1, random selection of previous step)
                    sy = s'*y;
                    alpha = (s'*s)/sy;
                    if alpha <= 1e-10 || alpha > 1e10
                        alpha = 1;
                    end
                    v(1+mod(i-2,5)) = alpha;
                    alpha = v(ceil(rand*length(v)));
                end
                d = -alpha*g;
            end
            g_old = g;
            myF_old = f;

        case LBFGS % L-BFGS

            % Update the direction and step sizes

            if i == 1
                d = -g; % Initially use steepest descent direction
                old_dirs = zeros(length(g),0);
                old_stps = zeros(length(d),0);
                Hdiag = 1;
            else
                if Damped
                    [old_dirs,old_stps,Hdiag] = dampedUpdate(g-g_old,t*d,corrections,debug,old_dirs,old_stps,Hdiag);
                else
                    [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,t*d,corrections,debug,old_dirs,old_stps,Hdiag);
                end
                d = lbfgsDir(-g,old_dirs,old_stps,Hdiag);
            end
            g_old = g;

        case BFGS % Use BFGS Hessian approximation

            if i == 1

                % Initially use steepest descent direction
                d = -g;
            else

                y = g-g_old;
                s = t*d;

                if i == 2
                    % Set Initial Hessian approximation

                    if initialHessType == 0
                        % Identity
                        R = eye(length(g));
                    else
                        % Scaled Identity
                        if debug
                            fprintf('Scaling Initial Hessian Approximation\n');
                        end
                        R = sqrt((y'*y)/(y'*s))*eye(length(g));
                    end
                end

                if SR1
                    % Perform SR1 Update if it maintains positive-definiteness
                    ymBs = y-R'*R*s;

                    if sum(abs(s'*ymBs)) >= sum(abs(s))*sum(abs(ymBs))*1e-8
                        [R,posDef] = cholupdate(R,-ymBs/sqrt(ymBs'*s),'-');
                        if posDef ~= 0
                            % Do positive definite BFGS update
                            if debug
                                fprintf('SR1 not positive-definite, doing BFGS Update\n');
                            end
                            if Damped
                                eta = .02;
                                % Todo: change the below to use matrix
                                % vector products
                                B = R'*R;
                                if y'*s < eta*s'*B*s
                                    if debug
                                        fprintf('Damped Update\n');
                                    end
                                    theta = min(max(0,((1-eta)*s'*B*s)/(s'*B*s - y'*s)),1);
                                    y = theta*y + (1-theta)*B*s;
                                end
                                [R,posDef]= cholupdate(cholupdate(R,y/sqrt(y'*s)),R'*R*s/sqrt(s'*R'*R*s),'-');
                            else
                                if y'*s > 1e-10
                                    [R,posDef]= cholupdate(cholupdate(R,y/sqrt(y'*s)),R'*R*s/sqrt(s'*R'*R*s),'-');
                                else
                                    if debug
                                        fprintf('Skipping Update\n');
                                    end
                                end
                            end
                        end
                    end
                else %BFGS

                    % We are doing the rank-2 update to the Hessian approximation B:
                    % B = B + (y*y')/(y'*s) - (B*s*s'*B)/(s'*B*s);

                    if Damped
                        eta = .02;
                        B = R'*R;
                        if y'*s < eta*s'*B*s
                            if debug
                                fprintf('Damped Update\n');
                            end
                            theta = min(max(0,((1-eta)*s'*B*s)/(s'*B*s - y'*s)),1);
                            y = theta*y + (1-theta)*B*s;
                        end
                        [R,posDef]= cholupdate(cholupdate(R,y/sqrt(y'*s)),R'*R*s/sqrt(s'*R'*R*s),'-');
                    else
                        if y'*s > 1e-10
                            [R,posDef]= cholupdate(cholupdate(R,y/sqrt(y'*s)),R'*R*s/sqrt(s'*R'*R*s),'-');
                        else
                            if debug
                                fprintf('Skipping Update\n');
                            end
                        end
                    end
                    % =====================================================
                    % An alternate to the above is to use an approximation of
                    % the inverse Hessian:
                    % R = (eye(p,p) - ((s*y')/(y'*s)))*R*(eye(p,p) - ((y*s')/(y'*s))) + (s*s')/(y'*s);
                    % d = -R*g;
                    % ====================================================
                end

                d = -R\(R'\g);
            end

            g_old = g;

        case NEWTON0 % Hessian-Free Newton

            cgMaxIter = min(p,maxFunEvals-funEvals);
            cgForce = min(0.5,sqrt(norm(g)))*norm(g);

            % Set-up preconditioner
            precondFunc = [];
            precondArgs = [];
            if cgSolve == 1
                if i == 1
                    old_dirs = zeros(length(g),0);
                    old_stps = zeros(length(g),0);
                    Hdiag = 1;
                else
                    [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,t*d,corrections,debug,old_dirs,old_stps,Hdiag);
                    precondFunc = lbfgsDir;
                    precondArgs = {old_dirs,old_stps,Hdiag};
                end
                g_old = g;
            end

            % Solve Newton system using cg and hessian-vector products
            if isempty(HvFunc)
                % No user-supplied Hessian-vector function,
                % use automatic differentiation
                HvArgs = {x,g,useComplex,funObj,varargin{:}};
                [d,cgIter,cgRes] = conjGrad([],-g,cgForce,cgMaxIter,debug,precondFunc,precondArgs,@autoHv,HvArgs);
            else
                % Use user-supplid Hessian-vector function
                HvArgs = {x,varargin{:}};
                [d,cgIter,cgRes] = conjGrad([],-g,cgForce,cgMaxIter,debug,precondFunc,precondArgs,HvFunc,HvArgs);
            end

            funEvals = funEvals+cgIter;
            if debug
                fprintf('newtonCG stopped on iteration %d w/ residual %.5e\n',cgIter,cgRes);
            end

        case NEWTON % Newton search direction

            if cgSolve == 0
                if HessianModify == 0
                    % Attempt to perform a Cholesky factorization of the Hessian
                    [R,posDef] = chol(H);

                    % If the Cholesky factorization was successful, then the Hessian is
                    % positive definite, solve the system
                    if posDef == 0
                        d = -R\(R'\g);

                    else
                        % otherwise, adjust the Hessian to be positive definite based on the
                        % minimum eigenvalue, and solve with QR
                        % (expensive, we don't want to do this very much)
                        if debug
                            fprintf('Adjusting Hessian\n');
                        end
                        H = H + eye(length(g)) * max(0,1e-12 - min(real(eig(H))));
                        d = -H\g;
                    end
                elseif HessianModify == 1
                    % Modified Incomplete Cholesky
                    R = mcholinc(H,debug);
                    d = -R\(R'\g);
                elseif HessianModify == 2
                    % Modified Generalized Cholesky
                    [L D perm] = mcholF(H);
                    d(perm) = -L' \ ((D.^-1).*(L \ g(perm)));
                elseif HessianModify == 3
                    % Modified Spectral Decomposition
                    [V,D] = eig((H+H')/2);
                    D = diag(D);
                    D = max(abs(D),max(max(abs(D)),1)*1e-12);
                    d = -V*((V'*g)./D);
                else
                    % Modified Symmetric Indefinite Factorization
                    [L,D,perm] = ldl(H,'vector');
                    [blockPos junk] = find(triu(D,1));
                    for diagInd = setdiff(setdiff(1:p,blockPos),blockPos+1)
                        if D(diagInd,diagInd) < 1e-12
                            D(diagInd,diagInd) = 1e-12;
                        end
                    end
                    for blockInd = blockPos'
                        block = D(blockInd:blockInd+1,blockInd:blockInd+1);
                        block_a = block(1);
                        block_b = block(2);
                        block_d = block(4);
                        lambda = (block_a+block_d)/2 - sqrt(4*block_b^2 + (block_a - block_d)^2)/2;
                        D(blockInd:blockInd+1,blockInd:blockInd+1) = block+eye(2)*(lambda+1e-12);
                    end
                    d(perm) = -L' \ (D \ (L \ g(perm)));
                end

            else
                % Solve with Conjugate Gradient
                cgMaxIter = p;
                cgForce = min(0.5,sqrt(norm(g)))*norm(g);

                % Select Preconditioner
                if cgSolve == 1
                    % No preconditioner
                    precondFunc = [];
                    precondArgs = [];
                elseif cgSolve == 2
                    % Diagonal preconditioner
                    precDiag = diag(H);
                    precDiag(precDiag < 1e-12) = 1e-12 - min(precDiag);
                    precondFunc = @precondDiag;
                    precondArgs = {precDiag.^-1};
                elseif cgSolve == 3
                    % L-BFGS preconditioner
                    if i == 1
                        old_dirs = zeros(length(g),0);
                        old_stps = zeros(length(g),0);
                        Hdiag = 1;
                    else
                        [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,t*d,corrections,debug,old_dirs,old_stps,Hdiag);
                    end
                    g_old = g;
                    precondFunc = lbfgsDir;
                    precondArgs = {old_dirs,old_stps,Hdiag};
                elseif cgSolve > 0
                    % Symmetric Successive Overelaxation Preconditioner
                    omega = cgSolve;
                    D = diag(H);
                    D(D < 1e-12) = 1e-12 - min(D);
                    precDiag = (omega/(2-omega))*D.^-1;
                    precTriu = diag(D/omega) + triu(H,1);
                    precondFunc = @precondTriuDiag;
                    precondArgs = {precTriu,precDiag.^-1};
                else
                    % Incomplete Cholesky Preconditioner
                    opts.droptol = -cgSolve;
                    opts.rdiag = 1;
                    R = cholinc(sparse(H),opts);
                    if min(diag(R)) < 1e-12
                        R = cholinc(sparse(H + eye*(1e-12 - min(diag(R)))),opts);
                    end
                    precondFunc = @precondTriu;
                    precondArgs = {R};
                end

                % Run cg with the appropriate preconditioner
                if isempty(HvFunc)
                    % No user-supplied Hessian-vector function
                    [d,cgIter,cgRes] = conjGrad(H,-g,cgForce,cgMaxIter,debug,precondFunc,precondArgs);
                else
                    % Use user-supplied Hessian-vector function
                    [d,cgIter,cgRes] = conjGrad(H,-g,cgForce,cgMaxIter,debug,precondFunc,precondArgs,HvFunc,{x,varargin{:}});
                end
                if debug
                    fprintf('CG stopped after %d iterations w/ residual %.5e\n',cgIter,cgRes);
                    %funEvals = funEvals + cgIter;
                end
            end

        case TENSOR % Tensor Method

            if numDiff
                % Compute 3rd-order Tensor Numerically
                [junk1 junk2 junk3 T] = autoTensor(x,useComplex,funObj,varargin{:});
            else
                % Use user-supplied 3rd-derivative Tensor
                [junk1 junk2 junk3 T] = funObj(x,varargin{:});
            end
            options_sub.Method = 'newton';
            options_sub.Display = 'none';
            options_sub.TolX = tolX;
            options_sub.TolFun = tolFun;
            d = minFunc(@taylorModel,zeros(p,1),options_sub,f,g,H,T);

            if any(abs(d) > 1e5) || all(abs(d) < 1e-5) || g'*d > -tolX
                if debug
                    fprintf('Using 2nd-Order Step\n');
                end
                [V,D] = eig((H+H')/2);
                D = diag(D);
                D = max(abs(D),max(max(abs(D)),1)*1e-12);
                d = -V*((V'*g)./D);
            else
                if debug
                    fprintf('Using 3rd-Order Step\n');
                end
            end
    end

    % ****************** COMPUTE STEP LENGTH ************************

    % Directional Derivative
    gtd = g'*d;

    % Check that progress can be made along direction
    if gtd > -tolX
        exitflag=2;
        msg = 'Directional Derivative below TolX';
        break;
    end

    % Select Initial Guess
    if i == 1
        if method < NEWTON0
            t = min(1,1/sum(abs(g)));
        else
            t = 1;
        end
    else
        if LS_init == 0
            % Newton step
            t = 1;
        elseif LS_init == 1
            % Close to previous step length
            t = t*min(2,(gtd_old)/(gtd));
        elseif LS_init == 2
            % Quadratic Initialization based on {f,g} and previous f
            t = min(1,2*(f-f_old)/(gtd));
        elseif LS_init == 3
            % Double previous step length
            t = min(1,t*2);
        elseif LS_init == 4
            % Scaled step length if possible
            dHd = d'*autoHv(d,x,g,0,funObj,varargin{:});
            funEvals = funEvals + 1;
            if dHd > 0
                t = -gtd/(dHd);
            else
                t = t*min(2,(gtd_old)/(gtd));
            end
        end

        if t <= 0
            t = 1;
        end
    end
    f_old = f;
    gtd_old = gtd;

    % Bound the initial step size
    if boundStepLength && method < NEWTON0
        t = min(t,1e4/(1+sum(abs(g))));
    end

    % Compute reference fr if using non-monotone objective
    if Fref == 1
        fr = f;
    else
        if i == 1
            old_fvals = repmat(-inf,[Fref 1]);
        end

        if i <= Fref
            old_fvals(i) = f;
        else
            old_fvals = [old_fvals(2:end);f];
        end
        fr = max(old_fvals);
    end

    computeHessian = 0;
    if method >= NEWTON
        if HessianIter == 1
            computeHessian = 1;
        elseif i > 1 && mod(i-1,HessianIter) == 0
            computeHessian = 1;
        end
    end

    % Line Search
    f_old = f;
    if LS < 3 % Use Armijo Bactracking
        % Perform Backtracking line search
        if computeHessian
            [t,x,f,g,LSfunEvals,H] = ArmijoBacktrack(x,t,d,f,fr,g,gtd,c1,LS,tolX,debug,doPlot,LS_saveHessianComp,funObj,varargin{:});
        else
            [t,x,f,g,LSfunEvals] = ArmijoBacktrack(x,t,d,f,fr,g,gtd,c1,LS,tolX,debug,doPlot,1,funObj,varargin{:});
        end
        funEvals = funEvals + LSfunEvals;

    elseif LS < 6
        % Find Point satisfying Wolfe

        if computeHessian
            [t,f,g,LSfunEvals,H] = WolfeLineSearch(x,t,d,f,g,gtd,c1,c2,LS,25,tolX,debug,doPlot,LS_saveHessianComp,funObj,varargin{:});
        else
            [t,f,g,LSfunEvals] = WolfeLineSearch(x,t,d,f,g,gtd,c1,c2,LS,25,tolX,debug,doPlot,1,funObj,varargin{:});
        end
        funEvals = funEvals + LSfunEvals;
        x = x + t*d;

    else
        % Use Matlab optim toolbox line search
        [t,f_new,fPrime_new,g_new,LSexitFlag,LSiter]=...
            lineSearch({'fungrad',[],funObj},x,p,1,p,d,f,gtd,t,c1,c2,-inf,maxFunEvals-funEvals,...
            tolFun,[],[],[],varargin{:});
        funEvals = funEvals + LSiter;
        if isempty(t)
            exitflag = -2;
            msg = 'Matlab LineSearch failed';
            break;
        end

        if method >= NEWTON
            [f_new,g_new,H] = funObj(x + t*d,varargin{:});
            funEvals = funEvals + 1;
        end
        x = x + t*d;
        f = f_new;
        g = g_new;
    end

    % Output iteration information
    if verboseI
        fprintf('%10d %10d %15.5e %15.5e %15.5e\n',i,funEvals*funEvalMultiplier,t,f,sum(abs(g)));
    end

    % Output Function
    if ~isempty(outputFcn)
        callOutput(outputFcn,x,'iter',i,funEvals,f,t,gtd,g,d,sum(abs(g)),varargin{:});
    end

    % Update Trace
    trace.fval(end+1,1) = f;
    trace.funcCount(end+1,1) = funEvals;

    % Check Optimality Condition
    if sum(abs(g)) <= tolFun
        exitflag=1;
        msg = 'Optimality Condition below TolFun';
        break;
    end

    % ******************* Check for lack of progress *******************

    if sum(abs(t*d)) <= tolX
        exitflag=2;
        msg = 'Step Size below TolX';
        break;
    end

    if abs(f-f_old) < tolFun
        exitflag=2;
        msg = 'Function Value changing by less than TolFun';
        break;
    end

    % ******** Check for going over iteration/evaluation limit *******************

    if funEvals*funEvalMultiplier > maxFunEvals
        exitflag = 0;
        msg = 'Exceeded Maximum Number of Function Evaluations';
        break;
    end

    if i == maxIter
        exitflag = 0;
        msg='Exceeded Maximum Number of Iterations';
        break;
    end

end

if verbose
    fprintf('%s\n',msg);
end
if nargout > 3
    output = struct('iterations',i,'funcCount',funEvals*funEvalMultiplier,...
        'algorithm',method,'firstorderopt',sum(abs(g)),'message',msg,'trace',trace);
end

% Output Function
if ~isempty(outputFcn)
    callOutput(outputFcn,x,'done',i,funEvals,f,t,gtd,g,d,sum(abs(g)),varargin{:});
end

end


function label = litekmeans(X, k)
% Perform k-means clustering.
%   X: d x n data matrix
%   k: number of seeds
% Written by Michael Chen (sth4nth@gmail.com).
n = size(X,2);
last = 0;
label = ceil(k*rand(1,n));  % random initialization
while any(label ~= last)
    [u,~,label] = unique(label);   % remove empty clusters
    k = length(u);
    E = sparse(1:n,label,1,n,k,n);  % transform label into indicator matrix
    m = X*(E*spdiags(1./sum(E,1)',0,k,k));    % compute m of each cluster
    last = label;
    [~,label] = max(bsxfun(@minus,m'*X,dot(m,m,1)'/2),[],1); % assign samples to the nearest centers
end
[~,~,label] = unique(label);


function protocol = FSL2Protocol(bvalfile, bvecfile)
%
% function protocol = FSL2Protocol(bvalfile, bvecfile)
%
% Note: for NODDI, the exact sequence timing is not important.
%  this function reverse-engineerings one possible sequence timing
%  given the b-values.
%
% author: Gary Hui Zhang (gary.zhang@ucl.ac.uk)
%

protocol.pulseseq = 'PGSE';
protocol.schemetype = 'multishellfixedG';
protocol.teststrategy = 'fixed';

% load bval
bval = load(bvalfile);
bval = bval';

% set total number of measurements
protocol.totalmeas = length(bval);

% set the b=0 indices
protocol.b0_Indices = find(bval==0);
protocol.numZeros = length(protocol.b0_Indices);

% find the unique non-zero b-values
B = unique(bval(bval>0));

% set the number of shells
protocol.M = length(B);
for i=1:length(B)
    protocol.N(i) = length(find(bval==B(i)));
end

% maximum b-value in the s/mm^2 unit
maxB = max(B);

% set maximum G = 40 mT/m
Gmax = 0.04;

% set smalldel and delta and G
GAMMA = 2.675987E8;
tmp = nthroot(3*maxB*10^6/(2*GAMMA^2*Gmax^2),3);
for i=1:length(B)
    protocol.udelta(i) = tmp;
    protocol.usmalldel(i) = tmp;
    protocol.uG(i) = sqrt(B(i)/maxB)*Gmax;
end

protocol.delta = zeros(size(bval))';
protocol.smalldel = zeros(size(bval))';
protocol.G = zeros(size(bval))';

for i=1:length(B)
    tmp = find(bval==B(i));
    for j=1:length(tmp)
        protocol.delta(tmp(j)) = protocol.udelta(i);
        protocol.smalldel(tmp(j)) = protocol.usmalldel(i);
        protocol.G(tmp(j)) = protocol.uG(i);
    end
end

% load bvec
bvec = load(bvecfile);
protocol.grad_dirs = bvec';

% make the gradient directions for b=0's [1 0 0]
for i=1:length(protocol.b0_Indices)
    protocol.grad_dirs(protocol.b0_Indices(i),:) = [1 0 0];
end

% make sure the gradient directions are unit vectors
for i=1:protocol.totalmeas
    protocol.grad_dirs(i,:) = protocol.grad_dirs(i,:)/norm(protocol.grad_dirs(i,:));
end


%
% Smooth point-set registration method using neighboring constraints
% -------------------------------------------------------------------
%
% Authors: Gerard Sanromà, René Alquézar and Francesc Serratosa
%
% Contact: gsanorma@gmail.com
% Date: 15/02/2012
%
% Centers and rescales input graphs according to the coefficients of the
% match matrix. It follows the ansatz by Softassign Procrustes.
%
% Input
%   g_M,g_D: 2D coordinates of the model and data graph nodes
%   S: matrix of matching coefficients
%
% Output
%   g_Mc,g_Dc: centered and rescaled coordinates of the model and data
%              graphs' nodes
%

function [g_Mc g_Dc] = center_rescale_weighted(g_M,g_D,S)
% Softassign Procrustes (Rangarajan)
% Update centroids and variances (eq.(5))
m_M = sum(g_M.*repmat(sum(S)',1,2))/(sum(S(:))+eps);
m_D = sum(g_D.*repmat(sum(S,2),1,2))/(sum(S(:))+eps);
r_M = g_M - repmat(m_M,size(g_M,1),1);
r_D = g_D - repmat(m_D,size(g_D,1),1);
v_M = sum(diag(r_M*r_M').*sum(S)');%/sum(S(:));
v_D = sum(diag(r_D*r_D').*sum(S,2));%/sum(S(:));
% Center and rescale
g_Mc = r_M/(sqrt(v_M)+eps);
g_Dc = r_D/(sqrt(v_D)+eps);


function demoNcutImage;
% demoNcutImage
%
% demo for NcutImage
% also initialize matlab paths to subfolders
% Timothee Cour, Stella Yu, Jianbo Shi, 2004.

disp('Ncut Image Segmentation demo');

%% read image, change color image to brightness image, resize to 160x160
I = imread_ncut('jpg_images/3.jpg',160,160);

%% display the image
figure(1);clf; imagesc(I);colormap(gray);axis off;
disp('This is the input image to segment, press Enter to continue...');
pause;

%% compute the edges imageEdges, the similarity matrix W based on
%% Intervening Contours, the Ncut eigenvectors and discrete segmentation
nbSegments = 5;
disp('computing Ncut eigenvectors ...');
tic;
[SegLabel,NcutDiscrete,NcutEigenvectors,NcutEigenvalues,W,imageEdges]= NcutImage(I,nbSegments);
disp(['The computation took ' num2str(toc) ' seconds on the ' num2str(size(I,1)) 'x' num2str(size(I,2)) ' image']);

%% display the edges
figure(2);clf; imagesc(imageEdges); axis off
disp('This is the edges computed, press Enter to continue...');
pause;

%% display the segmentation
figure(3);clf
bw = edge(SegLabel,0.01);
J1=showmask(I,imdilate(bw,ones(2,2))); imagesc(J1);axis off
disp('This is the segmentation, press Enter to continue...');
pause;

%% display Ncut eigenvectors
figure(4);clf;set(gcf,'Position',[100,500,200*(nbSegments+1),200]);
[nr,nc,nb] = size(I);
for i=1:nbSegments
    subplot(1,nbSegments,i);
    imagesc(reshape(NcutEigenvectors(:,i) , nr,nc));axis('image');axis off;
end
disp('This is the Ncut eigenvectors...');
disp('The demo is finished.');


function V_corr = make_vcorr(DD,pv,nb,nl,f,Zb)
%MAKE_VCORR  Voltage Correction used in distribution power flow
%
%   V_corr = make_vcorr(DD,pv,nb,nl,f,Zb)
%
%   Calculates voltage corrections with current generators placed at PV
%   buses. Their currents are calculated with the voltage difference at PV
%   buses break points and loop impedances. The slack bus voltage is set to
%   zero. Details can be seen in
%   D. Rajicic, R. Ackovski and R. Taleski, "Voltage correction power flow,"
%   IEEE Transactions on Power Delivery, vol. 9, no. 2, pp. 1056-1062, Apr 1994.
%   https://doi.org/10.1109/61.296308
%
%   See also RADIAL_PF.

V_corr = zeros(nb,1);
I = zeros(nb,1);
I(pv) = DD;
% backward sweep
for k = nl:-1:2
    i = f(k);
    I(i) = I(i) + I(k);
end
% forward sweep
for k = 2:nl
    i = f(k);
    V_corr(k) = V_corr(i) - Zb(k) * I(k);
end


function [D] = compute_diffusion_coefficient(I)

alpha = 5.0;
beta = 0.5;

wSize = 5;
sigma = wSize/6;

gI = imfilter(I, fspecial('gaussian', [wSize wSize], sigma),'replicate');
mask = [-1 0 1];
Ix = imfilter(gI, mask, 'replicate');
Iy = imfilter(gI, mask', 'replicate');

norm = sqrt(Ix.^2 + Iy.^2);

D = max(1e-06, exp(-alpha*norm.^beta));

end


function[]=makefigs_curvemoments
%MAKEFIGS_CURVEMOMENTS  Makes a sample figure for CURVEMOMENTS.

load qgsnapshot

[cv,zeta,N,S,P]=psi2fields(qgsnapshot.psi);
P=frac(P,std(P(:)));

[xc,yc]=closedcurves(qgsnapshot.x,qgsnapshot.y,P,-2);
[xo,yo,kappa,R,L,a,b,theta]=curvemoments(xc,yc);

figure,jpcolor(qgsnapshot.x,qgsnapshot.y,P),axis equal, axis tight,
hold on,colormap gray,flipmap,cellplot(xc,yc,'2b'),
[k,l]=ab2kl(a,b);ellipseplot(k,l,theta,xo+sqrt(-1)*yo,'r')
title('Curves of Okubo-Weiss (blue), moment ellipses (red), and centroids (black)')
caxis([-5 5]),xtick([-5:1:5]*1000),ytick([-5:1:5]*1000),plot(xo,yo,'k.')
noxlabels,noylabels,fontsize 11
h=colorbar;
h.Label.String='Okubo-Weiss Parameter normalized by its own standard deviation';

%To print
if 0
    currentdir=pwd;
    cd([whichdir('jlab_license') '/figures'])
    print -dpng curvemoments
    crop curvemoments.png
    cd(currentdir)
end


function [rI, Par]   =  MCWNNM_ADMM1_Denoising( nI, I, Par )
rI           = nI;   % Estimated Image
[h, w, ch]  = size(rI);
Par.h = h;
Par.w = w;
Par.ch = ch;
Par = SearchNeighborIndex( Par );
% noisy image to patch
NoiPat =	Image2Patch( nI, Par );
Par.TolN = size(NoiPat, 2);
Sigma_arrCh = zeros(Par.ch, Par.TolN);
for iter = 1 : Par.Iter
    Par.iter = iter;
    % iterative regularization
    rI =	rI + Par.delta * (nI - rI);
    % image to patch
    CurPat =	Image2Patch( rI, Par );
    % estimate local noise variance
    for c = 1:Par.ch
        if(iter == 1)
            TempSigma_arrCh = sqrt(max(0, repmat(Par.nSig0(c)^2, 1, size(CurPat, 2)) - mean((NoiPat((c-1)*Par.ps2+1:c*Par.ps2, :) - CurPat((c-1)*Par.ps2+1:c*Par.ps2, :)).^2)));
            %             TempSigma_arrCh = sqrt(abs(repmat(Par.nSig0(c)^2, 1, size(CurPat, 2)) - mean((NoiPat((c-1)*Par.ps2+1:c*Par.ps2, :) - CurPat((c-1)*Par.ps2+1:c*Par.ps2, :)).^2)));
        else
            TempSigma_arrCh = Par.lambda*sqrt(max(0, repmat(Par.nSig0(c)^2, 1, size(CurPat, 2)) - mean((NoiPat((c-1)*Par.ps2+1:c*Par.ps2, :) - CurPat((c-1)*Par.ps2+1:c*Par.ps2, :)).^2)));
            %             TempSigma_arrCh = Par.lambda*sqrt(abs(repmat(Par.nSig0(c)^2, 1, size(CurPat, 2)) - mean((NoiPat((c-1)*Par.ps2+1:c*Par.ps2, :) - CurPat((c-1)*Par.ps2+1:c*Par.ps2, :)).^2)));
        end
        Sigma_arrCh((c-1)*Par.ps2+1:c*Par.ps2, :) = repmat(TempSigma_arrCh, [Par.ps2, 1]);
    end
    if (mod(iter-1, Par.Innerloop) == 0)
        Par.nlsp = Par.nlsp - 10;  % Lower Noise level, less NL patches
        NL_mat  =  Block_Matching(CurPat, Par);% Caculate Non-local similar patches for each
    end
    % Denoising by MCWNNM
    [Y_hat, W_hat]  =  MCWNNM_ADMM1_Estimation( NL_mat, Sigma_arrCh, CurPat, Par );   % Estimate all the patches
    rI = PGs2Image(Y_hat, W_hat, Par);
    PSNR  = csnr( I, rI, 0, 0 );
    SSIM      =  cal_ssim( I, rI, 0, 0 );
    fprintf( 'Iter = %2.3f, PSNR = %2.2f, SSIM = %2.2f \n', iter, PSNR, SSIM );
    Par.PSNR(iter, Par.image)  =   PSNR;
    Par.SSIM(iter, Par.image)      =  SSIM;
end
return;


classdef one_dim_denoise_problem

    properties
        name;
        dim;
        samples;
        lambda;
        Lap;
        LaptLap;
        b;
        n;
        d;
        prox_flag;
    end

    methods
        function obj = one_dim_denoise_problem(b, lambda, varargin)

            obj.name = 'one_dim_denoise_problem';
            obj.b = b;
            obj.lambda = lambda;
            obj.samples = length(b);

            obj.n = obj.samples;
            obj.d = obj.samples;
            obj.dim = 1;
            obj.samples = obj.n;

            obj.Lap = zeros(obj.d-1, obj.d);
            for i = 1 : obj.d-1
                obj.Lap(i,i) = 1;
                obj.Lap(i,i+1) = -1;
            end
            obj.LaptLap = obj.Lap' * obj.Lap;

            obj.prox_flag = false;
        end

%         function v = prox_denoise(w, t)
%
%             v = soft_thresh(w, t * obj.lambda);
%
%         end

        function f = cost(obj, w)

            f = obj.loss(w) + obj.lambda * obj.reg(w);

        end

        function l = loss(obj, w)

            l = 1/2 * norm(obj.b-w)^2;
        end

        function r = reg(obj, w)

            % Lw = obj.Lap * w;
            % r = (Lw'*Lw)/2;
            % r = w'L'Lw/2
            r = w' * obj.LaptLap * w / 2;

        end

        function r = differentiable_reg(obj, w)

            r = w' * obj.LaptLap * w / 2;

        end

        function r = residual(obj, w)
            r = - (obj.b - w);
        end

        function g = full_grad(obj, w)
            g = - (obj.b - w) + obj.lambda * obj.reg_grad(w);
        end

        function g = grad(obj, w, indices)
            g = obj.full_grad(w);
        end

        function rg = reg_grad(obj, w)

            rg = obj.Lap' * obj.Lap * w;

         end

        function h = hess(obj, w, indices)
            error('Not implemted yet.');
        end

        function h = full_hess(obj, w)
            error('Not implemted yet.');
        end

        function hv = hess_vec(obj, w, v, indices)
            error('Not implemted yet.');
        end

        function w_opt = calc_solution(obj, w_init, options)

            w_opt = (eye(obj.d) + obj.lambda * obj.LaptLap) \ obj.b;

        end

    end

end


% revised on September 21, 2010 to make parameters consistent

% September 25, 2009
% written by Shuiwang Ji and Jieping Ye

% This function implements the accelerated gradient algorithm for
% multi-task learning regularized by trace norm as
% described in Ji and Ye (ICML 2009).

% References:
%Ji, S. and Ye, J. 2009. An accelerated gradient method for trace norm minimization.
%In Proceedings of the 26th Annual international Conference on Machine Learning
%(Montreal, Quebec, Canada, June 14 - 18, 2009). ICML '09, vol. 382. ACM, New York,
%NY, 457-464.

%[W, fval_vec,itr_counter] =
%accel_grad_mtl(A,Y,lambda,opt)

% required inputs:
% A: K x 1 cell in which each cell is N x D where N is the sample size
% and D is data dimensionality and K is the number of tasks
% Y: K x 1 cell in which each cell contains the output of the
% corresponding task
% lambda: regularization parameter

% optional inputs:
% opt.L0: Initial guess for the Lipschitz constant
% opt.gamma: the multiplicative factor for Lipschitz constant
% opt.W_init: initial weight matrix
% opt.epsilon: precision for termination
% opt.max_itr: maximum number of iterations

% outputs:
% W: the computed weight matrix
% fval_vec: a vector for the sequence of function values
% itr_counter: number of iterations executed

function [W,fval_vec,itr_counter] = accel_grad_mtl(A,Y,lambda,opt)

Xtrain = A;
Ytrain = Y;
clear A;
clear Y;

if nargin<4
    opt = [];
end

if isfield(opt, 'L0')
    L0 = opt.L0;
else
    L0 = 100;
end

if isfield(opt, 'gamma')
    gamma = opt.gamma;
else
    gamma = 1.1;
end

if isfield(opt, 'W_init')
    W_init = opt.W_init;
else
    W_init = zeros(size(Xtrain{1},2),length(Ytrain));
end

if isfield(opt, 'epsilon')
    epsilon = opt.epsilon;
else
    epsilon = 10^-5;
end

if isfield(opt, 'max_itr')
    max_itr = opt.max_itr;
else
    max_itr = 100;
end

W_old = W_init;
alpha = 1;
fval_vec = [];
L = L0;
fval_old = rand(1,1);
fval = rand(1,1);
itr_counter = 0;
Z_old = W_old;
while abs((fval_old-fval)/fval_old)>epsilon
    fval_old = fval;
    [Wp,P,sval] = ComputeQP(Xtrain,Ytrain,Z_old,L,lambda);
    f = 0;
    for i = 1:length(Xtrain)
        f = f+0.5*norm(Ytrain{i}-Xtrain{i}*Wp(:,i))^2;
    end
    fval = f+lambda*sval;
    Q = P+lambda*sval;
    while fval>Q
        fprintf('Searching step size (fval = %f, Q = %f)...\n',fval,Q);
        L = L*gamma;
        [Wp,P,sval] = ComputeQP(Xtrain,Ytrain,Z_old,L,lambda);
        f = 0;
        for i = 1:length(Xtrain)
            f = f+0.5*norm(Ytrain{i}-Xtrain{i}*Wp(:,i))^2;
        end
        fval = f+lambda*sval;
        Q = P+lambda*sval;
    end

    alpha_old = alpha;
    alpha = (1+sqrt(1+4*alpha_old^2))/2;
    Z_old = Wp+((alpha_old-1)/alpha)*(Wp-W_old);

    W_old = Wp;
    fval_vec = [fval_vec,fval];
    itr_counter = itr_counter+1;
    if itr_counter>max_itr
        break;
    end
    if mod(itr_counter,100)==0
        fprintf('Iteration = %8d,  objective = %f\n',itr_counter, fval);
    end

end
W = Wp;
return;

function [Wp,P,sval] = ComputeQP(X,Y,W,L,lambda)

[W1,delta_W] = ComputeGradStep(X,Y,W,L);
[U,D,V] = svd(W1,0);
D = diag(D);
D = D-(lambda/L);
idx = find(D>0);
sval = sum(D(idx));
Wp = U(:,idx)*diag(D(idx))*V(:,idx)';
P = 0;
for i = 1:length(X)
    P = P+0.5*norm(Y{i}-X{i}*W(:,i))^2;
end
P = P+trace(delta_W'*(Wp-W))+0.5*L*norm(Wp-W,'fro')^2;
return;

function [W1,delta_W] = ComputeGradStep(X,Y,W,L)
delta_W = ComputeDerivative(X,Y,W);
W1 = W-(1/L)*delta_W;
return;

function dev = ComputeDerivative(X,Y,W)

dev = [];
for i = 1:length(X)
    dev = [dev, X{i}'*(X{i}*W(:,i)-Y{i})];
end
return;


function [node,elem] = squaremesh(square,h)
%% SQUAREMESH uniform mesh of a square
%
% [node,elem] = squaremesh([x0,x1,y0,y1],h) generates a uniform mesh of the
% rectangle [x0,x1]*[y0,y1] with mesh size h.
%
% Example
%
%   [node,elem] = squaremesh([0,1,0,1],0.2);
%   showmesh(node,elem);
%   findnode(node);
%   findelem(node,elem);
%
% See also: squarequadmesh, cubehexmesh
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Generate nodes
x0 = square(1); x1 = square(2);
y0 = square(3); y1 = square(4);
[x,y] = meshgrid(x0:h:x1,y0:h:y1);
node = [x(:),y(:)];

%% Generate elements
ni = size(x,1); % number of rows
N = size(node,1);
t2nidxMap = 1:N-ni;
topNode = ni:ni:N-ni;
t2nidxMap(topNode) = [];
k = (t2nidxMap)';
elem = [k+ni k+ni+1 k; k+1 k k+ni+1];
% 4 k+1 --- k+ni+1 3
%    |        |
% 1  k  ---  k+ni  2


function G=constructMatrixOfMonomials(g,order)
%-fanDTasia ToolBox------------------------------------------------------------------
% This Matlab script is part of the fanDTasia ToolBox: a Matlab library for Diffusion
% Weighted MRI (DW-MRI) Processing, Diffusion Tensor (DTI) Estimation, High-order
% Diffusion Tensor Analysis, Tensor ODF estimation, Visualization and more.
%
% A Matlab Tutorial on DW-MRI can be found in:
% http://www.cise.ufl.edu/~abarmpou/lab/fanDTasia/tutorial.php
%
%-CITATION---------------------------------------------------------------------------
% If you use this software please cite the following work:
% A. Barmpoutis and B.C. Vemuri, "A Unified Framework for Estimating Diffusion Tensors
% of any order with Symmetric Positive-Definite Constraints",
% In the Proceedings of ISBI, 2010
%
%-DESCRIPTION------------------------------------------------------------------------
% This function computes all possible monomials in 3 variables of a certain order.
% The 3 variables are given in the form of 3-dimensional vectors.
%
%-USE--------------------------------------------------------------------------------
% G=constructMatrixOfMonomials(g,order);
%
% g: is a list of N 3-dimensional vectors stacked in a matrix of size Nx3
% order: is the order of the computed monomials
% G: contains the computed monomials. It is a matrix of size N x (2+order)!/2(order)!
%
%-DISCLAIMER-------------------------------------------------------------------------
% You can use this source code for non commercial research and educational purposes
% only without licensing fees and is provided without guarantee or warrantee expressed
% or implied. You cannot repost this file without prior written permission from the
% authors. If you use this software please cite the following work:
% A. Barmpoutis and B.C. Vemuri, "A Unified Framework for Estimating Diffusion Tensors
% of any order with Symmetric Positive-Definite Constraints", In Proc. of ISBI, 2010.
%
%-AUTHOR-----------------------------------------------------------------------------
% Angelos Barmpoutis, PhD
% Computer and Information Science and Engineering Department
% University of Florida, Gainesville, FL 32611, USA
% abarmpou at cise dot ufl dot edu
%------------------------------------------------------------------------------------

fprintf(1,'Constructing matrix of monomials G...');
for k=1:length(g)
    c=1;
    for i=0:order
		for j=0:order-i
			G(k,c)=(g(k,1)^i)*(g(k,2)^j)*(g(k,3)^(order-i-j));
			c=c+1;
        end
    end
end
fprintf(1,'Done\n');


function [xstar,fstar]=Bisection(a0,b0)

global counterf
counterf=0;

% initialization
epsilon=0.00001;
L=1e-4;

ak=a0;
bk=b0;

% iteration
while 1
    % stop criteria
    len=bk-ak;
    if len<L
        xstar=(ak+bk)/2;
        fstar=obj1(xstar);
        break;
    end

    mean=(ak+bk)/2;
    lambdak=mean-epsilon;
    muk=mean+epsilon;
    %compute two points every iteration
    flambdak=obj1(lambdak);
    fmuk=obj1(muk);

    if flambdak>fmuk
        ak=lambdak;
    else
        bk=muk;
    end
end

counterf


function blend_test04 ( )

%*****************************************************************************80
%
%% BLEND_TEST04 checks out BLEND_IJ_0D1 and BLEND_IJ_1D1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2008
%
%  Author:
%
%    John Burkardt
%
  m1 = 5;
  m2 = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BLEND_TEST04\n' );
  fprintf ( 1, '  BLEND_IJ_0D1 interpolates data in a table,\n' );
  fprintf ( 1, '  from corner data.\n' );
  fprintf ( 1, '  BLEND_IJ_1D1 interpolates data in a table,\n' );
  fprintf ( 1, '  from edge data.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The table is %d rows by %d columns.\n', m1, m2 );

  x = zeros ( m1, m2 );
%
%  Load data in the corners only.
%
  i = 1;
  j = 1;
  r = (  i - 1 ) / ( m1 - 1 );
  s = (  j - 1 ) / ( m2 - 1 );
  x(i,j) = cubic_rs ( r, s, 1 );

  i = m1;
  j = 1;
  r = (  i - 1 ) / ( m1 - 1 );
  s = (  j - 1 ) / ( m2 - 1 );
  x(i,j) = cubic_rs ( r, s, 1 );

  i = 1;
  j = m2;
  r = (  i - 1 ) / ( m1 - 1 );
  s = (  j - 1 ) / ( m2 - 1 );
  x(i,j) = cubic_rs ( r, s, 1 );

  i = m1;
  j = m2;
  r = (  i - 1 ) / ( m1 - 1 );
  s = (  j - 1 ) / ( m2 - 1 );
  x(i,j) = cubic_rs ( r, s, 1 );

  x = blend_ij_0d1 ( x, m1, m2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Values interpolated by BLEND_IJ_0D1:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : m1
    fprintf ( 1, '  %10f  %10f  %10f  %10f\n', x(i,1:m2) );
  end
%
%  Load data in the edges.
%
  j = 1;
  for i = 1 : m1
    r = (  i - 1 ) / ( m1 - 1 );
    s = (  j - 1 ) / ( m2 - 1 );
    x(i,j) = cubic_rs ( r, s, 1 );
  end

  j = m2;
  for i = 1 : m1
    r = (  i - 1 ) / ( m1 - 1 );
    s = (  j - 1 ) / ( m2 - 1 );
    x(i,j) = cubic_rs ( r, s, 1 );
  end

  i = 1;
  for j = 2 : m2 - 1
    r = (  i - 1 ) / ( m1 - 1 );
    s = (  j - 1 ) / ( m2 - 1 );
    x(i,j) = cubic_rs ( r, s, 1 );
  end

  i = m1;
  for j = 2 : m2 - 1
    r = (  i - 1 ) / ( m1 - 1 );
    s = (  j - 1 ) / ( m2 - 1 );
    x(i,j) = cubic_rs ( r, s, 1 );
  end

  x = blend_ij_1d1 ( x, m1, m2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Values interpolated by BLEND_IJ_1D1:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : m1
    fprintf ( 1, '  %10f  %10f  %10f  %10f\n', x(i,1:m2) );
  end
%
%  Compare with BLEND_RS_1D1
%
  for i = 1 : m1
    r = (  i - 1 ) / ( m1 - 1 );
    for j = 1 : m2
      s = (  j - 1 ) / ( m2 - 1 );
      x(i,j) = blend_rs_1dn ( r, s, 1, @cubic_rs );
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Data blended by BLEND_RS_1DN:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : m1
    fprintf ( 1, '  %10f  %10f  %10f  %10f\n', x(i,1:m2) );
  end
%
%  Load all data.
%
  for i = 1 : m1
    for j = 1 : m2
      r = (  i - 1 ) / ( m1 - 1 );
      s = (  j - 1 ) / ( m2 - 1 );
      x(i,j) = cubic_rs ( r, s, 1 );
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Exact data:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : m1
    fprintf ( 1, '  %10f  %10f  %10f  %10f\n', x(i,1:m2) );
  end

  return
end


function ihist = imghist(img)
% Author: Javier Montoya (jmontoyaz@gmail.com).
%         http://www.lis.ic.unicamp.br/~jmontoya
%
% IMGHIST calculates the histogram of a given image.
% Input parameters:
%    img: image I (passed as a bidimensional matrix).
% Ouput parameters:
%    ihist: histogram.
%
% Usage:
%    I     = imread('tire.tif');
%    ihist = imghist(I);
%    figure; stem(ihist); title('Image Histogram');

   if exist('img', 'var') == 0
      error('Error: Specify an input image.');
   end

   ihist       = [];
   [rows,cols] = size(img);
   maxgval     = 255;
   ihist       = zeros(1,maxgval);

   for i=0:maxgval
      ihist(i+1) = sum(img(:)==i);
   end
end


function val=calcRMSE(xTrue,xEst,is3D)
%%CALCRMSE Compute the scalar root-mean-squared error (RMSE) of estimates
%          compared to true values.
%
%INPUTS: xTrue The truth data. This is either an xDimXNumSamples matrix or
%              an xDimXNXnumSamples matrix. The latter formulation is
%              useful when the MSE over multiple Monte Carlo runs of an
%              entire length-N track is desired. In the first formulation,
%              we take N=1. Alternatively, if the same true value is used
%              for all numSamples, then xTrue can just be an xDimXN matrix.
%              Alternatively, if xTrue is the same for all numSamples and
%              all N, then just an xDimX1 matrix can be passed. N and
%              numSamples are inferred from xEst.
%         xEst An xDimXnumSamples set of estimates or an xDimXNXnumSamples
%              set of estimates (if values at N times are desired).
%         is3D An optional indicating that xEst is 3D. This is only used if
%              xEst is a matrix. In such an instance, there is an ambiguity
%              whether xEst is truly 2D, so N=1, or whether numSamples=1
%              and xEst is 3D with the third dimension being 1. If this
%              parameter is omitted or an empty matrix is passed, it is
%              assumed that N=1.
%
%OUTPUTS: val A 1XN vector of scalar RMSE values.
%
%The RMSE is discussed with relation to alternative error measured in [1]
%and is given in Equation 1 of [1].
%
%EXAMPLE:
%Here, we show that the RMSE of samples of a Gaussian random variable is
%related to the square root of the trace of a covariance matrix.
% R=[28,   4, 10;
%     4,  22, 16;
%     10, 16, 16];%The covariance matrix.
% xTrue=[10;-20;30];
% numRuns=100000;
% xEst=GaussianD.rand(numRuns,xTrue,R);
% rootTrace=sqrt(trace(R))
% val=calcRMSE(xTrue,xEst)
%One will see that the root-trace and the sample RMSE are close.
%
%REFERENCES:
%[1] X. R. Li and Z. Zhao, "Measures of performance for evaluation of
%    estimators and filters," in Proceedings of SPIE: Conference on Signal
%    and Data processing of Small Targets, vol. 4473, San Diego, CA, 29
%    Jul. 2001, pp. 530-541.
%
%February 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(is3D))
    is3D=false;
end

xDim=size(xEst,1);
if(ismatrix(xEst)&&is3D==false)
    N=1;
    numSamples=size(xEst,2);
    xEst=reshape(xEst,[xDim,1,numSamples]);

    if(size(xTrue,2)==1)
        %If the true values are the same for all samples.
        xTrue=repmat(xTrue,[1,1,numSamples]);
    else
        xTrue=reshape(xTrue,[xDim,1,numSamples]);
    end
else
    N=size(xEst,2);
    numSamples=size(xEst,3);

    if(ismatrix(xTrue))
        if(size(xTrue,2)==1)
            %If the true values are the same for all samples and for all N.
            xTrue=repmat(xTrue,[1,N,numSamples]);
        else
            %If the true values are the same for all samples.
            xTrue=repmat(xTrue,[1,1,numSamples]);
        end
    end
end

val=zeros(1,N);
for k=1:N
    val(k)=sqrt(sum(sum((xEst(:,k,:)-xTrue(:,k,:)).^2,3)/numSamples));
end
end

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


function dUpsilon = lfmvvGradientUpsilonMatrix(gamma, sigma2, t1, ...
    t2, mode, upsilon)

% LFMVVGRADIENTUPSILONMATRIX Gradient upsilon matrix vel. vel.
% FORMAT
% DESC computes the gradient of a portion of the LFMVV kernel.
% ARG gamma : Gamma value for system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG upsilon : precomputation of the upsilon matrix.
% ARG mode : operation mode, according to the derivative (mode 0,
% derivative wrt t1, mode 1 derivative wrt t2)
% RETURN upsilon : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio Alvarez, 2010
%
% SEEALSO : lfmvvComputeUpsilonMatrix.m

% KERN

sigma = sqrt(sigma2);

if nargin<6
    upsilon = lfmvpComputeUpsilonMatrix(gamma, sigma2, t1, t2, mode);
    if nargin <5
        mode =0;
    end
end

if mode ==0
    dUpsilon = upsilon + gamma*lfmvpGradientUpsilonMatrix(gamma, sigma2, t1, t2, mode) ...
        - (2/(sqrt(pi)*sigma))*((1-gamma*t1).*exp(-gamma*t1))*(exp(-(t2.^2)/sigma2)).';
else
    dUpsilon = -upsilon - gamma*lfmvpGradientUpsilonMatrix(gamma, sigma2, t1, t2, mode);
end


%THIS FUMCTION ILLUSTRATES HOW TO USE THE USER-DEFINED ALGORITHM
function [AH,XH] = user_alg3(Y,r,X)
%
% The example of implementing the user-defined algorithm (this is the regularized Fixed-Point algorithm based)
%
% INPUTS:
% Y - mixed signals (matrix of size [m by T])
% r - number of estimated signals
% X - true source signals
%
% OUTPUTS
% AH - estimated mixing matrix (matrix of size [m by r])
% XH - estimated source signals (matrix of size [r by T])
%
% #########################################################################
% Initialization
[m,T]=size(Y);
Y(Y <=0) = eps; % this enforces the positive value in the data
AH=rand(m,r);
XH=rand(r,T);

IterNo = 1000; % number of alternating steps

% Iterations
for k = 1:IterNo

    alpha_reg = 20*exp(-k/10); % regularization parameter
    XH = max(1E6*eps,pinv(AH'*AH +  alpha_reg)*AH'*Y);
    AH = max(1E6*eps, Y*XH'*pinv(XH*XH' + alpha_reg));
    AH = AH*diag(1./(sum(AH,1) + eps));

end


function [Spos,Sneg,vpos,vneg] = strengths_und_sign(W)
%STRENGTHS_UND_SIGN        Strength and weight
%
%   [Spos Sneg] = strengths_und_sign(W);
%   [Spos Sneg vpos vneg] = strengths_und_sign(W);
%
%   Node strength is the sum of weights of links connected to the node.
%
%   Inputs:     W,              undirected connection matrix with positive
%                               and negative weights
%
%   Output:     Spos/Sneg,      nodal strength of positive/negative weights
%               vpos/vneg,      total positive/negative weight
%
%
%   2011, Mika Rubinov, UNSW

%   Modification History:
%   Mar 2011: Original

n = length(W);              %number of nodes
W(1:n+1:end) = 0;           %clear diagonal
Spos = sum( W.*(W>0));      %positive strengths
Sneg = sum(-W.*(W<0));      %negative strengths

if nargout>2
    vpos = sum(Spos);       %positive weight
    vneg = sum(Sneg);       %negative weight
end


function [ xstar, seed ] = rk1_tv_step ( x, t, h, q, fv, gv, seed )

%*****************************************************************************80
%
%% RK1_TV_STEP takes one step of a stochastic Runge Kutta scheme.
%
%  Discussion:
%
%    The Runge-Kutta scheme is first-order, and suitable for time-varying
%    systems.
%
%    d/dx X(t,xsi) = F ( X(t,xsi), t ) + G ( X(t,xsi), t ) * w(t,xsi)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jeremy Kasdin,
%    Runge-Kutta algorithm for the numerical integration of
%    stochastic differential equations,
%    Journal of Guidance, Control, and Dynamics,
%    Volume 18, Number 1, January-February 1995, pages 114-120.
%
%    Jeremy Kasdin,
%    Discrete Simulation of Colored Noise and Stochastic Processes
%    and 1/f^a Power Law Noise Generation,
%    Proceedings of the IEEE,
%    Volume 83, Number 5, 1995, pages 802-827.
%
%  Parameters:
%
%    Input, real X, the value at the current time.
%
%    Input, real T, the current time.
%
%    Input, real H, the time step.
%
%    Input, real Q, the spectral density of the input white noise.
%
%    Input, external real FV, the name of the deterministic
%    right hand side function.
%
%    Input, external real GV, the name of the stochastic
%    right hand side function.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real XSTAR, the value at time T+H.
%
  a21 = 1.0;

  q1 = 1.0;

  t1 = t;
  x1 = x;
  [ n1, seed ] = r8_normal_01 ( seed );
  w1 = n1 * sqrt ( q1 * q / h );
  k1 = h * fv ( t1, x1 ) + h * gv ( t1, x1 ) * w1;

  xstar = x1 + a21 * k1;

  return
end


function y= fcnNormalize0_1(a)
% normalizes matrix between 100 and 0; input, output: matrix
% higheest value = 0; lowest = 100;
% use y= fcnNormalize0_1(-a) for high=100; low=0;

b=[];
for i=1:size(a)
    b = [b a(i,:)]; % converts matrix to a vector
end

n=(b-(min(b)))/max(b-min(b)); % normalizes between 0 and 1
n = abs(n-1);                 % changes the normaliation between 1 and 0
n= n.*100;                      % range = 0 :100

y = vec2mat ( n,(size(a,2))); % converts vector to same size matrix

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


function areaIntersection = computeIntersectionArea(bb1,bb2)
%compute intersection anrea of bb1 and bb2
%bb1 and bb2 - bounding boxes
%bbi = [xmin ymin xmax ymax] for i=1,2

xmin = max(bb1(1),bb2(1));
xmax = min(bb1(3),bb2(3));
ymin = max(bb1(2),bb2(2));
ymax = min(bb1(4),bb2(4));

areaIntersection = computeArea([xmin ymin xmax ymax]);

end


function [p,w, fVal, exitFlag] = fitBezierCurve(t,x,nPoint,tSpan, xBnd)
% [p,w, fVal, exitFlag] = fitBezierCurve(t,x,nPoint,tSpan, xBnd)
%
% This function fits a rational bezier curve to the data x(t), using a
% bezier curve with n points.
%
% INPUTS:
%   t = [1 x nTime] monotonically increasing time vector
%   x = [nCurve x nTime] vector function at each point in t
%   nPoint = number of points to use for bezier curve
%   tSpan = [1 x 2] domain of t
%   xBnd = [nCurve x 2] = [lower, upper] bound on function
%
% OUTPUTS:
%   p = [nCurve x nPoint] = bezier control points
%   w = [1 x nPoint] = weights for each control point
%   fVal = mean-square-error in function fit
%   exitFlag = fmincon exit flag
%
% See also: bezierCurve, rationalBezierCurve
%

nCurve = size(x,1);

tNormalized = (t-tSpan(1))/diff(tSpan);
wGuess = 0.5*ones(1,nPoint);
tGuess = linspace(0,1,nPoint);
pGuess = interp1(tNormalized',x',tGuess,'linear','extrap')';
zGuess = packDecVar(pGuess,wGuess);

wLow = zeros(1,nPoint);
wUpp = ones(1,nPoint);
pLow = xBnd(:,1)*ones(1,nPoint);
pUpp = xBnd(:,2)*ones(1,nPoint);

zLow = packDecVar(pLow,wLow);
zUpp = packDecVar(pUpp,wUpp);

options = optimset(...
    'Display','off');

problem.Aineq = []; problem.Aeq = [];
problem.bineq = []; problem.beq = [];
problem.lb = zLow;
problem.ub = zUpp;
problem.x0 = zGuess;
problem.options = options;
problem.objective = @(z)evalFit(z,t,x,nCurve,nPoint, tSpan);
problem.solver = 'fmincon';

[zSoln,fVal,exitFlag] = fmincon(problem);

[p,w] = unpackDecVar(zSoln,nCurve,nPoint);

end

function [p,w] = unpackDecVar(z,nCurve,nPoint)

w = z(1:nPoint)';

p = reshape(z(nPoint+1:end),nCurve,nPoint);

end

function z = packDecVar(p,w)

[nCurve, nPoint] = size(p);

z = [w'; reshape(p,nCurve*nPoint,1)];

end

function mse = evalFit(z,t,x,nCurve,nPoint,tSpan)

[p,w] = unpackDecVar(z,nCurve,nPoint);
xFit = rationalBezierCurve(p,w,t,tSpan);

mse = mean(mean((xFit-x).^2));

end


function varargout = qr(f, ignored)
%QR Orthogonal-triangular decomposition of a SEPARABLEAPPROX.
%
% [Q, R] = QR( F ), where F is a separableApprox, produces an unitary column
% quasimatrix Q and a upper-triangular row quasimatrix R so that F = Q * R. This
% is computed by a continuous analogue of QR.
%
% [Q, R] = QR( F, 0 ) is the same as QR( F ).
%
% [Q, R, E] = QR( F ) and [Q, R, E] = QR( F, 'vector') produces a vector E that
% stores the pivoting locations.
%
% For more information about this decomposition:
% A. Townsend and L. N. Trefethen, Continuous analogues of matrix
% factorizations, Proc. Royal Soc. A., 2015.
%
% See also LU, and CHOL.

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

% Empty check:
if ( isempty( f ) )
    varargout = cell(1, nargout);
    return
end

% As always start with the CDR decomposition:
[C, D, R] = cdr( f );

% Balance out the scaling, becareful about signs:
sgns = sign( diag( D ) );
C = C * diag(sgns) * sqrt( abs(D) );   % Put the signs into Q
R = R * sqrt( abs(D) );

% QR of the column
[Q, RC] = qr( C );

% Form R:
R = RC * R.';   % still an upper-triangular quasimatrix! :)

% Output to user:
if ( nargout <= 1 )
    varargout = { Q };
elseif ( nargout == 2 )
    varargout = { Q, R };
elseif ( nargout == 3 )
    E = f.pivotLocations;   % complete pivot Locations from GE.
    varargout = { Q, R, E( :, 1 ) };  % Only pass the ones in x.
end

end


clc;
clear;
close all;

T0 = 10000000 ; % initial temperature
r = 0.999 ; % temperature damping rate
Ts = 0.001 ; % stop temperature

model = initModel();

% initialization
while(1)
route = randomSol(model);
if(isFeasible(route,model))
    break;
end
end

cost = calculateCost(route,model);
T = T0;
min = cost;

cnt = 1;

% SA
while(T > Ts)
    flag = '#';
    mode = randi([1 3]);
    newRoute = createNeibor(route,model,mode);
    newCost = calculateCost(newRoute,model);
    delta = newCost - cost;

    if(delta < 0)
        cost = newCost;
        route = newRoute;
        flag = '*';
    else
        p=exp(-delta/T);
        if rand <= p
             cost = newCost;
             route = newRoute;
             flag = '^';
        end
    end

     if cost < min
         min = cost;
     end

     costArr(cnt) = cost;

    T = T*r; %  annealing
    disp([flag 'Iteration ' num2str(cnt) ': Best Cost = ' num2str(cost) ' T = ' num2str(T)]);
    cnt = cnt+1;

end
disp(min);
plot(costArr);


function c = tapas_logrt_linear_binary_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the linear log-reaction time response model according to as
% developed with Louise Marshall and Sven Bestmann
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The Gaussian noise observation model assumes that responses have a Gaussian distribution around
% the inferred mean of the relevant state. The only parameter of the model is the noise variance
% (NOT standard deviation) zeta.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2016 Christoph Mathys, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'Linear log-reaction time for binary models';

% Sufficient statistics of Gaussian parameter priors
%
% Beta_0
c.be0mu = log(500);
c.be0sa = 4;

% Beta_1
c.be1mu = 0;
c.be1sa = 4;

% Beta_2
c.be2mu = 0;
c.be2sa = 4;

% Beta_3
c.be3mu = 0;
c.be3sa = 4;

% Beta_4
c.be4mu = 0;
c.be4sa = 4;

% Zeta
c.logzemu = log(log(20));
c.logzesa = log(2);

% Gather prior settings in vectors
c.priormus = [
    c.be0mu,...
    c.be1mu,...
    c.be2mu,...
    c.be3mu,...
    c.be4mu,...
    c.logzemu,...
         ];

c.priorsas = [
    c.be0sa,...
    c.be1sa,...
    c.be2sa,...
    c.be3sa,...
    c.be4sa,...
    c.logzesa,...
         ];

% Model filehandle
c.obs_fun = @tapas_logrt_linear_binary;

% Handle to function that transforms observation parameters to their native space
% from the space they are estimated in
c.transp_obs_fun = @tapas_logrt_linear_binary_transp;

return;


function H = per(sequence,isplot)
%
% 'per' estimate the hurst parameter of a given sequence with periodogram
%     method.
%
% Inputs:
%     sequence: the input sequence for estimate
%     isplot: whether display the plot. without a plot if isplot equal to 0
% Outputs:
%     H: the estimated hurst coeffeient of the input sequence

%  Author: Chu Chen
%  Version 1.0,  03/10/2008
%  chen-chu@163.com
%

if nargin == 1
    isplot = 0;
end

n = length(sequence);
Xk = fft(sequence);
P_origin = abs(Xk).^2/(2*pi*n);
P = P_origin(1:floor(n/2)+1);

x = log10((pi/n)*[2:floor(0.5*n)]);
y = log10(P(2:floor(0.5*n)));

% Use the lowest 20% part of periodogram to estimate the similarity.
X = x(1:floor(length(x)/5));
Y = y(1:floor(length(y)/5));
p1 = polyfit(X,Y,1);
Yfit = polyval(p1,X);
H = (1-(Yfit(end)-Yfit(1))/(X(end)-X(1)))/2;

if isplot ~= 0
    figure,clf,hold on;
    plot(x,y,'b.');
    plot(X,Yfit,'r-','LineWidth',3);
    xlabel('log10(Frequency)'),ylabel('log10(Periodogram)'),title('Periodogram Method');
end


% demos for ch07

%% sparse signal recovery demo
clear; close all;

d = 512; % signal length
k = 20;  % number of spikes
n = 100; % number of measurements
%
% random +/- 1 signal
x = zeros(d,1);
q = randperm(d);
x(q(1:k)) = sign(randn(k,1));

% projection matrix
A = unitize(randn(d,n),1);
% noisy observations
sigma = 0.005;
e = sigma*randn(1,n);
y = x'*A + e;

[model,llh] = rvmRegVb(A,y);
plot(llh);
m = model.w;

h = max(abs(x))+0.2;
x_range = [1,d];
y_range = [-h,+h];
figure;
subplot(2,1,1);plot(x); axis([x_range,y_range]); title('Original Signal');
subplot(2,1,2);plot(m); axis([x_range,y_range]); title('Recovery Signal');


function [m2] = ft22m2(ft2)
% Convert area from square feet to square meters.
% Chad A. Greene 2012
m2 = ft2*0.09290304;