% VL_VLAD   VLAD feature encoding
%   ENC = VL_VLAD(X, MEANS, ASSIGNMENTS) computes the VLAD
%   encoding of the vectors X relative to cluster centers MEANS and
%   vector-to-cluster soft assignments ASSIGNMENTS.
%
%   X has one column per data vector (e.g. a SIFT descriptor), and
%   MEANS has one column per component. Usually one has one component
%   per KMeans cluster and MEANS are the KMeans centers. X and MEANS
%   have the same number of rows and the data class, which can be
%   either SINGLE or DOUBLE.
%
%   ASSIGNMENTS has as many rows as clusters and as many columns as
%   X. Its columns are non-negative and should sum to one,
%   representing the soft assignment of the corresponding vector in X
%   to each of the clusters. It is of the same class as X.
%
%   ENC is a vector of the same class of X of size equal to the
%   product of the data dimension and the number of clusters.
%
%   By default, ENC is L2 normalized. VL_VLAD() accepts the following
%   options:
%
%   Unnormalized::
%     If specified, no overall normalization is applied to ENC.
%
%   NormalizeComponents::
%     If specified, the part of the encoding corresponding to each
%     cluster is individually normalized.
%
%   NormalizeMass::
%     If specified, each component is re-normalized by the mass
%     of data vectors assigned to it. If NormalizedComponents is
%     also selected, this has no effect.
%
%   SquareRoot::
%     If specified, the signed square root function is applied to
%     ENC before normalization.
%
%   Verbose::
%     Increase the verbosity level (may be specified multiple times).
%
%   See: VLAD, VL_HELP().

% Authors: David Novotny and Andrea Vedaldi

% Copyright (C) 2013 David Novotny and Andrea Vedaldi
% 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).


function M = hmxSpy(varargin)
%+========================================================================+
%|                                                                        |
%|         OPENHMX - LIBRARY FOR H-MATRIX COMPRESSION AND ALGEBRA         |
%|           openHmx is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : hmxSpy.m                                      |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Spy H-Matrix architecture                     |
%|  `---'  |                                                              |
%+========================================================================+

% Input data
Mh = varargin{1};

% Initialize visu block
if (Mh.typ > 0)
    M        = sparse(size(Mh,1),size(Mh,2));
    M(:,1)   = 1;
    M(:,end) = 1;
    M(1,:)   = 1;
    M(end,:) = 1;
end

%%%% H-Matrix (recursion)
if (Mh.typ == 0)
    A = hmxSpy(Mh.chd{1},[]);
    B = hmxSpy(Mh.chd{2},[]);
    C = hmxSpy(Mh.chd{3},[]);
    D = hmxSpy(Mh.chd{4},[]);
    M = [A,B;C,D];

%%% Compressed leaf
elseif (Mh.typ == 1)
    rk = size(Mh.dat{1},2);
    if (rk > 0)
        M(:,rk) = 1;
        M(rk,:) = 1;
    else
        M = 3 .* M;
    end

%%% Full leaf
elseif (Mh.typ == 2)
    if issparse(Mh.dat)
        M = 4 .* (M + (abs(Mh.dat)>0));
    else
        M = 2 .* M;
    end

%%% Unknown type
else
    error('hmxSpy.m : unavailable case')
end

% Graphical representation
if (length(varargin) == 1)
    spy(M==1,'b')
    hold on
    spy(M==2,'r')
    spy(M==3,'g')
    spy(M==4,'m')
    hold off
    title('openHmx : H-Matrix structure');
end
end


function a = log10(a)
%LOG10        Gradient logarithm  log10(a)
%

% written  12/06/98     S.M. Rump
% modified 10/14/00     S.M. Rump  use Tony's trick
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/08/08     S.M. Rump  improved sparse multiplication: not using intval data type
% modified 05/22/09     S.M. Rump  improved log10
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/13/12     S.M. Rump  INTLAB_INTVAL_STDFCTS
%

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

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  wng = warning;
  warning off

  % use full(a.x(:)): cures Matlab V6.0 bug
  % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
  % ax is full anyway
  if isa(a.x,'intval')
    INTLAB_STDFCTS_LOG10_ = getappdata(0,'INTLAB_STDFCTS_LOG10_');
    clog10 = infsup(INTLAB_STDFCTS_LOG10_.INF,INTLAB_STDFCTS_LOG10_.SUP);
    ax = clog10 ./ full(a.x(:));
  else
    ax = 1./( log(10) * full(a.x(:)) );
  end
  a.x = log10(a.x);
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    if isa(a.x,'intval')
      adx = times(ax(ia),sa,0);
      if adx.complex
        a.dx = intval( sparse(ia,ja,adx.mid,sizeax,N) , sparse(ia,ja,adx.rad,sizeax,N) , 'midrad' );
      else
        a.dx = intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
      end
    else
      a.dx = sparse(ia,ja,ax(ia).*sa,sizeax,N);
    end
  else
    a.dx = a.dx .* ax(:,ones(1,N));
  end

  warning(wng)

  if rndold
    setround(rndold)
  end


function [controlFlux, objFlux] = runRobustnessAnalysis(model, controlRxn, nPoints, objRxn, plotRedCost)
% runRobustnessAnalysis
%   Performs robustness analysis for a reaction of interest and an objective
%   of interest. Modified from the COBRA robustnessAnalysis function.
%
% Input:
%   model           a model structure
%   controlRxn      reaction of interest whose value is to be controlled
%   nPoints         number of points to show on plot (opt, default 20)
%   objRxn          reaction identifier of objective to be maximized (opt,
%                   default it uses the objective defined in the model)
%   plotRedCost     logical whether reduced cost should also be plotted
%                   (opt, default false)
%
% Output:
%   controlFlux     flux values of the reaction of interest, ranging from
%                   its minimum to its maximum value
%   objFlux         optimal values of objective reaction at each control
%                   reaction flux value
%
% Modified from COBRA Toolbox robustnessAnalysis.m
%
% Usage: runRobustnessAnalysis(model, controlRxn, nPoints, objRxn)

if nargin < 3
    nPoints = 20;
end
if nargin < 4
    baseModel = model;
else
    baseModel = setParam(model,'obj',objRxn,1);
end
if nargin < 5
    plotRedCost = false;
end

if any(ismember(model.rxns,controlRxn))
    controlRxnIdx = getIndexes(model,controlRxn,'rxns');
    tmpModel = setParam(model,'obj',controlRxnIdx,1);
    solMax = solveLP(tmpModel);
    solMax = solMax.x(logical(tmpModel.c));
    tmpModel.c = -tmpModel.c;
    solMin = solveLP(tmpModel);
    solMin = solMin.x(logical(tmpModel.c));
else
    error('Control reaction does not exist!');
end

objFlux = zeros(nPoints,1);
redCost = zeros(nPoints,1);
controlFlux = linspace(solMin,solMax,nPoints)';

fprintf('Running robustness analysis...   0%% complete');
for i=1:length(controlFlux)
    progress=pad(num2str(floor(i/numel(controlFlux)*100)),3,'left');
    fprintf('\b\b\b\b\b\b\b\b\b\b\b\b\b%s%% complete',progress);
    modelControlled = setParam(baseModel,'eq',controlRxnIdx,controlFlux(i));
    solControlled = solveLP(modelControlled);
    objFlux(i) = solControlled.x(logical(modelControlled.c));
    redCost(i) = solControlled.rCost(controlRxnIdx);
end
fprintf('\b\b\b\b\b\b\b\b\b\b\b\b\bCOMPLETE\n');

if plotRedCost
    yyaxis right
    plot(controlFlux,redCost)
    ylabel([strrep(controlRxn,'_','-') ' reduced cost']);
    yyaxis left
end
plot(controlFlux,objFlux)
xlabel(strrep(controlRxn,'_','-'));
ylabel('Objective');
end


function P = calcP(j)
global uLINK

if j == 0
   P = [0 0 0]';
else
   c1 = uLINK(j).R * uLINK(j).c;
   P = uLINK(j).m * (uLINK(j).v + cross(uLINK(j).w, c1) );
   P = P + calcP(uLINK(j).sister) + calcP(uLINK(j).child);
end


function [data, opt] = GrayRawPixelExtractor(im, tmpl, opt)
sz = opt.FeatureExtractor.tmplsize;
if (ndims(im) == 3)
    im = rgb2gray(im);
end
data.tmpl = tmpl;

features = zeros(prod(sz)*size(im, 3), size(tmpl, 1));

% pad
minW = min(round(tmpl(:, 1) - tmpl(:, 3) / 2)) - 1;
maxW = max(round(tmpl(:, 1) + tmpl(:, 3) / 2)) + 1;
minH = min(round(tmpl(:, 2) - tmpl(:, 4) / 2)) - 1;
maxH = max(round(tmpl(:, 2) + tmpl(:, 4) / 2)) + 1;
[h, w, c] = size(im);
if (minW < 1)
    im_new = zeros(h, w + abs(minW) + 1, c);
    im_new(:, abs(minW) + 2:end, :) = im;
    im = im_new;
    tmpl(:, 1) = tmpl(:, 1) + abs(minW) + 1;
end
if (maxW > w)
    im_new = zeros(h, size(im, 2) + maxW - w, c);
    im_new(:, 1:size(im, 2), :) = im;
    im = im_new;
end
if (minH < 1)
    im_new = zeros(h + abs(minH) + 1, size(im, 2), c);
    im_new(abs(minH) + 2:end, :, :) = im;
    im = im_new;
    tmpl(:, 2) = tmpl(:, 2) + abs(minH) + 1;
end
if (maxH > h)
    im_new = zeros(size(im, 1) + maxH - h, size(im, 2), c);
    im_new(1:size(im, 1), :, :) = im;
    im = im_new;
end

for i = 1:size(tmpl, 1)
    midW    = tmpl(i, 1);
    midH    = tmpl(i, 2);
    w       = tmpl(i, 3);
    h       = tmpl(i, 4);

    tempIm = im(round(midH-h/2) : round(midH+h/2),...
                round(midW-w/2) : round(midW+w/2), :);
%     tempIm = imresize(tempIm, sz);
    tempIm = mexResize(tempIm, sz, 'auto');
    features(:, i) = tempIm(:);
    if (norm(features(:, i)) > 1e-6)
        features(:, i) = features(:, i) / norm(features(:, i));
    end
end
% features = features - 0.5;

data.feat = features;


%Set parameters: k and threshold of cof
k = 7;
thresholds = 0.9:0.01:1.10;

disp('Demo COF')

data=[(1:10)' rand(10,1)*0.1];
data=[data; 5 1.5;];
data=[data; [(1:1.5:10)' rand(7,1)*0.2 - 10]];

axis([0 10 -5 2]);
scatter(data(:, 1), data(:, 2));

for threshold=thresholds

    clf;
    subplot(121)
    [~, lof] = LOF(data, k);
    target_lof = data(lof>=threshold, :);
    normal_lof = data(lof<threshold, :);
    hold on
    scatter(normal_lof(:, 1), normal_lof(:, 2), 'b');
    scatter(target_lof(:, 1), target_lof(:, 2), 'rx')
    title(sprintf('LOF, threshold=%f', threshold));
    axis([0 10 -15 2]);
    hold off
    subplot(122)
    hold on
    [~, cof] = COF(data, k);
    target_cof = data(cof>=threshold, :);
    normal_cof = data(cof<threshold, :);

    scatter(normal_cof(:, 1), normal_cof(:, 2), 'b');
    scatter(target_cof(:, 1), target_cof(:, 2), 'rx')
    title(sprintf('COF, threshold=%f', threshold));
    axis([0 10 -15 2]);
    hold off
    pause
end


classdef BMatrixComputer < handle

    properties (Access = private)
        fun
        dNdx
        nVoigt
    end

    methods (Access = public)

        function obj = BMatrixComputer(cParams)
            obj.init(cParams);
        end

        function B = compute(obj,igaus)
            ndimf = obj.fun.ndimf;
            switch ndimf
                case 1
                    obj.nVoigt = 2;
                    B = obj.computeBin1D(igaus);
                case 2
                    obj.nVoigt = 3;
                    B = obj.computeBin2D(igaus);
                case 3
                    obj.nVoigt = 6;
                    B = obj.computeBin3D(igaus);
            end
        end

    end

    methods (Access = private)

        function init(obj, cParams)
            obj.fun  = cParams.fun;
            obj.dNdx = cParams.dNdx;
        end

        function B = computeBin2D(obj,iGaus)
            deriv = obj.dNdx;
            nStre = obj.nVoigt;
            nDimf = obj.fun.ndimf;
            nNodE = size(deriv,2);
            nDofE = nNodE*nDimf;
            nElem = size(deriv,3);
            B = zeros(nStre,nDofE,nElem);
            for iNode = 1:nNodE
                j = nDimf*(iNode-1)+1;
                B(1,j,:)   = deriv(1,iNode,:,iGaus);
                B(2,j+1,:) = deriv(2,iNode,:,iGaus);
                B(3,j,:)   = deriv(2,iNode,:,iGaus);
                B(3,j+1,:) = deriv(1,iNode,:,iGaus);
            end
        end

        function B = computeBin3D(obj,iGaus)
            deriv = obj.dNdx;
            nNode = size(deriv,2);
            nElem = size(deriv,3);
            B = zeros(obj.nVoigt,nNode,nElem);
            for inode = 1:nNode
                j = obj.fun.ndimf*(inode-1)+1;
                % associated to normal strains
                B(1,j,:)   = deriv(1,inode,:,iGaus);
                B(2,j+1,:) = deriv(2,inode,:,iGaus);
                B(3,j+2,:) = deriv(3,inode,:,iGaus);
                % associated to shear strain, gamma12
                B(4,j,:)   = deriv(2,inode,:,iGaus);
                B(4,j+1,:) = deriv(1,inode,:,iGaus);
                % associated to shear strain, gamma13
                B(5,j,:)   = deriv(3,inode,:,iGaus);
                B(5,j+2,:) = deriv(1,inode,:,iGaus);
                % associated to shear strain, gamma23
                B(6,j+1,:) = deriv(3,inode,:,iGaus);
                B(6,j+2,:) = deriv(2,inode,:,iGaus);
            end
        end

        function [B] = computeBin1D(obj, igaus)
            deriv  = obj.dNdx(:,:,:,igaus);
            nDimf = obj.fun.ndimf;
            nNode = size(deriv,2);
            nElem = size(obj.dNdx,3);
            nDofs = nDimf*nNode;
            B = zeros(obj.nVoigt,nDofs,nElem);
            for inode = 1:nNode
                j = nDimf*(inode-1) + 1;
                B(1,j,:) = deriv(1,inode,:);
                B(2,j,:) = deriv(2,inode,:);
            end
        end

    end
end


classdef Filter_PDE_Density < Filter

    properties (Access = private)
        epsilon
        Acomp
        Anodal2Gauss
        M
        x_reg
        LHS
        bc
        quadrature
    end

    methods (Access = public)

        function obj = Filter_PDE_Density(cParams)
            obj.init(cParams);
            obj.createQuadrature();
            obj.computeBoundaryConditions();
            obj.createMassMatrix();
            obj.epsilon = cParams.mesh.computeMeanCellSize();
            obj.Anodal2Gauss = obj.computeA();
            lhs = obj.createProblemLHS();
            obj.LHS = decomposition(lhs);
        end

        function x0 = getP0fromP1(obj,x)
            obj.x_reg =  obj.getP1fromP1(x);
            x0 = zeros(obj.mesh.nelem,obj.quadrature.ngaus);
            for igaus = 1:obj.quadrature.ngaus
                x0(:,igaus) = obj.Anodal2Gauss{igaus}*obj.x_reg;
            end
        end

        function RHS = integrate_L2_function_with_shape_function(obj,x)
            RHS = obj.M*x;
        end

        function obj = updateEpsilon(obj,epsilon)
            if obj.hasEpsilonChanged(epsilon)
                obj.epsilon = epsilon;
                lhs = obj.createProblemLHS();
                obj.LHS = decomposition(lhs);
            end
        end

        function x_reg = getP1fromP1(obj,x)
            RHS = obj.integrate_L2_function_with_shape_function(x);
            x_reg = obj.solveFilter(RHS);
        end

        function x_reg = getP1fromP0(obj,x0)
            quad = Quadrature.set(obj.mesh.type);
            quad.computeQuadrature('LINEAR');
            s.dV = obj.mesh.computeDvolume(quad)';
            s.x        = x0;
            RHS        = obj.Acomp.integrateP1FunctionWithShapeFunction(s);
            x_reg      = obj.solveFilter(RHS);
        end

    end

    methods (Access = private)

        function createQuadrature(obj)
            q = Quadrature.set(obj.mesh.type);
            q.computeQuadrature('LINEAR');
            obj.quadrature = q;
        end

        function createMassMatrix(obj)
            s.type   = 'MassMatrix';
            s.mesh   = obj.mesh;
            s.fun    = P1Function.create(obj.mesh, 1);
            s.quadratureOrder = 'QUADRATICMASS';
            lhs = LHSintegrator.create(s);
            obj.M = lhs.compute();
        end

        function A_nodal_2_gauss = computeA(obj)
            p1f = P1Function.create(obj.mesh,1);
            s.nnode   = obj.mesh.nnodeElem;
            s.nelem   = obj.mesh.nelem;
            s.npnod   = obj.mesh.nnodes;
            s.ngaus   = obj.quadrature.ngaus;
            s.connec  = obj.mesh.connec;
            s.shape   = p1f.computeShapeFunctions(obj.quadrature);
            obj.Acomp = Anodal2gausComputer(s);
            obj.Acomp.compute();
            A_nodal_2_gauss = obj.Acomp.A_nodal_2_gauss;
        end

        function itHas = hasEpsilonChanged(obj,eps)
            if isempty(obj.epsilon)
                obj.epsilon = 0;
            end
            var = abs(eps - obj.epsilon)/eps;
            itHas = var > 1e-15;
        end

        function x_reg = solveFilter(obj,RHS)
            RHS = obj.bc.fullToReducedVector(RHS);
            s.type = 'DIRECT';
            Solv = Solver.create(s);
            x = Solv.solve(obj.LHS,RHS);
            x_reg = obj.bc.reducedToFullVector(x);
        end

        function computeBoundaryConditions(obj)
            s.scale        = obj.femSettings.scale;
            s.mesh         = obj.mesh;
            s.bc{1}.dirichlet = [];
            s.bc{1}.pointload = [];
            s.bc{1}.ndimf     = [];
            s.bc{1}.ndofs     = [];
            s.ndofs        = obj.mesh.nnodes;
            obj.bc         = BoundaryConditions(s);
            obj.bc.compute();
        end

        function lhs = createProblemLHS(obj)
            s          = obj.femSettings;
            s.mesh     = obj.mesh;
            s.type     = obj.LHStype;
            problemLHS = LHSintegrator.create(s);
            lhs        = problemLHS.compute(obj.epsilon);
            lhs        = obj.bc.fullToReducedMatrix(lhs);
        end

    end

end


function B = thresholdLocally(A, blksz, varargin)
% Performs LOCAL Otsu thresholding on an image; user can specify blocksize
%
% SYNTAX: B = thresholdLocally(A,PV_Pairs)
%
% THRESHOLDLOCALLY processes an image, calling graythresh on LOCAL
% blocks in an image. This facilitates easy thresholding of images with
% uneven background illumination, for which global thresholding is
% inadequate. Uses the Image Processing Toolbox function BLOCKPROC
% (R2009b).
%
% INPUTS:
%    A:   Any image (or path/name of an image) suitable for processing
%         with im2bw()
%
%    BLKSZ: Blocksize ([M,N]) with which to process the image.
%           DEFAULT: [32 32]
%
%    (OPTIONAL):
%    PV_Pairs: Any valid parameter-value pairs accepted by blockproc.
%           DEFAULTS:
%             'BorderSize':       [6 6]
%             'PadPartialBlocks': true
%             'PadMethod':        'replicate'
%             'TrimBorder':       true
%             'Destination':      [NOT SPECIFIED] (See BLOCKPROC for usage)
%    FudgeFactor: As an additional PV_Pair, one may enter:
%             'FudgeFactor',       value (DEFAULT = 1),
%              You may provide a scalar multiplier for the local value
%              returned by graythresh.
%
% OUTPUT:
%    B:     Output image (Unless 'DESTINATION' output is specified.)
%
% USAGE NOTE: To specify any PV_Pairs, BLKSZ must be provided as the second
% input. If the default value of blksz is desired, an empty bracket ([]) may be
% provided. (See Example 3.)
%
% EXAMPLES:
%
% % NOTE: All examples use image 'rice.png', which ships with the Image
% %       Processing Toolbox.
%
% img = imread('rice.png');
%
% % EXAMPLE 1) Default usage:
% thresholded = thresholdLocally(img);
% imshow(thresholded)
%
% % EXAMPLE 2) Specifying non-default blocksize:
% thresholded = thresholdLocally(img,[16 16]);
%
% % EXAMPLE 3) Specifying non-default padmethod (using default blocksize):
% thresholded = thresholdLocally(img,[],'PadMethod','symmetric');
%
% % EXAMPLE 4) Comparing and local thresholding, and thresholding after
% %            background normalization using tophat filtering:
% figure
% subplot(2,2,1);imshow(img);title('Original Image');
% tmp = im2bw(img,graythresh(img));
% subplot(2,2,2);imshow(tmp);title('Globally Thresholded');
% tmp = imtophat(img,strel('disk',15));
% tmp = im2bw(tmp,graythresh(tmp));
% subplot(2,2,3);imshow(tmp);title('Globally Thresholded after Tophat')
% tmp = thresholdLocally(img);
% subplot(2,2,4);imshow(tmp);title('Locally Thresholded');
%
% Written by Brett Shoelson, PhD.
% 12/17/2010
% Modifications:
% * 12/20/2010   Modified significantly to accept as optional inputs all
%   parameter-value pairs accepted by BLOCKPROC, as well as an additional
%   "fudge factor" parameter that allows one to scale the local graythresh
%   value by a scalar multiple.
% * 02/08/2011   Modified default blocksize to that calculated by bestblk
%
% Copyright 2010 The MathWorks
%
% See also: blockproc, graythresh, im2bw

if ~nargin
    error('THRESHOLDLOCALLY: Requires at least one input argument.')
end

if ischar(A)
    A = imread(A);
end

% DEFAULTS
% M                = 32;
% N                = 32;
[M, N] = bestblk(size(A));
opts.BorderSize  = [6 6];
opts.PadPartialBlocks = true;
opts.PadMethod   = 'replicate';
opts.TrimBorder  = true;
opts.Destination = [];
opts.FudgeFactor = 1;

if nargin > 1 && ~isempty(blksz)
    if numel(blksz) == 1
        M = blksz; N = M;
    elseif numel(blksz) == 2
        M = blksz(1);N = blksz(2);
    else
        error('THRESHOLDLOCALLY: Improper specification of blocksize parameter');
    end
end

if nargin > 2
    opts = parsePV_Pairs(opts,varargin);
end

fun = @(block_struct) im2bw(block_struct.data,...
    min(max(opts.FudgeFactor*graythresh(block_struct.data),0),1));

if isempty(opts.Destination)
    B = blockproc(A,[M N],fun,...
        'BorderSize',opts.BorderSize,...
        'PadPartialBlocks',opts.PadPartialBlocks,...
        'PadMethod',opts.PadMethod,...
        'TrimBorder',opts.TrimBorder);
else
    B = [];
    blockproc(A,[M N],fun,...
        'BorderSize',opts.BorderSize,...
        'PadPartialBlocks',opts.PadPartialBlocks,...
        'PadMethod',opts.PadMethod,...
        'TrimBorder',opts.TrimBorder,...
        'Destination',opts.Destination);
end

end

function opts = parsePV_Pairs(opts,UserInputs)
    ind = find(strcmpi(UserInputs,'BorderSize'));
    if ~isempty(ind)
        opts.BorderSize = UserInputs{ind + 1};
    end
    ind = find(strcmpi(UserInputs,'PadPartialBlocks'));
    if ~isempty(ind)
        opts.PadPartialBlocks = UserInputs{ind + 1};
    end
    ind = find(strcmpi(UserInputs,'PadMethod'));
    if ~isempty(ind)
        opts.PadMethod = UserInputs{ind + 1};
    end
    ind = find(strcmpi(UserInputs,'TrimBorder'));
    if ~isempty(ind)
        opts.TrimBorder = UserInputs{ind + 1};
    end
    ind = find(strcmpi(UserInputs,'Destination'));
    if ~isempty(ind)
        opts.Destination = UserInputs{ind + 1};
    end
    ind = find(strcmpi(UserInputs,'FudgeFactor'));
    if ~isempty(ind)
        opts.FudgeFactor = UserInputs{ind + 1};
    end
end


function [imout,orderout,cimout,meta,thetaout]=scat_display(in,dirac,options,scatt)
%this function constructs the scattering logpolar display, presented
%in the paper 'Invariant Scattering Convolution Networks'.
%scatt=array of scattering coefficients to be displayed
%meta=additional information produced by the scattering function, which encodes the
%scattering path of each coefficient and other necessary information.
%
%options: options struct.
% options.type
%type=0 unified plot: all scattering coefficients are displayed together
%type=2 split plot: it generates a separate output for each scattering order (default option)
%type=1 concatenated plot: it concatenates the coefficients along annular rings.
% options.renorm_process: use dirac renormalisation for the coefficients (default 0)
% options.logpolar: use logpolar display versus cartesian display (default 1)
% options.display_size: size of the scattering display (default 512)
%imout contains the output image.

options.null=0;
use_whole_ener=getoptions(options,'use_whole_ener',0);
type=getoptions(options,'display_type',2);
logpolar=getoptions(options,'display_logpolar',1);
renorm_process=getoptions(options,'renorm_process',0);

maxsize=getoptions(options,'display_size',512);
J=in{1}.meta.j;
L=max(in{2}.meta.theta);

%create the legacy 'meta' structure
meta = recover_meta(in, dirac);
if nargin < 4
scatt = meta.ave;
end

%first_mask=find(meta.order==2);
%J=max(meta.scale(first_mask))+1;
%L=max(meta.orientation(first_mask))+1;

meta.covered=zeros(size(meta.order));
meta=effective_energy(meta,use_whole_ener);
meta=compute_rectangles(meta,use_whole_ener,type==0,logpolar);
maxorder=max(meta.order);

if renorm_process
  norm_ratio = scatt./meta.dirac_norm;
else
  l2_renorm = getoptions(options,'l2_renorm',0);
  denom=ones(size(meta.dirac_norm));
  if l2_renorm
    denom=sqrt(2.^(-mod(meta.scale,J)));
  end
  if (size(scatt)~=size(denom))
    scatt = scatt';
  end
  norm_ratio = scatt./denom;
end

heights=meta.rectangle(:,2)-meta.rectangle(:,1);
widths=meta.rectangle(:,4)-meta.rectangle(:,3);

fact_h = maxsize;
fact_w = maxsize;

if type < 2
  imout=zeros(fact_h+1,fact_w+1);
  orderout=zeros(fact_h+1,fact_w+1);
else
  for m=1:maxorder
    imout{m}=zeros(fact_h,fact_w);
  end
end

if type==1
  %concatenate the rectangles along orders
  %rescale the horizontal coordinate according to total energy of each order
  for ord=1:maxorder
    selected=find(meta.order==ord);
    ordener(ord)=sum(meta.dirac_norm(selected).^2);
  end
  ordener=ordener/sum(ordener);
  cordener=cumsum(ordener);
  cordener=[0 cordener];
  %concatenate and squeeze the rectangles
  lower=0;
  for ord=1:maxorder
    selected=find(meta.order==ord);
    if ordener(ord)>0
      if logpolar
        %obtain the upper and lower bounds of the annulus
        upper=sqrt(lower^2+ordener(ord));
        meta.rectangle(selected,3:4)=sqrt(meta.rectangle(selected,3:4).^2*upper^2+...
          (1-meta.rectangle(selected,3:4).^2)*lower^2);
        lower=upper;
      else
        meta.rectangle(selected,3:4)=ordener(ord)*meta.rectangle(selected,3:4)+cordener(ord);
      end
    end
  end
  type=0;
end

switch type
  case 0
    for l=1:length(norm_ratio)
      %extrema
      ext(1)=1+floor(fact_h*meta.rectangle(l,1));
      ext(2)=1+floor(fact_h*meta.rectangle(l,2));
      ext(3)=1+floor(fact_w*meta.rectangle(l,3));
      ext(4)=1+floor(fact_w*meta.rectangle(l,4));

      inthh=[ext(1):ext(2)];
      intww=[ext(3):ext(4)];
      imout(inthh,intww)=norm_ratio(l);
      orderout(inthh,intww)=meta.order(l);

    end
    if logpolar
      [imout,thetaout]=logpolar_conversion(imout,L);
      [orderout]=logpolar_conversion(orderout,L);
    end
    m1=(orderout==2);
    m2=(orderout==3);
    m3=(orderout>3);
    [gox,goy]=gradient(orderout);
    couronnes=(conv2(gox.^2+goy.^2,ones(5),'same') < .25);
    nimout=imout/max(imout(:));
    nimout=nimout.*couronnes+(1-couronnes);
    [NN,MM]=size(nimout);
    cimout=ones(NN,MM,3);
    cimout(:,:,1)=1-nimout.*(m2|m3);
    cimout(:,:,3)=1-nimout.*(m1|m3);
    cimout(:,:,2)=1-nimout.*(m1|m2);

  case 2
    for ord=2:maxorder
      selected=find(meta.order==ord);
      for l=selected
        inth=min(fact_h,[1+floor(fact_h*meta.rectangle(l,1)):floor(fact_h*meta.rectangle(l,2))]);
        intw=min(fact_w,[1+floor(fact_w*meta.rectangle(l,3)):floor(fact_w*meta.rectangle(l,4))]);
        imout{ord-1}(inth,intw)=norm_ratio(l);
      end
      if logpolar
        [imout{ord-1}]=logpolar_conversion(imout{ord-1},L);
      end
    end

end

end

function meta=compute_rectangles(meta,use_whole_ener,unified_plot, logpolar)

LP_correction=1;

R=length(meta.order);
maxorder=max(meta.order);
first_mask=find(meta.order==2);
J=max(meta.scale(first_mask))+1;
L=max(meta.orientation(first_mask))+1;

meta.lp_correction=1;%min(1,meta.lp_correction);
meta.rectangle(1,:)=[0 1 0 sqrt(meta.lp_correction)];
%meta.dirac_onorm(1)=meta.dirac_onorm(1)*LP_correction;
meta.dirac_onorm=meta.dirac_norm;

for o=1:maxorder-1
  slice=find(meta.order==o);
  for s=slice
    %find children
    if o==1
      children=find(meta.order==o+1);
    else
      children=find((floor(meta.scale/J)==meta.scale(s))&(floor(meta.orientation/L)==meta.orientation(s))&(meta.order==o+1));
    end
    if ~isempty(children)
      [newrectangles,outrect]=split_rectangle(meta.rectangle(s,:),meta.scale(children),...
        meta.orientation(children),meta.dirac_norm(s),meta.dirac_onorm(s),...
        meta.dirac_effnorms(children),J,L,use_whole_ener,unified_plot,logpolar,o);
      for c=1:length(children)
        meta.rectangle(children(c),:)=newrectangles(c,:);
      end
      meta.covered(s) = ((outrect(2)-outrect(1))*(outrect(4)-outrect(3))+...
        sum((newrectangles(:,2)-newrectangles(:,1)).*(newrectangles(:,4)-newrectangles(:,3))))/...
        ((meta.rectangle(s,2)-meta.rectangle(s,1))*(meta.rectangle(s,4)-meta.rectangle(s,3)));
      meta.rectangle(s,:)=outrect;
    end
  end
end

end

function [out,outlowp]=split_rectangle(inrectangle, scales, orientations, dirac_phi, dirac_orig,dirac_norms,J,L,use_whole_ener,unified_plot, logpolar,order)

%first step: we marginalize orientations in order to split scale axis:
C=length(dirac_norms);
if unified_plot | order==1
  ener=dirac_phi^2;
  totener=sum(dirac_norms)+ener;
else
  totener=sum(dirac_norms);
end
if use_whole_ener
  totener=dirac_orig^2;
end

totalheight=inrectangle(2)-inrectangle(1);
if logpolar
  totalwidth=inrectangle(end)^2-inrectangle(end-1)^2;
else
  totalwidth=inrectangle(end)-inrectangle(end-1);
end

outlowp=inrectangle;
if unified_plot | order==1
  if logpolar
    outlowp(end)=sqrt(outlowp(end-1)^2+totalwidth*(ener/totener));
  else
    outlowp(end)=outlowp(end-1)+totalwidth*(ener/totener);
  end
  rasterwidth=outlowp(end);
else
  rasterwidth=inrectangle(3);
end
scale_parent= mod(floor(scales/J),J);
orient_parent=mod(floor(orientations/L),L);
diffori=1;

for j=J-1:-1:0
  pack=find(mod(scales,J)==j);
  if ~isempty(pack)
    width=sum(dirac_norms(pack).^1);
    rasterheight=inrectangle(1);
    for l=0:L-1
      ind=find(mod(orientations(pack)-diffori*orient_parent(pack)+(order>1)*L/2,L)==l);
      out(pack(ind),1)=rasterheight;
      out(pack(ind),2)=rasterheight+totalheight*dirac_norms(pack(ind)).^1/width;
      out(pack(ind),3)=rasterwidth;
      if logpolar
        out(pack(ind),4)=sqrt(rasterwidth^2+totalwidth*width/totener);
      else
        out(pack(ind),4)=rasterwidth+totalwidth*width/totener;
      end
      rasterheight=out(pack(ind),2);
    end
    if logpolar
      rasterwidth=sqrt(rasterwidth^2+totalwidth*width/totener);
    else
      rasterwidth=rasterwidth+totalwidth*width/totener;
    end
  end
end

if order==3 & ~use_whole_ener
  %sanity check: conservation of energy
  in_area = totalheight*totalwidth;
  if unified_plot
    if logpolar
      out_area = (outlowp(2)-outlowp(1))*(outlowp(4)^2-outlowp(3)^2) + sum( (out(:,2)-out(:,1)).*(out(:,4).^2-out(:,3).^2));
    else
      out_area = (outlowp(2)-outlowp(1))*(outlowp(4)-outlowp(3)) + sum( (out(:,2)-out(:,1)).*(out(:,4)-out(:,3)));
    end
  else
    if logpolar
      out_area =  sum( (out(:,2)-out(:,1)).*(out(:,4).^2-out(:,3).^2));
    else
      out_area =  sum( (out(:,2)-out(:,1)).*(out(:,4)-out(:,3)));
    end
  end

  tol=1e-5;
  if abs(in_area-out_area) > tol*in_area
    in_area
    out_area
    out
    inrectangle
    error('sthg weird')
  end
end

end

function metaout=effective_energy(meta,use_whole_ener)

R=length(meta.order);
maxorder=max(meta.order);
first_mask=find(meta.order==2);
J=max(meta.scale(first_mask))+1;
L=max(meta.orientation(first_mask))+1;

last_mask=find(meta.order==maxorder);
metaout=meta;
metaout.dirac_effnorms(last_mask)=meta.dirac_norm(last_mask).^2;
if use_whole_ener
  metaout.dirac_effnorms(last_mask)=meta.dirac_onorm(last_mask).^2;
end

for o=maxorder-1:-1:1
  slice=find(meta.order==o);
  for s=slice
    %find children
    if o==1
      children=find(meta.order==o+1);
    else
      children=find((floor(meta.scale/J)==meta.scale(s))&(floor(meta.orientation/L)==meta.orientation(s))&(meta.order==o+1));
    end
    metaout.dirac_effnorms(s)=sum(metaout.dirac_effnorms(children))+meta.dirac_norm(s).^2;
    if use_whole_ener
      metaout.dirac_effnorms(s)=metaout.dirac_onorm(s).^2;
    end
  end
end

end

function [out,theta]=logpolar_conversion(in,L)

%out=in;
%theta=in;
%return;

[N,M]=size(in);

ix=-M:M;
iix=ones(length(ix),1)*ix;
iiy=iix';
r=sqrt(iix.^2+iiy.^2);
theta=angle(iiy+i*iix);

[N2,M2]=size(r);
%r=r(:,round(M2/2):end);
mask=(r>M);
%theta=theta(:,round(M2/2):end);
theta=mod(theta+(0*L+1)*pi/(2*L),pi);
theta=theta/pi;

code=min(N-2,max(1,round(theta*N)))+N*min(M-1,max(1,round(r)));
out=in(max(1,min(numel(in),1+code)));
out(mask)=0;
clear theta;
theta(:,:,1)=mod(code+1,N);
theta(:,:,2)=min(M-1,max(1,round(r)));

end


function [ori, Tori,i] = project(obj,ori)
% project an embedding back onto the manifold of orientations.
%
% Syntax
%   [ori, Tori] = project(e)
%
% Input
%  e - @embedding
%
% Output
%  ori - @orientation
%  Tori - projected @embedding
%

% get the embedding of the identity
Tid = embedding.id(obj.CS);

% normalize obj correctly
obj = obj .* norm(Tid)./norm(obj);

%get weights beta
%[~,~,weights]= embedding.coefficients(obj.CS);

% ensure obj is symmetric
%obj = obj.sym;

% special case for triclinic symmetry
if 0 && obj.CS.Laue.id ==2

  %weighted sum in Horn
  for i = 1:length(obj.u)
    r(i,:) = obj.u{i}(:);
  end

  ori = orientation(fit(obj.l,r,obj.CS),obj.CS);

  if nargout == 2, Tori = ori * Tid; end
  return

end

% initial guess - we need it to be sufficently close to avoid local extrema
if nargin == 1
  ori = equispacedSO3Grid(obj.CS,'points',10);

  d = norm(reshape(obj,[],1) - (ori * Tid).');

  [~,id] = min(d,[],2);

  ori = reshape(ori(id),[],1);
  %ori = orientation.id(obj.CS);
end

% basis of the tangential space
t = spinTensor([xvector,yvector,zvector]);

% perform steepest descent iteration to maximize dot(ori * Tid, obj)
maxIter = 200;
for i = 1:maxIter

  % compute the gradient in ori
  Tori = rotate_outer(Tid,ori);
  for k = 1:length(obj.u)
    Tori.u{k} = obj.rank(k) * EinsteinSum(t,[1,-1],Tori.u{k},[-1 2:obj.rank(k)]);
  end

  g = vector3d(dot(Tori,obj).').';

  % eradicate normalizing of embedding: adapt length ofs gradient
  %g = g * obj.rho^2;

  % stop if gradient is sufficently small
  if all(norm(g)<1e-10), break; end
  %disp([xnum2str(max(norm(g))) ' ' char(ori(1)) ' ' char(g(1))]);

  % update ori
  ori = exp(ori,g,'left');

end
end


function ierror = part_sf_check ( n, npart, a )

%*****************************************************************************80
%
%% PART_SF_CHECK checks a standard form partition of an integer.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the integer to be partitioned.
%    N must be positive.
%
%    Input, integer NPART, the number of parts of the partition.
%    1 <= NPART <= N.
%
%    Input, integer A(NPART), contains the partition.
%    A(1) through A(NPART) contain the nonzero integers which
%    sum to N.  The entries must be in DESCENDING order.
%
%    Output, integer IERROR, error flag.
%    0, no error.
%    -1, N is illegal.
%    -2, NPART is illegal.
%    -3, the entries do not add up to N.
%    I, the I-th entry of A is illegal.
%
  ierror = 0;

  if ( n < 1 )
    ierror = -1;
    return
  end

  if ( npart < 1 || n < npart )
    ierror = -2;
    return
  end
%
%  Every entry must lie between 1 and N.
%
  for i = 1 : npart
    if ( a(i) < 1 || n < a(i) )
      ierror = i;
      return
    end
  end
%
%  The entries must be in descending order.
%
  for i = 2 : npart
    if ( a(i-1) < a(i) )
      ierror = i;
      return
    end
  end
%
%  The entries must add up to N.
%
  if ( sum ( a(1:npart) ) ~= n )
    ierror = -3;
  end

  return
end


y1=user1sym();
y2=user3sym();
y3=user5sym();
y4=user7sym();
y5=user10sym();
y6=user12sym();
%//SYMBOL BASED CONFIGURATION\\
u1=[y1,y2,y3,y4,y5,y6];
v1=[1,3,5,7,10,12];

%// CHIP BASED CONFIGURATION\\
u2=[s1,s2,s3,s4,s5,s6];
v2=[1,3,5,7,10,12];
plot(u1,v1,'r-+',u2,v2,'g-*');
grid on;
legend('u1,v1','u2,v2');
title('simulation for No of users vs BER in  SYMBOL BASED & CHIP BASED ');
 xlabel('NUMBER USERS');
 ylabel('BER');


function [tfg, tfg_timestamp]=extract_tfg(peak,capbuf,fc,sampling_carrier_twist, nRB)

% add 100RB support. Jiao Xianjun(putaohsu@gmail.com)
% Convert from time domain to frequency domain and create the time/frequency
% grid.

% Copyright 2012 Evrytania LLC (http://www.evrytania.com)
%
% Written by James Peroulas <james@evrytania.com>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU Affero 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 Affero General Public License for more details.
%
% You should have received a copy of the GNU Affero General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

frame_start=peak.frame_start;
cp_type=peak.cp_type;
freq_fine=peak.freq_fine;

% nRB = 6;
if nRB == 6
    conv_idx_ratio = 1;
    decimation_ratio = 16;
elseif nRB == 100
    conv_idx_ratio = 16;
    decimation_ratio = 1;
else
    disp('nRB must be 6 or 100!');
    return;
end

fs = fs_lte/decimation_ratio;
fft_size = 2048/decimation_ratio;
len_cp_extended = 512/decimation_ratio;
len_cp_normal1 = 144/decimation_ratio;
len_cp_normal2 = 160/decimation_ratio;
nSC = nRB*12;

% fc*k_factor is the receiver's actual RX center frequency.
if sampling_carrier_twist==1
    k_factor=(fc-freq_fine)/fc;
else
    k_factor = peak.k_factor;
end

if (strcmpi(cp_type,'normal'))
  n_symb_dl=7;
  dft_location = conv_idx(frame_start+(len_cp_normal2/conv_idx_ratio), conv_idx_ratio);
elseif (strcmpi(cp_type,'extended'))
  n_symb_dl=6;
%   dft_location=frame_start+16; % wrong?
  dft_location = conv_idx(frame_start+(len_cp_extended/conv_idx_ratio), conv_idx_ratio);
else
 error('Check code...');
end

% See if we can advance the frame start
if (dft_location-k_factor*fs*.01>=0.5)
  dft_location=dft_location-k_factor*fs*.01;
end

% Perform FOC
capbuf=fshift(capbuf,-freq_fine,fs);

% Extract 6 frames + 2 slots worth of data
n_ofdm_sym=6*10*2*n_symb_dl+2*n_symb_dl;
tfg=NaN(n_ofdm_sym,fft_size);
tfg_timestamp=NaN(1,n_ofdm_sym);
sym_num=0;
for t=1:n_ofdm_sym
  indices=round(dft_location):round(dft_location)+fft_size-1;
  % Perform 2 sample coarse TOC
  %indices=[indices(3:end) indices(1:2)];
  %indices=[indices(end-2:end) indices(1:end-3)];
  tfg(t,:)=dft(capbuf(indices));
  tfg_timestamp(t)=dft_location;
  if (n_symb_dl==6)
%     dft_location=dft_location+k_factor*(128+16); % wrong?
    dft_location=dft_location+k_factor*(fft_size+len_cp_extended);
  else
    if (sym_num==6)
      dft_location=dft_location+k_factor*(fft_size+len_cp_normal2);
    else
      dft_location=dft_location+k_factor*(fft_size+len_cp_normal1);
    end
    sym_num=mod(sym_num+1,7);
  end
end

% Extract the columns of interest
tfg=[tfg(:,end-((nSC/2) - 1):end) tfg(:,2:((nSC/2) + 1))];

% Compensate for the residual time offset.
cn=[-(nSC/2):-1 1:(nSC/2)];
for t=1:n_ofdm_sym
  ideal_offset=tfg_timestamp(t);
  actual_offset=round(ideal_offset);
  % How late were we in locating the DFT
  late=actual_offset-ideal_offset;
  % Compensate for the improper location of the DFT
  tfg(t,:)=tfg(t,:).*exp(-1i*2*pi*cn*late/fft_size);
end
% At this point, tfg(t,:) contains the results of a DFT that was performed
% at time offset tfg_timestamp(t). Note that tfg_timestamp(t) is not an
% integer!

function out_idx = conv_idx(in_idx, decimation_ratio)
out_idx = (in_idx-1)*decimation_ratio + 1;


function f=fitmultiplegauss_test(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

Vxyt = 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{3}; %parameters

Vxt=reshape(Vxyt,peval.nx*peval.ny,1);
sigpsf_vec=repmat(sigpsf,peval.ncomp,1); %all psfs same sigma
a_vec=1./(sigpsf_vec.^2*2*pi); % all normalised to 1
cxy_vec=[x(1:peval.ncomp)'+1, x(peval.ncomp+1:end)'+1]; %different notation of the dip_image/new version

Wxkpix=gauss2dmultislice([peval.nx, peval.ny, peval.ncomp], cxy_vec, sigpsf_vec, a_vec);
Wxkpix=normalize(Wxkpix); %normalize PSFs to 1
Wxk=reshape(Wxkpix,peval.nx*peval.ny, peval.ncomp);

factor=(sum(Vxt(:))/sum(Wxk(:)))/peval.ncomp;
Hkt=factor*ones(peval.ncomp,1); %just sum the components

P=Wxk*Hkt+peval.bg; %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


function [t,q,R,Rt] = splitFrame(F)

% SPLITFRAME  Split frame information into useful matrices and vectors.
%   [T,Q,R,Rt] = SPLITFRAME(F), for a frame F, returns the translation
%   vector T, quaternion Q, rotation matrix R and its transpose Rt. The
%   frame F can be either a 7-vector F=[T;Q] or a structure containing, at
%   least, the fields F.t, F.q, F.R and F.Rt.
%
%   See also FRAME, UPDATEFRAME.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

if isa(F,'struct')
    t  = F.t;
    q  = F.q;
    R  = F.R;
    Rt = F.Rt;

else % F is a 7-vector
    t  = F(1:3);
    q  = F(4:7);
    R  = q2R(q);
    Rt = R';

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


classdef LMOCSO < ALGORITHM
% <multi/many> <real/integer> <large/none> <constrained/none>
% Large-scale multi-objective competitive swarm optimization algorithm

%------------------------------- Reference --------------------------------
% Y. Tian, X. Zheng, X. Zhang, and Y. Jin, Efficient large-scale multi-
% objective optimization based on a competitive swarm optimizer, IEEE
% Transactions on Cybernetics, 2020, 50(8): 3696-3708.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Generate random population
            [V,Problem.N] = UniformPoint(Problem.N,Problem.M);
            Population    = Problem.Initialization();
            Population    = EnvironmentalSelection(Population,V,(Problem.FE/Problem.maxFE)^2);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                Fitness = calFitness(Population.objs);
                if length(Population) >= 2
                    Rank = randperm(length(Population),floor(length(Population)/2)*2);
                else
                    Rank = [1,1];
                end
                Loser  = Rank(1:end/2);
                Winner = Rank(end/2+1:end);
                Change = Fitness(Loser) >= Fitness(Winner);
                Temp   = Winner(Change);
                Winner(Change) = Loser(Change);
                Loser(Change)  = Temp;
                Offspring      = Operator(Problem,Population(Loser),Population(Winner));
                Population     = EnvironmentalSelection([Population,Offspring],V,(Problem.FE/Problem.maxFE)^2);
            end
        end
    end
end

function Fitness = calFitness(PopObj)
% Calculate the fitness by shift-based density

    N      = size(PopObj,1);
    fmax   = max(PopObj,[],1);
    fmin   = min(PopObj,[],1);
    PopObj = (PopObj-repmat(fmin,N,1))./repmat(fmax-fmin,N,1);
    Dis    = inf(N);
    for i = 1 : N
        SPopObj = max(PopObj,repmat(PopObj(i,:),N,1));
        for j = [1:i-1,i+1:N]
            Dis(i,j) = norm(PopObj(i,:)-SPopObj(j,:));
        end
    end
    Fitness = min(Dis,[],2);
end


clear
clc
A=[2 1 5 3 4];
B=[1 5 3 4 2];
C=[4 1 2 3 5];
D=[1 4 2 3 5];
E=[5 4 3 1 2];
neighbork=[A;B;C;D;E];

center_route=Center(neighbork);


function [x,zo]=overlapadd(f,win,inc)
%OVERLAPADD join overlapping frames together X=(F,WIN,INC)
%
% Inputs:  F(NR,NW) contains the frames to be added together, one
%                   frame per row.
%          WIN(NW)  contains a window function to multiply each frame.
%                   WIN may be omitted to use a default rectangular window
%                   If processing the input in chunks, WIN should be replaced by
%                   ZI on the second and subsequent calls where ZI is the saved
%                   output state from the previous call.
%          INC      gives the time increment (in samples) between
%                   succesive frames [default = NW].
%
% Outputs: X(N,1) is the output signal. The number of output samples is N=NW+(NR-1)*INC.
%          ZO     Contains the saved state to allow a long signal
%                 to be processed in chunks. In this case X will contain only N=NR*INC
%                 output samples.
%
% Example of frame-based processing:
%          INC=20       													% set frame increment
%          NW=INC*2     													% oversample by a factor of 2 (4 is also often used)
%          S=cos((0:NW*7)*6*pi/NW);								% example input signal
%          W=sqrt(hamming(NW+1)); W(end)=[];      % sqrt hamming window of period NW
%          F=enframe(S,W,INC);               			% split into frames
%          ... process frames ...
%          X=overlapadd(F,W,INC);           			% reconstitute the time waveform (omit "X=" to plot waveform)

%	   Copyright (C) Mike Brookes 2009
%      Version: $Id: overlapadd.m,v 1.2 2009/06/08 16:21:49 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[nr,nf]=size(f);            % number of frames and frame length
if nargin<2
    win=nf;                 % default increment
end
if isstruct(win)
    w=win.w;
    if ~numel(w) && length(w)~=nf
        error('window length does not match frames size');
    end
    inc=win.inc;
    xx=win.xx;
else
    if nargin<3
        inc=nf;
    end
    if numel(win)==1 && win==fix(win) && nargin<3       % win has been omitted
        inc=win;
        w=[];
    else
        w=win(:).';
        if length(w)~=nf
            error('window length does not match frames size');
        end
        if all(w==1)
            w=[];
        end
    end
    xx=[];      % partial output from previous call is null
end
nb=ceil(nf/inc);        % number of overlap buffers
no=nf+(nr-1)*inc;       % buffer length
z=zeros(no,nb);                      % space for overlapped output speech
if numel(w)
    z(repmat(1:nf,nr,1)+repmat((0:nr-1)'*inc+rem((0:nr-1)',nb)*no,1,nf))=f.*repmat(w,nr,1);
else
    z(repmat(1:nf,nr,1)+repmat((0:nr-1)'*inc+rem((0:nr-1)',nb)*no,1,nf))=f;
end
x=sum(z,2);
if ~isempty(xx)
    x(1:length(xx))=x(1:length(xx))+xx;     % add on leftovers from previous call
end
if nargout>1            % check if we want to preserve the state
    mo=inc*nr;          % completed output samples
    if no<mo
        x(mo,1)=0;
        zo.xx=[];
    else
        zo.xx=x(mo+1:end);
        zo.w=w;
        zo.inc=inc;
        x=x(1:mo);
    end
elseif ~nargout
    if isempty(xx)
        k1=nf-inc;  % dubious samples at start
    else
        k1=0;
    end
    k2=nf-inc;      % dubious samples at end
    plot(1+(0:nr-1)*inc,x(1+(0:nr-1)*inc),'>r',nf+(0:nr-1)*inc,x(nf+(0:nr-1)*inc),'<r', ...
        1:k1+1,x(1:k1+1),':b',k1+1:no-k2,x(k1+1:end-k2),'-b',no-k2:no,x(no-k2:no),':b');
    xlabel('Sample Number');
    title(sprintf('%d frames of %d samples with %.0f%% overlap = %d samples',nr,nf,100*(1-inc/nf),no));
end


function zL = extrap(z, limtype);	% Slope-limited extrapolation (MUSCL)
					% Staggered scheme
% Function called: limiter

% Numbering for pressure nodes:
%   1 1 2 2                     J J
% |-o-|-o-|-o-|-o-|-o-|-o-|-o-|-o-|
% Numbering for momentum nodes:
% 1 1 2 2                        J-1  J
% |-o-|-o-|-o-|-o-|-o-|-o-|-o-|---o---|

J = length(z);
zL(1) = z(1);
for j = 2:J-1
  b     = (z(j+1) - z(j)); a = (z(j) - z(j-1));
  zL(j) = z(j) + 0.5*limiter(a, b, limtype)*(z(j+1)-z(j));
end
zL(J) = z(J);


function [slcstack] = SLC_filt(mlistack,slcstack,SHP,Coh_cal,BroNumthre,Cohthre_slc_filt)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   This file is part of TomoSAR.
%
%   TomoSAR is distributed in the hope that it will be useful,
%   but without warranty of any kind; without even the implied
%   warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
%   See the Apache License for more details.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Author : Dinh Ho Tong Minh (INRAE) and Yen Nhi Ngo, Jan. 2022
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[~,~,n_slc] = size(slcstack);

mask_coh = Coh_cal > Cohthre_slc_filt;
mask_PS = SHP.BroNum>BroNumthre;
mask = and(mask_PS,mask_coh);
mask = repmat(mask,[1,1,n_slc]);

slcstack(mask) = abs(mlistack(mask)).*exp(1i*angle(slcstack(mask)));


function cuts = knapsack_add_cover_cut(p,x,alg,upper)

if ~isinf(upper) && p.LinearBinaryPositiveCost
    % We can add a global knapsack from c'*x <= upper
    relevant_to_use = find(p.c & p.c <= upper);
    p.knapsack.a{end+1} = p.c(relevant_to_use);
    p.knapsack.b{end+1} = upper;
    p.knapsack.variables{end+1} = relevant_to_use;
    p.knapsack.type(end+1) = 0;
end

cuts = [];
for i = find(p.knapsack.type == 0)
    a = p.knapsack.a{i};
    b = p.knapsack.b{i};
    v = p.knapsack.variables{i};
    x_ = x(v);

    cut_ = knapsack_create_cover_cut(a,b,x_,alg,p.gubs);
    if ~isempty(cut_)
        cut = spalloc(1,length(p.c)+1,0);
        cut(1) = cut_(1);
        cut(1 + v) = cut_(2:end);
        cuts = [cuts;cut];
    end
end


function d = get_dtw_distance(d1, d2)
    SM = simmx(abs(d1), abs(d2));
%     [~, ~, D] = dpfast(1-SM);
    [~, ~, D] = dp(1-SM);
    d = D(size(D,1), size(D,2));
end


function x = fnlCg(x0,params)
%-----------------------------------------------------------------------
%
% res = fnlCg(x0,params)
%
% implementation of a L1 penalized non linear conjugate gradient reconstruction
%
% The function solves the following problem:
%
% given k-space measurments y, and a fourier operator F the function
% finds the image x that minimizes:
%
% Phi(x) = ||F* W' *x - y||^2 + lambda1*|x|_1 + lambda2*TV(W'*x)
%
%
% the optimization method used is non linear conjugate gradient with fast&cheap backtracking
% line-search.
%
% (c) Michael Lustig 2007
%-------------------------------------------------------------------------
x = x0;

% line search parameters
maxlsiter = params.lineSearchItnlim ;
gradToll = params.gradToll ;
alpha = params.lineSearchAlpha; ,    beta = params.lineSearchBeta;
t0 = params.lineSearchT0;
k = 0;
t = 1;

% copmute g0  = grad(Phi(x))

g0 = wGradient(x,params);

dx = -g0;

% iterations
while(1)

% backtracking line-search

	% pre-calculate values, such that it would be cheap to compute the objective
	% many times for efficient line-search
	[FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx] = preobjective(x, dx, params);
	f0 = objective(FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx,x,dx, 0, params);
	t = t0;
        [f1, ERRobj, RMSerr]  =  objective(FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx,x,dx, t, params);

	lsiter = 0;

	while (f1 > f0 - alpha*t*abs(g0(:)'*dx(:)))^2 & (lsiter<maxlsiter)
		lsiter = lsiter + 1;
		t = t * beta;
		[f1, ERRobj, RMSerr]  =  objective(FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx,x,dx, t, params);
	end

	if lsiter == maxlsiter
		disp('Reached max line search,.... not so good... might have a bug in operators. exiting... ');
		return;
	end

	% control the number of line searches by adapting the initial step search
	if lsiter > 2
		t0 = t0 * beta;
	end

	if lsiter<1
		t0 = t0 / beta;
	end

	x = (x + t*dx);

	%--------- uncomment for debug purposes ------------------------
	disp(sprintf('%d   , obj: %f, RMS: %f, L-S: %d', k,f1,RMSerr,lsiter));

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

    %conjugate gradient calculation

	g1 = wGradient(x,params);
	bk = g1(:)'*g1(:)/(g0(:)'*g0(:)+eps);
	g0 = g1;
	dx =  - g1 + bk* dx;
	k = k + 1;

	%TODO: need to "think" of a "better" stopping criteria ;-)
	if (k > params.Itnlim) | (norm(dx(:)) < gradToll)
		break;
	end

end

return;

function [FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx] = preobjective(x, dx, params)

% precalculates transforms to make line search cheap

FTXFMtx = params.FT*(params.XFM'*x);
FTXFMtdx = params.FT*(params.XFM'*dx);

if params.TVWeight
    DXFMtx = params.TV*(params.XFM'*x);
    DXFMtdx = params.TV*(params.XFM'*dx);
else
    DXFMtx = 0;
    DXFMtdx = 0;
end

function [res, obj, RMS] = objective(FTXFMtx, FTXFMtdx, DXFMtx, DXFMtdx, x,dx,t, params);
%calculated the objective function

p = params.pNorm;

obj = FTXFMtx + t*FTXFMtdx - params.data;
obj = obj(:)'*obj(:);

if params.TVWeight
    w = DXFMtx(:) + t*DXFMtdx(:);
    TV = (w.*conj(w)+params.l1Smooth).^(p/2);
else
    TV = 0;
end

if params.xfmWeight
   w = x(:) + t*dx(:);
   XFM = (w.*conj(w)+params.l1Smooth).^(p/2);
else
    XFM=0;
end

TV = sum(TV.*params.TVWeight(:));
XFM = sum(XFM.*params.xfmWeight(:));
RMS = sqrt(obj/sum(abs(params.data(:))>0));

res = obj + (TV) + (XFM) ;

function grad = wGradient(x,params)

gradXFM = 0;
gradTV = 0;

gradObj = gOBJ(x,params);
if params.xfmWeight
gradXFM = gXFM(x,params);
end
if params.TVWeight
gradTV = gTV(x,params);
end

grad = (gradObj +  params.xfmWeight.*gradXFM + params.TVWeight.*gradTV);

function gradObj = gOBJ(x,params)
% computes the gradient of the data consistency

	gradObj = params.XFM*(params.FT'*(params.FT*(params.XFM'*x) - params.data));

gradObj = 2*gradObj ;

function grad = gXFM(x,params)
% compute gradient of the L1 transform operator

p = params.pNorm;

grad = p*x.*(x.*conj(x)+params.l1Smooth).^(p/2-1);

function grad = gTV(x,params)
% compute gradient of TV operator

p = params.pNorm;

Dx = params.TV*(params.XFM'*x);

G = p*Dx.*(Dx.*conj(Dx) + params.l1Smooth).^(p/2-1);
grad = params.XFM*(params.TV'*G);


function [Population,FrontNo,CrowdDis] = LCSA_NSGAIIEnvironmentalSelection(Population,N)
% -----------------------------------------------------------------------
%  Copyright (C) 2020 Heiner Zille
%
%  This work is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0
%  International License. (CC BY-NC-SA 4.0). To view a copy of this license,
%  visit http://creativecommons.org/licenses/by-nc-sa/4.0/ or see the
%  pdf-file "License-CC-BY-NC-SA-4.0.pdf" that came with this code.
%
%  You are free to:
%  * Share ? copy and redistribute the material in any medium or format
%  * Adapt ? remix, transform, and build upon the material
%  Under the following terms:
%  * Attribution ? You must give appropriate credit, provide a link to the
%     license, and indicate if changes were made. You may do so in any reasonable
%     manner, but not in any way that suggests the licensor endorses you or your use.
%  * NonCommercial ? You may not use the material for commercial purposes.
%  * ShareAlike ? If you remix, transform, or build upon the material, you must
%    distribute your contributions under the same license as the original.
%  * No additional restrictions ? You may not apply legal terms or technological
%    measures that legally restrict others from doing anything the license permits.
%
%  Author of this Code:
%   Heiner Zille <heiner.zille@ovgu.de> or <heiner.zille@gmail.com>
%
%  This code is based on the following publications:
%
%  1) Heiner Zille
%     "Large-scale Multi-objective Optimisation: New Approaches and a Classification of the State-of-the-Art"
%     PhD Thesis, Otto von Guericke University Magdeburg, 2019
%     http://dx.doi.org/10.25673/32063
%
%  2) Heiner Zille and Sanaz Mostaghim
%     "Linear Search Mechanism for Multi- and Many-Objective Optimisation"
%     10th International Conference on Evolutionary Multi-Criterion Optimization (EMO 2019),
%        Lecture Notes in Computer Science, vol 11411.
%        Deb K. et al. (eds), Springer, Cham, East Lansing, Michigan, USA, March 2019
%     https://doi.org/10.1007/978-3-030-12598-1_32.
%
%  This file is intended to work with the PlatEMO framework version 2.5.
%  Date of publication of this code: 06.04.2020
%  Last Update of this code: 06.04.2020
%  A newer version of this algorithm may be available. Please contact the author
%  or see http://www.ci.ovgu.de/Research/Codes.html.
%
% The files may have been modified in Feb 2021 by the authors of the Platemo framework to work with the Platemo 3.0 release.
% -----------------------------------------------------------------------
% This file is derived from its original version containied in the PlatEMO
% framework.
% -----------------------------------------------------------------------

    %% Non-dominated sorting
    [FrontNo,MaxFNo] = NDSort(Population.objs,Population.cons,N);
    Next = FrontNo < MaxFNo;

    %% Calculate the crowding distance of each solution
    CrowdDis = CrowdingDistance(Population.objs,FrontNo);

    %% Select the solutions in the last front based on their crowding distances
    Last     = find(FrontNo==MaxFNo);
    [~,Rank] = sort(CrowdDis(Last),'descend');
    Next(Last(Rank(1:N-sum(Next)))) = true;

    %% Population for next generation
    Population = Population(Next);
    FrontNo    = FrontNo(Next);
    CrowdDis   = CrowdDis(Next);
end


function element_stress = elementStress(matrixD, element_strain, element_num)
% 该函数用于计算单元应力
% 输入为
%   D矩阵 matriD
%   单元节点应变 element_strain
%   单元编号element_num
% 输出为
%   单元应力 element_stress

element_stress = matrixD * element_strain(:, element_num);


function[]=makefigs_closedcurves
%MAKEFIGS_CLOSEDCURVES  Makes a sample figure for CLOSEDCURVES.

load qgsnapshot, use qgsnapshot
dx=qgsnapshot.x(2)-qgsnapshot.x(1);
[cv,zeta,N,S,P]=psi2fields(dx,qgsnapshot.psi);
P=frac(P,std(P(:)));

[xc,yc]=closedcurves(x,y,P,-2);
[xp,yp]=closedcurves(x,y,P,-2,'periodic',100,100);
[xpax,ypax,fp]=periodize(100,100,x,y,P);

figure,jpcolor(xpax,ypax,fp),axis equal,axis tight
colormap gray, flipmap, caxis([-5 5]),
vlines(xpax([100 end-100]),'w'),hlines(ypax([100 end-100]),'w')
xtick([-5:1:5]*1000),ytick([-5:1:5]*1000),noxlabels,noylabels
cellplot(xp,yp,'2b'),hold on,cellplot(xc,yc,'r'),fontsize 11
title('Periodized (blue) non-periodized (red) curves for a QG model')
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 closedcurves
    crop closedcurves.png
    cd(currentdir)
end


function pde = TorusTimeInitial4(mum,mup,sigm,sigp,epsm,epsp,omega,x0,y0,z0,r1,r2,a,b,intPt)
%% USAGE: polynomial solution for Poisson equation
%  Last Modified: 02/21/2020 by Xu Zhang

%% PDE Structure
pde = struct('intf',@intf,...
    'exactu1',@exactu1,'exactu2',@exactu2,'exactu3',@exactu3,...
    'um1',@um1,'um2',@um2,'um3',@um3,'up1',@up1,'up2',@up2,'up3',@up3,...
    'Dxu',@Dxu,'Dxum',@Dxum,'Dxup',@Dxup,'Dyu',@Dyu,...
    'Dyum',@Dyum,'Dyup',@Dyup,'Dzu',@Dzu,'Dzum',@Dzum,'Dzup',@Dzup,...
    'f1',@f1,'f2',@f2,'f3',@f3,...
    'fm1',@fm1,'fm2',@fm2,'fm3',@fm3,...
    'fp1',@fp1,'fp2',@fp2,'fp3',@fp3,...
    'Mu',@Mu,'Mum',@Mum,'Mup',@Mup,'one',@one,...
    'Epslon',@Epslon,'Epslonm',@Epslonm,'Epslonp',@Epslonp,...
    'Sig',@Sig,'Sigm',@Sigm,'Sigp',@Sigp,...
    'E1',@E1,'E2',@E2,'E3',@E3,...
    'Et1',@Et1,'Et2',@Et2,'Et3',@Et3);

pde.mum = mum;
pde.mup = mup;
pde.sigm = sigm;
pde.sigp = sigp;
pde.epsm = epsm;
pde.epsp = epsp;
%% interface function
    function u = intf(x,y,z)
        u = (sqrt((x-x0).^2+(y-y0).^2)-r2).^2 + (z-z0).^2 - r1^2;
    end

%% exact solution
    function u = exactu1(x,y,z,t)
        u = um1(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = up1(x(id),y(id),z(id),t);
        %u = t*ones(size(x));
    end
    function u = exactu2(x,y,z,t)
        u = um2(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = up2(x(id),y(id),z(id),t);
        %u = t*ones(size(x));
    end
    function u = exactu3(x,y,z,t)
        u = um3(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = up3(x(id),y(id),z(id),t);
        %u = t*ones(size(x));
    end
    function u = um1(x,y,z,t)
        u = zeros(size(x));
    end
    function u = um2(x,y,z,t)
        u = exp(-b*(a*(x-intPt)-omega*t).^2);
    end
    function u = um3(x,y,z,t)
        u = zeros(size(x));
    end
    function u = up1(x,y,z,t)
        u = zeros(size(x));
        %u = t*ones(size(x));
    end
    function u = up2(x,y,z,t)
        u = exp(-b*(a*(x-intPt)-omega*t).^2);
    end
    function u = up3(x,y,z,t)
        u = zeros(size(x));
    end
%% Derivative of the exact solution
    function u = Dxu(x,y,z,t)
        u = Dxum(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = Dxup(x(id),y(id),z(id),t);
    end
    function u = Dyu(x,y,z,t)
        u = Dyum(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = Dyup(x(id),y(id),z(id),t);
    end
    function u = Dzu(x,y,z,t)
        u = Dzum(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = Dzup(x(id),y(id),z(id),t);
    end
    function u = Dxum(x,y,z,t)
        u = zeros(size(x));
    end
    function u = Dyum(x,y,z,t)
        u = zeros(size(x));
    end
    function u = Dzum(x,y,z,t)
        u = -exp(-b*(a*(x-intPt)-omega*t).^2)*2*a*b.*(a*(x-intPt)-omega*t);
    end
    function u = Dxup(x,y,z,t)
        u = zeros(size(x));
    end
    function u = Dyup(x,y,z,t)
        u = zeros(size(x));
    end
    function u = Dzup(x,y,z,t)
        u = -exp(-b*(a*(x-intPt)-omega*t).^2)*2*a*b.*(a*(x-intPt)-omega*t);
    end

%% right hand side function
    function u = f1(x,y,z,t)
        u = fm1(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = fp1(x(id),y(id),z(id),t);
    end
    function u = f2(x,y,z,t)
        u = fm2(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = fp2(x(id),y(id),z(id),t);
    end
    function u = f3(x,y,z,t)
        u = fm3(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = fp3(x(id),y(id),z(id),t);
    end

    function u = fm1(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fm2(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fm3(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fp1(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fp2(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fp3(x,y,z,t)
        u = zeros(size(x));
    end

%% Diffusion coefficient function
    function u = Mu(x,y,z)
        u = Mum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Mup(x(id),y(id),z(id));
    end
    function u = Mum(x,y,z)
        u = mum^(-1)*ones(size(x));
    end
    function u = Mup(x,y,z)
        u = mup^(-1)*ones(size(x));
    end

%% Mass coefficient function
    function u = Epslon(x,y,z)
        u = Epslonm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Epslonp(x(id),y(id),z(id));
    end
    function u = Epslonm(x,y,z)
        u = epsm*ones(size(x));
    end
    function u = Epslonp(x,y,z)
        u = epsp*ones(size(x));
    end

    function u = Sig(x,y,z)
        u = Sigm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Sigp(x(id),y(id),z(id));
    end
    function u = Sigm(x,y,z)
        u = sigm*ones(size(x));
    end
    function u = Sigp(x,y,z)
        u = sigp*ones(size(x));
    end

%% Other function
    function u = one(x,y,z)
        u = ones(size(x));
    end
end


%DEMOEXPANDPOLYGON  Expand a polygon by a given distance
%   demoExpandPolygon
%
%   Example
%   demoExpandPolygon
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2009-06-17,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

%% Base polygon

% create a non-convex polygon
poly = [200 100;200 150;150 150;150 200;50 200;150 100];

% set up display
figure(1); clf; hold on;
axis equal; axis([20 230 70 230]);

% show the polygon
drawPolygon(poly, 'linewidth', 2);

print(gcf, 'expandPolygon_initial.png', '-dpng');

%% Positive expansion

% expand the polygon by a positive distance (outside of the polygon)
polyOut = expandPolygon(poly, 10);

% draw the expanded polygon
drawPolygon(polyOut, 'k');

print(gcf, 'expandPolygon_outer.png', '-dpng');

%% Negative expansion

% expand the polygon by a negative distance (inside the polygon)
polyIn = expandPolygon(poly, -20, 'cleanuploops', true);

% draw the expanded polygon
drawPolygon(polyIn, 'g');

% decorate
legend('Original', 'Outside', 'Inside')

print(gcf, 'expandPolygon_OuterAndInner.png', '-dpng');


function [tm,im] = mapvert(tr,pi)
%MAPVERT find the tree-to-vertex mappings.
%   [TM,IM] = MAPVERT(TR,PI) returns the tree-to-vertex and
%   vertex-to-tree mappings for a given aabb-tree TR and a
%   collection of query vertices PI.
%
%   The tree-to-item mapping TM is a structure representing
%   the intersection of the items PI with the tree TR. TM.II
%   is an M-by-1 array of tree indices and TM.LL is an
%   M-by-1 cell array of item lists. Specifically, items in
%   the list TM.LL{JJ} intersect with the node TM.II(JJ).
%
%   The item-to-tree mapping IM is a structure representing
%   the inverse mapping. IM.II is an N-by-1 array of item
%   indices and IM.LL is an N-by-1 cell array of node lists.
%   Specifically, nodes in the list IM.LL{JJ} intersect with
%   the item IM.II(JJ).
%
%   See also QUERYSET, MAPRECT, MAKETREE

%   Darren Engwirda : 2014 --
%   Email           : de2363@columbia.edu
%   Last updated    : 06/04/2017

%----------------------- call SCANTREE to do the actual work
    if (nargout == +1)
       [tm   ] = scantree(tr,pi,@partvert);
    else
       [tm,im] = scantree(tr,pi,@partvert);
    end

end

function [j1,j2] = partvert(pi,b1,b2)
%PARTVERT partition points between boxes B1,B2 for SCANTREE.

    j1 = true(size(pi,1),1);
    j2 = true(size(pi,1),1);

    nd = size(b1,2) / +2;

    for ax = +1 : nd
%--------------- remains TRUE if inside bounds along axis AX
    j1 = j1 & pi(:,ax)>=b1(ax+nd*0) ...
            & pi(:,ax)<=b1(ax+nd*1) ;

%--------------- remains TRUE if inside bounds along axis AX
    j2 = j2 & pi(:,ax)>=b2(ax+nd*0) ...
            & pi(:,ax)<=b2(ax+nd*1) ;
    end

end


function [D,u,X,div_X,phi,pre,B,t] = heat_geodesic(varargin)
  % HEAT_GEODESIC  geodesic distance approximation following the method of
  % "Geodesics in Heat" [Krane et al. 2013] D from all points V in the domain
  % (V,F) to a source point/set of points, gamma. The method is motivated by
  % heat using a time parameter t to guide the heat diffusion.
  %
  % D = heat_geodesic(V,F,gamma,t)
  % [D,u,X,div_X,phi,pre] = heat_geodesic(V,F,gamma,t,'ParamName',ParamValue)
  %
  % Inputs:
  %   V  #V by 3 set of vertex positions
  %   F  #F by 3 set of face indices
  %     or
  %      #T by 4 set of tetrahedra indices
  %   gamma  #gamma list of vertex indices of source points
  %   t  time parameter
  %   Optional:
  %     'BoundaryConditions'  followed by one of the following strings
  %       {'average'}:
  %       'dirichlet'  domain boundaries set to 0 when computing heat
  %         diffusion
  %       'neumann'  heat diffusion solved with implicit neumann conditions
  %       'average'  Uniform average of neumann and Dirichlet solutions
  %       'robin'  The original ArXiv paper called the method of averaging the
  %         Dirichlet and Neumann solutions 'robin'. This was later corrected
  %         (this solution cannot be created by boundary conditions to the heat
  %         equation alone: average solutions is a non-linear operation).  In
  %         any case, 'robin' is treated as 'average'.
  %     'Precomputation' Followed by pre struct as returned by this function
  %     'IntrinsicDelaunay'  followed by whether to use intrinsic Delaunay
  %       Laplacian {false}
  %     'Legacy' followed by bool telling whether to use Alec's legacy
  %       implmentation. In particular this is useful because the original
  %       paper is unclear how the boundary of the domain should be handle with
  %       respect to the seed locations (gamma). What if they overlap? What
  %       should be the boundary conditions for the final poisson solve?
  % Outputs:
  %   D  #V list of geodesic distances from vertices to gamma
  %   u  #V list of results of heat diffusion step
  %   X  #F by 3 list of reversed normalized gradients of u
  %   div_X  #V list od divergence of X
  %   phi  #V list of solution to final poisson equation solve
  %
  D=[];
  % Note: This is Alec's previous implmentation. He is still convinced that
  % this is more exact/correct despite not what's written in the article (and
  % implying that refactoring is necessary).
  legacy = false;
  use_intrinsic = false;

  % mandatory input
  V = varargin{1};
  F = varargin{2};

  % number of domain vertices
  n = size(V,1);
  % simplex size
  ss = size(F,2);
  gamma = varargin{3};
  if nargin>=4 && ~isempty(varargin{4})
    t = varargin{4};
  else
    switch ss
    case 3
      % Section 3.1.1
      AM = sum(doublearea(V,F))/2;
      sF = size(F,1);
      c = 5;
      t = c * AM / sF;
      %t = 20*mean(doublearea(V,F));
    case 4
      AM = sum(volume(V,F));
      sF = size(F,1);
      c = 1e3;
      t = c * AM / sF;
      %t = 20*mean(volume(V,F));
    end
  end

  % option parameter default values
  bc_type = 'average';
  pre = [];
  pre.L = [];
  % precomputation for Dirichlet solve
  pre.D = [];
  % precomputation for Neumann solve
  pre.N = [];
  % precomputation for Poisson solve
  pre.poisson = [];
  u = [];

  ii = 5;
  while(ii <= nargin)
    switch varargin{ii}
    case 'BoundaryConditions'
      ii = ii + 1;
      assert(ii<=nargin);
      bc_type = varargin{ii};
    case 'Precomputation'
      ii = ii + 1;
      assert(ii<=nargin);
      % skip empty input
      if ~isempty(varargin{ii})
        pre = varargin{ii};
      end
    case 'Legacy'
      ii = ii + 1;
      assert(ii<=nargin);
      legacy = varargin{ii};
    case 'u'
      ii = ii + 1;
      assert(ii<=nargin);
      u = varargin{ii};
    case 'IntrinsicDelaunay'
      ii = ii + 1;
      assert(ii<=nargin);
      use_intrinsic = varargin{ii};
    otherwise
      error(['Unsupported parameter: ' varargin{ii}]);
    end
    ii = ii + 1;
  end

  % Algorithm 1 in Section 3:
  %   Integrate the heat flow ?? = ??u for time t
  %   Evaluate the vector field X = -???u/|???u|
  %   Solve the Poisson equation ???? = ??????X

  % Integrate the heat flow: "We discretize the heat equation from step I of
  % Algorithm 1 using a single backward Euler step"

  % "... with initial conditions u0 = ??(x)"
  % "Note that a Dirac delta appears as a literal one in this system since we
  % are effectively working with integrated quantities"
  u0 = zeros(n,1);
  u0(gamma) = 1;

  % "where ???????? is one third the area of all triangles incident on vertex ...
  % where ???? ??? R|????|??|????| is a diagonal matrix containing the vertex areas"
  if isempty(pre.L)
    if use_intrinsic
    pre.L = intrinsic_delaunay_cotmatrix(V,F);
    else
    pre.L = cotmatrix(V,F);
    end
    pre.M = massmatrix(V,F,'barycentric');
    pre.Q = pre.M - t*pre.L;
    pre.G = grad(V,F);
    pre.Div = div(V,F);
    pre.Tik = 1e-8*speye(size(V,1));
    pre.P = -pre.L*0.5 + pre.Tik;
    % get outline ("boundary") of mesh
    switch ss
    case 3
      pre.out = unique(reshape(outline(F),[],1));
    case 4
      pre.out = unique(boundary_faces(F));
    end
  end

  % 11/7/2017: Keenan writes that the mass matrix should not be here.
  % B = M*u0;
  B = u0;

  if isempty(u)
    if strcmp(bc_type,'dirichlet') || strcmp(bc_type,'average') || strcmp(bc_type,'robin')

      if legacy
        % remove any gamma from outline
        pre.out = setdiff(pre.out(:),gamma(:));
        % find all boundary vertices and append to gamma
        b = [gamma(:); pre.out];
        % "zero Dirichlet conditions"
        bc = [ones(numel(gamma),1); zeros(numel(b)-numel(gamma),1)];
        [uD,pre.D] = min_quad_with_fixed(pre.Q,B,b,bc,[],[],pre.D);
      else
        b = pre.out;
        % Q: How should we deal with gamma ??? out ?
        % This gives *reasonable* results, but doesn't look right for fixing
        % entire boundary. Probably more because of the lack of correct boundary
        % conditions for the final poisson solve.
        bc = -1*ismember(pre.out,gamma);
        %assert(~any(ismember(gamma,out)));
        [uD,pre.D] = min_quad_with_fixed(pre.Q,B,b,bc,[],[],pre.D);
      end
    end
    if strcmp(bc_type,'neumann') || strcmp(bc_type,'average') || strcmp(bc_type,'robin')
      % See Figure 9, pretty sure these are implicit ???x/???n = 0 neumann
      % conditions, though it is not stated in the text. At best, "Neumann
      % conditions prevent heat from flowing out of the domain..."
      if legacy
        b = gamma(:);
        bc = ones(numel(gamma),1);
        [uN,pre.N] = min_quad_with_fixed(pre.Q,B,b,bc,[],[],pre.N);
      else
        [uN,pre.N] = min_quad_with_fixed(pre.Q,B,[],[],[],[],pre.N);
      end
    end

    if strcmp(bc_type,'natural')
      K = keenan(V,F);
      Q = M - t*K;
      B = M*u0;
      G = grad(V,F);
      switch size(F,2)
      case 4
        vol = volume(V,F);
      case 3
        vol = doublearea(V,F);
      end
      vol = vol/sum(vol);
      % kill off affine functions
      A = [
        sum(M)/sum(M(:)); ...
        kron(speye(size(V,2)),vol)'*G];
      uT = min_quad_with_fixed(Q,B,[],[],A,[0;zeros(size(V,2),1)]);
    end

    switch bc_type
    case 'dirichlet'
      u = uD;
    case 'neumann'
      u = uN;
    case {'average','robin'}
      % "We advocate the use of the Robin boundary conditions obtained by taking
      % the mean of the Neumann solution uN and the Dirichlet solution uD, i.e.,
      % u = 0.5*(uN + uD)"
      u = 0.5*(uN+uD);
    case 'natural'
      u = uT;
    otherwise
      error(['Unsupported BoundaryCondtions value: ' bc_type]);
    end
  end

  robust_X = false;
  if robust_X
    X = robust_heat_gradients(V,F,uN,uD,t);
    warning('robust');
  else
    % Evaluate the vector field X
    grad_u = reshape(pre.G*u,size(F,1),size(V,2));
    %grad_u_norm = sqrt(sum(grad_u.^2,2));
    %% normalize grad_u
    %normalized_grad_u = bsxfun(@rdivide,grad_u,grad_u_norm);
    %% correct any zero-norm gradients
    %normalized_grad_u(grad_u_norm == 0,:) = 0;
    % normalize reverse direction; normalizerow will use robust norm that
    % avoids underflow.
    X = -normalizerow(grad_u);
    X(isnan(X)) = 0;
  end

  % Solve the Poisson equation
  % divergence of X
  div_X = pre.Div*X(:);
  if legacy
    [phi,pre.poisson] = min_quad_with_fixed( ...
      -pre.L*0.5,2*div_X,gamma,zeros(numel(gamma),1),[],[],pre.poisson);
  else
    [phi,pre.poisson] = ...
      min_quad_with_fixed(pre.P,2*div_X,[],[],[],[],pre.poisson);
    %Aeq = sparse(ones(1,size(V,1)));
    %Beq = 0;
    %[phi,pre.poisson] = ...
    %  min_quad_with_fixed(-pre.L*0.5,2*div_X,[],[],Aeq,Beq,pre.poisson);
  end
  D = phi;
  % "Note that ???? is unique only up to an additive constant and should be
  % shifted such that the smallest distance value is zero."
  %
  % D should be zero at gamma
  D = D - mean(D(gamma));
  % Flip sign so farthest point is positive distance
  [~,mi] = max(abs(D));
  D = D*sign(D(mi));

end


function [ gd ] = gsp_design_can_dual( g ,tol)
%GSP_DESIGN_CAN_DUAL This function return the canonical dual filters of g
%   Usage:  gd = gsp_design_can_dual( g );
%
%   Inputs parameters:
%       g       : cell array of filters
%       tol     : tolerance for the pseudo-inverse
%
%   Ouputs parameters:
%       g       : cell array of filters
%
%   This function returns the canonical dual filterbank g. Note that it
%   might not be the be the optimal solution in term of computation.
%
%   Example:::
%
%             N = 100;
%             G = gsp_sensor(N);
%             G = gsp_compute_fourier_basis(G);
%             g = gsp_design_abspline(G,8);
%             gd = gsp_design_can_dual(g);
%             paramplot.show_sum = 0;
%             figure(1)
%             gsp_plot_filter(G,g,paramplot);
%             title('Original filters')
%             figure(2)
%             gsp_plot_filter(G,gd,paramplot);
%             title('Canonical dual filters');
%
%             x = rand(N,1);
%             param.method = 'exact';
%             coeff = gsp_filter_analysis(G,g,x,param);
%             xs = gsp_filter_synthesis(G,gd,coeff,param);
%             norm(xs-x)
%
%   See also: gsp_evaluate_can_dual

% Author: Nathanael Perraudin
% Date  : 30 December 2014
% Testing: test_dual

if nargin<2
    tol = 1e-8;
end

Nf = length(g);
gd = cell(Nf,1);

for ii = 1:Nf
    gd{ii} = @(x) can_dual(g,ii,x,tol);
end

end

function ret = can_dual(g,n,x,tol)
    [N1, N2] = size(x);
    x = x(:);
    sol = gsp_evaluate_can_dual( g,x,tol );
    ret = sol(:,n);
    ret = reshape(ret,N1,N2);
end


function g = heavisideNoiseGradientParam(noise, mu, varsigma, y)

% HEAVISIDENOISEGRADIENTPARAM Gradient of the heaviside noise model's parameters.

% IVM

c = y./sqrt(varsigma);
denom = zeros(size(c));
u = zeros(size(c));
for i = 1:size(mu, 2)
  u(:, i) = c(:, i).*(mu(:, i) + noise.bias(i));
  denom(:, i) = ((1-2*noise.eta)*cumGaussian(u(:, i))+noise.eta);
end

gnoise.bias = (1-2*noise.eta).*sum(c.*ngaussian(u)./denom, 1);
gnoise.eta = sum(sum((-2*cumGaussian(u)+1)./denom, 1));
gnoise.eta = gnoise.eta.*(noise.eta.*(1-2*noise.eta));
g = [gnoise.eta gnoise.bias];


%% FUNCTION Logistic_CMTL
%   Convex-relaxed Clustered Multi-Task Learning with Logistic Loss.
%
%% OBJECTIVE
%   argmin_{W,M,C} { sum_i^t (- sum(log (1./ (1+ exp(-X{i}*W(:, i) - Y{i} .* C(i)))))/length(Y{i}))
%            + rho1 * eta (1+eta) trace(W (eta I + M)^-1 W')
%     subject to: trace (M) = k, M \preceq I, M \in S_+^t, eta = rho2/rho1
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   k: cluster number
%   rho1: clustering penalty controlling parameter (rho1=0 then tasks are not
%   related)
%   rho2: L2 norm regularization on model W (rho2=0 then reduce to ASO but
%   this solver does not support this case)
%
%% OUTPUT
%   W: model: d * t
%   C: model: 1 * t
%   funcVal: function value vector.
%   M: relaxed F*F', where F is the clustering assignment matrix
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou, Jianhui Chen and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% RELATED PAPERS
%
%   [1] J. Zhou, J. Chen and J. Ye, Clustered Multi-Task Learning via
%       Alternating Structure Optimization, NIPS 2011.
%
%% RELATED FUNCTIONS
%   Least_CMTL, init_opts

%% Code starts here
function [W, C, funcVal, M] = Logistic_CMTL(X, Y, rho1, rho2, k, opts)

if nargin <5
    error('\n Inputs: X, Y, rho1, rho2 and k should be specified!\n');
end
X = multi_transpose(X);

if nargin <6
    opts = [];
end

if rho2<=0 || rho1<=0
    error('rho1 and rho2 should both greater than zero.');
end

% if exist('mosekopt','file')==0
%     error('Mosek is not found. Please install Mosek first. \n')
% end

% initialize options.
opts=init_opts(opts);

task_num  = length (X);
dimension = size(X{1}, 1);
funcVal = [];

eta = rho2 / rho1;
c = rho1 * eta * (1 + eta);

%initialize a starting point
C0_prep = zeros(1, task_num);
for t_idx = 1: task_num
    m1 = nnz(Y{t_idx} == 1);
    m2 = nnz(Y{t_idx} == -1);
    if ( m1==0 || m2==0 )
        C0_prep(t_idx) = 0;
    else
        C0_prep(t_idx) = log(m1/m2);
    end
end

if opts.init==2
    W0 = zeros(dimension, task_num);
    C0 = zeros(1, task_num);
elseif opts.init== 0
    W0 = randn(dimension, task_num);
    C0 = C0_prep;
else
    if isfield(opts,'W0')
        W0=opts.W0;
        if (nnz(size(W0)-[dimension, task_num]))
            error('\n Check the input .W0');
        end
    else
        W0 = zeros(dimension, task_num);
    end
    if isfield(opts,'C0')
        C0=opts.C0;
    else
        C0=C0_prep;
    end
end

M0 = speye (task_num) * k / task_num;

bFlag=0; % this flag tests whether the gradient step only changes a little

Wz= W0;
Cz= C0;
Mz = M0;

Wz_old = W0;
Cz_old = C0;
Mz_old = M0;

t = 1;
t_old = 0;
iter = 0;
gamma = 1;
gamma_inc = 2;

while iter < opts.maxIter
    alpha = (t_old - 1) /t;

    Ws = (1 + alpha) * Wz - alpha * Wz_old;
    Cs = (1 + alpha) * Cz - alpha * Cz_old;
    Ms = (1 + alpha) * Mz - alpha * Mz_old;

    % compute function value and gradients of the search point
    [gWs, gCs, gMs, Fs ]  = gradVal_eval(Ws, Cs, Ms);

    % the Armijo Goldstein line search scheme
    while true
        %[Wzp l1c_wzp] = l1_projection(Ws - gWs/gamma, 2 * rho2 / gamma);
        %Fzp = funVal_eval  (Wzp, Czp, rho1);

        Wzp = Ws - gWs/gamma;
        Czp = Cs - gCs/gamma;
        [Mzp Mzp_Pz Mzp_DiagSigz ] = singular_projection (Ms - gMs/gamma, k);
        Fzp = funVal_eval (Wzp, Czp, Mzp_Pz, Mzp_DiagSigz);

        delta_Wzp = Wzp - Ws;
        delta_Czp = Czp - Cs;
        delta_Mzp = Mzp - Ms;

        nrm_delta_Wzp = norm(delta_Wzp, 'fro')^2;
        nrm_delta_Czp = norm(delta_Czp, 'fro')^2;
        nrm_delta_Mzp = norm(delta_Mzp, 'fro')^2;

        r_sum = (nrm_delta_Wzp+nrm_delta_Czp+nrm_delta_Mzp)/3;

        Fzp_gamma = Fs + sum(sum(delta_Wzp .* gWs)) ...
            + sum(sum(delta_Czp .* gCs))...
            + sum(sum(delta_Mzp .* gMs)) ...
            + gamma/2 * nrm_delta_Wzp ...
            + gamma/2 * nrm_delta_Mzp ...
            + gamma/2 * nrm_delta_Czp;

        if (r_sum <=eps)
            bFlag=1; % this shows that, the gradient step makes little improvement
            break;
        end

        if (Fzp <= Fzp_gamma)
            break;
        else
            gamma = gamma * gamma_inc;
        end
    end

    Wz_old = Wz;
    Cz_old = Cz;
    Mz_old = Mz;

    Wz = Wzp;
    Cz = Czp;
    Mz = Mzp;

    funcVal = cat(1, funcVal, Fzp);

    if (bFlag)
        % fprintf('\n The program terminates as the gradient step changes the solution very small.');
        break;
    end

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    iter = iter + 1;
    t_old = t;
    t = 0.5 * (1 + (1+ 4 * t^2)^0.5);

end

W = Wzp;
C = Czp;
M = Mzp;

% private functions

    function [Mzp Mzp_Pz Mzp_DiagSigz ] = singular_projection (Msp, k)
        [EVector EValue] = eig(Msp);
        Pz = real(EVector);  diag_EValue = real(diag(EValue));
        %DiagSigz = SingVal_Projection(diag_EValue, k);
        DiagSigz = bsa_ihb(diag_EValue, ones(size(diag_EValue)), k, ones(size(diag_EValue)));
        Mzp = Pz * diag(DiagSigz) *Pz';
        Mzp_Pz = Pz;
        Mzp_DiagSigz = DiagSigz;
    end

    function [grad_W, grad_C, grad_M, funcVal] = gradVal_eval(W, C, M)
        IM = (eta * speye(task_num) + M);
        invEtaMWt = IM\W';

        grad_W = zeros(dimension, task_num);
        grad_C = zeros(1, task_num);
        lossValVect = zeros (1 , task_num);
        if opts.pFlag
            parfor i = 1:task_num
                [ grad_W(:, i), grad_C(:, i), lossValVect(:, i)] = unit_grad_eval( W(:, i), C(i), X{i}, Y{i});
            end
        else
            for i = 1:task_num
                [ grad_W(:, i), grad_C(:, i), lossValVect(:, i)] = unit_grad_eval( W(:, i), C(i), X{i}, Y{i});
            end
        end
        grad_W = grad_W + 2 * c * invEtaMWt';   %W component
        grad_M = - c * (W' * W / IM /IM );      %M component

        funcVal = sum(lossValVect) + c * trace( W * invEtaMWt);
    end

    function [funcVal] = funVal_eval (W, C, M_Pz, M_DiagSigz)
        invIM = M_Pz * (diag( 1./(eta + M_DiagSigz))) * M_Pz';
        invEtaMWt = invIM * W';

        funcVal = 0;
        if opts.pFlag
            parfor i = 1: task_num
                funcVal = funcVal + unit_funcVal_eval( W(:, i), C(i), X{i}, Y{i});
            end
        else
            for i = 1: task_num
                funcVal = funcVal + unit_funcVal_eval( W(:, i), C(i), X{i}, Y{i});
            end
        end
        funcVal = funcVal + c * trace( W * invEtaMWt);
    end

end

function [ grad_w, grad_c, funcVal ] = unit_grad_eval( w, c, x, y)
%gradient and logistic evaluation for each task
m = length(y);
weight = ones(m, 1)/m;
weighty = weight.* y;
aa = -y.*(x'*w + c);
bb = max( aa, 0);
funcVal = weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb );
pp = 1./ (1+exp(aa));
b = -weighty.*(1-pp);
grad_c = sum(b);
grad_w = x * b;
end
function [ funcVal ] = unit_funcVal_eval( w, c, x, y)
%function value evaluation for each task
m = length(y);
weight = ones(m, 1)/m;
aa = -y.*(x'*w + c);
bb = max( aa, 0);
funcVal = weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb );
end


function domi_factor = dominant_slope(data,fs)
% decide whch side has the dominant slope (up or down)
%
% return domi_factor = 1, up slope is dominant
%                     -1, down slope is dominant
% so the data may be multiplied by domi_factor

domi_factor=1;
up_slope=0;
down_slope=0;
% split data to every 2s and find the median amplitude of each segment
d=reshape(data(1:floor(length(data)/(fs*2))*fs*2),fs*2,[]);
amplitude=median(max(d)-min(d));
% use amplitude/3 as MinPeakProminence to find peaks
[pks locs]=findpeaks(data,'MinPeakProminence',amplitude/3);
% calculate x-coordinate of each slope
for i=1:length(pks)-1
    [mn mni]=min(data(locs(i):locs(i+1)));
    l=length(data(locs(i):locs(i+1)));
    up_slope(i)=l-mni;
    down_slope(i)=mni-1;
end
% sum(up_slope)
% sum(down_slope)
% median(up_slope)
% median(down_slope)
if median(up_slope)>median(down_slope)
    domi_factor=-1;
end
if median(up_slope)==median(down_slope)
    if sum(up_slope)>sum(down_slope)
        domi_factor=-1;
    end
end
end


function [P,F] = spm_MH(L,B,y,M)
% The Rejection-Metropolis-Hastings Algorithm
% FORMAT [P,F] = spm_MH(L,P,y)
%
% L   - likelihood function: inline(P,y,M)
% B   - free parameter [structure]
% Y   - response  [stucture]
% M   - model [structure]
%
% P   - Sample from posterior p(P|y,M)
% F   - marginal likelihood p(y|M) using harmonic mean
%--------------------------------------------------------------------------
%
% Returns a harmonic mean estimate of the log-marginal likelihood or
% log-evidence and a sample from the posterior density of the free parameters
% of a model.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_MH.m 1143 2008-02-07 19:33:33Z spm $

% initialise parameters
%--------------------------------------------------------------------------
P(:,1) = spm_vec(B);

% MCMC - RMH
%--------------------------------------------------------------------------
n     = 2^8;                                          % number of burn in
N     = 2^16;                                         % number of samples
for i = 1:N

    % sample from proposal
    %----------------------------------------------------------------------
    pi = P(:,i);
    pp = pi + randn(size(P,1),1)/32;

    % compute importnace ratio
    %----------------------------------------------------------------------
    Lp = feval(L,spm_unvec(pp,B),y,M);
    Li = feval(L,spm_unvec(pi,B),y,M);
    r  = Lp/Li;

    % accept and marginal likelihood
    %----------------------------------------------------------------------
    if rand < r
        P(:,i + 1) = pp;
        F(i + 1)   = Lp;
    else
        P(:,i + 1) = pi;
        F(i + 1)   = Li;
    end

end

% burn in
%--------------------------------------------------------------------------
P = P(:,n:end);
F = F(n:end);
F = -log(mean(mean(F)./F)) + log(mean(F));


function rr = SFu(data, up)
%SFu fuses RR estimates using an implementation of smart fusion.

bw_est = data.bw.v;
am_est = data.am.v;
fm_est = data.fm.v;

%% Find times at which all three RRs were estimated
mutual_times = intersect(intersect(data.bw.t, data.fm.t), data.am.t);
for mod = {'bw', 'am', 'fm'}
    eval(['temp_data = data.' mod{1,1} ';']);
    [rel_times,rel_els,~] = intersect(temp_data.t,mutual_times);

    eval([mod{1,1} '_est = data.' mod{1,1} '.v(rel_els);']);
end

%% Cycle through times at which RR was estimated
% Find out min length incase one has fewer estimates.
min_length = min([length(bw_est), length(am_est), length(fm_est)]);
rr.v = nan(min_length,1);
rr.t = rel_times;
for s = 1 : min_length

    rrEst_values = [bw_est(s), am_est(s), fm_est(s)];

    rel_els = ~isnan(rrEst_values);

    standev = std(rrEst_values);
    % is - 3/22/2019
    %standev = nanstd(rrEst_values);
    % is - end

    if sum(rel_els) < 3 || standev > 4
        rr.v(s) = nan;
    else
        rr.v(s) = mean(rrEst_values);
    end

end

end


%sim_coherenceOrder.m
%Dana Goerzen and Jamie Near, 2021.
%
% USAGE:
% out = sim_coherenceOrder(spinSys);
%
% DESCRIPTION:
% Creates an n x n matrix that represents the coherence order of each element
% of the density matrix for the given spin system For use in nulling signal
% incoherences during pulse sequence simulations
%
% INPUTS:
% spinSys     = spin system definition structure.
%
% OUTPUTS:
% out       = n x n coherence order matrix for spin system.

function out=sim_coherenceOrder(spinSys)
out=cell(length(spinSys),1);
for m=1:length(spinSys)
    Nspins=length(spinSys(m).J);

    p0=[0 -1;1 0];
    p=p0;
    for n=1:Nspins-1
        p=kron(ones(2),p)+kron(p0,ones(size(p)));
    end
    out{m,1}=p;
end
out=p;
end


function test11results
%TEST11RESULTS analyze results from test11.m
% Example:
%   test11results
% See also test11, cholmod_test

% Copyright 2006-2007, Timothy A. Davis, University of Florida

load Results
index = UFget ;

c = E1(1:kkk) < 1 & T1(1:kkk) > 0 ;
m = E2(1:kkk) < 1 & T2(1:kkk) > 0 ;
cgood = find (c) ;	%#ok
mgood = find (m) ;	%#ok
good  = find (c | m) ;
bad = find (~(c|m)) ;

fl_per_lnz = FL(1:kkk) ./ LNZ(1:kkk) ;
speedup = T1(1:kkk) ./ T2(1:kkk) ;

[ignore ii] = sort (fl_per_lnz (good)) ;
good = good (ii) ;

fprintf ('MATLABtime CHOLMOD(time,flop,nnz(L)) speedup problem\n') ;
for k = good
    i = f (k) ;
%    fprintf ('%4d: t1 %10.2f t2 %10.2f fl %6.1e lnz %6.1e   %s/%s\n', ...
%	i, T1(k), T2(k), FL(k), LNZ(k), index.Group{i}, index.Name{i}) ;
     fprintf ('%10.4f %10.4f  %6.1e  %6.1e  %5.2f   %s/%s\n', ...
	T1(k), T2(k), FL(k), LNZ(k), speedup(k), ...
	index.Group{i}, index.Name{i}) ;
end

fprintf ('\nfailed in both:\n') ;
for k = bad
     i = f (k) ;
     fprintf ('%10.4f %10.4f  %6.1e  %6.1e  %5.2f   %s/%s\n', ...
	T1(k), T2(k), FL(k), LNZ(k), speedup(k), ...
	index.Group{i}, index.Name{i}) ;
end

% figure (3)
clf
loglog (fl_per_lnz (good), speedup (good), 'x') ;
axis ([1 4000 .1 50]) ;
xlabel ('Cholesky flop count / nnz(L)') ;
ylabel ('(MATLAB x=A\\b time) / (CHOLMOD time)') ;

drawnow


function [divTr, divTe]= sample_chronKfold(label, folds)
%SAMPLE_CHRONKFOLD - Sampling function: chronological consequtive folds
%
%Synopsis:
%  [PARTR, PARTE]= sample_chronKfold(LABEL, FOLDS)
%
% IN  LABEL   - class labels, but here only the length of the 2nd dim is
%               used (to determine the number of samples)
%     FOLDS   - DOUBLE nFolds: number of folds into which the samples are
%               divided. Or FOLDS can be [nShifts nFolds] in which case
%               all partitions will also be generated in shifted versions.
%
% OUT PARTR   - Partitions of the training set
%               PARTR{n}: cell array holding the training sets
%               folds for shuffle #n, more specificially
%               PARTR{n}{m} holds the indices of the training set of
%               the m-th fold of shuffle #n.
%     PARTE   - analogue to PARTR, for the test sets

% Benjamin Blankertz

misc_checkType(label, 'DOUBLE[- -]');
misc_checkType(folds, 'DOUBLE|DOUBLE[2]');

nSamples= size(label,2);

if length(folds)==1
  folds= [1 folds];
end

divTr= {cell(1,folds(2))};
divTe= {cell(1,folds(2))};
for nn= 1:folds(1)
  div= round(linspace(0+(nn-1), nSamples-(folds(1)-nn), folds(2)+1));
  for kk= 1:folds(2)
    divTe{nn}{kk}= div(kk)+1:div(kk+1);
    divTr{nn}{kk}= setdiff(nn:nSamples-(folds(1)-nn), divTe{nn}{kk});
    % check that all classes are inhabited
    if ~all(sum(label(:,divTr{nn}{kk}))),
      error('empty classes in training set');
    end
  end
end


function imgOut = bilateralFilterSI(img, imgEdges, sigma_s, sigma_r)
%
%		 imgOut = bilateralFilterSI(img, imgEdges, sigma_s, sigma_r)
%
%        This function implements a bilateral filter without
%        approximations. Note this function is very slow!
%
%		 Input:
%			-img: is an image to be filtered.
%           -imgEdges: is an edge image, where its edges will be transfered
%           to img. Note set this to [] if you do not want to transfer
%           edges.
%           -sigma_s: is the spatial sigma value.
%           -sigma_r: is the range sigma value.
%
%		 Output:
%			-imgOut: is the filtered image.
%
%     Copyright (C) 2014-18  Francesco Banterle
%
%     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/>.
%

output_class = class(img);

if ~isa(img, 'double')
    img = double(img);
end

if ~isa(imgEdges, 'double')
    imgEdges = double(imgEdges);
end

if ~isa(sigma_s, 'double')
    sigma_s = double(sigma_s);
end

if ~isa(sigma_r, 'double')
    sigma_r = double(sigma_r);
end

imgOut = bilateralFilterS(img, imgEdges, sigma_s, sigma_r);

if strcmp(output_class, class(imgOut)) == 0
    imgOut = cast(imgOut, output_class);
end

end