function [newPsi, Stats] = sampleSplitMerge_NoQRev( Psi, data, algParams )
% Split Merge via Not-Quite-Valid Sequential Allocation
% Proposes new candidate configuration for feat matrix F and stateSeq z
%  and accepts or rejects via Metropolis-Hastings
% HOWEVER, this *does not* account for the probability of the reverse move
%  in the acceptance ratio... there is still an acceptance ratio, but
%  it is entirely based on the joint probability under the model,
%  not on the proposal probabilities.
% Proposal selects anchor objects and features to split/merge at random,
%   unless provided (see DEBUG NOTE below)
%INPUT:
%  Psi  : model config input
%  data : SeqData object
%  algParams : relevant params for this method
%    ** SM.featSelectDistr can be one of
%       'random' : choose features to merge at random
%       'splitBias+margLik' (default) : use smart criteria to
%                    make successful proposals
%    ** SM.doSeqUpdateThetaHatOnMerge
%       if 1, refine Theta after visiting every seq. during merge proposal
%          0 (default), just estimate Theta initially and stick with it
%OUTPUT:
%  newPsi : model configuration as result of MH proposal
%    equals a new configuration when proposal accepted
%    or the input config Psi when rejected
%  Stats  : struct indicating type of move and whether accepted/rejected
%ASSUMES:
%  Psi comes with correct sufficient statistics for observed data (Xstats)
%DEBUG NOTE:
%   Can provide input "Psi" with fields "anchorIDs" and "activeFeatIDs"
%     to indicatea specific proposal to try.
%   E.g. to merge features 1 and 5 using objects 101 and 202 as seeds, set
%    Psi.anchorIDs = [101 202];
%    Psi.activeFeatIDs = [1 5];  Note F(101,1) and F(202,5) must be ON.

if isfield( Psi, 'activeFeatIDs' )
    [anchorIDs, featIDs, qFWD] = sampleAnchorAndFeatIDsToSplitMerge( Psi, data, algParams );
else
    [anchorIDs, featIDs, qFWD] = sampleAnchorAndFeatIDsToSplitMerge( Psi, data, algParams );
    Psi.anchorIDs        = anchorIDs;
    Psi.activeFeatIDs = featIDs;
end

if featIDs(1) == featIDs(2)
    % =========================================== SPLIT
    moveDescrStr = 'ADD';
    [propPsi] = sampleSplitConfig( Psi, data, anchorIDs, featIDs(1), algParams );
    %[propPsi, logQ] = sampleSplitConfig( Psi, data, anchorIDs, featIDs(1), algParams );
    %[~, logQ_Rev]   = sampleMergeConfig( propPsi, data, anchorIDs, propPsi.activeFeatIDs, algParams, Psi );
else
    % =========================================== MERGE
    moveDescrStr = 'DEL';
    [propPsi] = sampleMergeConfig( Psi, data, anchorIDs, featIDs, algParams );

    %[propPsi, logQ] = sampleMergeConfig( Psi, data, anchorIDs, featIDs, algParams );
    %[~, logQ_Rev]   = sampleSplitConfig( propPsi, data, anchorIDs, propPsi.activeFeatIDs, algParams, Psi );
end

% Total up probabilities of FORWARD (Q) and REVERSE (Q_Rev) moves
[~, ~, qREV] = sampleAnchorAndFeatIDsToSplitMerge( propPsi, data, algParams );
logQ_Rev.all = log(qREV); % + logQ_Rev.F + logQ_Rev.z;
logQ.all     = log(qFWD); % + logQ.F + logQ.z;

% NB: not passing data as an arg here
%   means that we trust the stored X suff stats in Psi! Yay efficiency.
logPr_Cur  = calcJointLogPr_BPHMMState( Psi );
logPr_Prop = calcJointLogPr_BPHMMState( propPsi );

logPrAccept = logPr_Prop.all - logPr_Cur.all + logQ_Rev.all - logQ.all;
rho = exp( logPrAccept );
rho = min(1, rho);
doAccept = rand < rho;

if (  doAccept )
    newPsi = propPsi;
    % Remove empty columns of F, and rename state sequence appropriately
    newPsi = reallocateFeatIDs( newPsi );
else
    newPsi = Psi;
end

% Strip off info used internally to identify how to construct proposals
newPsi = rmfield( newPsi, 'anchorIDs');
newPsi = rmfield( newPsi, 'activeFeatIDs');

Stats.nAccept = doAccept;
Stats.rho = rho;
Stats.moveDescr = moveDescrStr;

end % main function


function N1 = plus(N1, N2)
%PLUS   Plus for CHEBOP2.
%
% N = PLUS(N1, N2) is the same as N = N1 + N2.

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

% Check domains are the same.
if ( all( N1.domain ~= N1.domain ) )
    error('CHEBFUN:CHEBOP2:plus:domain', ...
        'Domains of operators must be identical.');
end

% Plus the coefficient matrices together.
A = N1.coeffs;
B = N2.coeffs;

% PLUS the coefficient cellarrays or matrices together:
if ( iscell(A) && iscell(B) )
    C = cell(max(size(A, 1), size(B, 1)), max(size(A, 2), size(B, 2)));
    if size(B, 1) < size(C, 1)
        B{size(C, 1),1} = [];
    end
    if ( size(B, 2) < size(C, 2) )
        B{1,size(C, 2)} = [];
    end
    if ( size(A, 1) < size(C, 1) )
        A{size(C, 1),1} = [];
    end
    if ( size(A, 2) < size(C, 2) )
        A{1,size(C, 2)} = [];
    end
    for jj = 1:size(C, 1)
        for kk = 1:size(C, 2)
            if ( isempty(B{jj,kk}) )
                C{jj,kk} = A{jj,kk};
            elseif ( isempty(A{jj,kk}) )
                C{jj,kk} = B{jj,kk};
            else
                C{jj,kk} = A{jj,kk} + B{jj,kk};
            end
        end
    end

elseif ( iscell(A) )
    BB = cell(size(B, 2), size(B, 1));
    for jj = 1:size(B, 1)
        for kk = 1:size(B, 2)
            BB(kk,jj) = {B(jj,kk)};
        end
    end
    N2.coeffs = BB;
    N1 = plus(N1, N2);
    return

elseif ( iscell(B) )
    AA = cell(size(A, 2), size(A, 1));
    for jj = 1:size(A, 1)
        for kk = 1:size(A, 2)
            AA(kk,jj) = {A(jj,kk)};
        end
    end
    N1.coeffs = AA;
    N1 = plus(N1, N2);
    return

else
    C = zeros(max(size(A, 1), size(B, 1)), max(size(A, 2), size(B, 2)));
    if size(B, 1) < size(C, 1)
        B(size(B, 1)+1:size(C, 1),:) = 0;
    end
    if size(B, 2) < size(C, 2)
        B(:,size(B, 2)+1:size(C, 2)) = 0;
    end
    if size(A, 1) < size(C, 1)
        A(size(A, 1)+1:size(C, 1),:) = 0;
    end
    if size(A, 2) < size(C, 2)
        A(:,size(A, 2)+1:size(C, 2)) = 0;
    end
    C = A + B;
end

% Loop over to remove empty cells (replace with zeros).
for jj = 1:size(C, 1)
    for kk = 1:size(C, 2)
        if ( isempty(C{jj,kk}) )
            C{jj,kk} = 0;
        end
    end
end

% Do not add boundary conditions together. Ignore them.
if ( ~isempty(N1.lbc) || ~isempty(N1.rbc) ||...
        ~isempty(N1.ubc) || ~isempty(N1.dbc) )
    warning('CHEBFUN:CHEBOP2:plus:ignoredBCs', 'BCs were ignored.');
    N1.lbc =[];
    N1.rbc =[];
    N1.ubc =[];
    N1.dbc =[];
end
if ( ~isempty(N2.lbc) || ~isempty(N2.rbc) ||...
        ~isempty(N2.ubc) || ~isempty(N2.dbc) )
    warning('CHEBFUN:CHEBOP2:plus:ignoredBCs', 'BCs were ignored.');
    N2.lbc =[];
    N2.rbc =[];
    N2.ubc =[];
    N2.dbc =[];
end

% Assign to output:
N1.coeffs = C;
N1.xorder = max(N1.xorder, N2.xorder);
N1.yorder = max(N1.yorder, N2.yorder);
% Plus the ops together:
op1 = N1.op;
op2 = N2.op;
N1.op = @(u) op1(u) + op2(u);

end


% this script tests NGLDM features between CERR and pyradiomics on wavelet filtered image.
%
% RKP, 03/22/2018

%% Load image
gldmParamFileName = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing','tests_for_cerr','test_ngldm_radiomics_extraction_settings.json');
cerrFileName = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing','data_for_cerr_tests','CERR_plans','head_neck_ex1_20may03.mat.bz2');

planC = loadPlanC(cerrFileName,tempdir);
indexS = planC{end};

paramS = getRadiomicsParamTemplate(gldmParamFileName);
strNum = getMatchingIndex(paramS.structuresC{1},{planC{indexS.structures}.structureName});
scanNum = getStructureAssociatedScan(strNum,planC);

scanType = 'wavelet';
dirString = 'HHH';

%% NGLDM features CERR

ngldmS = calcGlobalRadiomicsFeatures...
            (scanNum, strNum, paramS, planC);
ngldmS = ngldmS.Wavelets_Coif1__HHH.ngldmFeatS;

cerrNgldmV = [ngldmS.lde, ngldmS.hde, ngldmS.lgce, ngldmS.hgce, ...
    ngldmS.ldlge, ngldmS.ldhge, ngldmS.hdlge, ngldmS.hdhge, ...
    ngldmS.gln, ngldmS.glnNorm, ngldmS.dcn, ngldmS.dcnNorm,...
    ngldmS.dcp, ngldmS.glv, ngldmS.dcv, ngldmS.entropy, ngldmS.energy];

% %% Calculate features using pyradiomics
%
% testM = single(planC{indexS.scan}(scanNum).scanArray) - ...
%     single(planC{indexS.scan}(scanNum).scanInfo(1).CTOffset);
% mask3M = zeros(size(testM),'logical');
% [rasterSegments, planC, isError] = getRasterSegments(strNum,planC);
% [maskBoundBox3M, uniqueSlices] = rasterToMask(rasterSegments, scanNum, planC);
% mask3M(:,:,uniqueSlices) = maskBoundBox3M;
%
% dx = planC{indexS.scan}(scanNum).scanInfo(1).grid1Units;
% dy = planC{indexS.scan}(scanNum).scanInfo(1).grid1Units;
% dz = mode(diff([planC{indexS.scan}(scanNum).scanInfo(:).zValue]));
% pixelSize = [dx dy dz]*10;
%
% teststruct = PyradWrapper(testM, mask3M, pixelSize, scanType, dirString);
% %teststruct = PyradWrapper(testM, mask3M, scanType, dirString);
%
% pyradNgldmNamC = {'SmallDependenceEmphasis', 'LargeDependenceEmphasis',...
%     'LowGrayLevelCountEmphasis', 'HighGrayLevelCountEmphasis',  'SmallDependenceLowGrayLevelEmphasis', ...
%     'SmallDependenceHighGrayLevelEmphasis', 'LargeDependenceLowGrayLevelEmphasis', ...
%     'LargeDependenceHighGrayLevelEmphasis', 'GrayLevelNonUniformity', 'GrayLevelNonUniformityNorm', ...
%     'DependenceNonUniformity', 'DependenceNonUniformityNormalized', ...
%     'DependencePercentage', 'GrayLevelVariance', 'DependenceVariance', ...
%     'DependenceEntropy', 'DependenceEnergy'};
%
%
% pyradNgldmNamC = strcat(['wavelet','_', dirString, '_gldm_'],pyradNgldmNamC);
%
% pyRadNgldmV = [];
% for i = 1:length(pyradNgldmNamC)
%     if isfield(teststruct,pyradNgldmNamC{i})
%         pyRadNgldmV(i) = teststruct.(pyradNgldmNamC{i});
%     else
%         pyRadNgldmV(i) = NaN;
%     end
% end
%
% %% Compare
% ngldmDiffV = (cerrNgldmV - pyRadNgldmV) ./ cerrNgldmV * 100

%% Compare with previously calculated pyradiomics NGLDM features
saved_pyRadNgldmV = [0.204701151831240,55.0338803599788,NaN,NaN,0.000365710992103713,196.983572569714,0.0594517626789526,51247.9161461091,1583.25410269984,NaN,817.939756484913,0.0866002918459410,NaN,11.2964611875247,18.6018880476280,6.32953752322967,NaN];
ngldmDiffV = (cerrNgldmV - saved_pyRadNgldmV) ./ cerrNgldmV * 100


function [TorF, vstr, rdate] = have_feature_ipopt()
%HAVE_FEATURE_IPOPT  Detect availability/version info for IPOPT
%
%   Feature detection function implementing 'ipopt' tag for HAVE_FEATURE
%   to detect availability/version of IPOPT, a nonlinear programming
%   solver from COIN-OR (https://github.com/coin-or/Ipopt).
%
%   See also HAVE_FEATURE, NLPS_MASTER, QPS_MASTER, IPOPT.

%   MP-Opt-Model
%   Copyright (c) 2004-2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MP-Opt-Model.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/mp-opt-model for more info.

TorF = exist('ipopt', 'file') == 3;
vstr = '';
rdate = '';
if TorF
    if have_feature('evalc')
        str = evalc('qps_ipopt([],[1; 1],[1 1],[2],[2],[1; 1],[1; 1],[1; 1],struct(''verbose'', 2))');
        pat = 'Ipopt version ([^\s,]+)';
        [s,e,tE,m,t] = regexp(str, pat);
        if ~isempty(t)
            vstr = t{1}{1};
            if vstr2num(vstr) >= 3.011 && ~exist('ipopt_auxdata', 'file')
                TorF = 0;
                warning('Improper installation of IPOPT. Version %s detected, but IPOPT_AUXDATA.M is missing.', vstr);
            end
        end
    else
        try
            x = feval('qps_ipopt', [],[1; 1],[1 1],[2],[2],[1; 1],[1; 1],[1; 1],struct('verbose', 0));
            if ~isequal(x, [1;1])
                TorF = 0;
            end
        catch
            TorF = 0;
        end
    end
end

function num = vstr2num(vstr)
% Converts version string to numerical value suitable for < or > comparisons
% E.g. '3.11.4' -->  3.011004
pat = '\.?(\d+)';
[s,e,tE,m,t] = regexp(vstr, pat);
b = 1;
num = 0;
for k = 1:length(t)
    num = num + b * str2num(t{k}{1});
    b = b / 1000;
end


function [IDX,C] = meanShiftPost( X, IDX, C, minCl, forceOutl )
% Some post processing routines for meanShift not currently being used.
%
% USAGE
%  [IDX,C] = meanShiftPost( X, IDX, C, minCl, forceOutl )
%
% INPUTS
%  see meanShift
%
% OUTPUTS
%  see meanShift
%
% EXAMPLE
%
% See also MEANSHIFT, KMEANS2
%
% Piotr's Image&Video Toolbox      Version 2.0
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

%%% force outliers to belong to IDX (mainly for visualization)
if( forceOutl )
  for i=find(IDX<0)'
    D = pdist2( X(i,:), C );
    [mind IDx] = min(D,[],2);
    IDX(i) = IDx;
  end
end;

%%% Delete smallest cluster, reassign points, re-sort, repeat...
k = max(IDX);
ticId = ticStatus('meanShiftPost',[],5); kinit = k;
while( 1 )
  % sort clusters [largest first]
  cnts = zeros(1,k); for i=1:k; cnts(i) = sum( IDX==i ); end
  [cnts,order] = sort( -cnts ); cnts = -cnts; C = C(order,:);
  IDX2 = IDX;  for i=1:k; IDX2(IDX==order(i))=i; end; IDX = IDX2;

  % stop if smallest cluster is big enough
  if( cnts(k)>= minCl ); break; end;

  % otherwise discard smallest [last] cluster
  C( end, : ) = [];
  for i=find(IDX==k)'
    D = pdist2( X(i,:), C );
    [mind IDx] = min(D,[],2);
    IDX(i) = IDx;
  end;
  k = k-1;

  tocStatus( ticId, (kinit-k)/kinit );
end
tocStatus( ticId, 1 );


function alist_generate

load H
% H = dvbs2ldpc(1/2);
% H = [0 0 1 0 0 0 0 1 0 1 0 0 1 0 0 0; ...
%  0 0 0 1 0 0 1 0 1 0 0 0 1 0 0 0; ...
%  0 1 0 0 1 0 1 0 0 1 0 0 0 0 0 0; ...
%  0 0 0 1 0 1 0 0 0 0 1 0 0 1 0 0; ...
%  0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 1; ...
%  1 0 0 0 0 1 0 0 1 1 0 0 0 0 0 0; ...
%  0 0 0 1 0 0 0 1 0 0 0 1 0 0 1 0; ...
%  0 1 0 0 0 1 0 0 0 0 0 1 0 0 0 1; ...
%  1 0 0 0 0 0 1 0 0 0 0 0 0 1 0 1; ...
%  0 0 1 0 1 0 0 0 0 0 0 1 0 1 0 0; ...
%  0 1 0 0 0 0 0 0 0 0 1 0 1 0 1 0; ...
%  1 0 0 0 1 0 0 1 0 0 1 0 0 0 0 0];

[N, M] = size(H);

fid = fopen('NR_ldpc.alist', 'w');
% fid = fopen('dvbs2_ldpc_12.alist', 'w');
fprintf(fid, '%d %d\n', N, M);

H = full(H);

row_non_zero_entry_number = sum(H, 2).';
column_non_zero_entry_number = sum(H, 1);

fprintf(fid, '%d %d\n', max(row_non_zero_entry_number), max(column_non_zero_entry_number));

for index = 1:N
  fprintf(fid, '%d ', row_non_zero_entry_number(index));
end

fprintf(fid, '\n');

for index = 1:M
  fprintf(fid, '%d ', column_non_zero_entry_number(index));
end

fprintf(fid, '\n');

for row_index = 1:N
  non_zero_entry_positions = find(H(row_index, :) ~= 0);

  for index = 1:max(row_non_zero_entry_number) % length(non_zero_entry_positions)
    if index <= length(non_zero_entry_positions)
      fprintf(fid, '%d ', non_zero_entry_positions(index));
    else
      fprintf(fid, '0 ');
    end
  end

  fprintf(fid, '\n');
end

for column_index = 1:M
  non_zero_entry_positions = find(H(:, column_index) ~= 0);

  for index = 1:max(column_non_zero_entry_number)
    if index <= length(non_zero_entry_positions)
      fprintf(fid, '%d ', non_zero_entry_positions(index));
    else
      fprintf(fid, '0 ');
    end
  end

  fprintf(fid, '\n');
end

fclose(fid);

end


function [ sx, varargout ] = cvx_get_dimension( args, darg, varargin )
opts = struct( 'zero', false, 'vec', false, 'nox', false );
if nargin > 2,
    for k = 1 : 2 : nargin - 2,
        opts.(varargin{k}) = varargin{k+1};
    end
end
narg = length( args );
if narg < 1,
    cvx_throw( 'Not enough input arguments.' );
end
if narg < darg,
    dim = [];
else
    dim = args{darg};
end
x = args{1};
if opts.nox,
    sx = x;
    nx = numel(sx);
    if ~( isnumeric(sx) && nx==length(sx) && isreal(sx) && all(sx>=0) && all(sx==floor(sx)) ),
        cvx_throw( 'Size argument must be a vector of nonnegative integers.' );
    elseif nx < 2,
        sx(end+1:2) = 1;
    elseif nx > 2 && sx(nx) == 1,
        sx = sx(max([2,find(sx~=1,1,'last')]));
    end
else
    sx = size( x );
end
if isempty( dim ),
    dim = find( sx ~= 1, 1, 'first' );
    if isempty( dim ), dim = 1; end
elseif ~( isnumeric(dim) && numel(dim)==1 && isreal(dim) && dim > -opts.zero && dim==floor(dim) ),
    cvx_throw( 'Dimension argument must be a positive integer.' );
end
if dim == 0,
    dim = find( sx == 1, 1, 'first' );
    if isempty( dim ), dim = length(sx) + 1; end
end
sx(end+1:dim) = 1;
args{darg} = dim;
if opts.vec,
    args([1,darg]) = [];
    if opts.nox,
        varargout = { dim, args };
    else
        varargout = { x, dim, args };
    end
else
    no = nargout - 1;
    if opts.nox, args(1) = []; end
    if no > length(args), args{no} = []; end
    varargout = args;
end


function net = cnn_initNet_pairwiseModel( networkFile, structuredLoss, numPairwiseClusters, extraLayers, meanImage )
%cnn_initNet_pairwiseModel initializes the pairwise model
%
% net = cnn_initNet_pairwiseModel( networkFile, structuredLoss, numPairwiseClusters, extraLayers )
%
% Input:
%   networkFile - file with the pretrained network (the dropout layers should already be inside)
%   structuredLoss - type of structure loss to use: 'svmStructCompact' or 'logisticScoresCompact'
%   numPairwiseClusters - number of clusters in the pairwise potentials
%   extraLayers - structure containing information about extra layers (default: [] - no extra layers). Fields:
%       unary - vector containing numbers of nodes in the extra layers for the unary potentials
%       pairwise - vector containing numbers of nodes in the extra layers for the pairwise potentials
%       useDropout - flag showing whether to use dropout in the extra layers
%       cutoffConvolutions - parameter to show if there is need to cut off more layers with weights from the pretrained net
%           One such layer is cut off anyway (connected to the classes from the pretraining task)
%   meanImage - if provided, sets the new mean image for the normalization
%
% Output:
%   net - structure containing the pairwise model

if ~exist('extraLayers', 'var') || isempty(extraLayers)
    extraLayers = struct;
    extraLayers.unary = [];
    extraLayers.pairwise = [];
    extraLayers.useDropout = true;
    extraLayers.cutoffConvolutions = 0;
end

% the initialization parameters
scal = 1 ;
init_bias = 0.1;
dropout_rate = 0.5;

%% read the network file
pretrainedNet = load( networkFile, '-mat' );
if ~isfield(pretrainedNet, 'layers')
    if ~isfield(pretrainedNet, 'net')
        error('cnn_initNet_pairwiseModel:incorrectInitialization', 'File to initialize the network is of incorrect format')
    else
        pretrainedNet = pretrainedNet.net;
    end
end
pretrainedNet.layers(end) = []; % cut off the loss layer
if ~exist('meanImage', 'var')
    meanImage = pretrainedNet.normalization.averageImage;
end

%% determine the number of output features
% find the last convolutional layer
lastConvolutionalLayer = length(pretrainedNet.layers);
cutoffConvolutions = 0;
while (~isequal( pretrainedNet.layers{ lastConvolutionalLayer }.type, 'conv' ) || cutoffConvolutions < extraLayers.cutoffConvolutions) ...
        && lastConvolutionalLayer > 1
    if isequal( pretrainedNet.layers{ lastConvolutionalLayer }.type, 'conv' )
        cutoffConvolutions = cutoffConvolutions + 1;
    end
    lastConvolutionalLayer = lastConvolutionalLayer - 1;
end
if isequal( pretrainedNet.layers{ lastConvolutionalLayer }.type, 'conv' )
    numFeatures = size( pretrainedNet.layers{ lastConvolutionalLayer }.weights{1}, 3 );
else
    error('cnn_initNet_pairwiseModel:wrongNetwork', 'Cannot determine the number of features from the pretrained network');
end

%% create the feature extractor network
net = struct;
net.featureExtractor = struct;
net.featureExtractor.layers = pretrainedNet.layers;
net.featureExtractor.layers(lastConvolutionalLayer : 1 : end) = []; % cut off the last layer

%% add the loss layer
net.lossLayer = struct;
net.lossLayer.type = structuredLoss;

numClasses = 2;
switch net.lossLayer.type
    case 'svmStructCompact'
        numUnaryOutputs = 1;
        numPairwiseOutputs = numPairwiseClusters;
    case 'logisticScoresCompact'
        numUnaryOutputs = 1;
        numPairwiseOutputs = numPairwiseClusters;
    otherwise
        error('cnn_initNet_pairwiseModel:unknownLoss',['The structured loss is not recognised: ', net.lossLayer.type]);
end

%% add the unary network
net.unaryNetwork = struct;
net.unaryNetwork.layers = cell(0, 0);

numExtraLayers = length(extraLayers.unary);
numNodesExtraLayersUnary = [ numFeatures; extraLayers.unary(:)];
for iLayer = 1 : numExtraLayers
    net.unaryNetwork.layers{end+1} = struct('type', 'conv', ...
        'weights', {{ 0.01/scal * randn(1, 1, numNodesExtraLayersUnary(iLayer), numNodesExtraLayersUnary(iLayer+1),'single'),... % filters
        init_bias*ones(1,numNodesExtraLayersUnary(iLayer+1),'single') }}, ... % biases
        'stride', 1, ...
        'pad', 0, ...
        'learningRate', [1, 2], ...
        'weightDecay', [1, 0], ...
        'name', ['unaryNetwork_conv', num2str(iLayer)]) ;
    net.unaryNetwork.layers{end+1} = struct('type', 'relu') ;
    if extraLayers.useDropout
        net.unaryNetwork.layers{end+1} = struct('type', 'dropout', 'rate', dropout_rate) ;
    end
end
net.unaryNetwork.layers{end+1} = struct('type', 'conv', ...
    'weights', {{ 0.01/scal * randn(1, 1, numNodesExtraLayersUnary(end), numUnaryOutputs,'single'),... % weights
    init_bias * ones(1, numUnaryOutputs, 'single') }}, ... % biases
    'stride', 1, ...
    'pad', 0, ...
    'learningRate', [1, 2], ...
    'weightDecay', [1, 0], ...
    'name', 'unaryNetwork_convLast') ;

%% add the pairwise network
net.pairwiseNetwork = struct;
net.pairwiseNetwork.layers = cell(0, 0);

numExtraLayers = length(extraLayers.pairwise);
numNodesExtraLayersPairwise = [ 2 * numFeatures; extraLayers.pairwise(:)];
for iLayer = 1 : numExtraLayers
    net.pairwiseNetwork.layers{end+1} = struct('type', 'conv', ...
        'weights', {{ 0.01/scal * randn(1, 1, numNodesExtraLayersPairwise(iLayer), numNodesExtraLayersPairwise(iLayer+1),'single'), ... % filters
        init_bias*ones(1,numNodesExtraLayersPairwise(iLayer+1),'single') }}, ... % biases
        'stride', 1, ...
        'pad', 0, ...
        'learningRate', [1, 2], ...
        'weightDecay', [1, 0], ...
        'name', ['pairwiseNetwork_conv', num2str(iLayer)]) ;
    net.pairwiseNetwork.layers{end+1} = struct('type', 'relu') ;
    if extraLayers.useDropout
        net.pairwiseNetwork.layers{end+1} = struct('type', 'dropout', 'rate', dropout_rate) ;
    end
end
net.pairwiseNetwork.layers{end+1} = struct('type', 'conv', ...
    'weights', {{ 0.01/scal * randn(1, 1, numNodesExtraLayersPairwise(end), numPairwiseOutputs,'single'), ... % filters
    init_bias * ones(1, numPairwiseOutputs, 'single') }}, ... % biases
    'stride', 1, ...
    'pad', 0, ...
    'learningRate', [1, 2], ...
    'weightDecay', [1, 0], ...
    'name', 'pairwiseNetwork_convLast') ;

%% collect all trainable layers
net.layers = cell(0,0);

for iLayer = 1 : length(net.featureExtractor.layers)
    if isequal( net.featureExtractor.layers{iLayer}.type, 'conv' )
        net.layers{end+1} = net.featureExtractor.layers{iLayer};
        net.featureExtractor.layers{iLayer} = struct( 'type', 'convPtr', 'index', length(net.layers) );
    end
end
for iLayer = 1 : length(net.unaryNetwork.layers)
    if isequal( net.unaryNetwork.layers{iLayer}.type, 'conv' )
        net.layers{end+1} = net.unaryNetwork.layers{iLayer};
        net.unaryNetwork.layers{iLayer} = struct( 'type', 'convPtr', 'index', length(net.layers) );
    end
end
for iLayer = 1 : length(net.pairwiseNetwork.layers)
    if isequal( net.pairwiseNetwork.layers{iLayer}.type, 'conv' )
        net.layers{end+1} = net.pairwiseNetwork.layers{iLayer};
        net.pairwiseNetwork.layers{iLayer} = struct( 'type', 'convPtr', 'index', length(net.layers) );
    end
end

%% mean image normalization
% set the new mean image
net.normalization.averageImage = meanImage;

%% class order
net.classes = struct;
net.classes.name = {'head', 'background'};
net.classes.description = {'head', 'background'};

end


function r = Tsqrt(x)

%function r = Tsqrt(x)
%
%The same as CHOL, only
%R*R' = X;
%instead of
%R'*R = X.
%
%See also CHOL.

r = chol(x)';


classdef NaiveClustererX < ClustererX
% NAIVECLUSTERERX Class
%
% Summary of NaiveClustererX:
% This is a class implementation of a naive clusterer that generates clusters
% of tracks sharing common measurements.
%
% NaiveClustererX Properties:
%   + NumMeasDims - The number of observation dimensions.
%
% NaiveClustererX Methods:
%   + NaiveClustererX  - Constructor method
%   + cluster - Perform clustering and generate a list of clusters
%
% (+) denotes public properties/methods
%
% See also SystematicResamplerX
    properties
    end

    properties (SetAccess = immutable)
        NumMeasDims
    end

    methods
        function this = NaiveClustererX(varargin)
        % ELLIPSOIDALGATERX Constructor method
        %
        % Usage
        % -----
        % * nv = NaiveClustererX() returns a NaiveClustererX object
        %
        % See also NaiveClustererX/cluster

        end

        function [ClusterList,UnassocTrackInds] = cluster(this,ValidationMatrix)
        % CLUSTER Perform naive clustering to generate clusters of tracks
        % sharing common measurements.
        %
        % Parameters
        % ----------
        % ValidationMatrix: matrix
        %   A (Nt x Nm) validation matrix, Nt being the number of tracks and
        %   Nm being the number of measurements, where each element (t,m) is
        %   a binary variable (0 or 1), representing whether measurement m
        %   fell in the gate of target t.
        %
        % Returns
        % -------
        % ClusterList: cell vector
        %   A (1 x Nc) cell vector, where each cell represents one of Nc Cluster
        %   objects, with the following fields:
        %       - MeasIndList: A list of the indices of all measurements
        %                     contained within the cluster.
        %       - TrackIndList: A list of indices of all tracks belonging
        %                       to the cluster.
        %
        % UnassocTrackInds: column vector
        %  A (1 x Nu) column vector, where each element contains the index
        %  (as ordered in ValidationMatrix) of any tracks that have not
        %  been associated to any measurements. As such, 0<= Nu <= Nt.
        %
        % Usage
        % -----
        % * [ClusterList,UnassocTracks] = cluster(this,ValidationMatrix)
        %   returns a list of clusters ClusterList and a list of unassociated
        %   track indices UnassocTracks (corresponding to the row indices of
        %   ValidationMatrix).
        %   ClusterList is a list of Cluster objects, where each cluster
        %   object has two properties:
        %       - MeasIndList: A list of the indices of all measurements
        %                     contained within the cluster.
        %       - TrackIndList: A list of indices of all tracks belonging
        %                       to the cluster.
        %
        % See also NaiveClustererX/NaiveClustererX

            % Initiate parameters
            NumTracks = size(ValidationMatrix,1); % Number of measurements

            % Form clusters of tracks sharing measurements
            UnassocTrackInds = [];
            ClusterList = [];
            ClusterObj.MeasIndList = [];
            ClusterObj.TrackIndList = [];

            % Iterate over all tracks
            for trackInd=1:NumTracks
                % Extract valid measurement indices
                validMeasInds = find(ValidationMatrix(trackInd,:));

                % If there exist valid measurements
                if (~isempty(validMeasInds))

                    % Check if matched measurements are members of any clusters
                    NumClusters = numel(ClusterList);
                    matchedClusterIndFlags = zeros(1, NumClusters);
                    for ClusterInd=1:NumClusters
                        if (sum(ismember(validMeasInds, ClusterList(ClusterInd).MeasIndList)))
                            matchedClusterIndFlags(ClusterInd) = 1; % Store matched cluster ids
                        end
                    end

                    NumMatchedClusters = sum(matchedClusterIndFlags);
                    matchedClusterInds = find(matchedClusterIndFlags);

                    % If only matched with a single cluster, join.
                    switch(NumMatchedClusters)
                        case(1)
                            ClusterList(matchedClusterInds).TrackIndList = union(ClusterList(matchedClusterInds).TrackIndList, trackInd);
                            ClusterList(matchedClusterInds).MeasIndList = union(ClusterList(matchedClusterInds).MeasIndList, validMeasInds);
                        case(0)
                            ClusterList(end+1).TrackIndList = trackInd;
                            ClusterList(end).MeasIndList = validMeasInds;
                            %ClusterList(end+1) = ClusterObj;
                        otherwise
                            % Start from last cluster, joining each one with the previous
                            %   and removing the former.
                            for matchedClusterInd = NumMatchedClusters-1:-1:1
                                ClusterList(matchedClusterInds(matchedClusterInd)).TrackIndList = ...
                                    union(ClusterList(matchedClusterInds(matchedClusterInd)).TrackIndList, ...
                                        ClusterList(matchedClusterInds(matchedClusterInd+1)).TrackIndList);
                                ClusterList(matchedClusterInds(matchedClusterInd)).MeasIndList = ...
                                    union(ClusterList(matchedClusterInds(matchedClusterInd)).MeasIndList, ...
                                        ClusterList(matchedClusterInds(matchedClusterInd+1)).MeasIndList);
                                ClusterList(matchedClusterInds(matchedClusterInd+1)) = [];
                            end
                            % Finally, join with associated track.
                            ClusterList(matchedClusterInds(matchedClusterInd)).TrackIndList = ...
                                union(ClusterList(matchedClusterInds(matchedClusterInd)).TrackIndList, trackInd);
                            ClusterList(matchedClusterInds(matchedClusterInd)).MeasIndList = ...
                                union(ClusterList(matchedClusterInds(matchedClusterInd)).MeasIndList, validMeasInds);
                    end
                else
                    ClusterList(end+1).TrackIndList = trackInd;
                    ClusterList(end).MeasIndList = [];
                    %ClusterList(end+1) = ClusterObj;
                    UnassocTrackInds = [UnassocTrackInds trackInd];
                end
                this.ClusterList = ClusterList;
                this.UnassocTrackInds = UnassocTrackInds;
            end
        end
    end
end


function [listDuplicates] = check4DuplicatesInList(list)
% This function checks for duplicate entries in a list.
%
% INPUT
% list              List of e.g. metabolite abbr
%
% OUTPUT
% listDuplicates    List of duplicated entries. Second (or more) occurance
%                   of the duplicate is provided.
%
% Ines Thiele, 09/2021

listDuplicates = [];
cnt = 1;
[D,IA,ID]= duplicates((list));
Di = find(IA);
if isempty(Di)
    fprintf('No duplicate metabolites exists.\n')
else
    for i = 1 : length(Di)
        fprintf([list{Di(i)} ' appears more than once.\n'])
        listDuplicates{cnt,1} = list{Di(i)};
        listDuplicates{cnt,2} = num2str(Di(i));
        cnt = cnt +1;
    end
end


function best_acc = LRSDL_top(dataset, N_train, k, k0, lambda1, lambda2, lambda3)
% function LRSDL_FDDL_top(dataset, N_train, k, k0, lambda1, lambda2, lambda3)

%% Dependencies
    addpath('utils');
    addpath('LRSDL_FDDL');
    addpath('ODL');
    %% test mode
    if nargin == 0
        dataset = 'myYaleB';
        N_train = 10;
        k = 8;
        k0 = 5;
        lambda1 = 0.001;
        lambda2 = 0.01;
        lambda3 = 0.02;
    end
    if ~exist('lambda3', 'var')
        lambda3 = 0;
    end
    %%
    t = getTimeStr();
    [dataset, Y_train, Y_test, label_train, label_test] = ...
        train_test_split(dataset, N_train);
    %% Parameter preparation
    fprintf('starting... %s\n', dataset)    ;
    [acc, rt] = LRSDL_wrapper(Y_train, label_train, Y_test , label_test, ...
                            k, k0, lambda1, lambda2, lambda3);
    fprintf('rt = %5.1f\n', rt);
    %% output filename
    if ~exist('results', 'dir')
        mkdir('results');
    end
    if k0 > 0
        if ~exist(fullfile('results', 'LRSDL'), 'dir')
            mkdir('results', 'LRSDL');
        end
        fn = fullfile('results', 'LRSDL', strcat(dataset, ...
            '_N_', num2str(N_train), '_k_', num2str(k), ...
            '_k0_',num2str(k0),'_l1_', num2str(lambda1), ...
            '_l2_', num2str(lambda2), '_l3_', num2str(lambda3), ...
            '_', t, '.mat'));
    else
        if ~exist(fullfile('results', 'FDDL'), 'dir')
            mkdir('results', 'FDDL');
        end
        fn = fullfile('results', 'FDDL', strcat(dataset, ...
            '_N_', num2str(N_train), '_k_', num2str(k),'_l1_', ...
            num2str(lambda1), '_l2_', num2str(lambda2),'_', t, '.mat'));
    end
    disp(fn);
    save(fn, 'acc', 'rt');
    best_acc = max(acc);
end


function y=pulse(t,f,w,Ein)
% unit pulse; starts at f ends at w; pulsewidth = w - f
% amplitude when on = Ein
if t < f | t > w
   y=0;
else
   y=Ein;
end


function ph = montage_clusters_points(ovl,clusters,XYZpts,varargin)
% ::
%
%    ph = montage_clusters_points(ovl,clusters,XYZpts,varargin)
%
% :Inputs:
%
%   **varargin:**
%        additional clusters structures
%
%   **XYZpts:**
%        XYZ mm coordinates of points to plot
%
%   **ph:**
%        point handles
%
% ..
%    Tor Wager
% ..

if isempty(ovl), ovl = which('scalped_single_subj_T1.img');, end
myc = {'b' 'r' 'y' 'g' 'm' 'c'};

if size(XYZpts,1) ~= 3, XYZpts = XYZpts';,end

XYZmm = cat(2,clusters.XYZmm);
XYZmm = [XYZmm XYZpts];

if length(varargin) > 0
    for i = 1:length(varargin)
        cl = varargin{i};
        clXYZmm = cat(2,cl.XYZmm);
        XYZmm = [XYZmm clXYZmm];
    end
end

% ----------------------------------------
% * overlay image
% ----------------------------------------
V = spm_vol(ovl);
oimg = spm_read_vols(V);
V.M = V.mat;

textx = size(oimg,2) - 50;
texty = size(oimg,1) - 6;

%[array,hdr,h,whichslices,rows,cols,figh] = readim2(ovl,'p');

% how many slices, which ones

XYZ = mm2voxel(XYZmm,V)';
whsl = unique(XYZ(3,:));
nsl = length(whsl) + 1;
rc = ceil(sqrt(nsl));
h = [];

figure; colormap gray; set(gcf,'Color','w')

% plot first cluster structure

index = plot_cluster(clusters,oimg,rc,V,whsl,myc{1},textx,texty,1,size(oimg));
ph = plot_points(XYZpts,rc,V,whsl,myc{2});

% plot additional cluster structures

if length(varargin) > 0
    for i = 1:length(varargin)
        cl = varargin{i};
        index = plot_cluster(cl,[],rc,V,whsl,myc{i+1},textx,texty,i+1,size(oimg));

    end
end

return

% sub-functions
%

function index = plot_cluster(clusters,oimg,rc,V,whsl,myc,textx,texty,clind,odims)

XYZmm = cat(2,clusters.XYZmm);
XYZ = mm2voxel(XYZmm,V)';

% surface patch method
% ----------------------------------------------------------------------------------------
vol = voxel2mask(XYZ',odims);
vol = smooth3(vol);

index = 1;
for z = whsl

    subplot(rc,rc,index);

    if ~isempty(oimg)
        set(gca,'YDir','reverse');
        imagesc(oimg(:,:,z))
        hold on; axis image; axis off
        zmm = voxel2mm([1 1 z]',V.mat);
        text(textx,texty,['z = ' num2str(zmm(3))],'Color','w')
    else
        hold on
    end

    if z>1,FVC = isocaps(vol(:,:,z-1:z),0,'zmax');
    else FVC = isocaps(vol(:,:,z:z+1),0,'zmax');
    end
    try
	patch(FVC,'EdgeColor','none','FaceColor',myc,'FaceAlpha',.7)
    catch
	patch(FVC,'EdgeColor','none','FaceColor',myc)
    end

    % plot method

    %myXYZ = XYZ(1:2,XYZ(3,:) == z);
    %plot(myXYZ(2,:),myXYZ(1,:),[myc 's'],'MarkerFaceColor',myc,'MarkerSize',3)

    index = index + 1;
    drawnow

end

subplot(rc,rc,index)
a = pwd; a = a(end-6:end);
b = num2str(clusters(1).threshold);

c = num2str(length(clusters));
text(0,clind-1,[myc ': ' clusters(1).title ' ' a ' u = ' b ', ' c ' clusters'])
axis off
axis([0 1 -1 clind])

return

function ph = plot_points(XYZmm,rc,V,whsl,myc)

XYZ = mm2voxel(XYZmm,V)';
index = 1;
phind = 1;
for z = whsl

    subplot(rc,rc,index);
    hold on
    myXYZ = XYZ(:,XYZ(3,:) == z);

    for i = 1:size(myXYZ,2)
        ph(phind) = plot3(myXYZ(2,i),myXYZ(1,i),myXYZ(3,i),[myc(1) '.'],'MarkerFaceColor',myc(1),'MarkerSize',8);
        phind = phind + 1;
    end

    index = index + 1;

end

return


function nlp = imposeNLPConstraint(obj, nlp, ep, nzy, load_path)
    % impose virtual constraints as NLP constraints in the trajectory
    % optimization problem 'nlp' of the dynamical system
    %
    % For each (vector) virtual constraints, we will enforce the following
    % constraints at the first node:
    % {y, y', ..., y^N} % where N = RelativeDegree - 1
    % and this constraint:
    % y^(N+1) + ep_N y^N + ... ep_1 y = 0
    % at all nodes.
    %
    % Parameters:
    % nlp: the trajectory optimization NLP @type TrajectoryOptimization
    % ep: the coefficients of the derivatives @type rowvec
    % nzy: the derivatives that does not requires to be zero at the first
    % node @type rowvec
    %
    % @note For example, ep = [kp, kd] for typical vector relative degree 2
    % holonomic virtual constraints.
    %
    % @note For example, nzy = [0] for a relative degree 1 nonholonomic
    % virtual constraint (velocity outputs) means we do not require y to
    % be zero at the first node. Another example, nzy = [0,0] for a
    % relative degree 2 virtual constraints, we do not require both y and
    % y' to be zero at the first node, on the other hand, nzy = [1,0] for a
    % relative degree 2 virtual constraints, we only enforce y being zero
    % at the first node.

    if nargin < 5
        load_path = [];
    end

    % local variables for speed
    rel_deg = obj.RelativeDegree;
    is_holonomic = obj.Holonomic;
    is_state_based = strcmp(obj.PhaseType, 'StateBased');
    model = obj.Model;
    ya = obj.ActualFuncs;
    yd = obj.DesiredFuncs;

    assert(~isempty(ya) || ~isempty(yd),...
        'The virtual constraint functions are not compiled yet. Please run compile method first.');

    %% validate input arguments
    validateattributes(nlp, {'NonlinearProgram'},...
        {'scalar'},'VirtualConstraint.imposeNLPConstraint',...
        'nlp');
    nlpOptions = nlp.Options;
    n_node     = nlp.NumNode;
    %%
    if strcmp(nlpOptions.CollocationScheme,'PseudoSpectral')
        error('The PseudoSpectral is incompatible with virtual constraints.');
    end

    if nargin > 2
        validateattributes(ep, {'double'},...
            {'vector','numel',rel_deg,'positive','real'},...
            'VirtualConstraint.imposeNLPConstraint','ep');
    else
        warning('The coefficient vector (ep) of outputs are not defined. Use 1 for all derivatives.');
        ep = ones(1,rel_deg);
        for l= 1:rel_deg
            ep(l) = nchoosek(rel_deg,l-1)*10^(rel_deg - l + 1);
        end
    end
    if nargin > 3
        validateattributes(nzy, {'double'},...
            {'vector','numel',rel_deg,'binary'},...
            'VirtualConstraint.imposeNLPConstraint','nzy');
    else
        nzy = ones(1,rel_deg);

    end

    % the name suffix of functions
    name = [obj.Name '_' model.Name];

    % desired output parameters and its name in the var table of the NLP
    if isa(obj.OutputParams,'SymVariable')
        a_var = {SymVariable(tomatrix(obj.OutputParams(:)))};
        a_name = obj.OutputParamName;
    else
        a_var = {};
        a_name = {};
    end

    % phase variable parameters and its name in the var table of the NLP
    if isa(obj.PhaseParams,'SymVariable')
        p_var = {SymVariable(tomatrix(obj.PhaseParams(:)))};
        p_name = obj.PhaseParamName;
    else
        p_var = {};
        p_name = {};
    end

    % offset variable parameters and its name in the var table of the NLP
    if isa(obj.OffsetParams,'SymVariable')
        c_var = {SymVariable(tomatrix(obj.OffsetParams(:)))};
        c_name = obj.OffsetParamName;
    else
        c_var = {};
        c_name = {};
    end

    % states and their name representation in the var table of the NLP
    switch model.Type
        case 'FirstOrder'
            x_var = {model.States.x};
            dx_var = {model.States.dx};
            x_name = {'x'};
            dx_name = {'dx'};

            dX = model.States.dx;
        case 'SecondOrder'
            x_var = {model.States.x, model.States.dx};
            dx_var = {model.States.ddx};
            x_name = {'x','dx'};
            dx_name = {'ddx'};

            dX = [model.States.dx; model.States.ddx];
    end

    if is_holonomic
        q_var = {model.States.x};
        q_name = {'x'};
    else
        q_var = {model.States.x,model.States.dx};
        q_name = {'x','dx'};
    end

    % if the desired outputs are time-based, get the function for
    % converting the horizon time T to the actual node time t
    if is_state_based
        t_var = {};
        t_name = {};
        aux_var = {};
    else
        t = SymVariable('t');
        k = SymVariable('k');
        T  = SymVariable('t',[2,1]);
        nNode = SymVariable('nNode');
        tsubs = T(1) + ((k-1)./(nNode-1)).*(T(2)-T(1));

        if ~isnan(nlpOptions.ConstantTimeHorizon)
            t_var = {};
            t_name = {};
            aux_var = {T,k,nNode};
        else
            t_var = {T};
            t_name = 'T';
            aux_var = {k,nNode};
        end
    end

    y_fun = cell(rel_deg+1,1);

    %% y(x,a,p) = 0
    if nzy(1)==1
        if isempty(load_path)
            y = ya{1} - yd{1};
            if ~is_state_based
                %% Time-based outputs, need to incoorporates the time variable
                y = subs(y,t,tsubs);
            end

            % holonomic virtual constraints
            y_fun{1} = SymFunction(['y_' name], y, [t_var, q_var, a_var, p_var, c_var], aux_var);
        else
            % holonomic virtual constraints
            y_fun{1} = SymFunction(['y_' name], [], [t_var, q_var, a_var, p_var, c_var], aux_var);
            y_fun{1} = load(y_fun{1},load_path);
        end
        if ~isempty(aux_var)
            switch length(aux_var)
                case 2
                    % k, nNode
                    aux_data = {1,n_node};

                case 3
                    % T, k, nNode
                    aux_data = {nlpOptions.ConstantTimeHorizon, 1,n_node};

            end
        else
            aux_data = [];
        end
        % add constraint at the first node
        nlp = addNodeConstraint(nlp, y_fun{1}, [t_name, q_name ,a_name, p_name, c_name], 'first',...
            0, 0, 'Nonlinear', aux_data);

    end

    %% higher order derivatives
    if rel_deg > 1

        for i=2:rel_deg
            if nzy(i) == 1
                %% state-based output, no need to use time variable
                if isempty(load_path)
                    dy = ya{i} - yd{i};
                    if ~is_state_based
                        %% Time-based outputs, need to incoorporates the time variable
                        dy = subs(dy,t,tsubs);
                    end
                    y_fun{i} = SymFunction(['d' num2str(i-1) 'y_' name], dy, [t_var, x_var, a_var, p_var], aux_var);
                else
                    y_fun{i} = SymFunction(['d' num2str(i-1) 'y_' name], [], [t_var, x_var, a_var, p_var], aux_var);
                    y_fun{i} = load(y_fun{i}, load_path);
                end
                % add constraint at the first node

                nlp = addNodeConstraint(nlp, y_fun{i}, [t_name, x_name ,a_name, p_name], 'first',...
                    0, 0, 'Nonlinear', aux_data);
            end
        end
    end

    %% the highest order derivatives imposed at all nodes (feedback linearization)
    node_list = 1:1:n_node;
    dim = obj.Dimension;
    vars   = nlp.OptVarTable;
    ceq_err_bound = nlpOptions.EqualityConstraintBoundary;

    y_dynamics(1,n_node) = NlpFunction();

    if isempty(load_path)
        % state-based output, no need to use time variable
        if is_state_based
            ddy = (ya{rel_deg+1} - yd{rel_deg+1})*dX;
        else
            ddy = ya{rel_deg+1}*dX - yd{rel_deg+1};
        end

        %         ep_s = SymVariable('k',[rel_deg,1]);

        %         for j=1:1:rel_deg
        %             ddy = ddy + ep_s(j)*(ya{j} - yd{j});
        %         end
        % Time-based outputs, need to incoorporates the time variable
        if ~is_state_based
            ddy = subs(ddy,t,tsubs);
        end

        %         ddy_fun = SymFunction(['d' num2str(rel_deg) 'y_' name], ddy, [t_var, x_var, dx_var, a_var, p_var, c_var], [aux_var,{ep_s}]);
        ddy_fun = SymFunction(['d' num2str(rel_deg) 'y_' name], ddy, [t_var, x_var, dx_var, a_var, p_var, c_var], [aux_var]);
    else
        %         ep_s = SymVariable('k',[rel_deg,1]);

        %         ddy_fun = SymFunction(['d' num2str(rel_deg) 'y_' name], [], [t_var, x_var, dx_var, a_var, p_var, c_var], [aux_var,{ep_s}]);
        ddy_fun = SymFunction(['d' num2str(rel_deg) 'y_' name], [], [t_var, x_var, dx_var, a_var, p_var, c_var], [aux_var]);
        ddy_fun = load(ddy_fun, load_path);

    end

    for i=node_list
        idx = node_list(i);
        if nlpOptions.DistributeParameters
            param_index = idx;
        else
            param_index = 1;
        end

        if ~isempty(a_name)
            a_deps = vars.(a_name)(param_index);
        else
            a_deps = {};
        end
        if ~isempty(p_name)
            p_deps = vars.(p_name)(param_index);
        else
            p_deps = {};
        end
        if ~isempty(c_name)
            c_deps = vars.(c_name)(param_index);
        else
            c_deps = {};
        end

        if nlpOptions.DistributeTimeVariable
            time_index = idx;
        else
            time_index = 1;
        end

        if ~isempty(t_name)
            t_deps = vars.(t_name)(time_index);
        else
            t_deps = {};
        end
        if ~isempty(aux_var)
            switch length(aux_var)
                case 2
                    % k, nNode
                    aux_data = {idx,n_node};

                case 3
                    % T, k, nNode
                    aux_data = {nlpOptions.ConstantTimeHorizon, idx,n_node};

            end
        else
            aux_data = [];
        end
        x_deps = cellfun(@(x)vars.(x)(idx),x_name,'UniformOutput',false);
        dx_deps = cellfun(@(x)vars.(x)(idx),dx_name,'UniformOutput',false);

        y_dynamics(i) = NlpFunction('Name',[obj.Name '_output_dynamics'],...
            'Dimension',dim,'SymFun',ddy_fun,'lb',-ceq_err_bound,...
            'ub',ceq_err_bound,'Type','Nonlinear',...
            'DepVariables',[t_deps, x_deps{:}, dx_deps{:}, a_deps, p_deps, c_deps]',...
            'AuxData', {aux_data});
        %             'AuxData', {[aux_data,{ep}]});

    end

    obj.OutputFuncs = [y_fun;{ddy_fun}];
    %     obj.OutputFuncsName_ = cellfun(@(f)f.Name, obj.OutputFuncs,'UniformOutput',false);

    % add output dynamics at all nodes
    % add dynamical equation constraints
    nlp = addConstraint(nlp,[obj.Name '_output_dynamics'],'all',y_dynamics);

end


function dtiFiberGroupMniTransform(data_directory, fgfilename)

%Input is a file with a fiber group
%can be either .mat or .pdb file
%It is assumed that is is located in dtidir/fibers
%Output is the same fiber structure tranformed into MNI space.

%ER 04/2008
[pathstr, name, ext, versn] = fileparts(fgfilename);

    dt6File=fullfile(data_directory, 'dt6.mat');
    [sn, def]=dtiComputeDtToMNItransform(dt6File);

    filename=fullfile(data_directory, 'fibers', fgfilename);

    if strcmp(ext, '.pdb')
    [fg,filename] = mtrImportFibers(filename, eye(4));
    elseif strcmp(ext, '.mat')
        load(filename);
    else
display('Only .mat and .pdb types are supported for fgfilename');
    end

    fg = dtiXformFiberCoords(fg, def); %fg_sn = dtiXformFiberCoords(fg, def);

    coordinateSpace='MNI'; versionNum=1;
    newfilename=[filename(1:end-4) 'MNI'];
    save(newfilename, 'fg', 'coordinateSpace', 'versionNum');


% Matlab-Script: seisshift.m
% Script to evaluate increase and decrease of seismicity
% Author: J. Woessner
% woessner@seismo.ifg.ethz.ch
% last update: 04.09.02

% Track changes:
% 04.09.02 : Changed fcumulsum to calc_cumulsum

report_this_filefun(mfilename('fullpath'));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% END Header %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% Load data file %%%%%%%%
%%  Data is stored in catalog matrix a in the ZMAP form
%%% lon lat year month day mag depth hour min
mCatalog = ga; % Catalog initialazation
fSplitTime = mean(mCatalog(:,3));
fLastDate = max(mCatalog(:,3));
fFirstDate = min(mCatalog(:,3));

% [mCatalog1, mCatalog2, fPeriodExact1, fPeriodExact2, fPeriod, fPeriod2] = ...
%     ex_SplitCatalog(mCatalog, fSplitTime, 0, 0, 0, 0);
[mCatalog1, mCatalog2, fPeriodExact1, fPeriodExact2, fPeriod, fPeriod2] = ...
     ex_SplitCatalog(ga, 1984.7, 1, 1, 1,1);
maxmag = max(mCatalog1(:,6))
maxmag2 = max(mCatalog2(:,6))

[ev_val mags ev_valsum ev_valsum_rev,  mags_rev] =calc_cumulsum(mCatalog1);
[ev_val2 mags2 ev_valsum2 ev_valsum_rev2,  mags_rev2] =calc_cumulsum(mCatalog2);

% Figures
if exist('h_mag_fig','var') & ishandle(h_mag_fig)
    set(0,'Currentfigure',h_mag_fig);
    disp('Figure exists');
else
    h_mag_fig=figure_w_normalized_uicontrolunits('tag','ev_hist','Name','Magnitude histogram','Units','normalized','Nextplot','add',...
        'Numbertitle','off');
    h_mag_axs=axes('tag','ax_ev_hist','Nextplot','add','box','on');
end

xmag=max([maxmag maxmag2]); % Define length of x-axis

% Figure: Magnitude historgrams

subplot(2,1,1);
set(gca,'tag','ax_ev_hist1','Nextplot','replace','box','on');
axs1=findobj('tag','ax_ev_hist1');
axes(axs1(1));
histogram(mCatalog1(:,6),mags)
axis([0 xmag 0 max(ev_val)]);
ylabel('Events');
subplot(2,1,2);
set(gca,'tag','ax_ev_hist2','Nextplot','replace','box','on');
axs2=findobj('tag','ax_ev_hist2');
axes(axs2(1));
histogram(mCatalog2(:,6),mags2);
fig_hd1 = findobj(gca,'Type','patch');
set(fig_hd1,'FaceColor',[0 0.5 0])
axis([0 xmag 0 max(ev_val2)]);
ylabel('Events');
xlabel('Magnitude')

if exist('cum_mag_fig','var') &  ishandle(cum_mag_fig)
   set(0,'Currentfigure',cum_mag_fig);
   disp('Figure exists');
else
    cum_mag_fig=figure_w_normalized_uicontrolunits('tag','cumFMD','Name','Cumulative FMD','Units','normalized','Nextplot','add','Numbertitle','off');
    cum_mag_axs=axes('tag','ax_cumFMD','Nextplot','add','box','on');
end
% Figure: Bar and cumulative sums

subplot(2,1,1);
set(gca,'tag','ax_cumFMD1','Nextplot','replace','box','on');
axs3=findobj('tag','ax_cumFMD1')
axes(axs3(1));
plot(mags,ev_val,'-o',mags2,ev_val2,'-*')
ylabel('Non-cumulative number of events ');
subplot(2,1,2);
set(gca,'tag','ax_cumFMD2','Nextplot','replace','box','on','Yscale','log');
axs4=findobj('tag','ax_cumFMD2')
axes(axs4(1));
semilogy(mags,ev_valsum,'-o','Color',[0 0 1])
hold on;
semilogy(mags_rev,ev_valsum_rev,'-o','Color',[0 0 1])
semilogy(mags2,ev_valsum2,'-*','Color',[0 0.5 0])
semilogy(mags_rev2,ev_valsum_rev2,'-*','Color',[0 0.5 0])
ylabel('Cumulative sum');
xlabel('Magnitude')
hold off;

%% Difference illustrations
[params] = calc_totdiff(mCatalog, fSplitTime);

params.dNdiffsum
params.dNdiffsumYearVal
params.dNdiffsumMonthVal

if exist('hDiff_fig','var') &  ishandle(hDiff_fig)
   set(0,'Currentfigure',hDiff_fig);
   disp('Figure exists');
else
    hDiff_fig=figure_w_normalized_uicontrolunits('tag','hDiff','Name','Normalized differences','Units','normalized','Nextplot','add','Numbertitle','off');
    hDiff_axs=axes('tag','ax_hDiff','Nextplot','add','box','on');
end

subplot(3,1,1);
set(gca,'tag','ax_hDiff1','Nextplot','replace','box','on');
axs5=findobj('tag','ax_hDiff1')
axes(axs5(1));
bar(params.dMags,params.dNdiff);
ylabel('Seismicity difference');

subplot(3,1,2);
set(gca,'tag','ax_hDiff2','Nextplot','replace','box','on');
axs6=findobj('tag','ax_hDiff2')
axes(axs6(1));
bar(params.dMags,params.dNdiffYear)
ylabel(' Normalized per year');

subplot(3,1,3);
set(gca,'tag','ax_hDiff3','Nextplot','replace','box','on');
axs7=findobj('tag','ax_hDiff3')
axes(axs7(1));
bar(params.dMags,params.dNdiffMonth)
ylabel(' Normalized per month');


function [V,net,genFlag] = TrainGrowingGasNet(V,temp1,net,scale,params,Problem,wholeObj,genFlag,zmin)

%--------------------------------------------------------------------------
% 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".
%--------------------------------------------------------------------------

% This function is written by Qiqi Liu

    %% Parameters
    N = params.N;
    MaxIt = params.MaxIt;
    L = params.L;
    epsilon_b = params.epsilon_b;
    epsilon_n = params.epsilon_n;
    alpha = params.alpha;
    delta = params.delta;
    T = params.T;

    t = net.t;
    w = net.w;
    E = net.E;
    C = net.C;
    nx = net.nx;
    ageSumBefore = net.ageSumBefore;
    flag = net.flag;

    % select the corner solutions using GNG
    output=[];
    fm =[];
    C = net.C;
    w= net.w;
    for i = 1:size(w,1)
        neighbor = find(C(i,:)==1);

        ageSum(i,:) = sum(t(i,neighbor,:),2);
        if ageSum(i,:) == ageSumBefore(i,:)
            flag(i,:) = flag(i,:) + 1;
        end

    end

    cw = w(output,:);
    d = pdist2(cw,temp1);

    ageSumBefore = ageSum;
    maxN = 1.5;

    gen    = ceil(Problem.FE/Problem.N);
    maxgen = ceil(Problem.maxFE/Problem.N);
    if gen <= round(0.9*maxgen)
        maxIter = 1;
        maxPZ = maxN;
        if size(w,1) == round(maxN*N)
            %      PlotResults(w, C)
            % plot(w(:,1),w(:,2),'k*')
            % hold on
            % plot(temp1(:,1),temp1(:,2),'rs')
            [~,rankFlag] = sort(flag,'descend');
            r = rankFlag(1:round(maxN*N)-N);
            %     [~,index] = max(flag,[],1);
            %     r = index;
            C(r, :) = [];
            C(:, r) = [];
            t(r, :) = [];
            t(:, r) = [];
            w(r, :) = [];
            E(r) = [];
            ageSumBefore(r,:) = [];
            flag(r,:) = [];
            flag = zeros(N,1);

        end
    else
        if size(w,1) < round(maxN*N) && isempty(genFlag)
            maxPZ = maxN;
            maxIter = 1;
        else
            maxPZ = 1;
            maxIter = 0;
        end
        if size(w,1) == round(maxN*N)
            maxPZ = 1;
            maxIter = 0;
            genFlag = gen;
        end
    end
    % plot(w(:,1),w(:,2),'k*')

    if isempty(genFlag)
        for iter = 1:maxIter
            for kk = 1:size(temp1,1)
                nx = nx + 1;
                x = temp1(kk,:);
                w = w./sum(w,2);
                d = pdist2(x, w);
                [~, SortOrder] = sort(d);
                s1 = SortOrder(1);
                s2 = SortOrder(2);

                % Aging: the age of all neighbours of s1 is increased by 1

                t(s1, :) = t(s1, :) + 1;
                t(:, s1) = t(:, s1) + 1;

                % Add Error
                E(s1) = E(s1) + d(s1)^2;

                % Adaptation
                w(s1,:) = w(s1,:) + epsilon_b*(x-w(s1,:));
                Ns1 = find(C(s1,:)==1);
                for j=Ns1
                    w(j,:) = w(j,:) + epsilon_n*(x-w(j,:));
                end

                % Create Link
                C(s1,s2) = 1;
                C(s2,s1) = 1;
                t(s1,s2) = 0;
                t(s2,s1) = 0;

                % Remove Old Links
                C(t>T) = 0;
                nNeighbor = sum(C);
                AloneNodes = (nNeighbor==0);
                if ~isempty(find(AloneNodes == true))
                    %         AloneNodes
                end
                C(AloneNodes, :) = [];
                C(:, AloneNodes) = [];
                t(AloneNodes, :) = [];
                t(:, AloneNodes) = [];
                w(AloneNodes, :) = [];
                E(AloneNodes) = [];
                ageSumBefore(AloneNodes,:) = [];
                flag(AloneNodes,:) = [];

                % Add New Nodes

                if mod(nx, L) == 0 && size(w,1) < round(maxPZ*N)
                    [~, q] = max(E);
                    [~, f] = max(C(:,q).*E);
                    r = size(w,1) + 1;
                    w(r,:) = (w(q,:) + w(f,:))/2;
                    C(q,f) = 0;
                    C(f,q) = 0;
                    C(q,r) = 1;
                    C(r,q) = 1;
                    C(r,f) = 1;
                    C(f,r) = 1;
                    t(r,:) = 0;
                    t(:, r) = 0;
                    E(q) = alpha*E(q);
                    E(f) = alpha*E(f);
                    E(r) = E(q);
                    ageSumBefore(r,:) = 0;
                    flag(r,:) = 0;
                end

                % Decrease Errors
                E = delta*E;
            end
        end

        w = w./sum(w,2);
        net.w = w;
        net.E = E;
        net.C = C;
        net.t = t;
        net.nx = nx;
        net.ageSumBefore = ageSumBefore;
        net.flag = flag;
    end

    if isempty(genFlag)
        output = [];
        for i = 1:size(w,1)
            neighbor = find(C(i,:)==1);
            [~,minInd] = min(w([i neighbor],:),[],1);
            [~,maxInd] = max(w([i neighbor],:),[],1);
            if ~isempty(find(minInd==1))
                output = [output i];
            elseif ~isempty(find(maxInd==1))
                output = [output i];
            end

        end

        for t = 1:size(output,2)
            s = output(:,t);
            x = mean(w(find(C(s,:)==1),:),1);
            w(s,:) = w(s,:) - 1*(x-w(s,:));
        end

        t = any(w<0,2);
        l = find(t == true);
        if ~isempty(l)
            for tt = 1:size(l,1)
                w(l(tt,1),any(w(l(tt,1),:)<0,1)) = 0;
            end
        end

        V = w;
        V = V.*repmat(scale,size(V,1),1);
    end

    if gen == genFlag
        N = size(wholeObj,1);
        wholeObj1 = (wholeObj - repmat(zmin,N,1));
        wholeObj = wholeObj1./scale;
        temp2 = wholeObj1./sum(wholeObj1,2);

        output = [];
        for i = 1:size(w,1)
            neighbor = find(C(i,:)==1);
            [~,minInd] = min(w([i neighbor],:),[],1);
            [~,maxInd] = max(w([i neighbor],:),[],1);
            if ~isempty(find(minInd==1))
                output = [output i];
            elseif ~isempty(find(maxInd==1))
                output = [output i];
            end

        end

        for t = 1:size(output,2)
            s = output(:,t);
            x = mean(w(find(C(s,:)==1),:),1);
            w(s,:) = w(s,:) - 1*(x-w(s,:));
        end

        t = any(w<0,2);
        l = find(t == true);
        if ~isempty(l)
            for tt = 1:size(l,1)
                w(l(tt,1),any(w(l(tt,1),:)<0,1)) = 0;
            end
        end

        V = [w.*repmat(scale,size(w,1),1);temp2];
    end
end


function [data_train, labels_train, data_devel, labels_devel, raw_devel, PC, means_norm, stds_norm, vid_ids_devel_string] = ...
    Prepare_HOG_AU_data_generic_dynamic(train_users, devel_users, au_train, rest_aus, semaine_dir, feature_dir)

%%
addpath(genpath('../data extraction/'));

% First extracting the labels
[ labels_train, valid_ids_train, vid_ids_train ] = extract_SEMAINE_labels(semaine_dir, train_users, au_train);

[ labels_other, ~, ~ ] = extract_SEMAINE_labels(semaine_dir, train_users, rest_aus);
labels_other = cat(1, labels_other{:});

% Reading in the HOG data (of only relevant frames)
[train_appearance_data, valid_ids_train_hog, vid_ids_train_string] = Read_HOG_files_dynamic(train_users, vid_ids_train, feature_dir);

[train_geom_data] = Read_geom_files_dynamic(train_users, vid_ids_train, feature_dir);

% Subsample the data to make training quicker
labels_train = cat(1, labels_train{:});
valid_ids_train = logical(cat(1, valid_ids_train{:}));

reduced_inds = false(size(labels_train,1),1);
reduced_inds(labels_train == 1) = true;

% make sure the same number of positive and negative samples is taken
pos_count = sum(labels_train == 1);
neg_count = sum(labels_train == 0);

num_other = floor(pos_count / (size(labels_other, 2)));

inds_all = 1:size(labels_train,1);

for i=1:size(labels_other, 2)+1

    if(i > size(labels_other, 2))
        % fill the rest with a proportion of neutral
        inds_other = inds_all(sum(labels_other,2)==0 & ~labels_train );
        num_other_i = min(numel(inds_other), pos_count - sum(labels_train(reduced_inds,:)==0));
    else
        % take a proportion of each other AU
        inds_other = inds_all(labels_other(:, i) & ~labels_train );
        num_other_i = min(numel(inds_other), num_other);
    end
    inds_other_to_keep = inds_other(round(linspace(1, numel(inds_other), num_other_i)));
    reduced_inds(inds_other_to_keep) = true;

end

% Remove invalid ids based on CLM failing or AU not being labelled
reduced_inds(~valid_ids_train) = false;
reduced_inds(~valid_ids_train_hog) = false;

labels_other = labels_other(reduced_inds, :);
labels_train = labels_train(reduced_inds,:);
train_appearance_data = train_appearance_data(reduced_inds,:);
train_geom_data = train_geom_data(reduced_inds,:);
vid_ids_train_string = vid_ids_train_string(reduced_inds,:);

%% Extract devel data

% First extracting the labels
[ labels_devel, valid_ids_devel, vid_ids_devel ] = extract_SEMAINE_labels(semaine_dir, devel_users, au_train);

% Reading in the HOG data (of only relevant frames)
[devel_appearance_data, valid_ids_devel_hog, vid_ids_devel_string] = Read_HOG_files_dynamic(devel_users, vid_ids_devel, feature_dir);

[devel_geom_data] = Read_geom_files_dynamic(devel_users, vid_ids_devel, feature_dir);

labels_devel = cat(1, labels_devel{:});

% Peforming zone specific masking
if(au_train < 8 || au_train == 43 || au_train == 45) % upper face AUs ignore bottom face
    % normalise the data
    pca_file = '../../pca_generation/generic_face_upper.mat';
    load(pca_file);
elseif(au_train > 9) % lower face AUs ignore upper face and the sides
    % normalise the data
    pca_file = '../../pca_generation/generic_face_lower.mat';
    load(pca_file);
elseif(au_train == 9) % Central face model
    % normalise the data
    pca_file = '../../pca_generation/generic_face_rigid.mat';
    load(pca_file);
end

% Grab all data for validation as want good params for all the data
raw_devel = cat(2, devel_appearance_data, devel_geom_data);

devel_appearance_data = bsxfun(@times, bsxfun(@plus, devel_appearance_data, -means_norm), 1./stds_norm);
train_appearance_data = bsxfun(@times, bsxfun(@plus, train_appearance_data, -means_norm), 1./stds_norm);

data_train = train_appearance_data * PC;
data_devel = devel_appearance_data * PC;

data_train = cat(2, data_train, train_geom_data);
data_devel = cat(2, data_devel, devel_geom_data);

PC_n = zeros(size(PC)+size(train_geom_data, 2));
PC_n(1:size(PC,1), 1:size(PC,2)) = PC;
PC_n(size(PC,1)+1:end, size(PC,2)+1:end) = eye(size(train_geom_data, 2));
PC = PC_n;

means_norm = cat(2, means_norm, zeros(1, size(train_geom_data,2)));
stds_norm = cat(2, stds_norm, ones(1, size(train_geom_data,2)));

end


function scores=test_svms(models, imnames,featdir)
W=cat(2, models.w);
b=cat(2, models.b);
for i=1:numel(imnames)
    feats=load_feats(featdir, imnames{i});
    feats=xform_feat(feats, models(1).meannrm);
    scores{i}=bsxfun(@plus, feats*W,b);
    fprintf('Done %d/%d\n',i, numel(imnames));
end

function feats=xform_feat(feats, nrm)
feats=feats*20./nrm;

function feats=load_feats(featdir, image_id)
x1=load(fullfile(featdir,'CPMC_segms_150_sp_approx_LBP_f_pca_2500_noncent',[image_id '.mat']));
x2=load(fullfile(featdir,'CPMC_segms_150_sp_approx_SIFT_GRAY_f_g_pca_5000_noncent', [image_id '.mat']));
x3=load(fullfile(featdir,'CPMC_segms_150_sp_approx_SIFT_GRAY_mask_pca_5000_noncent', [image_id '.mat']));
 feats= [x1.D;x2.D; x3.D]';


function [d pred f]=astar_search(A,s,h,varargin)
% ASTAR_SEARCH Perform a heuristically guided (A*) search on the graph.
%
% [d pred rank]=astar_search(A,s,h,optionsu) returns the distance map,
% search tree and f-value of each node in an astar_search.
% The search begins at vertex s.  The heuristic h guides the search,
% h(v) should be small close to a goal and large far from a goal.  The
% heuristic h can either be a vector with an entry for each vertex in the
% graph or a function which maps vertices to values.
%
% This method works on non-negatively weighted directed graphs.
% The runtime is O((E+V)log(V)).
%
% ... = astar_search(A,u,...) takes a set of
% key-value pairs or an options structure.  See set_matlab_bgl_options
% for the standard options.
%   options.visitor: a visitor to use with the A* search (see Note)
%   options.inf: the value to use for unreachable vertices
%       [double > 0 | {Inf}]
%   options.target: a special vertex that will stop the search when hit
%       [{'none'} | any vertex number besides the u]; this is ignored if
%       a visitor is set.
%   options.edge_weight: a double array over the edges with an edge
%       weight for each node, see EDGE_INDEX and EXAMPLES/REWEIGHTED_GRAPHS
%       for information on how to use this option correctly
%       [{'matrix'} | length(nnz(A)) double vector]
%
% Note: You can specify a visitor for this algorithm.  The visitor has the
% following optional functions.
%    vis.initialize_vertex(u)
%    vis.discover_vertex(u)
%    vis.examine_vertex(u)
%    vis.examine_edge(ei,u,v)
%    vis.edge_relaxed(ei,u,v)
%    vis.edge_not_relaxed(ei,u,v)
%    vis.black_target(ei,u,v)
%    vis.finish_vertex(u)
% Each visitor parameter should be a function pointer, which returns 0
% if the search should stop.  (If the function does not return anything,
% the algorithm continues.)
%
% Example:
%   load graphs/bgl_cities.mat
%   goal = 11; % Binghamton
%   start = 9; % Buffalo
%   % Use the euclidean distance to the goal as the heuristic
%   h = @(u) norm(xy(u,:) - xy(goal,:));
%   % Setup a routine to stop when we find the goal
%   ev = @(u) (u ~= goal);
%   [d pred f] = astar_search(A, start, h, ...
%       struct('visitor', struct('examine_vertex', ev)));

% David Gleich
% Copyright, Stanford University, 2006-2008

%% History
%  2007-04-20: Added edge weight option
%  2007-07-12: Fixed edge_weight documentation.
%  2008-10-07: Changed options parsing
%%

[trans check full2sparse] = get_matlab_bgl_options(varargin{:});
if full2sparse && ~issparse(A), A = sparse(A); end

options = struct('inf', Inf, 'edge_weight', 'matrix', 'target', 'none');
options = merge_options(options,varargin{:});

edge_weight_opt = 'matrix';

if strcmp(options.edge_weight, 'matrix')
    % do nothing if we are using the matrix weights
else
    edge_weight_opt = options.edge_weight;
end

if strcmp(options.target,'none')
    target = 0; % a flag used to denote "no target" to the mex
elseif isa(options.target, 'double')
    target = options.target;
else
    error('matlab_bgl:invalidParameter', ...
        'options.target is not ''none'' or a vertex number.');
end

if check, check_matlab_bgl(A,struct()); end
if trans, A = A'; end

function hi=vec2func(u)
    hi = h(u);
end

if isa(h,'function_handle')
    hfunc = h;
else
    hfunc = @vec2func;
end

if isfield(options,'visitor')
    [d pred f] = astar_search_mex(A,s,target,hfunc,options.inf,edge_weight_opt,options.visitor);
else
    [d pred f] = astar_search_mex(A,s,target,h,options.inf,edge_weight_opt);
end

% end the main function
end


function trnslate(H,X)
    %
    %    trnslate(Handle,offset)
    %    translate Handle by amount offset=[x_offset, y_offset, z_offest]
    %
    %    See also ROTATE, SCALHAND
    %
    %    Richard G. Cobb    3/96
    %
    dad=get(H,'Parent');
    xlim=get(dad,'Xlim');
    xm=max(get(H,'Xdata'));
    xn=min(get(H,'Xdata'));
    if xm + X(1) > max(xlim)
        X(1) = max(xlim) - xm ;
    elseif xn + X(1) < min(xlim)
        X(1) = min(xlim) - xn ;
    end
    xlim=get(dad,'Ylim');
    xm=max(get(H,'Ydata'));
    xn=min(get(H,'Ydata'));
    if xm + X(2) > max(xlim)
        X(2) = max(xlim) - xm ;
    elseif xn + X(2) < min(xlim)
        X(2) = min(xlim) - xn ;
    end
    xlim=get(dad,'Zlim');
    xm=max(get(H,'Zdata'));
    xn=min(get(H,'Zdata'));
    if xm + X(3) > max(xlim)
        X(3) = max(xlim) - xm ;
    elseif xn + X(3) < min(xlim)
        X(3) = min(xlim) - xn ;
    end
    nx = (get(H,'Xdata')+X(1));
    ny = (get(H,'Ydata')+X(2));
    nz = (get(H,'Zdata')+X(3));
    set(H,'Xdata',nx)
    set(H,'Ydata',ny)
    set(H,'Zdata',nz)
    ud=get(H,'Userdata');
    ud(1)=nx(6);
    ud(2)=ny(6);
    ud(3)=nz(6);
    ud(4)=nx(1);
    ud(5)=ny(1);
    ud(6)=nz(1);

    set(H,'Userdata',ud);

    %eof


function [area smoothedArea] = roiSurfaceArea(vw, roi, msh)
% Get the surface area of an ROI on a mesh
%
% [area smoothedArea] = roiSurfaceArea(vw, roi, msh)
%

% Argument checks
if notDefined('vw'), vw = getSelectedGray; end
switch lower(viewGet(vw, 'viewType'))
    case {'gray' 'volume'}
        % ok
    otherwise
        error('Must be in Gray/Volume view to get ROI surface area');
end
if notDefined('msh'), msh = viewGet(vw, 'curmesh'); end
if notDefined('roi'), roi = viewGet(vw, 'selectedROI'); end

% Set current mesh in view struct
if isnumeric(msh),
    vw = viewSet(vw, 'curmeshn', msh);
    msh = viewGet(vw, 'curmesh');
end

verts = viewGet(vw, 'roivertinds', roi);

if isempty(verts),
    area = 0;
    smoothedArea = 0;
    return;
end

[areaList, smoothAreaList] = mrmComputeMeshArea(msh, verts);
area            = sum(areaList);
smoothedArea    = sum(smoothAreaList);

fprintf('ROI surface area: %0.1f mm^2 (%0.1f mm^2 on smoothed mesh)\n', area, smoothedArea);

end


function X = diff(varargin)
% DIFF (overloaded)

Y = varargin{1};
X = Y;
X.basis = [];
for i = 1:size(Y.basis,2)
    base = reshape(full(Y.basis(:,i)),X.dim);
    base = diff(base,varargin{2:end});
    X.basis = [X.basis sparse(base(:))];
end
X.dim = size(base);


clear all;
%%%====== Settings ======%%%
model = 'Deep SR-ITM'; % Deep SR-ITM (ICCV'19) or Multi-purpose CNN (ACCV'18)
yuv_format = '420'; % YUV file format
SDR_file = './data/test/testset_SDR.yuv'; % input .yuv file
wid = 1920; % width
hei = 1080; % height
num_fr = 28; % number of frames in the YUV file
scale = 2; % scale factor for SR
pred_file = sprintf('./pred/pred_x%d.yuv', scale); % result .yuv file
gpuDevice(1); % GPU ID
%%%======================%%%
addpath('utils');
disp(['Testing for scale ', num2str(scale), '...'])
% initialize
fclose(fopen(pred_file, 'w'));
[fwidth,fheight] = yuv_factor(yuv_format);

% load net
disp('Loading net...')
if strcmp(model, 'Deep SR-ITM')
    netstruct = load(sprintf('./net/x%d.mat', scale));
elseif strcmp(model, 'Multi-purpose CNN')
    netstruct = load(sprintf('./net/Multi-purpose_CNN_x%d.mat', scale));
end
net = dagnn.DagNN.loadobj(netstruct.net);
move(net,'gpu');
net.mode = 'test' ;
pred_index = net.getVarIndex('pred');
net.conserveMemory = true;

% test
disp('Testing starts...')
for fr = 1:num_fr
    % read frames
    SDR_YUV = uint8(load_yuv(SDR_file, fr, hei, wid, fheight, fwidth, 'SDR'));
    % normalize
    SDR_YUV = single(SDR_YUV)/255;
    % change type
    SDR_YUV = gpuArray(SDR_YUV);
    % prediction
    net.eval({'input', SDR_YUV});
    pred = gather(net.vars(pred_index).value);
    pred = min(max(pred, 0), 1);
    % save yuv file
    save_yuv(pred*1023, pred_file, hei, wid, fheight, fwidth, 'HDR');
    disp('Frame saved!')
end
disp('Done!')


function model = rmSearchFit_twoGaussiansPosOnly(model,data,params,wProcess,t)
% rmSearchFit_twoGaussiansPosOnly - wrapper for 'fine' one Gaussian fit
%
% model = rmSearchFit_twoGaussiansPosOnly(model,prediction,data,params);
%
% 2008/01 SOD: split of from rmSearchFit.

% now get original sigmas:
gridSigmas_unique = double(unique(params.analysis.sigmaMajor));
% add upper and lower limit:
expandRange    = double(params.analysis.fmins.expandRange);
gridSigmas = double([0.001.*ones(expandRange,1); ...
    gridSigmas_unique; ...
    params.analysis.maxRF.*ones(expandRange,1)]);

% fminsearch options
searchOptions         = params.analysis.fmins.options;
vethresh              = params.analysis.fmins.vethresh;

% amount of negative fits
nNegFit  = 0;
trends   = t.trends;
t_id     = t.dcid+2;

% convert to double just in case
params.analysis.X = double(params.analysis.X);
params.analysis.Y = double(params.analysis.Y);
params.analysis.allstimimages = double(params.analysis.allstimimages);
data = double(data);

%-----------------------------------
% Go for each voxel
%-----------------------------------
progress = 0;tic;
for ii = 1:numel(wProcess),

    % progress monitor (10 dots)
    if floor(ii./numel(wProcess)*10)>progress,
        % print out estimated time left
        if progress==0,
            esttime = toc.*10;
            if floor(esttime./3600)>0,
                fprintf(1,'[%s]:Estimated processing time: %d voxels: %d hours.\n',...
                    mfilename,numel(wProcess),ceil(esttime./3600));
            else
                fprintf(1,'[%s]:Estimated processing time: %d voxels: %d minutes.\n',...
                    mfilename,numel(wProcess),ceil(esttime./60));
            end;
            fprintf(1,'[%s]:Nonlinear optimization (x,y,sigma):',mfilename);
        end;
        fprintf(1,'.');drawnow;
        progress = progress + 1;
    end;

    % volume index
    vi = wProcess(ii);
    vData = data(:,ii);

    % raw rss value (non-squared) - faster than sum(data(:,vi).^2)
    rawrss     = norm(vData);

    % reset tolFun: Precision of evaluation function. We define RMS
    % improvement relative to the initial raw 'no-fit' data RMS. So, 1
    % means stop if there is less than 1% improvement on the fit:
    searchOptions.TolFun = params.analysis.fmins.options.TolFun.*rawrss;

    % start point from grid fit
    startParams = double([model.x0(vi); model.y0(vi); model.s(vi);...
        model.x02(vi); model.y02(vi); model.s2(vi)]);

    % tight search region [lowerbound upperbound]
    if params.analysis.scaleWithSigmas,
        step = params.analysis.relativeGridStep.*max(startParams([3 6]));
        minstep = params.analysis.maxXY./2./params.analysis.minimumGridSampling;
        step = min(step,minstep);
        maxstep = params.analysis.maxXY./2./params.analysis.maximumGridSampling;
        step = max(step,maxstep);
    else
        step = params.analysis.maxXY./2./params.analysis.maximumGridSampling;
    end;
    boundary.xy    = startParams([1 2 4 5])*[1 1] + ones(4,1)*[-1 1].*(step.*expandRange);

    % interpolated sigmas, so we'll look for the closest one.
    closestvalue = zeros(2,1);
    for n=1:2,
        [tmp,tmp2] = sort(abs(gridSigmas_unique-startParams(3*n)));
        closestvalue(n)       = tmp2(1)+expandRange;
    end
    boundary.sigma     = gridSigmas(closestvalue*[1 1]+[-1 1;-1 1].*expandRange);
    bndParams          = [boundary.xy([1 2],:);boundary.sigma(1,:);...
                          boundary.xy([3 4],:);boundary.sigma(2,:)];

    % actual fitting routine
    if searchOptions.MaxIter>0
        outParams = ...
            fmincon(@(x) rmModelSearchFit_twoGaussiansPosOnly(x,vData,...
            params.analysis.X,...
            params.analysis.Y,...
            params.analysis.allstimimages,trends),...
            startParams,[],[],[],[],bndParams(:,1),bndParams(:,2),...
            @(x) distanceCon(x,startParams,step.*expandRange),searchOptions);
    else
        outParams = startParams;
    end
    %[outParams bndParams(:,1) startParams bndParams(:,2)]

    % make RF, prediction and get rss,b
    Xi = params.analysis.X - outParams(1);   % positive x0 moves center right
    Yi = params.analysis.Y - outParams(2);   % positive y0 moves center up
    rf(:,1) = exp( (Yi.*Yi + Xi.*Xi) ./ (-2.*(outParams(3).^2)) );

    Xi = params.analysis.X - outParams(4);   % positive x0 moves center right
    Yi = params.analysis.Y - outParams(5);   % positive y0 moves center up
    rf(:,2) = exp( (Yi.*Yi + Xi.*Xi) ./ (-2.*(outParams(6).^2)) );

    X = [params.analysis.allstimimages * rf trends];
    b    = pinv(X)*vData;
    rss  = norm(vData-X*b).^2;

    % store results only if the first beta is positive, somehow fmincon
    % outputs negative fits. If the fit is negative keep old (grid) fit.
    if all(b(1:2)>0),
        model.x0(vi)   = outParams(1);
        model.y0(vi)   = outParams(2);
        model.s(vi)    = outParams(3);
        model.x02(vi)  = outParams(4);
        model.y02(vi)  = outParams(5);
        model.s2(vi)   = outParams(6);
        model.rss(vi)  = rss;
        model.b([1 2 t_id],vi)  = b;
    else
        % change the percent variance explained to be just under the
        % current vethresh. So it counts as a 'coarse'-fit but can still be
        % included in later 'fine'-fits
        model.rss(vi)  = (1-max((vethresh-0.01),0)).*model.rawrss(vi);
        nNegFit = nNegFit + 1;
    end
end

% just in case
model.s_major = model.s;
model.s_minor = model.s;

% end time monitor
et  = toc;
if floor(et/3600)>0,
    fprintf(1,'Done [%d hours].\n',ceil(et/3600));
else
    fprintf(1,'Done [%d minutes].\n',ceil(et/60));
end;
fprintf(1,'[%s]:Removed negative fits: %d (%.1f%%).\n',...
    mfilename,nNegFit,nNegFit./numel(wProcess).*100);
return;

%-----------------------------------
% make sure that the pRF can only be moved "step" away from original
% position "startParams" - for the one Gaussian model
function [C, Ceq]=distanceCon(x,startParams,step)
Ceq = [];
dist = x([1 2;4 5])-startParams([1 2;4 5]);
C = sqrt(sum(dist.^2,2)) - step;
return;
%-----------------------------------


function varargout = spm_render(dat,brt,rendfile)
% Render blobs on surface of a 'standard' brain
% FORMAT spm_render(dat,brt,rendfile)
%
% dat      - a struct array of length 1 to 3
%            each element is a structure containing:
%            - XYZ - the x, y & z coordinates of the transformed SPM{.}
%                    values in units of voxels.
%            - t   - the SPM{.} values.
%            - mat - affine matrix mapping from XYZ voxels to MNI.
%            - dim - dimensions of volume from which XYZ is drawn.
% brt      - brightness control:
%            If NaN, then displays using the old style with hot
%            metal for the blobs, and grey for the brain.
%            Otherwise, it is used as a ``gamma correction'' to
%            optionally brighten the blobs up a little.
% rendfile - the file containing the images to render on to (see also
%            spm_surf.m) or a surface mesh file.
%
% Without arguments, spm_render acts as its own UI.
%__________________________________________________________________________
%
% spm_render prompts for details of up to three SPM{.}s that are then
% displayed superimposed on the surface of a 'standard' brain.
%
% The first is shown in red, then green then blue.
%
% The blobs which are displayed are the integral of all transformed t
% values, exponentially decayed according to their depth. Voxels that
% are 10mm behind the surface have half the intensity of ones at the
% surface.
%__________________________________________________________________________
% Copyright (C) 1996-2019 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_render.m 7577 2019-04-24 08:59:56Z guillaume $

SVNrev = '$Rev: 7577 $';

global prevrend
if ~isstruct(prevrend)
    prevrend = struct('rendfile','', 'brt',[], 'col',[]);
end

varargout = {};

%-Parse arguments, get data if not passed as parameters
%==========================================================================

%-Get surface
%--------------------------------------------------------------------------
if nargin < 3 || isempty(prevrend.rendfile)
    spm('FnBanner',mfilename,SVNrev);
    [rendfile, sts] = spm_select(1,{'mat','mesh'},'Render file');
    if ~sts, return; end
end
prevrend.rendfile = rendfile;

ext = spm_file(rendfile,'ext');
loadgifti = false;
if strcmpi(ext,'mat')
    load(rendfile);
    if ~exist('rend','var') && ~exist('Matrixes','var')
        loadgifti = true;
    end
end
if ~strcmpi(ext,'mat') || loadgifti
    try
        rend = gifti(rendfile);
    catch
        error('Cannot read  render file "%s".\n', rendfile);
    end
    if ~isfield(rend,'vertices')
        try
            M = rend;
            rend = gifti(rend.private.metadata(1).value);
            try, rend.cdata = full(M.cdata); end
        catch
            error('Cannot find a surface mesh to be displayed.');
        end
    end
    rend = export(rend,'patch');
end

%-Get data
%--------------------------------------------------------------------------
if isfield(rend,'facevertexcdata')
    dat = rend.facevertexcdata;
    num = 1;
elseif nargin < 1
    spm('FigName','Results: render');

    num   = spm_input('Number of sets',1,'1 set|2 sets|3 sets',[1 2 3],1);

    for i = 1:num
        [SPM,xSPM] = spm_getSPM;
        dat(i) = struct('XYZ', xSPM.XYZ,...
                        't',   xSPM.Z',...
                        'mat', xSPM.M,...
                        'dim', xSPM.DIM);
    end
    showbar = 1;
else
    if isstruct(dat)
        num     = length(dat);
        showbar = 0;
    else
        files = cellstr(dat);
        clear dat
        num = numel(files);
        for i=1:num
            V  = spm_vol(files{i});
            d  = spm_read_vols(V);
            id = find(isfinite(d));
            [X,Y,Z]    = ind2sub(V.dim,id);
            dat(i).XYZ = [X Y Z]';
            dat(i).t   = d(id);
            dat(i).mat = V.mat;
            dat(i).dim = V.dim';
            clear X Y Z id
        end
        showbar = 1;
    end
end

%-Get brightness & colours (mesh)
%--------------------------------------------------------------------------
if isfield(rend,'vertices')
    if num == 1
        col = hot(256);
    else
        col = eye(3);
        if spm_input('Which colours?','+1','b',{'RGB','Custom'},[0 1],1)
            for k = 1:num
                col(k,:) = uisetcolor(col(k,:),sprintf('Colour of blob set %d',k));
            end
        end
    end
    surf_rend(dat,rend,col);
    return
end

%-Get brightness & colours (image)
%--------------------------------------------------------------------------
if nargin < 2  || isempty(prevrend.brt)
    brt = 1;
    if num==1
        brt = spm_input('Style',1,'new|old',[1 NaN], 1);
    end

    if isfinite(brt)
        brt = spm_input('Brighten blobs','+1','none|slightly|more|lots',[1 0.75 0.5 0.25], 1);
        col = eye(3);
        % ask for custom colours & get rgb values
        %------------------------------------------------------------------
        if spm_input('Which colours?','+1','b',{'RGB','Custom'},[0 1],1)
            for k = 1:num
                col(k,:) = uisetcolor(col(k,:),sprintf('Colour of blob set %d',k));
            end
        end
    else
        col = [];
    end
elseif isfinite(brt) && isempty(prevrend.col)
    col = eye(3);
elseif isfinite(brt)  % don't need to check prevrend.col again
    col = prevrend.col;
else
    col = [];
end
prevrend.brt = brt;
prevrend.col = col;

%-Perform the rendering
%==========================================================================
spm('Pointer','Watch');

if showbar, spm_progress_bar('Init', size(dat,1)*length(rend),...
            'Formatting Renderings', 'Number completed'); end
for i=1:length(rend)
    rend{i}.max=0;
    rend{i}.data = cell(size(dat,1),1);
    if issparse(rend{i}.ren)
        % Assume that images have been DCT compressed
        % - the SPM99 distribution was originally too big.
        d = size(rend{i}.ren);
        B1 = spm_dctmtx(d(1),d(1));
        B2 = spm_dctmtx(d(2),d(2));
        rend{i}.ren = B1*rend{i}.ren*B2';
        % the depths did not compress so well with
        % a straight DCT - therefore it was modified slightly
        rend{i}.dep = exp(B1*rend{i}.dep*B2')-1;
    end
    rend{i}.ren(rend{i}.ren>=1) = 1;
    rend{i}.ren(rend{i}.ren<=0) = 0;
    if showbar, spm_progress_bar('Set', i); end
end
if showbar, spm_progress_bar('Clear'); end

if showbar, spm_progress_bar('Init', length(dat)*length(rend),...
            'Making pictures', 'Number completed'); end

mx = zeros(length(rend),1)+eps;
mn = zeros(length(rend),1);

for j=1:length(dat)
    XYZ = dat(j).XYZ;
    t   = dat(j).t;
    dim = dat(j).dim;
    mat = dat(j).mat;

    for i=1:length(rend)

        % transform from Talairach space to space of the rendered image
        %------------------------------------------------------------------
        M1  = rend{i}.M*mat;
        zm  = sum(M1(1:2,1:3).^2,2).^(-1/2);
        M2  = diag([zm' 1 1]);
        M  = M2*M1;
        cor = [1 1 1 ; dim(1) 1 1 ; 1 dim(2) 1; dim(1) dim(2) 1 ;
               1 1 dim(3) ; dim(1) 1 dim(3) ; 1 dim(2) dim(3); dim(1) dim(2) dim(3)]';
        tcor= M(1:3,1:3)*cor + M(1:3,4)*ones(1,8);
        off = min(tcor(1:2,:)');
        M2  = spm_matrix(-off+1)*M2;
        M  = M2*M1;
        xyz = (M(1:3,1:3)*XYZ + M(1:3,4)*ones(1,size(XYZ,2)));
        d2  = ceil(max(xyz(1:2,:)'));

        % Calculate 'depth' of values
        %------------------------------------------------------------------
        if ~isempty(d2)
            dep = spm_slice_vol(rend{i}.dep,spm_matrix([0 0 1])*inv(M2),d2,1);
            z1  = dep(round(xyz(1,:))+round(xyz(2,:)-1)*size(dep,1));

            if ~isfinite(brt), msk = find(xyz(3,:) < (z1+20) & xyz(3,:) > (z1-5));
            else msk = find(xyz(3,:) < (z1+60) & xyz(3,:) > (z1-5)); end
        else
            msk = [];
        end

        if ~isempty(msk)

            % Generate an image of the integral of the blob values.
            %--------------------------------------------------------------
            xyz = xyz(:,msk);
            if ~isfinite(brt), t0  = t(msk);
            else
                dst = xyz(3,:) - z1(msk);
                dst = max(dst,0);
                t0  = t(msk).*exp((log(0.5)/10)*dst)';
            end
            X0  = full(sparse(round(xyz(1,:)), round(xyz(2,:)), t0, d2(1), d2(2)));
            hld = 1; if ~isfinite(brt), hld = 0; end
            X   = spm_slice_vol(X0,spm_matrix([0 0 1])*M2,size(rend{i}.dep),hld);
            msk = find(X<0);
            X(msk) = 0;
        else
            X = zeros(size(rend{i}.dep));
        end

        % Brighten the blobs
        %------------------------------------------------------------------
        if isfinite(brt), X = X.^brt; end

        mx(j) = max([mx(j) max(max(X))]);
        mn(j) = min([mn(j) min(min(X))]);

        rend{i}.data{j} = X;

        if showbar, spm_progress_bar('Set', i+(j-1)*length(rend)); end
    end
end

mxmx = max(mx);
mnmn = min(mn);

if showbar, spm_progress_bar('Clear'); end

%-Display
%==========================================================================
if ~spm('CmdLine')
    Fgraph = spm_figure('GetWin','Graphics');
    spm_results_ui('Clear',Fgraph);

    nrow = ceil(length(rend)/2);
    if showbar, hght = 0.95; else hght = 0.5; end
    % subplot('Position',[0, 0, 1, hght]);
    ax = axes('Parent',Fgraph,...
        'units','normalized',...
        'Position',[0, 0, 1, hght],...
        'Visible','off');
    image(0,'Parent',ax);
    set(ax,'YTick',[],'XTick',[]);
end

rgb = cell(1,length(rend));

if ~isfinite(brt)
    % Old style split colourmap display
    %----------------------------------------------------------------------
    split = [gray(64); hot(64)];
    if ~spm('CmdLine'), colormap(split); end
    for i=1:length(rend)
        ren = rend{i}.ren;
        X   = (rend{i}.data{1}-mnmn)/(mxmx-mnmn);
        msk = find(X);
        ren(msk) = X(msk)+(1+1.51/64);
        rgb{i} = ind2rgb(round(ren*64),split);
        if ~spm('CmdLine')
            ax = axes('Parent',Fgraph,...
                'units','normalized',...
                'Position',[rem(i-1,2)*0.5, floor((i-1)/2)*hght/nrow, 0.5, hght/nrow],...
                'Visible','off');
            image(ren*64,'Parent',ax);
            set(ax,'DataAspectRatio',[1 1 1],...
                'PlotBoxAspectRatioMode','auto',...
                'YTick',[],'XTick',[],...
                'XDir','normal','YDir','normal');
        end
    end
else
    % Combine the brain surface renderings with the blobs, and display
    % using 24 bit colour
    %----------------------------------------------------------------------
    for i=1:length(rend)
        ren = rend{i}.ren;
        X = cell(3,1);
        for j=1:length(rend{i}.data)
            X{j} = rend{i}.data{j}/(mxmx-mnmn)-mnmn;
        end
        for j=(length(rend{i}.data)+1):3
            X{j}=zeros(size(X{1}));
        end

        rgb{i} = zeros([size(ren) 3]);
        tmp = ren.*max(1-X{1}-X{2}-X{3},0);
        for k = 1:3
            rgb{i}(:,:,k) = tmp + X{1}*col(1,k) + X{2}*col(2,k) +X{3}*col(3,k);
        end
        rgb{i}(rgb{i}>1) = 1;
        if ~spm('CmdLine')
            ax = axes('Parent',Fgraph,...
                'units','normalized',...
                'Position',[rem(i-1,2)*0.5, floor((i-1)/2)*hght/nrow, 0.5, hght/nrow],...
                'nextplot','add', ...
                'Visible','off');
            image(rgb{i},'Parent',ax);
            set(ax,'DataAspectRatio',[1 1 1],...
                'PlotBoxAspectRatioMode','auto',...
                'YTick',[],'XTick',[],...
                'XDir','normal','YDir','normal');
        end
    end
end

spm('Pointer','Arrow');

if nargout
    for i=1:numel(rgb)
        for ch=1:size(rgb{i},3)
            rgb{i}(:,:,ch) = flipud(rgb{i}(:,:,ch));
        end
    end
    varargout = { rgb };
end

%==========================================================================
% function surf_rend(dat,rend,col)
%==========================================================================
function surf_rend(dat,rend,col)

%-Setup figure and axis
%--------------------------------------------------------------------------
Fgraph = spm_figure('GetWin','Graphics');
spm_results_ui('Clear',Fgraph);

ax0 = axes(...
    'Tag',      'SPMMeshRenderBackground',...
    'Parent',   Fgraph,...
    'Units',    'normalized',...
    'Color',    [1 1 1],...
    'XTick',    [],...
    'YTick',    [],...
    'Position', [-0.05, -0.05, 1.05, 0.555]);

ax = axes(...
    'Parent',   Fgraph,...
    'Units',    'normalized',...
    'Position', [0.05, 0.05, 0.9, 0.4],...
    'Visible',  'off');

H = spm_mesh_render('Disp',rend,struct('parent',ax));
spm_mesh_render('Overlay',H,dat,col);
camlight(H.light);

try
    setAllowAxesRotate(H.rotate3d, setxor(findobj(Fgraph,'Type','axes'),ax), false);
end

%-Register with MIP
%--------------------------------------------------------------------------
try % meaningless when called outside spm_results_ui
    hReg = spm_XYZreg('FindReg',spm_figure('GetWin','Interactive'));
    xyz  = spm_XYZreg('GetCoords',hReg);
    hs   = mydispcursor('Create',ax,dat.mat,xyz);
    spm_XYZreg('Add2Reg',hReg,hs,@mydispcursor);
end

%==========================================================================
function varargout = mydispcursor(varargin)

switch lower(varargin{1})
    %======================================================================
    case 'create'
    %======================================================================
    % hMe = mydispcursor('Create',ax,M,xyz)
    ax  = varargin{2};
    M   = varargin{3};
    xyz = varargin{4};

    [X,Y,Z] = sphere;
    vx = sqrt(sum(M(1:3,1:3).^2));
    X = X*vx(1) + xyz(1);
    Y = Y*vx(2) + xyz(2);
    Z = Z*vx(3) + xyz(3);
    hold(ax,'on');
    hs = surf(X,Y,Z,'parent',ax,...
        'EdgeColor','none','FaceColor',[1 0 0],'FaceLighting', 'gouraud');
    set(hs,'UserData',xyz);

    varargout = {hs};

    %======================================================================
    case 'setcoords'    % Set co-ordinates
    %======================================================================
    % [xyz,d] = mydispcursor('SetCoords',xyz,hMe,hC)
    hMe  = varargin{3};
    pxyz = get(hMe,'UserData');
    xyz  = varargin{2};

    set(hMe,'XData',get(hMe,'XData') - pxyz(1) + xyz(1));
    set(hMe,'YData',get(hMe,'YData') - pxyz(2) + xyz(2));
    set(hMe,'ZData',get(hMe,'ZData') - pxyz(3) + xyz(3));
    set(hMe,'UserData',xyz);

    varargout = {xyz,[]};

    %======================================================================
    otherwise
    %======================================================================
    error('Unknown action string')

end


function [gradS,gradV,gradAlpha] = refineShape(S,V,alpha,stateFiles,lambda,shapePrior,deformationPrior,shapePriorInstance,iter)

% V is (numpoints*3 X K)
% alpha is K X length(fnames)

%% Initializations
params = get_params();

%numcores = max(matlabpool('size'),1);

poolobj = gcp('nocreate'); % If no pool, do not create new one.
if isempty(poolobj)
    numcores = 0;
else
    numcores = poolobj.NumWorkers;
end

gradICPS = zeros(size(S));
gradICPV = zeros(size(V));
gradICPAlpha = zeros(size(alpha));
numpoints = size(S,1);
K = size(V,2);
numimages = length(stateFiles);

gOS = zeros([size(S) numcores]);
gOV = zeros([size(V) numcores]);
gOA = zeros([size(alpha) numcores]);
cO = zeros([numpoints,1,numcores]);

gSS = zeros([size(S) numcores]);
gSV = zeros([size(V) numcores]);
gSA = zeros([size(alpha) numcores]);

gPS = zeros([size(S) numcores]);
gPV = zeros([size(V) numcores]);
gPA = zeros([size(alpha) numcores]);

gKS = zeros([size(S) numcores]);
gKV = zeros([size(V) numcores]);
gKA = zeros([size(alpha) numcores]);

normS = sqrt(sum(S.^2,2));
directions = S./repmat(normS,1,3);

%% Parallely computing gradients

%h = tic;
localParams = params;
parfor c = 1:numcores
    [gKS(:,:,c),gKV(:,:,c),gKA(:,:,c),gOS(:,:,c),gOV(:,:,c),gOA(:,:,c),cO(:,:,c),...
        gSS(:,:,c),gSV(:,:,c),gSA(:,:,c),gPS(:,:,c),gPV(:,:,c),gPA(:,:,c)]...
        =computeGrads(S,V,alpha,stateFiles,lambda,shapePriorInstance,c,numcores,localParams);
end

gradOcclusionS = sum(gOS,3);
gradOcclusionV = sum(gOV,3);
gradOcclusionAlpha = sum(gOA,3);
occlusionCount = sum(cO,3);

gradSilS = sum(gSS,3);
gradSilV = sum(gSV,3);
gradSilAlpha = sum(gSA,3);

gradShapePriorS = sum(gPS,3);
gradShapePriorV = sum(gPV,3);
gradShapePriorAlpha = sum(gPA,3);

gradKeypointS = sum(gKS,3);
gradKeypointV = sum(gKV,3);
gradKeypointAlpha = sum(gKA,3);

%% Computing soft occlusion grad for the shape
gradOcclusionS = repmat(double((occlusionCount/numimages-0.02)>0),1,3).*gradOcclusionS; %hacking here

%% Computing gradients from priors
%h = tic;
gradPriorS = lambda(3)*shapePrior(S);
[gradPriorV,deformationPriorGradAlpha] = deformationPrior(V,alpha);
gradPriorV = lambda(4)*gradPriorV;
deformationPriorGradAlpha = lambda(4)*deformationPriorGradAlpha;
%fprintf('Prior computation: %.3fs\n',toc(h));

%% Display all gradients
fprintf('%d\t%3.3f\t%3.3f\t%3.3f\t%3.3f\t%3.3f\t%3.3f\t',...
iter, mean(sqrt(sum(mean(gKS,3).^2,2))),mean(sqrt(sum(mean(gOS,3).^2,2)))...
    ,mean(sqrt(sum(mean(gSS,3).^2,2))),mean(sqrt(sum(mean(gPS,3).^2,2))),...
    mean(sqrt(sum(gradPriorS.^2,2))),mean(sqrt(sum(gradPriorV.^2,2))));

%% Computing overall change in shape
gradS = gradICPS + gradOcclusionS + gradPriorS + gradSilS + gradShapePriorS + gradKeypointS;
gradV = gradICPV + gradOcclusionV + gradPriorV + gradSilV + gradShapePriorV + gradKeypointV;
gradAlpha = gradICPAlpha + gradOcclusionAlpha + deformationPriorGradAlpha + gradSilAlpha...
    + gradShapePriorAlpha + gradKeypointAlpha;

%% Computing grad along radial direction to preserve mesh
if(params.opt.spherical)
    projections = sum(gradS.*directions,2);
    gradS = directions.*repmat(projections,1,3);
    for k=1:K
        deformation = reshape(gradV(:,k),numpoints,3);
        projections = sum(deformation.*directions,2);
        deformation = directions.*repmat(projections,1,3);
        gradV(:,k)=deformation(:);
    end
end

end


function output = callopticsdp(interfacedata)

% Retrieve needed data
options = interfacedata.options;
F_struc = interfacedata.F_struc;
c       = interfacedata.c;
K       = interfacedata.K;
x0      = interfacedata.x0;
ub      = interfacedata.ub;
lb      = interfacedata.lb;

model = yalmip2opticsdp(interfacedata);
if options.showprogress;showprogress(['Calling ' interfacedata.solver.tag],options.showprogress);end

if options.savedebug
    save csdpdebug model
end

solvertime = tic;
model.ops = csdpset;
[y,fvals,exitflag,stats,X] = csdp(model.f,model.A,model.b,model.lb,model.ub,model.sdcone,model.y0,model.ops);
solvertime = toc(solvertime);

% Create dual variable in internal format
if options.saveduals
    if any(K.l)
        top = 1;
        D_struc = X{1};
    else
        top = 0;
        D_struc = [];
    end
    if any(K.s)
        for j = 1:length(K.s)
            D_struc = [D_struc;X{j+top}(:)];
        end
    end
else
    D_struc = [];
end

x = y;  % Our notation do not coincide ...
switch exitflag
    case 0
       problem = 0;
    case 1
        problem = 2;
    case 2
        problem = 1;
    case {3,5,6,7,8,9,10}
        problem = 4;
    case {4,-27}
        problem = 3;
    case -50
        problem = 16;
    case 11
        problem = 7;
    otherwise
        problem = -1;
end

if options.savesolveroutput
	solveroutput.y = y;
    solveroutput.fvals = fvals;
    solveroutput.exitflag = exitflag;
    solveroutput.stats = stats;
    solveroutput.X = X;
else
	solveroutput = [];
end

if options.savesolverinput
	solverinput.model = model;
else
	solverinput = [];
end

% Standard interface
output = createOutputStructure(x,D_struc,[],problem,interfacedata.solver.tag,solverinput,solveroutput,solvertime);