function f = chebfun3double(f, op, dom, pref, isEqui)
%CHEBFUN3DOUBLE   CHEBFUN3 constructor for discrete tensor of values
%   This algorithm is equivalent to chebfun3classic for double inputs.

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

% The input is a discrete tensor of values.
pseudoLevel = pref.cheb3Prefs.chebfun3eps;
tech = pref.tech();

f = chebfun3();
if ( ~isEqui && numel(op) == 1 )
    f = constructor(f, @(x,y,z) op + 0*x, dom);
    return;
end

% N.B. We cannot detect if MESHGRID was used to generate values unless we
% know also the (x,y,z) points used to generate those values. If we knew
% beforehand that ALL users WILL generate their tensor of values ONLY from
% meshgrid pts, all we need is to say vals = permute(vals,[2 1 3]); to
% generate a tensor corresponding to ''meshgrid'', in which case a copy of
% ``tensorGrid`` should also be used accordingly. op = permute(op,[2 1 3]);
if ( ~isEqui )
    m = size(op, 1);
    n = size(op, 2);
    p = size(op, 3);
    out = tech.tensorGrid([m, n, p], dom);
    xx = out{1};
    yy = out{2};
    zz = out{3};
else
    % Equispaced points from ndgrid, not meshgrid!
    x = linspace(dom(1), dom(2), size(op, 1));
    y = linspace(dom(3), dom(4), size(op, 2));
    z = linspace(dom(5), dom(6), size(op, 3));
    [xx, yy, zz] = ndgrid(x, y, z);
end

% Calculate a tolerance and find numerical rank to this tolerance: The
% tolerance assumes the samples are generated by NDGRID from a function.
% It depends on the size of the sample tensor, hscale of domain, vscale of
% the samples, condition number of the function, and the accuracy target in
% chebfun3 preferences.
[relTol, absTol] = getTol3D(xx, yy, zz, op, max(size(op)), dom, pseudoLevel);
pref.chebfuneps = relTol;

% Perform 3D ACA with complete pivoting:
factor = 0;
[colsValues, rowsValues, pivotVals2D, ~, tubesValues, pivotVals3D, ~, ~, ...
    ~] = completeACA3D(op, absTol, factor, dom, pref);

sepRank = numel(pivotVals3D); % first separation rank
diagValues2D = cell(sepRank, 1);
for k=1:sepRank
    diagValues2D{k} = diag(1./pivotVals2D{k});
end

% BTD ---> Tucker compression:
[core, colsValues, rowsValues] = btd2tucker(colsValues, rowsValues, ...
    diagValues2D, pivotVals3D, absTol);

% Construct a CHEBFUN3 object and call a sampleTest.
if ( ~isEqui )
    f.cols = chebfun(colsValues, dom(1:2), pref);
    f.rows = chebfun(rowsValues, dom(3:4), pref);
    f.tubes = chebfun(tubesValues, dom(5:6), pref);
else
    f.cols = chebfun(colsValues, dom(1:2), 'equi', pref);
    f.rows = chebfun(rowsValues, dom(3:4), 'equi', pref);
    f.tubes = chebfun(tubesValues, dom(5:6), 'equi', pref);
end
f.core = core;
f.domain = dom;
return

end

%%
function [relTol, absTol] = getTol3D(xx, yy, zz, vals, grid, dom, ...
    pseudoLevel)

relTol = 2*grid^(4/5) * pseudoLevel; % this should be vscale and hscale invariant
vscale = max(abs(vals(:)));
[m,n,p] = size(vals);
% Remove some edge values so that df_dx, df_dy and df_dz have the same size.
% xx changes in the first mode:
df_dx = diff(vals(:, 1:n-1, 1:p-1), 1, 1) ./ diff(xx(:, 1:n-1, 1:p-1), 1, 1);
% yy changes row-wise (2nd mode):
df_dy = diff(vals(1:m-1, :, 1:p-1), 1, 2) ./ diff(yy(1:m-1, :, 1:p-1), 1, 2);
% zz changes tube-wise (3rd mode):
df_dz = diff(vals(1:m-1, 1:n-1, :), 1, 3) ./ diff(zz(1:m-1, 1:n-1, :), 1, 3);
gradNorms = [max(abs(df_dx(:))), max(abs(df_dy(:))), max(abs(df_dz(:)))];
% A vector of gradient information over the domain.
if ( isempty(gradNorms) )
    % This happens if the input in not a trivariate function in which case
    % we basically disable using gradient information:
    gradNorms = 1;
end
domDiff = [diff(dom(1:2)) diff(dom(3:4)) diff(dom(5:6))];
absTol = max(max(gradNorms.*domDiff), vscale) * relTol;
% absTol should depend on the vscale of the function while it also uses
% derivative information to prevent issues like the one mentioned in
% https://github.com/chebfun/chebfun/issues/1491.

end

%%
function [colsBtd, rowsBtd, pivotValues2D, pivotIndices2D, fibers, ...
    pivotValues3D, pivotIndices3D, ifail3D, ifail2D] = completeACA3D(A, ...
    tol, factor, dom, pref)
%   Non-adaptive (fixed-size) MACA, i.e., a 3D analogue of Gaussian
%   elimination with complete pivoting.
%
%   INPUTS:     A:        A given tensor of function values at 3D chebpts.
%
%               tol:      A given tolerance on the magnitude of the pivots.
%
%               factor:   The ratio between the width of A and the number
%                         of iterations (rank) allowed in Gaussian elimination.
%
%  OUTPUTS:     colsBtd: A cell-array containing skeleton columns of slices
%                        in block term decomposition.
%
%               rowsBtd: A cell-array containing skeleton rows of slices in
%                        block term decomposition.
%
%               pivotValues2D: A cell array containing the values of pivots
%                              in 2D ACAs.
%
%               pivotIndices2D: A cell array containing indices i.e.,
%                               locations of of 2D pivot points.
%
%                       fibers: A matrix of size n1 x ITER. Each of its columns
%                             contains the values of the updated (residue)
%                             tensor at the pivot fiber.
%
%               pivotValues3D: A row vector containing the values of the
%                             pivot entries during the iterations.
%
%               pivotIndices3D: A matrix of size rank x 3, where rank = iter.
%                         Each of its rows contain the index of one 3D pivotValues.
%
%                      ifail: We fail if iter >= (width/factor).

% Developer Note: The output of this code should satisfy the following
% slice decomposition:
%       AA \approx temp,
% where AA is a copy of A from input, and temp is computed as follows:
%   temp = zeros(size(A2));
%   for i = 1:3,
%    temp = temp + chebfun3.outerProd(slices(:,:,i),fibers(:,i)./pivotValues3D(i));
%   end
%   norm(AA(:) - temp(:))
%
% An analogous BTD decomposotion should also hold.

pseudoLevel = pref.cheb3Prefs.chebfun3eps;
tech = pref.tech();

% Set up output variables.
[n1, n2, n3] = size(A);
width = min(n3, n1*n2);        % Use to tell us how many pivots we can take
                               % See Developer note in the following.
pivotValues3D = zeros(1);      % Store an unknown number of Pivot values
pivotIndices3D = zeros(1, 3);  % Store (col, row, tube) = entries of pivot location
ifail3D = 1;                   % Assume we fail in 3D ACA
ifail2D = 1;                   % Assume we also fail in the 2D ACAs
globalTol = [];
sliceDim = [1 2];              % See Developer note in the following.

% Main algorithm
iter = 0;                      % Count number of interpolated rows/slices.
[infNorm, ind] = max(abs(reshape(A, numel(A), 1))); % Complete pivoting
[col, row, tube] = ind2sub(size(A), ind);

scl = infNorm;
% If the function is the zero function.
if ( scl == 0 )
    pivotValues3D = 0;
    fibers = 0;
    colsBtd{1} = 0;
    rowsBtd{1} = 0;
    pivotValues2D = 0;
    pivotIndices2D = [0 0];
    ifail3D = 0;
    ifail2D = 0;
else
    fibers(:,1) = zeros(size(A, 3), 1);
    colsBtd{1} = zeros(size(A, sliceDim(1)), 1);
    rowsBtd{1} = zeros(size(A, sliceDim(2)), 1);
    pivotValues2D{1} = 0;
    pivotIndices2D{1} = [0 0];
    slices(:,:,1) = zeros(size(A,sliceDim(1)), size(A, sliceDim(2)), 1);
end
dom2D = dom(1:4);

while ( ( infNorm > tol ) && ( iter < width / factor) ...
        && ( iter < width ) )
    fibers(:, iter+1) = A(col, row, :);  % Extract skeleton tubes. Each
    % column in "fibers" is N3 x 1.

    slices(:,:,iter+1) = A(:,:,tube);    % Extract skeleton slices to be
    % decomposed further. Each slice in "slices" is N1 x N2.

    % Developer Note: As the above lines show, we are always separating the
    % last variable from the first two. The point is that the function
    % handle has already been permuted outside this subroutine and
    % therefore the tensor A here contains values of the permuted function.
    % In this sense, here we are in essense separating the variable chosen
    % by the dimension clustering step, and not necessarily the last
    % variable z.

    PivVal3D = A(col, row, tube);        % = f(X(col), Y(row), Z(tube)) in
                                         % the 1st iteration.

    % Use the first slice to compute globalTol for 2D ACAs applied to all
    % slices.
    if ( iter == 0 )
        out = tech.tensorGrid([n1, n2], dom2D);
        xx2D = out{1};
        yy2D = out{2};
        globalTol = GetTol2D(xx2D, yy2D, slices(:,:,1), dom2D, pseudoLevel);
    end

    % Apply 2D ACA to each slice to form columns and rows in block term
    % decomposition:
    [colsBtd{iter+1}, pivotValues2D{iter+1}, rowsBtd{iter+1}, ...
        pivotIndices2D{iter+1}, ifail2DIter] = ...
        chebfun2ACA(slices(:, :, iter+1), globalTol, factor);

    % Developer Note: Since we use globalTol for slices after 1st
    % iteration, it might be that these 2D ACA's don't fail, while with a
    % localTol they would fail. So, it is auaully the 1st slice which shows
    % whether or not we got the 2D rank right. So, just use that one:
    if ( iter == 0 )
        ifail2D = ifail2DIter;
    end

    % Update the tensor, i.e., compute the residual tensor:
    A = A - chebfun3.outerProd(colsBtd{iter+1} * ...
        (diag(1./(pivotValues2D{iter+1}))) * (rowsBtd{iter+1}.'), ...
        fibers(:,iter+1) ./ PivVal3D);
    % Equivalently, we have:
    % A = A - chebfun3.outerProd(slices(:,:,iter+1),fibers(:,iter+1)./PivVal3D);

    % Keep track of progress in 3D ACA:
    iter = iter + 1;              % One more fiber and slice are removed from A
    pivotValues3D(iter) = PivVal3D;           % Store pivot value in 3D ACA
    pivotIndices3D(iter, :)=[col row tube];    % Store pivot location in 3D ACA

    % Find next 3D pivot value and its location:
    [infNorm, ind] = max(abs(A(:)));
    [col, row, tube] = ind2sub(size(A), ind);
end

if ( ( iter > 0 ) && ( all(pivotValues2D{iter} == 0) ) )
    % If the last 2D pivot was zero, remove it
    colsBtd=colsBtd(1:iter-1);
    rowsBtd=rowsBtd(1:iter-1);
    pivotValues2D = pivotValues2D(1:iter-1);
    pivotIndices2D = pivotIndices2D(1:iter-1);
    fibers = fibers(:, 1:iter-1);
    pivotValues3D = pivotValues3D(1:iter-1);
    pivotIndices3D=pivotIndices3D(1:iter-1,:);
    infNorm = 0; % If the last 2D pivot was zero, infNorm will be NaN and
    % it makes the next statement result in ifail3D \neq 0; So, put it 0 to
    % get ifail3D = 0;
end

if ( infNorm <= tol )
    ifail3D = 0;                               % We didn't fail in 3D ACA.
    if ( iter == 0 )
        ifail2D = 0;
    end
end
if ( iter > width/factor )
    ifail3D = 1;                               % We did fail in 3D ACA.
end

end
%%

%%
function absTol = GetTol2D(xx, yy, vals, dom, pseudoLevel)
% GETTOL2D   Calculate a tolerance for the Chebfun2 constructor.
%
%  This is the 2D analogue of the tolerance employed in the chebtech
%  constructors. It is based on a finite difference approximation to the
%  gradient, the size of the approximation domain, the internal working
%  tolerance, and an arbitrary (4/5) exponent.

[m, n] = size(vals);
grid = max(m, n);
relTol = grid^(4/5) * pseudoLevel; % this should be vscale and hscale invariant

% Remove some edge values so that df_dx and df_dy have the same size.
% xx is generated by ndgrid, i.e., xx changes in the first mode:
dfdx = diff(vals(:, 1:n-1), 1, 1) ./ diff(xx(:, 1:n-1), 1, 1);
% yy is generated by ndgrid, i.e., yy changes row-wise (2nd mode):
dfdy = diff(vals(1:m-1, :), 1, 2) ./ diff(yy(1:m-1, :), 1, 2);
gradNorms = [max(abs(dfdx(:))), max(abs(dfdy(:)))];
% A vector of gradient information over the domain.
if ( isempty(gradNorms) )
    % This happens if the input in not a trivariate function.
    gradNorms = 1;
end

vscale = max(abs(vals(:)));
domDiff = [diff(dom(1:2)) diff(dom(3:4))];
absTol = max(max(gradNorms.*domDiff), vscale) * relTol;
end

%%
function [col, pivotVals, row, pivotLoc, ifail2D] = chebfun2ACA(op, ...
    tol, factor)
% Perform GE with complete pivoting:

if ( factor ~= 0 )
    % FACTOR in the 3D steps is either 0 (in case of constructionFromDoubles)
    % or 2sqrt(2) otherwise. For 2D steps however, we are happy with
    % FACTOR = 0 or 2 as in Chebfun2. This IF conditional, makes it
    % possible to rewrite FACTOR in 2D steps, but at the same time keeping
    % it zero fro constructionFromDoubles.
    factor = 2;
end
[pivotVals, pivotLoc, row, col, ifail2D] = completeACA2D(op, tol, factor);
end

%%
function [pivotValue, pivotElement, rows, cols, ifail2D] = ...
    completeACA2D(A, tol, factor)
% 2D ACA with complete pivoting which is the continuous analogue of
% Gaussian elimination with complete pivoting.
% We attempt to adaptively find the numerical rank of function in the 2D
% level. This is _almost_ the same as the one in chebfun2/constructor.

% Set up output variables.
[nx, ny] = size(A);
width = min(nx, ny);        % Use to tell us how many pivots we can take.
pivotValue = zeros(1);      % Store an unknown number of Pivot values.
pivotElement = zeros(1, 2); % Store (j,k) entries of pivot location.
ifail2D = 1;                  % Assume we fail.

% Main algorithm
zRows = 0;                  % count number of zero cols/rows.
[infNorm, ind] = max(abs(reshape(A, numel(A), 1)));
[row, col] = chebfun3.myind2sub(size(A) , ind);

% Bias toward diagonal for square matrices (see reasoning below):
if ( ( nx == ny ) && ( max(abs(diag(A))) - infNorm) > -tol )
    [infNorm, ind] = max(abs(diag(A)));
    row = ind;
    col = ind;
end

scl = infNorm;
% If the function is the zero function
if ( scl == 0 )
    pivotValue = 0;
    cols = 0;
    rows = 0;
    ifail2D = 0;
else
    cols(:,1) = zeros(size(A, 1), 1);
    rows(1,:) = zeros(1, size(A, 2));
end

while ( ( infNorm > tol ) && ( zRows < width / factor) ...
        && ( zRows < min(nx, ny) ) )

    cols(:, zRows+1) = A(:, col);             % Extract skeleton columns
    rows(zRows+1, :) = A(row, :);             % Extract skeleton rows
    PivVal = A(row, col);
    A = A - cols(:, zRows+1)*(rows(zRows+1,:)./PivVal); % One step of GE

    % Keep track of progress.
    zRows = zRows + 1;                       % One more row is interpolated
    pivotValue(zRows) = PivVal;              % Store value of 2D pivot
    pivotElement(zRows, :)=[row col];        % Store index of 2D pivot

    % Find value and index of next 2D pivot
    [infNorm, ind] = max(abs(A(:))); % Slightly faster
    [row, col] = chebfun3.myind2sub(size(A), ind);

    % Have a bias towards the diagonal of A, so that it can be used as a test
    % for nonnegative definite functions. (Complete GE and Cholesky are the
    % same as nonnegative definite functions have an absolute maximum on the
    % diagonal, except there is the possibility of a tie with an off-diagonal
    % absolute maximum. Bias toward diagonal maxima to prevent this.)
    if ( ( nx == ny ) && ( max(abs(diag(A))) - infNorm) > -tol )
        [infNorm, ind] = max(abs(diag(A)));
        row = ind;
        col = ind;
    end
end

if ( infNorm <= tol )
    ifail2D = 0;                               % We didn't fail in 2D ACA
end
if ( zRows >= (width/factor) )
    ifail2D = 1;                               % We did fail in 2D ACA
end

rows = rows.';                               % To unify all the columns,
                                             % rows and tubes, store
                                             % skeleton rows also as column
                                             % vectors.
end

%%
function [core, colsTucker, rowsTucker] = btd2tucker(colsValues, ...
    rowsValues, diagValues2D, pivotVals3D, absTol)
allCols = [];
allRows = [];
allDiags = [];
nn = numel(pivotVals3D);
sizeIndex = zeros(nn, 1);

for kkk = 1:nn
    sizeIndex(kkk) = size(rowsValues{kkk}, 2);
    allCols = [allCols, colsValues{kkk}];
    allRows = [allRows, rowsValues{kkk}];
    allDiags = [allDiags; diag(diagValues2D{kkk})];
end
sizeIndex = cumsum([0; sizeIndex]);

% Compress allCols and allRows:
if ( size(allCols, 2) > 1 )
    [Su, ~, Vu, colsTucker] = completeACA2D(allCols, absTol, 0);
    % factor = 0, because we want the ACA to be applied even if op is not
    % low-rank. In contrast to Chebfun2, we now have
    % allCols = colsTucker * diag(1./Su) * Vu'.
    % Moreover, we use the same absTol to chop columns, the same tolerance
    % used to chop fibers so that ranks are less inconsistent for symmetric
    % functions.
    Vu = Vu*diag(1./Su);
else
    colsTucker = allCols;
    Vu = 1;
end

if ( size(allRows, 2) > 1 )
    [Sv, ~, Vv, rowsTucker] = completeACA2D(allRows, absTol, 0);
    Vv = Vv*diag(1./Sv);
else
    rowsTucker = allRows;
    Vv = 1;
end

% Form the core tensor:
core = zeros(size(colsTucker, 2), size(rowsTucker, 2), nn);
for kkk = 1:nn
    core(:,:,kkk) = Vu(sizeIndex(kkk)+1:sizeIndex(kkk+1), :).' * ...
        diag(allDiags(sizeIndex(kkk)+1:sizeIndex(kkk+1))) * ...
        Vv(sizeIndex(kkk)+1:sizeIndex(kkk+1), :)./pivotVals3D(kkk);
end

if ( (max(abs(allCols(:))) == 0) || (max(abs(allRows(:))) == 0) )
    % zero input
    core = 0;
end

end


function h=vl_plotsiftdescriptor(d,f,varargin)
% VL_PLOTSIFTDESCRIPTOR   Plot SIFT descriptor
%   VL_PLOTSIFTDESCRIPTOR(D) plots the SIFT descriptor D. If D is a
%   matrix, it plots one descriptor per column. D has the same format
%   used by VL_SIFT().
%
%   VL_PLOTSIFTDESCRIPTOR(D,F) plots the SIFT descriptors warped to
%   the SIFT frames F, specified as columns of the matrix F. F has the
%   same format used by VL_SIFT().
%
%   H=VL_PLOTSIFTDESCRIPTOR(...) returns the handle H to the line
%   drawing representing the descriptors.
%
%   The function assumes that the SIFT descriptors use the standard
%   configuration of 4x4 spatial bins and 8 orientations bins. The
%   following parameters can be used to change this:
%
%   NumSpatialBins:: 4
%     Number of spatial bins in both spatial directions X and Y.
%
%   NumOrientationBins:: 8
%     Number of orientation bis.
%
%   MagnificationFactor:: 3
%     Magnification factor. The width of one bin is equal to the scale
%     of the keypoint F multiplied by this factor.
%
%   See also: VL_SIFT(), VL_PLOTFRAME(), VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

opts.magnificationFactor = 3.0 ;
opts.numSpatialBins = 4 ;
opts.numOrientationBins = 8 ;
opts.maxValue = 0 ;

if nargin > 1
  if ~ isnumeric(f)
    error('F must be a numeric type (use [] to leave it unspecified)') ;
  end
end

opts = vl_argparse(opts, varargin) ;

% --------------------------------------------------------------------
%                                                  Check the arguments
% --------------------------------------------------------------------

if(size(d,1) ~= opts.numSpatialBins^2 * opts.numOrientationBins)
  error('The number of rows of D does not match the geometry of the descriptor') ;
end

if nargin > 1
  if (~isempty(f) & (size(f,1) < 2 | size(f,1) > 6))
    error('F must be either empty of have from 2 to six rows.');
  end

  if size(f,1) == 2
    % translation only
    f(3:6,:) = deal([10 0 0 10]') ;
    %f = [f; 10 * ones(1, size(f,2)) ; 0 * zeros(1, size(f,2))] ;
  end

  if size(f,1) == 3
    % translation and scale
    f(3:6,:) = [1 0 0 1]' * f(3,:) ;
    %f = [f; 0 * zeros(1, size(f,2))] ;
  end

  if size(f,1) == 4
    c = cos(f(4,:)) ;
    s = sin(f(4,:)) ;
    f(3:6,:) = bsxfun(@times, f(3,:), [c ; s ; -s ; c]) ;
  end

  if size(f,1) == 5
    assert(false) ;
    c = cos(f(4,:)) ;
    s = sin(f(4,:)) ;
    f(3:6,:) = bsxfun(@times, f(3,:), [c ; s ; -s ; c]) ;
  end

  if(~isempty(f) & size(f,2) ~= size(d,2))
    error('D and F have incompatible dimension') ;
  end
end

% Descriptors are often non-double numeric arrays
d = double(d) ;
K = size(d,2) ;

if nargin < 2 | isempty(f)
  f = repmat([0;0;1;0;0;1],1,K) ;
end

% --------------------------------------------------------------------
%                                                           Do the job
% --------------------------------------------------------------------

xall=[] ;
yall=[] ;

for k=1:K
  [x,y] = render_descr(d(:,k), opts.numSpatialBins, opts.numOrientationBins, opts.maxValue) ;
  xall = [xall opts.magnificationFactor*f(3,k)*x + opts.magnificationFactor*f(5,k)*y + f(1,k)] ;
  yall = [yall opts.magnificationFactor*f(4,k)*x + opts.magnificationFactor*f(6,k)*y + f(2,k)] ;
end

h=line(xall,yall) ;

% --------------------------------------------------------------------
function [x,y] = render_descr(d, numSpatialBins, numOrientationBins, maxValue)
% --------------------------------------------------------------------

% Get the coordinates of the lines of the SIFT grid; each bin has side 1
[x,y] = meshgrid(-numSpatialBins/2:numSpatialBins/2,-numSpatialBins/2:numSpatialBins/2) ;

% Get the corresponding bin centers
xc = x(1:end-1,1:end-1) + 0.5 ;
yc = y(1:end-1,1:end-1) + 0.5 ;

% Rescale the descriptor range so that the biggest peak fits inside the bin diagram
if maxValue
    d = 0.4 * d / maxValue ;
else
    d = 0.4 * d / max(d(:)+eps) ;
end

% We scramble the the centers to have them in row major order
% (descriptor convention).
xc = xc' ;
yc = yc' ;

% Each spatial bin contains a star with numOrientationBins tips
xc = repmat(xc(:)',numOrientationBins,1) ;
yc = repmat(yc(:)',numOrientationBins,1) ;

% Do the stars
th=linspace(0,2*pi,numOrientationBins+1) ;
th=th(1:end-1) ;
xd = repmat(cos(th), 1, numSpatialBins*numSpatialBins) ;
yd = repmat(sin(th), 1, numSpatialBins*numSpatialBins) ;
xd = xd .* d(:)' ;
yd = yd .* d(:)' ;

% Re-arrange in sequential order the lines to draw
nans = NaN * ones(1,numSpatialBins^2*numOrientationBins) ;
x1 = xc(:)' ;
y1 = yc(:)' ;
x2 = x1 + xd ;
y2 = y1 + yd ;
xstars = [x1;x2;nans] ;
ystars = [y1;y2;nans] ;

% Horizontal lines of the grid
nans = NaN * ones(1,numSpatialBins+1);
xh = [x(:,1)' ; x(:,end)' ; nans] ;
yh = [y(:,1)' ; y(:,end)' ; nans] ;

% Verical lines of the grid
xv = [x(1,:) ; x(end,:) ; nans] ;
yv = [y(1,:) ; y(end,:) ; nans] ;

x=[xstars(:)' xh(:)' xv(:)'] ;
y=[ystars(:)' yh(:)' yv(:)'] ;


function [recall, precision, info] = vl_pr(labels, scores, varargin)
%VL_PR   Precision-recall curve.
%   [RECALL, PRECISION] = VL_PR(LABELS, SCORES) computes the
%   precision-recall (PR) curve. LABELS are the ground truth labels,
%   greather than zero for a positive sample and smaller than zero for
%   a negative one. SCORES are the scores of the samples obtained from
%   a classifier, where lager scores should correspond to positive
%   samples.
%
%   Samples are ranked by decreasing scores, starting from rank 1.
%   PRECISION(K) and RECALL(K) are the precison and recall when
%   samples of rank smaller or equal to K-1 are predicted to be
%   positive and the remaining to be negative. So for example
%   PRECISION(3) is the percentage of positive samples among the two
%   samples with largest score. PRECISION(1) is the precision when no
%   samples are predicted to be positive and is conventionally set to
%   the value 1.
%
%   Set the zero the lables of samples that should be ignored in the
%   evaluation. Set to -INF the scores of samples which are not
%   retrieved. If there are samples with -INF score, then the PR curve
%   may have maximum recall smaller than 1, unless the INCLUDEINF
%   option is used (see below). The options NUMNEGATIVES and
%   NUMPOSITIVES can be used to specify additional samples with -INF
%   score (see below).
%
%   [RECALL, PRECISION, INFO] = VL_PR(...) returns an additional
%   structure INFO with the following fields:
%
%   info.auc::
%     The area under the precision-recall curve. If the INTERPOLATE
%     option is set to FALSE, then trapezoidal interpolation is used
%     to integrate the PR curve. If the INTERPOLATE option is set to
%     TRUE, then the curve is piecewise constant and no other
%     approximation is introduced in the calculation of the area. In
%     the latter case, INFO.AUC is the same as INFO.AP.
%
%   info.ap::
%     Average precision as defined by TREC. This is the average of the
%     precision observed each time a new positive sample is
%     recalled. In this calculation, any sample with -INF score
%     (unless INCLUDEINF is used) and any additional positive induced
%     by NUMPOSITIVES has precision equal to zero. If the INTERPOLATE
%     option is set to true, the AP is computed from the interpolated
%     precision and the result is the same as INFO.AUC. Note that AP
%     as defined by TREC normally does not use interpolation [1].
%
%   info.ap_interp_11::
%     11-points interpolated average precision as defined by TREC.
%     This is the average of the maximum precision for recall levels
%     greather than 0.0, 0.1, 0.2, ..., 1.0. This measure was used in
%     the PASCAL VOC challenge up to the 2008 edition.
%
%   info.auc_pa08::
%     Deprecated. It is the same of INFO.AP_INTERP_11.
%
%   VL_PR(...) with no output arguments plots the PR curve in the
%   current axis.
%
%   VL_PR() accepts the following options:
%
%   Interpolate:: false
%     If set to true, use interpolated precision. The interpolated
%     precision is defined as the maximum precision for a given recall
%     level and onwards. Here it is implemented as the culumative
%     maximum from low to high scores of the precision.
%
%   NumPositives:: []
%   NumNegatives:: []
%     If set to a number, pretend that LABELS contains this may
%     positive/negative labels. NUMPOSITIVES/NUMNEGATIVES cannot be
%     smaller than the actual number of positive/negative entrires in
%     LABELS. The additional positive/negative labels are appended to
%     the end of the sequence, as if they had -INF scores (not
%     retrieved). This is useful to evaluate large retrieval systems
%     for which one stores ony a handful of top results for efficiency
%     reasons.
%
%   IncludeInf:: false
%     If set to true, data with -INF score SCORES is included in the
%     evaluation and the maximum recall is 1 even if -INF scores are
%     present. This option does not include any additional positive or
%     negative data introduced by specifying NUMPOSITIVES and
%     NUMNEGATIVES.
%
%   Stable:: false
%     If set to true, RECALL and PRECISION are returned the same order
%     of LABELS and SCORES rather than being sorted by decreasing
%     score (increasing recall). Samples with -INF scores are assigned
%     RECALL and PRECISION equal to NaN.
%
%   About the PR curve::
%     This section uses the same symbols used in the documentation of
%     the VL_ROC() function. In addition to those quantities, define:
%
%       PRECISION(S) = TP(S) / (TP(S) + FP(S))
%       RECALL(S) = TPR(S) = TP(S) / P
%
%     The precision is the fraction of positivie predictions which are
%     correct, and the recall is the fraction of positive labels that
%     have been correctly classified (recalled). Notice that the recall
%     is also equal to the true positive rate for the ROC curve (see
%     VL_ROC()).
%
%   REFERENCES:
%   [1] C. D. Manning, P. Raghavan, and H. Schutze. An Introduction to
%   Information Retrieval. Cambridge University Press, 2008.
%
%   See also VL_ROC(), VL_HELP().

% Author: Andrea Vedaldi

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

[tp, fp, p, n, perm, varargin] = vl_tpfp(labels, scores, varargin{:}) ;
opts.stable = false ;
opts.interpolate = false ;
opts = vl_argparse(opts,varargin) ;

% compute precision and recall
small = 1e-10 ;
recall = tp / max(p, small) ;
precision = max(tp, small) ./ max(tp + fp, small) ;

% interpolate precision if needed
if opts.interpolate
  precision = fliplr(vl_cummax(fliplr(precision))) ;
end

% --------------------------------------------------------------------
%                                                      Additional info
% --------------------------------------------------------------------

if nargout > 2 || nargout == 0

  % area under the curve using trapezoid interpolation
  if ~opts.interpolate
    info.auc = 0.5 * sum((precision(1:end-1) + precision(2:end)) .* diff(recall)) ;
  end

  % average precision (for each recalled positive sample)
  sel = find(diff(recall)) + 1 ;
  info.ap = sum(precision(sel)) / p ;
  if opts.interpolate
    info.auc = info.ap ;
  end

  % TREC 11 points average interpolated precision
  info.ap_interp_11 = 0.0 ;
  for rc = linspace(0,1,11)
    pr = max([0, precision(recall >= rc)]) ;
    info.ap_interp_11 = info.ap_interp_11 + pr / 11 ;
  end

  % legacy definition
  info.auc_pa08 = info.ap_interp_11 ;
end

% --------------------------------------------------------------------
%                                                                 Plot
% --------------------------------------------------------------------

if nargout == 0
  cla ; hold on ;
  plot(recall,precision,'linewidth',2) ;
  spline([0 1], [1 1] * p / length(labels), 'r--', 'linewidth', 2) ;
  axis square ; grid on ;
  xlim([0 1]) ; xlabel('recall') ;
  ylim([0 1]) ; ylabel('precision') ;
  title(sprintf('PR (AUC: %.2f%%, AP: %.2f%%, AP11: %.2f%%)', ...
                info.auc * 100, ...
                info.ap * 100, ...
                info.ap_interp_11 * 100)) ;
  if opts.interpolate
    legend('PR interp.', 'PR rand.', 'Location', 'SouthEast') ;
  else
    legend('PR', 'PR rand.', 'Location', 'SouthEast') ;
  end
  clear recall precision info ;
end

% --------------------------------------------------------------------
%                                                        Stable output
% --------------------------------------------------------------------

if opts.stable
  precision(1) = [] ;
  recall(1) = [] ;
  precision_ = precision ;
  recall_ = recall ;
  precision = NaN(size(precision)) ;
  recall = NaN(size(recall)) ;
  precision(perm) = precision_ ;
  recall(perm) = recall_ ;
end

% --------------------------------------------------------------------
function h = spline(x,y,spec,varargin)
% --------------------------------------------------------------------
prop = vl_linespec2prop(spec) ;
h = line(x,y,prop{:},varargin{:}) ;


function [cmap] = makeColorGradient(col1, col2, ncol)
% Generates a color gradient in hex format, based on color 1 and 2
%
% USAGE:
%
%    [cmap] = makeColorGradient(col1, col2, ncol)
%
% INPUTS:
%    col1:                      First color
%    col2:                      Last color
%    ncol:                      Number of colors in between
%
% OUTPUT:
%    cmap:                      Color gradient map with the corresponding
%                               colors in hex format
%
% .. Author: - Nicolas Mendoza-Mejia May/2021

hexformat = "#%02x%02x%02x";

rgb1 = sscanf(col1, hexformat);
rgb2 = sscanf(col2, hexformat);

T = (rgb2 -rgb1)/(ncol - 1);
rgb = zeros(3, ncol);

for i=1:3
   if T(i) ~= 0
    rgb(i, :) =  rgb1(i):T(i):rgb2(i);
   end
end

rgb = round(rgb);
cmap = cell(1, ncol);
for i=1:ncol
   cmap{i} = sprintf(hexformat, rgb(1, i), rgb(2, i), rgb(3, i));
end

end


function [M, moral_edges] = moralize(G)
% MORALIZE Ensure that for every child, all its parents are married, and drop directionality of edges.
% [M, moral_edges] = moralize(G)

M = G;
n = length(M);
for i=1:n
  fam = family(G,i);
  M(fam,fam)=1;
end
M = setdiag(M,0);
moral_edges = sparse(triu(max(0,M-G),1));


function [no,el]=fillsurf(node,face)
%
% [no,el]=fillsurf(node,face)
%
% calculate the enclosed volume for a closed surface
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%    node:  node coordinates
%    face:  surface triangle list
%
% output:
%    vol:   total volume of the enclosed space
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

[no,el]=surf2mesh(node,face,[],[],1,1,[],[],0,'tetgen','-YY');


function unprocessed = find_unprocessed(dag)
% unprocessed = find_unprocessed(dag)
%
% francois.olivier.c.h@gmail.com

undirected_edges = dag.*dag';
[unprocessed, tmp] = find(undirected_edges)
unprocessed = unique(unprocessed);
%  N = size(dag,1);
%  unprocessed = [];
%  for i=1:(N-1)
%      for j=(i+1):N
%          if dag(i,j)==1 & dag(j,i)==1
%             unprocessed = [unprocessed,i,j];
%          end
%      end
%  end
%  unprocessed = unique(unprocessed);


% SELFINTERSECT Given a triangle mesh (V,F) compute a new mesh (VV,FF) which is the same as
% (V,F) except that any self-intersecting triangles in (V,F) have been
% subdivided (new vertices and face created) so that the self-intersection
% contour lies exactly on edges in (VV,FF). New vertices will appear in
% original faces or on original edges. New vertices on edges are "merged" only
% across original faces sharing that edge. This means that if the input
% triangle mesh is a closed manifold the output will be too.
%
% [VV,FF,IF] = selfintersect(V,F,'ParameterName',ParameterValue, ...)
%
% Inputs:
%   V  #V by 3 list of vertex positions
%   F  #F by 3 list of triangle indices into V
%   Optional:
%     'DetectOnly'  followed by bool. Whether to only detect intersecting pairs
%       (sets IF but not VV or FF) {false}
%     'FirstOnly'  followed by bool whether to only detect the first
%       intersection. {false}
%     'StitchAll'  followed by whether to stitch all vertices in the output, if
%       true then IM will be 1:size(VV,1) {false}
% Outputs:
%   VV  #VV by 3 list of vertex positions
%   FF  #FF by 3 list of triangle indices into V
%   IF  #intersecting face pairs by 2  list of intersecting face pairs,
%     indexing F
%   J   #FF list of indices into F of birth parents
%   IM  #VV list of indices into VV of unique vertices
%
% Example:
%   [SV,SF,~,~,IM] = selfintersect(V,F);
%   FF = IM(SF);
%   [U,IM] = remove_unreferenced(SV,FF);
%   G = IM(FF);
%
%   % Self-Intersect (V,F)+(U,G) and separate
%   VU = [V;U];
%   FG = [F;size(V,1)+G];
%   [SVU,SFG,~,J] = selfintersect(VU,FG);
%   SF = SFG(J<=size(F,1),:);
%   SG = SFG(J>size(F,1),:);
%   [SV,IM] = remove_unreferenced(SVU,SF);
%   SF = IM(SF);
%   [SU,IM] = remove_unreferenced(SVU,SG);
%   SG = IM(SG);
%
%

% See selfintersect.h, selfintersect.cpp for mex implementation


function [res,qualMeasOut]=MLEM(proj,geo,angles,niter,varargin)
%MLEM solves the tomographic problem by using Maximum Likelihood Expectation
% Maximisation algorithm.
%
%   MLEM(PROJ,GEO,ALPHA,NITER,opt) solves the reconstruction problem
%   using the projection data PROJ taken over ALPHA angles, corresponding
%   to the geometry described in GEO, using NITER iterations.
%
% 'verbose': Get feedback or not. Default: 1
%
% 'init':    Describes different initialization techniques.
%             •  'none'     : Initializes the image to ones (default)
%             •  'FDK'      : Initializes image to FDK reconstruction
%
% 'QualMeas':    Asks the algorithm for a set of quality measurement
%                parameters. Input should contain a cell array of desired
%                quality measurement names. Example: {'CC','RMSE','MSSIM'}
%                These will be computed in each iteration.
%
% 'groundTruth':  An image as ground truth, to be used if quality measures
%                 are requested, to plot their change w.r.t. this known
%                 data.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------
[verbose,res,QualMeasOpts,gpuids,gt]=parse_inputs(proj,geo,angles,varargin);
measurequality=~isempty(QualMeasOpts) | ~any(isnan(gt(:)));
if ~any(isnan(gt(:)))
    QualMeasOpts{end+1}='error_norm';
    res_prev=gt;
    clear gt
end
if nargout<2 && measurequality
    warning("Image metrics requested but none caught as output. Call the algorithm with 3 outputs to store them")
    measurequality=false;
end
qualMeasOut=zeros(length(QualMeasOpts),niter);

res = max(res,0);

% Back-projection weight, V
V = Atb(ones(size(proj),'single'),geo,angles,'matched','gpuids',gpuids);
V(V<=0.) = inf;

for ii=1:niter
    if measurequality && ~strcmp(QualMeasOpts,'error_norm')
        res_prev = res; % only store if necessary
    end
    if (ii==1);tic;end

    den = Ax(res,geo,angles,'gpuids',gpuids);
    den(den<=0.)=inf;

    imgupdate = Atb(proj./den, geo,angles,'matched','gpuids',gpuids)./V;
    res = max(res.*imgupdate,0.);

    if measurequality
        qualMeasOut(:,ii)=Measure_Quality(res_prev,res,QualMeasOpts);
    end

    if (ii==1)&&(verbose==1)
        expected_time=(toc)*niter;
        disp('MLEM');
        disp(['Expected duration   :    ',secs2hms(expected_time)]);
        disp(['Expected finish time:    ',datestr(datetime('now')+seconds(expected_time))]);
        disp('');
    end

end
end

%% Parse inputs
function [verbose,f0,QualMeasOpts,gpuids,gt]=parse_inputs(proj,geo,angles,argin)
opts = {'verbose','init','qualmeas','gpuids','groundtruth'};
defaults=ones(length(opts),1);
% Check inputs
nVarargs = length(argin);
if mod(nVarargs,2)
    error('TIGRE:MLEM:InvalidInput','Invalid number of inputs')
end

% check if option has been passed as input
for ii=1:2:nVarargs
    ind=find(ismember(opts,lower(argin{ii})));
    if ~isempty(ind)
        defaults(ind)=0;
    else
        error('TIGRE:MLEM:InvalidInput',['Optional parameter "' argin{ii} '" does not exist' ]);
    end
end

for ii=1:length(opts)
    opt=opts{ii};
    default=defaults(ii);
    % if one option is not default, then extract value from input
    if default==0
        ind=double.empty(0,1);jj=1;
        while isempty(ind)
            ind=find(isequal(opt,lower(argin{jj})));
            jj=jj+1;
        end
        if isempty(ind)
            error('TIGRE:MLEM:InvalidInput',['Optional parameter "' argin{jj} '" does not exist' ]);
        end
        val=argin{jj};
    end

    switch opt
        % % % % % % % Verbose
        case 'verbose'
            if default
                verbose=1;
            else
                verbose=val;
            end
            if ~is2014bOrNewer
                warning('TIGRE: Verbose mode not available for older versions than MATLAB R2014b');
                verbose=false;
            end
        % Initial image
        %  =========================================================================
        case 'init'
            if default || strcmp(val,'none')
                f0=ones(geo.nVoxel','single');
            else
                if strcmp(val,'FDK')
                    f0=FDK(proj, geo, angles);
                else
                    error('TIGRE:MLEM:InvalidInput','Invalid init')
                end
            end
        % Image Quality Measure
        %  =========================================================================
        case 'qualmeas'
            if default
                QualMeasOpts={};
            else
                if iscellstr(val)
                    QualMeasOpts=val;
                else
                    error('TIGRE:MLEM:InvalidInput','Invalid quality measurement parameters');
                end
            end
        % GPUIDS
        % =========================================================================
        case 'gpuids'
            if default
                gpuids = GpuIds();
            else
                gpuids = val;
            end
        case 'groundtruth'
            if default
                gt=nan;
            else
                gt=val;
            end
        otherwise
            error('TIGRE:MLEM:InvalidInput',['Invalid input name:', num2str(opt),'\n No such option in MLEM()']);
    end
end
end


classdef SVM < handle

  properties

    svm;

  end

  methods

    function a = SVM()
      %Wrapper on libsvm.

    end

    function train(a, inputs, outputs)

      a.svm = svmtrain(outputs, inputs);

    end

    function outputs = test(a, inputs)

      outputs = svmpredict(ones(size(inputs, 1), 1), inputs, a.svm);

    end

    function margins = margins(a, inputs)

      [guess acc margins] = svmpredict(ones(size(inputs, 1), 1), inputs, a.svm);

    end

    function [weights theta] = weights(a)
        weights = (a.svm.sv_coef' * full(a.svm.SVs));
        theta = a.svm.rho;
    end

  end

end


Fs=44100;
dt=1/Fs;

T=1/f;

sls=get(handles.sl,'string');
sl=str2num(sls); % signal length

xl=get(handles.axes1,'Xlim');
yl=get(handles.axes1,'Ylim');
dxl=xl(2)-xl(1);
dyl=yl(2)-yl(1);

xc1=0:dt:sl;
xc=mod(xc1,T); % turn to one period
if iscnt
    yc=interp1([xys(1,:)-dxl xys(1,:) xys(1,:)+dxl],[xys(2,:) xys(2,:) xys(2,:)],xc,mth,'extrap');
else
    yc=interp1(xys(1,:),xys(2,:),xc,mth,'extrap');
end

yc=(yc-mean(yc))/max(abs(yc)); % mormalize, debias


classdef ApoapsisAltitudeTermCondition < AbstractEventTerminationCondition
    %PeriapsisAltitudeTermCondition Summary of this class goes here
    %   Detailed explanation goes here

    properties
        apoalt(1,1) double = 0; %km
        bodyInfo KSPTOT_BodyInfo
    end

    methods
        function obj = ApoapsisAltitudeTermCondition(apoalt)
            obj.apoalt = apoalt;
        end

        function evtTermCondFcnHndl = getEventTermCondFuncHandle(obj)
            evtTermCondFcnHndl = @(t,y) obj.eventTermCond(t,y);
        end

        function initTermCondition(obj, initialStateLogEntry)
            obj.bodyInfo = initialStateLogEntry.centralBody;
        end

        function name = getName(obj)
            name = sprintf('Apoapsis Altitude (%.3f km)', obj.apoalt);
        end

        function tf = shouldBeReinitOnRestart(obj)
            tf = true;
        end

        function params = getTermCondUiStruct(obj)
            params = struct();

            params.paramName = 'Apo. Altitude';
            params.paramUnit = 'km';
            params.useParam = 'on';
            params.useStages = 'off';
            params.useTanks = 'off';
            params.useEngines = 'off';
            params.useStopwatches = 'off';

            params.value = obj.apoalt;
            params.refStage = LaunchVehicleStage.empty(1,0);
            params.refTank = LaunchVehicleEngine.empty(1,0);
            params.refEngine = LaunchVehicleEngine.empty(1,0);
            params.refStopwatch = LaunchVehicleStopwatch.empty(1,0);
        end

        function optVar = getNewOptVar(obj)
            optVar = ApoapsisAltitudeOptimizationVariable(obj);
        end

        function optVar = getExistingOptVar(obj)
            optVar = obj.optVar;
        end

        function tf = usesStage(obj, stage)
            tf = false;
        end

        function tf = usesEngine(obj, engine)
            tf = false;
        end

        function tf = usesTank(obj, tank)
            tf = false;
        end

        function tf = usesEngineToTankConn(obj, engineToTank)
            tf = false;
        end

        function tf = usesStopwatch(obj, stopwatch)
            tf = false;
        end
    end

    methods(Static)
        function termCond = getTermCondForParams(paramValue, stage, tank, engine, stopwatch)
            termCond = ApoapsisAltitudeTermCondition(paramValue);
        end
    end

    methods(Access=private)
        function [value,isterminal,direction] = eventTermCond(obj, t,y)
            rVect = y(1:3);
            vVect = y(4:6);
            cartElem = CartesianElementSet(t, rVect(:), vVect(:), obj.bodyInfo.getBodyCenteredInertialFrame());
            kepElem = cartElem.convertToFrame(obj.frame).convertToKeplerianElementSet();

            value =  kepElem.getAltitudeApoapsis() - obj.apoalt;
            isterminal = 1;
            direction = 0;
        end
    end
end


function C=cutcategories(t,j)
%CUTCATEGORIES Categories used for branches in decision tree.
%   C=CUTCATEGORIES(T) returns an N-by-2 cell array C of the categories
%   used at branches in the decision tree T, where N is the number of
%   nodes in the tree.  For each branch node K based on a categorical
%   predictor variable Z, the left child is chosen if Z is among the
%   categories listed in C{K,1}, and the right child is chosen if Z is
%   among those listed in C{K,2}.  Both columns of C are empty for branch
%   nodes based on continuous predictors and for non-branch (leaf) nodes.
%
%   C=CUTCATEGORIES(T,J) takes an array J of node numbers and returns the
%   categories for the specified nodes.
%
%   See also CLASSREGTREE, CLASSREGTREE/CUTVAR, CLASSREGTREE/CUTPOINT, CLASSREGTREE/CUTTYPE.

%   Copyright 2006-2007 The MathWorks, Inc.
%   $Revision: 1.1.6.6 $  $Date: 2009/01/08 18:59:32 $

if nargin>=2 && ~validatenodes(t,j)
    error('stats:classregtree:cutcategories:InvalidNode',...
          'J must be an array of node numbers or a logical array of the proper size.');
end

% Get variable numbers
if nargin<2
    n = t.var;
    cut = t.cut;
else
    n = t.var(j,:);
    cut = t.cut(j);
end

% Set up array for output
C = repmat({[] []},numel(n),1);

% Fetch category list pairs for categorical branches
catvar = find(n<0);
if ~isempty(catvar)
    for i=1:length(catvar)
        C(catvar(i),:) = cut{catvar(i)};
    end
end


function Dual_TVL1_optic_flow( I0,I1,     u1, u2, nx, ny,     tau,    lambda, theta,   warps,  epsilon,verbose,iflagMedian)
size = round(nx * ny);
l_t = lambda * theta;
if isa(u1,'cell') && isa(u2,'cell')
   u1 = cell2mat(u1);
   u2 = cell2mat(u2);
end
MAX_ITERATIONS= 300;
PRESMOOTHING_SIGMA =0.8;
GRAD_IS_ZERO = 1E-10;
[I1x, I1y] = centered_gradient(I1, 0, 0, nx, ny);
  for  i = 1: size
        p11(i) =0.0;
        p12(i) = 0.0;
        p21(i) =0.0;
        p22(i) = 0.0;
  end
  rho_c = zeros(1,size);
  grad = zeros(1,size);
  for  warpings = 1: warps
      I1w = bicubic_interpolation_warp(I1,  u1, u2, 0,  nx, ny, 1);
      I1wx =  bicubic_interpolation_warp(I1x, u1, u2, 0, nx, ny, 1);
     I1wy = bicubic_interpolation_warp(I1y, u1, u2, 0, nx, ny, 1);
      for  i = 1:size
           Ix2 = I1wx(i) * I1wx(i);
             Iy2 = I1wy(i) * I1wy(i);

            grad(i) = (Ix2 + Iy2);
             sizes = [length(u2),length(I1w),length(I1wx),length(u1),length(I1wy),length(I0)];
if i <= min(sizes)
            rho_c(i) = (I1w(i) - I1wx(i) * u1(i)- I1wy(i) * u2(i) - I0(i));
end
      end
     n = 0;
       error = Inf;
        while (error > epsilon * epsilon && n < MAX_ITERATIONS)
            n = n +1;

            for  i = 1 : size
                 sizes = [length(u2),length(I1w),length(I1wx),length(u1),length(I1wy),length(I0)];
if i <= min(sizes)
               rho = rho_c(i)+ (I1wx(i) * u1(i) + I1wy(i) * u2(i));

               d1 = 0.0;  d2 = 0.0;

                if (rho < - l_t * grad(i))
                    d1 = l_t * I1wx(i);
                    d2 = l_t * I1wy(i);
                 else
                    if (rho > l_t * grad(i))
                        d1 = -l_t * I1wx(i);
                        d2 = -l_t * I1wy(i);
                     else
                        if (grad(i) < GRAD_IS_ZERO)
                            d1 = 0;
                            d2 = 0;
                        else
                             fi = -rho/grad(i);
                            d1 = fi * I1wx(i);
                            d2 = fi * I1wy(i);
                        end

                    end
                end
end
if(i <= length(u1))
                v1(i) = u1(i) + d1;
end
if(i <= length(u2))
                v2(i) = u2(i) + d2;
end
            end
           div_p1 =  divergence(p11, p12, 0, nx ,ny);
           div_p2 = divergence(p21, p22, 0, nx ,ny);
            error = 0.0;

            for  i = 1:  size-1
                if(i <= length(u1))
                u1k = u1(i);
                u2k = u2(i);

                u1(i) = v1(i) + theta * div_p1(i);
                u2(i) = v2(i) + theta * div_p2(i);

                error = error + (u1(i) - u1k) * (u1(i) - u1k) +(u2(i) - u2k) * (u2(i) - u2k);
                end
            end
            error =  error / size;
           [u1x, u1y ] = forward_gradient(u1, 0, 0, nx ,ny);
           [u2x, u2y] =  forward_gradient(u2, 0, 0, nx ,ny);
          for (i = 1:  size-1)
                 taut = tau / theta;
                 if i < length(u1x)
                g1   = hypot(u1x(i), u1y(i));
                g2   = hypot(u2x(i), u2y(i));
                 end
               ng1  = 1.0 + taut * g1;
                 ng2  = 1.0 + taut * g2;

                p11(i) = (p11(i) + taut * u1x(i)) / ng1;
                p12(i) = (p12(i) + taut * u1y(i)) / ng1;
                p21(i) = (p21(i) + taut * u2x(i)) / ng2;
                p22(i) = (p22(i) + taut * u2y(i)) / ng2;
          end

        end

        %applay the filter and continue
end


function varargout = cgal_meshseg(varargin)
% CGAL_MESHSEG  Surface meshing of an isosurface from a segmentation or
% grayscale image.
%
% This function is a Matlab wrapper of the CGAL 3D Surface Mesh Generation
% for a grayscale input image.
%
% http://www.cgal.org/Manual/latest/doc_html/cgal_manual/Surface_mesher/Chapter_main.html
%
% Note that even though CGAL internally associates x <-> rows, y <-> cols,
% this functions presents to the user the usual Matlab convention of
% x <-> cols, y <-> rows.
%
% [TRI, X] = cgal_meshseg(IM, ISOVAL)
%
%   IM is a 3D array or a SCIMAT struct with a segmentation or a grayscale
%   image.
%
%   ISOVAL is a scalar value that defines the isosurface to be meshed.
%
%   Note that if you have a binary segmentation (background=0, segmented
%   voxels=1), ISOVAL=1 will give a very tight surface connecting the
%   centres of the segmented voxels on the boundary. With small values,
%   ISOVAL=0.01, the surface will be close to the centres of the adjacent
%   background voxels. With ISOVAL=0.5, the surface will be halfway between
%   the centres of the background and segmented voxels. The latter is
%   usually the desired result.
%
%   TRI is a 3-column matrix. Each row represents the indices of the three
%   vertices that form a triangle. TRI as a whole represents the closed
%   surface.
%
%   X is a 3-column matrix. Each row represents the Cartesian coordinates
%   of a vertex on the surface, indexed by TRI values.
%
% ... = cgal_meshseg(IM, ISOVAL, MINALPHA, MAXRAD, MAXD, C, MANIFOLD)
%
%   MINALPHA, MAXRAD, MAXD are scalars that implement the three meshing
%   criteria in CGAL::Surface_mesh_default_criteria_3
%
%   http://www.cgal.org/Manual/latest/doc_html/cgal_manual/Surface_mesher_ref/Class_Surface_mesh_default_criteria_3.html#Cross_link_anchor_1519
%
%   MINALPHA is "a lower bound on the minimum angle in degrees of the
%   surface mesh facets". By default, MINALPHA = 30.0.
%
%   MAXRAD is "an upper bound on the radius of surface Delaunay balls. A
%   surface Delaunay ball is a ball circumscribing a facet, centered on the
%   surface and empty of vertices. Such a ball exists for each facet of the
%   current surface mesh. Indeed the current surface mesh is the Delaunay
%   triangulation of the current sampling restricted to the surface which
%   is just the set of facets in the three dimensional Delaunay
%   triangulation of the sampling that have a Delaunay surface ball". By
%   default, MAXRAD is 1/2 of the minimum voxel size dimension. For
%   example, if voxels have size [0.1 0.2 0.5], then by default
%   MAXRAD=0.05.
%
%   MAXD is "an upper bound on the center-center distances of the surface
%   mesh facets. The center-center distance of a surface mesh facet is the
%   distance between the facet circumcenter and the center of its surface
%   Delaunay ball". By default, MAXD is computed the same as MAXRAD.
%
%   C is a 3-vector with the coordinates of the centre of the bounding
%   sphere used by the meshing algorithm. This is an important parameter.
%   If C is close to the surface, it can produce lots of little triangles
%   in that area. If C is outside the segmentation, the computed mesh may
%   be incomplete.
%
%   MANIFOLD is a boolean flag.
%
%     MANIFOLD=false, the mesher uses CGAL::Non_manifold_tag: "When
%     instantiated with the tag Non_manifold_tag the function template
%     make_surface_mesh does not ensure that the output mesh is a manifold
%     surface. The manifold property of output mesh may nevertheless result
%     from the choice of appropriate meshing criteria".
%
%     MANIFOLD=true, the mesher uses CGAL::Manifold_tag: "When instantiated
%     with the tag Manifold_tag the function template make_surface_mesh
%     ensures that the output mesh is a manifold surface without boundary".
%
% Important!
%
% Note that this function can produce meshes with (1) stray vertices that
% belong to no triangle, (2) triangles not oriented with respect to the
% manifold, and (3) holes in the surface. These problems can be solved with
% the following functions available in Gerardus:
%
%   [tri, x] = tri_squeeze(tri, x); % remove stray vertices
%   [x, tri] = meshcheckrepair(x, tri, 'deep'); % correct orientation
%   tri = cgal_tri_fillholes(tri, x); % fill holes in the surface
%
% See also: bwmesh, tri_squeeze, meshcheckrepair, cgal_tri_fillholes.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2013 University of Oxford
% Version: 0.1.1
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see
% <http://www.gnu.org/licenses/>.

error('MEX function not found')


function [VI,L]=TriScatteredInterp_nat_near(X,V,XI)

%Natural neighbour interpolation
F=scatteredInterpolant(X,V,'natural');
VI=F(XI);

%Use nearest neighbour for points outside of convex-hull
L=any(isnan(VI),2);
if any(L)
    F=scatteredInterpolant(X,V,'nearest');
    VI(L,:)=F(XI(L,:));
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [ res ] = point_in_rect( pt, rect )
% Tests if the point pt is within rectangle rect
%  rect is [left_x top_y right_x bottom_y]

res = (pt(:,1) >= rect(1)) & (pt(:,2) >= rect(2)) & (pt(:,1) <= rect(3)) & (pt(:,2) <= rect(4));

end


function result = avoidcopies_functions
% AVOIDCOPIES_FUNCTIONS
% Compares the time of code segment 3, which does not use in-place function calling, to
% the time for code segment 4, which does. It returns a structure with the
% fields:
%
%   timeSegment3= time for code segment 3
%   timeSegment4= time for code segment 4
%   speedUp = timeSegment3/timeSegment4;
%   percentLessTime = 100*(timeSegment3-timeSegment4)/timeSegment3;
%
% The speedUp value is good for characterizing large improvements, whereas
% the percentLessTime is good at characterizing small improvements. Run a
% few times to remove first time costs.
%
%   Example:
%   >> avoidcopies_functions

%% Call code segments
result.timeSegment3=segment3;
result.timeSegment4=segment4;

%% Calculate performance improvement
% In terms of speed up and percentage less time
result.speedUp=result.timeSegment3/result.timeSegment4;
result.percentLessTime=100*(result.timeSegment3-result.timeSegment4)/result.timeSegment3;

%% Code segment 3
function time=segment3
N=3e3;
x=rand(N);
tic;
y=myfun(x);
time=toc;

function y=myfun(x)
y=1.2*x;

%% Code segment 4
function time=segment4
N=3e3;
x=rand(N);
tic
x=myfun_ip(x);
time=toc;

function x=myfun_ip(x)
x=1.2*x; % In-place


function [Y, optinf] = bpdngrp(D, S, lambda, mu, g, opt)

% bpdngrp -- Basis Pursuit DeNoising with l2,1 group sparsity
%
%         argmin_x (1/2)||D*x - s||_2^2 + lambda*||x||_1 +
%                  mu * \sum_l ||G_l(x)||_2
%
%         The solution is computed using the ADMM approach (see
%         boyd-2010-distributed for details).
%
% Usage:
%       [Y, optinf] = bpdngrp(D, S, lambda, mu, g, opt)
%
% Input:
%       D           Dictionary matrix
%       S           Signal vector (or matrix)
%       lambda      Regularization parameter
%       mu          l2,1 regularization parameter
%       g           Vector containing index values indicating the
%                   group number for each dictionary element. The
%                   first group index is 1 (0 indicates no group).
%                   Numbers must be contiguous. Overlapping groups
%                   are not supported.
%       opt         Options/algorithm parameters structure (see below)
%
% Output:
%       Y           Dictionary coefficient vector (or matrix)
%       optinf      Details of optimisation
%
%
% Options structure fields:
%   Verbose           Flag determining whether iteration status is displayed.
%                     Fields are iteration number, functional value,
%                     data fidelity term, l1 regularisation term, l2,1
%                     regularisation term, and primal and dual residuals
%                     (see Sec. 3.3 of boyd-2010-distributed). The value of
%                     rho is also displayed if options request that it is
%                     automatically adjusted.
%   MaxMainIter       Maximum main iterations
%   AbsStopTol        Absolute convergence tolerance (see Sec. 3.3.1 of
%                     boyd-2010-distributed)
%   RelStopTol        Relative convergence tolerance (see Sec. 3.3.1 of
%                     boyd-2010-distributed)
%   Y0                Initial value for Y
%   U0                Initial value for U
%   rho               ADMM penalty parameter
%   AutoRho           Flag determining whether rho is automatically updated
%                     (see Sec. 3.4.1 of boyd-2010-distributed)
%   AutoRhoPeriod     Iteration period on which rho is updated
%   RhoRsdlRatio      Primal/dual residual ratio in rho update test
%   RhoScaling        Multiplier applied to rho when updated
%   AutoRhoScaling    Flag determining whether RhoScaling value is
%                     adaptively determined (see wohlberg-2015-adaptive). If
%                     enabled, RhoScaling specifies a maximum allowed
%                     multiplier instead of a fixed multiplier.
%   RhoRsdlTarget     Residual ratio targeted by auto rho update policy.
%   StdResiduals      Flag determining whether standard residual definitions
%                     (see Sec 3.3 of boyd-2010-distributed) are used instead
%                     of normalised residuals (see wohlberg-2015-adaptive)
%   RelaxParam        Relaxation parameter (see Sec. 3.4.3 of
%                     boyd-2010-distributed)
%   AuxVarObj         Flag determining whether objective function is computed
%                     using the auxiliary (split) variable
%
%
% Author: Brendt Wohlberg <brendt@lanl.gov>  Modified: 2015-07-10
%
% This file is part of the SPORCO library. Details of the copyright
% and user license can be found in the 'License' file distributed with
% the library.

if nargin < 6,
  opt = [];
end
checkopt(opt, defaultopts([]));
opt = defaultopts(opt);

% Set up status display for verbose operation
hstr = 'Itn   Fnc       DFid      l1        l2,1      r         s      ';
sfms = '%4d %9.2e %9.2e %9.2e %9.2e %9.2e %9.2e';
nsep = 64;
if opt.AutoRho,
  hstr = [hstr '   rho   '];
  sfms = [sfms ' %9.2e'];
  nsep = nsep + 10;
end
if opt.Verbose && opt.MaxMainIter > 0,
  disp(hstr);
  disp(char('-' * ones(1,nsep)));
end

% Start timer
tstart = tic;

% Set up algorithm parameters and initialise variables
rho = opt.rho;
if isempty(rho), rho = 50*lambda+1; end;
[Nr, Nc] = size(D);
Nm = size(S,2);
Nx = Nc*Nm;
Ng = max(g);
DTS = D'*S;
[luL, luU] = factorise(D, rho);
optinf = struct('itstat', [], 'opt', opt);
r = Inf;
s = Inf;
epri = 0;
edua = 0;

% Initialise main working variables
X = [];
if isempty(opt.Y0),
  Y = zeros(Nc,Nm);
else
  Y = opt.Y0;
end
Yprv = Y;
if isempty(opt.U0),
  if isempty(opt.Y0),
    U = zeros(Nc,Nm);
  else
    U = (lambda/rho)*sign(Y);
  end
else
  U = opt.U0;
end

% Main loop
k = 1;
while k <= opt.MaxMainIter && (r > epri | s > edua),

  % Solve X subproblem
  X = linsolve(D, rho, luL, luU, DTS + rho*(Y - U));

  % See pg. 21 of boyd-2010-distributed
  if opt.RelaxParam == 1,
    Xr = X;
  else
    Xr = opt.RelaxParam*X + (1-opt.RelaxParam)*Y;
  end

  % Solve Y subproblem
  Y = shrink_groups(Xr + U, g, Ng, lambda/rho, mu/rho);

  % Update dual variable
  U = U + Xr - Y;

  % Objective function and convergence measures
  if opt.AuxVarObj,
    Jdf = sum(vec(abs(D*Y - S).^2))/2;
    Jl1 = sum(abs(vec(Y)));
    Jl21 = norm21(Y, g, Ng);
  else
    Jdf = sum(vec(abs(D*X - S).^2))/2;
    Jl1 = sum(abs(vec(X)));
    Jl21 = norm21(X, g, Ng);
  end
  Jfn = Jdf + lambda*Jl1 + mu*Jl21;

  nX = norm(X(:)); nY = norm(Y(:)); nU = norm(U(:));
  if opt.StdResiduals,
    % See pp. 19-20 of boyd-2010-distributed
    r = norm(vec(X - Y));
    s = norm(vec(rho*(Yprv - Y)));
    epri = sqrt(Nx)*opt.AbsStopTol+max(nX,nY)*opt.RelStopTol;
    edua = sqrt(Nx)*opt.AbsStopTol+rho*nU*opt.RelStopTol;
  else
    % See wohlberg-2015-adaptive
    r = norm(vec(X - Y))/max(nX,nY);
    s = norm(vec(Yprv - Y))/nU;
    epri = sqrt(Nx)*opt.AbsStopTol/max(nX,nY)+opt.RelStopTol;
    edua = sqrt(Nx)*opt.AbsStopTol/(rho*nU)+opt.RelStopTol;
  end

  % Record and display iteration details
  tk = toc(tstart);
  optinf.itstat = [optinf.itstat;[k Jfn Jdf Jl1 Jl21 r s epri edua rho tk]];
  if opt.Verbose,
    if opt.AutoRho,
      disp(sprintf(sfms, k, Jfn, Jdf, Jl1, Jl21, r, s, rho));
    else
      disp(sprintf(sfms, k, Jfn, Jdf, Jl1, Jl21, r, s));
    end
  end

  % See wohlberg-2015-adaptive and pp. 20-21 of boyd-2010-distributed
  if opt.AutoRho,
    if k ~= 1 && mod(k, opt.AutoRhoPeriod) == 0,
      if opt.AutoRhoScaling,
        rhomlt = sqrt(r/(s*opt.RhoRsdlTarget));
        if rhomlt < 1, rhomlt = 1/rhomlt; end
        if rhomlt > opt.RhoScaling, rhomlt = opt.RhoScaling; end
      else
        rhomlt = opt.RhoScaling;
      end
      rsf = 1;
      if r > opt.RhoRsdlTarget*opt.RhoRsdlRatio*s, rsf = rhomlt; end
      if s > (opt.RhoRsdlRatio/opt.RhoRsdlTarget)*r, rsf = 1/rhomlt; end
      rho = rsf*rho;
      U = U/rsf;
      if rsf ~= 1,
        [luL, luU] = factorise(D, rho);
      end
    end
  end

  Yprv = Y;
  k = k + 1;

end

% Record run time and working variables
optinf.runtime = toc(tstart);
optinf.X = X;
optinf.Y = Y;
optinf.U = U;
optinf.lambda = lambda;
optinf.rho = rho;

% End status display for verbose operation
if opt.Verbose && opt.MaxMainIter > 0,
  disp(char('-' * ones(1,nsep)));
end

return

function u = vec(v)

  u = v(:);

return

function u = shrink1(v, a)

  u = sign(v).*max(0, abs(v) - a);

return

function U = shrink2_col_vec(V, a)

  % Additional complexity here allows simultaenous shrinkage of a
  % set of column vectors
  n2v = sqrt(sum(V.^2,1));
  n2v(n2v == 0) = 1;
  U = bsxfun(@times, V, max(0, n2v - a)./n2v);

return

function U = shrink21(V, a, b)

  % See wohlberg-2012-local and chartrand-2013-nonconvex
  U = shrink2_col_vec(shrink1(V,  a), b);

return

function U = shrink_groups(V, g, Ng, a, b)

  U = zeros(size(V));
  U(g==0,:) = shrink1(V(g==0,:), a);
  for l = 1:Ng,
    U(g==l,:) = shrink21(V(g==l,:), a, b);
  end

return

function x = norm21(u, g, Ng)

  x = 0;
  for l = 1:Ng,
    x = x + sqrt(sum(u(g==l,:).^2, 1));
  end
  x = sum(x); % In case u is a matrix (i.e. not a column vector)

return

function [L,U] = factorise(A, c)

  [N,M] = size(A);
  % If N < M it is cheaper to factorise A*A' + cI and then use the
  % matrix inversion lemma to compute the inverse of A'*A + cI
  if N >= M,
    [L,U] = lu(A'*A + c*eye(M,M));
  else
    [L,U] = lu(A*A' + c*eye(N,N));
  end

return

function x = linsolve(A, c, L, U, b)

  [N,M] = size(A);
  if N >= M,
    x = U \ (L \ b);
  else
    x = (b - A'*(U \ (L \ (A*b))))/c;
  end

return

function opt = defaultopts(opt)

  if ~isfield(opt,'Verbose'),
    opt.Verbose = 0;
  end
  if ~isfield(opt,'MaxMainIter'),
    opt.MaxMainIter = 1000;
  end
  if ~isfield(opt,'AbsStopTol'),
    opt.AbsStopTol = 0;
  end
  if ~isfield(opt,'RelStopTol'),
    opt.RelStopTol = 1e-4;
  end
  if ~isfield(opt,'Y0'),
    opt.Y0 = [];
  end
  if ~isfield(opt,'U0'),
    opt.U0 = [];
  end
  if ~isfield(opt,'rho'),
    opt.rho = [];
  end
  if ~isfield(opt,'AutoRho'),
    opt.AutoRho = 1;
  end
  if ~isfield(opt,'AutoRhoPeriod'),
    opt.AutoRhoPeriod = 10;
  end
  if ~isfield(opt,'RhoRsdlRatio'),
    opt.RhoRsdlRatio = 1.2;
  end
  if ~isfield(opt,'RhoScaling'),
    opt.RhoScaling = 100;
  end
  if ~isfield(opt,'AutoRhoScaling'),
    opt.AutoRhoScaling = 1;
  end
  if ~isfield(opt,'RhoRsdlTarget'),
    opt.RhoRsdlTarget = 1;
  end
  if ~isfield(opt,'StdResiduals'),
    opt.StdResiduals = 0;
  end
  if ~isfield(opt,'RelaxParam'),
    opt.RelaxParam = 1.8;
  end
  if ~isfield(opt,'AuxVarObj'),
    opt.AuxVarObj = 1;
  end

return


function f=loglikGaPExp(x, varargin)
% f=loglikGaP(x, varargin)
% complete log likellihood of the GaP model
% Vxt = varargin{1};      %data
% sigpsf = varargin{2};  %std deviation of the PSF gaussian approx
% alpha = varargin{3}; %parameters of the Gamma prior on the blinking
% beta = varargin{4}; %parameters of the Gamma prior on the blinking
% peval = varargin{5}; %parameters
% x(1:end-2*peval.ncomp) is Hkt

Vxt = varargin{1};      %data
sigpsf = varargin{2};  %std deviation of the PSF gaussian approx
alpha = varargin{3}; %parameters of the Gamma prior on the blinking
beta = varargin{4}; %parameters of the Gamma prior on the blinking
peval = varargin{5}; %parameters

[Hkt_r, cx, cy, Wxk] = loglikGaPreadparam(x,varargin);
Hkt=exp(Hkt_r); %nonnegativity constrains

[Wxkbg,Hktbg]=addbg(Wxk, Hkt, peval.bg);
P=Wxkbg*Hktbg; %current approximation

%Poisson contribution
t1=Vxt.*log(P) - P;
%Gamma contribution
t2=(alpha-1)*log(Hkt)-1/beta*Hkt-alpha*log(beta)-log(gamma(alpha));

f=sum(t1(:))+sum(t2(:));
f=-f; %conjugate gradient is mimimizing!
end


classdef testEnergyEquivalenceVoigtAndTensorNotationForIsoTensor  ...
         < testEnergyEquivalenceVoigtAndTensorNotation

    methods (Access = public)

        function obj = testEnergyEquivalenceVoigtAndTensorNotationForIsoTensor()
            obj@testEnergyEquivalenceVoigtAndTensorNotation();
        end

    end

    methods
        function generateFourthOrderTensor(obj)
             obj.Ch = IsotropicConstitutiveTensor(1,1/3);
        end
    end

end


%SerialLink.INERTIA Manipulator inertia matrix
%
% I = R.inertia(Q) is the symmetric joint inertia matrix (NxN) which relates
% joint torque to joint acceleration for the robot at joint configuration Q.
%
% If Q is a matrix (KxN), each row is interpretted as a joint state
% vector, and the result is a 3d-matrix (NxNxK) where each plane corresponds
% to the inertia for the corresponding row of Q.
%
% Notes::
% - The diagonal elements I(J,J) are the inertia seen by joint actuator J.
% - The off-diagonal elements I(J,K) are coupling inertias that relate
%   acceleration on joint J to force/torque on joint K.
% - The diagonal terms include the motor inertia reflected through the gear
%   ratio.
%
% See also SerialLink.RNE, SerialLink.CINERTIA, SerialLink.ITORQUE.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function M = inertia(robot, q)
    if numcols(q) ~= robot.n
        error('q must have %d columns', robot.n);
    end

    if numrows(q) > 1
        M = [];
        for i=1:numrows(q)
            M = cat(3, M, robot.inertia(q(i,:)));
        end
        return
    end

	n = robot.n;

	if numel(q) == robot.n
		q = q(:).';
	end

	M = zeros(n,n,0);
	for Q = q.'
		m = rne(robot, ones(n,1)*Q.', zeros(n,n), eye(n), 'gravity', [0 0 0]);
		M = cat(3, M, m);
	end


function hyperball_integrals_test ( )

%*****************************************************************************80
%
%% HYPERBALL_INTEGRALS_TEST tests the HYPERBALL_INTEGRALS library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 January 2014
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HYPERBALL_INTEGRALS_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the HYPERBALL_INTEGRALS library.\n' );

  hyperball_integrals_test01 ( );
  hyperball_integrals_test02 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HYPERBALL_INTEGRALS_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return;
end


function dres = build_graph(dres)
ov_thresh = 0.5;
dnum = length(dres.x);
time1 = tic;
len1 = max(dres.fr);
for fr = 2:max(dres.fr)
  if toc(time1) > 2
    fprintf('%0.1f%%\n', 100*fr/len1);
    time1 = tic;
  end
  f1 = find(dres.fr == fr);     %% indices for detections on this frame
  f2 = find(dres.fr == fr-1);   %% indices for detections on the previous frame
  for i = 1:length(f1)
    ovs1  = calc_overlap(dres, f1(i), dres, f2);
    inds1 = find(ovs1 > ov_thresh);                       %% find overlapping bounding boxes.

    ratio1 = dres.h(f1(i))./dres.h(f2(inds1));
    inds2  = (min(ratio1, 1./ratio1) > 0.8);          %% we ignore transitions with large change in the size of bounding boxes.

    dres.nei(f1(i),1).inds  = f2(inds1(inds2))';      %% each detction window will have a list of indices pointing to its neighbors in the previous frame.
%     dres.nei(f1(i),1).ovs   = ovs1(inds1(inds2));
  end
end


load('data');
[Rtilt,R] = rectify(cat(3,X,Y,Z));

figure,
XYZnew = Rtilt*[X(:),Y(:),Z(:)]';
scatter3(XYZnew(1,1:10:end),XYZnew(2,1:10:end),XYZnew(3,1:10:end),1,rgb(1:10:end,:));
axis equal;

figure,
XYZnew = R*[X(:),Y(:),Z(:)]';
scatter3(XYZnew(1,1:10:end),XYZnew(2,1:10:end),XYZnew(3,1:10:end),1,rgb(1:10:end,:));
axis equal;


function [refMesh, distListIters, refVerticesIters] = averageMesh(meshList, varargin)
%AVERAGEMESH Compute average mesh from a list of meshes.
%
%   AVG = averageMesh(MESHLIST)
%
%   Example
%   averageMesh
%
%   See also
%     meshes3d

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2020-01-31, using Matlab 9.7.0.1247435 (R2019b) Update 2
% Copyright 2020-2022 INRAE - BIA Research Unit - BIBS Platform (Nantes)

%% Parse input values

% default values
nIters = 10;
verbose = false;

% parse input arguments
while length(varargin) > 1
    name = varargin{1};
    if ~ischar(name)
        error('require parameter name-value pairs');
    end

    if strcmpi(name, 'verbose')
        verbose = varargin{2};
    elseif strcmpi(name, 'nIters')
        nIters = varargin{2};
    else
        error(['Unknown parameter name: ' name]);
    end
    varargin(1:2) = [];
end

%% Initialisations

nMeshes = length(meshList);

% initialize kd-trees to accelerate nearest-neighbor searches
treeList = cell(nMeshes, 1);
for iMesh = 1:nMeshes
    treeList{iMesh} = KDTreeSearcher(meshList{iMesh}.vertices);
end

% choose arbitrary initial mesh
refMesh = struct('vertices', meshList{1}.vertices, 'faces', meshList{1}.faces);

refVerticesIters = cell(1, nIters);
distListIters = cell(1, nIters);

%% Main iteration

for iIter = 1:nIters
    if verbose
        fprintf('iter %d/%d\n', iIter, nIters);
    end
    refMesh = smoothMesh(refMesh);

    % create new array for average vertices
    newVerts = zeros(size(refMesh.vertices));
    distList = zeros(size(refMesh.vertices, 1), 1);

    % iterate over all meshes
    for iMesh = 1:nMeshes
        if verbose
            fprintf('    mesh %d/%d\n', iMesh, nMeshes);
        end

%         mesh = meshList{iMesh};
        inds = knnsearch(treeList{iMesh}, refMesh.vertices);

%         closest = mesh.vertices(inds,:);
        closest = treeList{iMesh}.X(inds,:);
        newVerts = newVerts + closest;
        distList = distList + sum((closest - refMesh.vertices).^2, 2);
    end

    % update new vertices
    newVerts = newVerts / nMeshes;
    refVerticesIters{iIter} = newVerts;
    refMesh.vertices = newVerts;

    % keep list of distances
    distList = sqrt(distList / nMeshes);
    distListIters{iIter} = distList;
end

% figure; drawMesh(refMesh, 'lineStyle', 'none', 'faceColor', [.5 .5 .5])
% axis equal; view(3); hold on; axis([-2.5 2.5 -2 2 -3.5 3.5]); light;
% lighting gouraud
% title('Average mesh');
% print(gcf, 'averageMesh_initial.png', '-dpng');


function [ unaryTermsNew, pairwiseTermsNew, energyConstant ] = projectEnergyBinaryPairwise( unaryTerms, pairwiseTerms, partialLabels )
%projectEnergyBinaryPairwise assigns spesified values to the subset of variables and constructs the energy from the unlabelled ones

maskUnlabeled = partialLabels == 0;
numUnlabeled = sum(maskUnlabeled);
numNodes = size( unaryTerms, 1 );
numEdges = size( pairwiseTerms, 1);
numLabels = 2;

maskEdgesNew = maskUnlabeled( pairwiseTerms(:, 1) ) & maskUnlabeled( pairwiseTerms(:, 2) );

pairwiseTermsNew = pairwiseTerms(maskEdgesNew, :);
newIds = nan(numUnlabeled, 1);
newIds(maskUnlabeled) = 1 : numUnlabeled;
pairwiseTermsNew(:, 1) = newIds( pairwiseTermsNew(:, 1) );
pairwiseTermsNew(:, 2) = newIds( pairwiseTermsNew(:, 2) );

energyConstant = 0;
unaryTermsNew = unaryTerms(maskUnlabeled, :);
labeledUnary = unaryTerms(~maskUnlabeled, :);
goodLabels = partialLabels(~maskUnlabeled);
energyConstant = energyConstant + ...
    sum( labeledUnary( (1 : numNodes - numUnlabeled)' + (goodLabels - 1) * (numNodes - numUnlabeled) ) );

labelMap = [ 3, 4; 5, 6];

for iEdge = 1 : numEdges
    node1 = pairwiseTerms(iEdge, 1);
    node2 = pairwiseTerms(iEdge, 2);
    label1 = partialLabels(node1);
    label2 = partialLabels(node2);
    if ~maskUnlabeled(node1) && ~maskUnlabeled(node2)
        energyConstant = energyConstant + pairwiseTerms( iEdge, labelMap(label1, label2) );
    elseif maskUnlabeled(node1) && ~maskUnlabeled(node2)
        unaryTermsNew( newIds(node1), : ) = unaryTermsNew( newIds(node1), : ) + ...
            reshape( pairwiseTerms(iEdge, labelMap(:, label2)), 1, numLabels);
    elseif ~maskUnlabeled(node1) && maskUnlabeled(node2)
        unaryTermsNew( newIds(node2), : ) = unaryTermsNew( newIds(node2), : ) + ...
            reshape( pairwiseTerms(iEdge, labelMap(label1, :)), 1, numLabels);
    end
end

end


function fh = AWB_fh(A,B,w)
% This is almost an MV2DF, but it does not return derivatives on numeric
% input, w.
%
% Algorithm: Y = A*reshape(w,..)*B

if nargin==0
    test_this();
    return;
end

[m,n] = size(A);
[r,s] =  size(B);

    function y = map_this(w)
        w = reshape(w,n,r);
        y = A*w*B;
    end

    function w = transmap_this(y)
        y = reshape(y,m,s);
        w = A.'*y*B.';
    end

map = @(y) map_this(y);
transmap = @(y) transmap_this(y);

fh = linTrans([],map,transmap);

if exist('w','var') && ~isempty(w)
    fh = fh(w);
end

end

function test_this()
A = randn(2,3);
B = randn(4,5);
f = AWB_fh(A,B);
test_MV2DF(f,randn(3*4,1));
end


% I = PATCH_INPAINT(Iorg, Mask, verbose) inpaints an image
%
%   Iorg  -  original image
%   Mask  -  replacement mask; pixels where mask==1 will be inpainted
%
%   I     - inpainted image
function A = patch_inpaint(Aorg,Morg, verbose, sigma)

if nargin < 3
    verbose = true;
end
if nargin < 4
    sigma = 0.25;
end

if ~exist('CSH_nn.m','file')
    error('CSH_nn not found. Please download and add to path:  http://www.eng.tau.ac.il/~simonk/CSH/index.html');
end

width = 8;
csh_iterations = 5;
k = 1;
calcBnn = 0;

diffthresh = 10;

% Also accept RGB masks, but only use first channel
if size(Morg,3) > 1
    Morg = Morg(:,:,1);
end

% Determinte starting scale
[m n rgb] = size(Aorg);
startscale = -ceil(log2(min(m,n))) + 5;
scale = 2^(startscale);

% Resize image to starting scale
A = imresize(Aorg,scale);
M = imresize(Morg,scale);
M(M>0)=1;
M3 = repmat(M,[1 1 3])==1;

% Random starting guess for inpainted image
[m n rgb] = size(A);
Rnd = uint8(255*rand(m,n,rgb));
A(M3) = Rnd(M3);

% Go through all scales
for logscale = startscale:0

    scale = 2^(logscale);

    % Maximum number of iterations on this scale;
    % oscillations are possible
    iterations = 30;

    if verbose
        fprintf('Scale = 2^%d\n',logscale);
    end

    for iter = 1:iterations
        if verbose
            fprintf('  Iteration %2d/%2d',iter,iterations);
            imshow(A);
            pause(0.001)
        end

        B = A;
        B(M3)=0;

        % Compute NN field
        CSH_ann = CSH_nn(A,B,width,csh_iterations,k,calcBnn,M);

        % Now be work in double precision
        A = double(A)./255;

        % Create new image by letting each patch vote
        R = zeros(size(A));
        Rcount = zeros(m,n);
        for i = 1:m
            for j = 1:n
                if M(i,j) == 1
                    if 1<=i && i+width-1<=m && 1<=j && j+width-1<=n
                        patch = A(i:i+width-1,j:j+width-1,:);
                        i2 = CSH_ann(i,j,2);
                        j2 = CSH_ann(i,j,1);
                        patch2 = A(i2:i2+width-1,j2:j2+width-1,:);

                        d = sum( (patch(:)-patch2(:)).^2 );
                        sim = exp( -d / (2*sigma^2) );

                        pi = i:i+width-1;
                        pj = j:j+width-1;
                        R(pi,pj,:) = R(pi,pj,:) + sim*patch2;
                        Rcount(pi,pj) = Rcount(pi,pj) + sim;
                    end
                end
            end
        end

        % Normalize and
        Rcount = repmat(Rcount,[1 1 3]);
        R(Rcount>0) = R(Rcount>0) ./ Rcount(Rcount>0);
        % Keep pixels outside mask
        R(~M3)=A(~M3);
        % Convert back to uint8
        Aprev = 255*A;
        A = uint8(255*R);

        if iter>1
            % Measure how much image has changed
            diff = sum( (double(A(:))-double(Aprev(:))).^2 ) / sum(M(:)>0);
            if verbose
                fprintf(' diff = %f\n',diff);
            end
            % Stop iterating if change is low
            if diff < diffthresh
                break;
            end
        elseif verbose
            fprintf('\n');
        end
    end

    %Upsample A for the next scale
    if logscale < 0
        Adata = imresize(Aorg,2*scale);
        [m n rgb] = size(Adata);
        A = imresize(A,[m n]);

        M = imresize(Morg,[m n]);
        M(M>0)=1;
        M3 = repmat(M,[1 1 3])==1;

        % Outside mask, A is equal to original image
        A(~M3) = Adata(~M3);
    end
end


function rhs = globRHSIFE3DFace(fun, funB, fem, femIF, d1)

%% USAGE: generate global matrix on a 3D mesh Face
%
% INPUTS:
% fun --- coefficient function
% mesh --- a struct data contains very rich mesh information.
% fem1 --- global DoF for test function space
% fem2 --- global DoF for trial function space
% d1 --- derivative info for test function
% d2 --- derivative info for trial function
%            d = [0,0,0]: function value
%            d = [1,0,0]: Dx value
%            d = [0,1,0]: Dy value
%            d = [0,0,1]: Dz value
%
% OUTPUTS:
% matrix --- global (mass, stiffness, ...) matrix.

% Last Modified: 08/02/2020 by Xu Zhang

%% 1. RHS on Boundary Interface Faces
dof = fem.ldof;
if strcmp(fem.type,'P1')||strcmp(fem.type,'DGP1')||strcmp(fem.type,'CR')
    feEvalBas = @evalP1Bas3D;
elseif strcmp(fem.type,'P2')||strcmp(fem.type,'DGP2')
    feEvalBas = @evalP2Bas3D;
end
rhs = zeros(length(fem.p),1);

if size(femIF.tB,1) ~= 0

    nmB = femIF.normalB;
    AB = femIF.areaB; gxB = femIF.gxB; gyB = femIF.gyB; gzB = femIF.gzB; gw = femIF.gw;
    nt = size(femIF.tB,1); % not number of interface element, but quadrature element
    ntot = nt*dof;
    IB = zeros(ntot,1); XB = zeros(ntot,1);

    coef = feval(fun,gxB,gyB,gzB);
    bval = feval(funB,gxB,gyB,gzB);
    IbasB = cell(dof,1);
    if d1 == 0
        for i = 1:dof
            IbasB{i} = feEvalBas(femIF.basB(:,:,i), gxB, gyB, gzB, [0,0,0]);
        end
    elseif d1 == 1
        for i = 1:dof
            IbasBx = feEvalBas(femIF.basB(:,:,i), gxB, gyB, gzB, [1,0,0]);
            IbasBy = feEvalBas(femIF.basB(:,:,i), gxB, gyB, gzB, [0,1,0]);
            IbasBz = feEvalBas(femIF.basB(:,:,i), gxB, gyB, gzB, [0,0,1]);
            IbasB{i} = IbasBx.*nmB(:,1) + IbasBy.*nmB(:,2) + IbasBz.*nmB(:,3);
        end
    end

    ind = 0;
    for i = 1:dof
        IB(ind+1:ind+nt) = femIF.tB(:,i);
        XB(ind+1:ind+nt) = AB.*sum((((IbasB{i}.*coef).*bval).*gw'),2);
        ind = ind + nt;
    end
    ID = find(XB~=0);
    rhs = sparse(IB(ID),1,XB(ID),length(fem.p),1);
end


function [EI,Pth,Qthth] = lme_EI(X,Zcols,W,CBhat,SIGMA,L,phi,ni)
% [EI,Pth,Qthth] = lme_EI(X,Zcols,W,CBhat,SIGMA,L,phi,ni)
%
% Expected information matrix for the restricted log-likelihood.
%
% Input
% X: Ordered design Matrix (according to time for each subject).
% Zcols: Vector with the indices of the colums of X that will be considered
% as random effects.
% W: Inverses of the estimated marginal covariance matrices for each
% subject stacked in W.
% CBhat: Asymptotic covariance matrix of the fixed effects.
% SIGMA: Estimated marginal covariance matrices for each subject
% stacked in SIGMA.
% L: Cholesky factor of the covariance matrix of the random effects (D).
% phi: Within-subject standard deviation of the errors.
% ni: Vector whose entries are the number of repeated measures for each
% subject in the study (ordered according to X).
%
% Output
% EI: Expected information matrix.
% Pth,Qthth: Matrices that are useful for inferences on the fixed effects.
%
% $Revision: 1.2 $  $Date: 2015/01/06 17:14:57 $
% Original Author: Jorge Luis Bernal Rusiel
% CVS Revision Info:
%    $Author: mreuter $
%    $Date: 2015/01/06 17:14:57 $
%    $Revision: 1.2 $
% Reference: Kenward MG and Roger JH, 1997. Small sample inference for fixed
% effects from restricted maximum likelihood. Biometrics,Vol. 53, No.3.
%
m = length(ni);
n = sum(ni);
q = length(Zcols);
nth = q*(q+1)/2+1;
p = size(X,2);
Pth = zeros(nth,p,p);
Qthth = zeros(nth,nth,p,p);
Der = zeros(nth,n,size(W,2));
%Computation of the first order derivatives of W
jk = 0;
for k=1:q
    for j=1:k
        jk = jk + 1;
        posi = 1;
        for i=1:m
            posf = posi+ni(i)-1;
            Zi = X(posi:posf, Zcols);
            Zki = Zi(:,k);
            Mjki = Zki*L(j,:)*Zi';
            Mjki = Mjki + Mjki';
            Wi = W(posi:posf,1:ni(i));
            Der(jk,posi:posf,1:ni(i)) = -Wi*Mjki*Wi;
            posi = posf+1;
        end;
    end;
end;
posi = 1;
for i=1:m
    posf = posi+ni(i)-1;
    Wi = W(posi:posf,1:ni(i));
    Der(nth,posi:posf,1:ni(i)) = -2*phi*Wi*Wi;
    posi = posf+1;
end;
%Computation of Pis,Qijs and the expected information matrix EI.
for j=1:nth
    posi = 1; Pj = 0;
    Bj = squeeze(Der(j,:,:));
    for i=1:m
        posf = posi+ni(i)-1;
        Pj = Pj + X(posi:posf,:)'*Bj(posi:posf,1:ni(i))*X(posi:posf,:);
        posi = posf+1;
    end;
    Pth(j,:,:) = Pj;
end;
EI = zeros(nth,nth);
for k=1:nth
    Bk = squeeze(Der(k,:,:));
    Pk = squeeze(Pth(k,:,:));
    for j=1:k
        Bj = squeeze(Der(j,:,:));
        Pj = squeeze(Pth(j,:,:));
        posi = 1; Qkj = 0;
        traceBkj = 0;
        for i=1:m
            posf = posi+ni(i)-1;
            Vi = SIGMA(posi:posf,1:ni(i));
            Bkji = Bk(posi:posf,1:ni(i))*Vi*Bj(posi:posf,1:ni(i));
            traceBkj = traceBkj + trace(Bkji*Vi);
            Qkji = X(posi:posf,:)'*Bkji*X(posi:posf,:);
            Qkj = Qkj + Qkji;
            posi = posf+1;
        end;
        Qthth(k,j,:,:) = Qkj;
        Qthth(j,k,:,:) = Qkj;
        EI(k,j) = 0.5*(traceBkj - trace(CBhat*(2*Qkj-Pk*CBhat*Pj)));
        EI(j,k) = EI(k,j);
    end;
end;
end


function [X_scal] = test_pretreatment(X,param)

% pretreatment for test data
%
% INPUT
% X:        data matrix (samples x variables)
% param:    output data structure from data_pretreatment routine
%
% OUTPUT
% X_scal:       pretreated data matrix (samples x variables)
%
% version 1.0 - september 2009
% Davide Ballabio
% Milano Chemometrics and QSAR Research Group
% www.disat.unimib.it/chm

a = param.a;
s = param.s;
m = param.m;
M = param.M;
pret_type = param.pret_type;

if strcmp(pret_type,'cent')
    amat = repmat(a,size(X,1),1);
    X_scal = X - amat;
elseif strcmp(pret_type,'scal')
    f = find(s>0);
    smat = repmat(s,size(X,1),1);
    X_scal = zeros(size(X,1),size(X,2));
    X_scal = X(:,f)./smat(:,f);
elseif strcmp(pret_type,'auto')
    f = find(s>0);
    amat = repmat(a,size(X,1),1);
    smat = repmat(s,size(X,1),1);
    X_scal = zeros(size(X,1),size(X,2));
    X_scal(:,f) = (X(:,f) - amat(:,f))./smat(:,f);
elseif strcmp(pret_type,'rang')
    f = find(M - m > 0);
    mmat = repmat(m,size(X,1),1);
    Mmat = repmat(M,size(X,1),1);
    X_scal = zeros(size(X,1),size(X,2));
    X_scal(:,f) = (X(:,f) - mmat(:,f))./(Mmat(:,f) - mmat(:,f));
else
    X_scal = X;
end


function y=PhaseDemod(x, bits)
%function y=PhaseDemod(x, bits)
%
% This function demodulates M-PSK with M=2^bits symbols
% This function should be used with PhaseMod.m

M = 2^bits;

if size(x,1) ==1
   x = x.';
end

ss = 0:M-1;
symbols = exp(j*2*pi*ss/M);
b = dec2bin(0:M-1);

for i=1:M
   e(i,:) = symbols(i)-x.';
end

[min,index] = min(abs(e));

y = b(index,:);

y = reshape(y, bits*size(y,1),1);

y = str2num(y).';


% resize image to 256*256
% and calculate the image mean
mkdir imgs_256
p = dir('./imgs/');
m1 = [];
for i = 3:numel(p)  %remove . & ..
    str = [ './imgs/',p(i).name,'/*.jpg'];
    pp = dir(str);
    if(numel(pp)==0)
        str = [ './imgs/',p(i).name,'/*.bmp'];
        pp = dir(str);
    end
    if(numel(pp)==0)
        str = [ './imgs/',p(i).name,'/*.png'];
        pp = dir(str);
    end
    for j = 1:numel(pp)
        img_str = [ './imgs/',p(i).name,'/',pp(j).name];
        wimg_str = [ './imgs_256/',p(i).name,'/',pp(j).name(1:end-3),'jpg'];
        wdir_str = ['./imgs_256/',p(i).name];
        mkdir(wdir_str);
        im = imresize(imread(img_str),[256,256]);
        imwrite(im,wimg_str);
        m = mean(mean(im,1),2);
        m1 = cat(4,m1,m);
    end
end

mm = mean(m1,4);