function TS_ClassifyLowDim(whatData,cfnParams,numPCs,doComputeAtMaxInSample)
% TS_ClassifyLowDim compare performance of reduced PCs from the data matrix
%-------------------------------------------------------------------------------
% 'whatData', HCTSA data file (or structure).
% 'cfnParams', parameters of the classification to be performed (cf.
%                   GiveMeDefaultClassificationParams)
% 'numPCs', investigate classification using up to this many PCs of the data
%              matrix (default: 5).
% 'docomputeAtMaxInsample', compute accuracy at a a number of PCs that hits 100%
%                           in-sample accuracy.

%-------------------------------------------------------------------------------
% If you use this code for your research, please cite these papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This work is licensed under the Creative Commons
% Attribution-NonCommercial-ShareAlike 4.0 International License. To view a copy of
% this license, visit http://creativecommons.org/licenses/by-nc-sa/4.0/ or send
% a letter to Creative Commons, 444 Castro Street, Suite 900, Mountain View,
% California, 94041, USA.
%-------------------------------------------------------------------------------

%-------------------------------------------------------------------------------
%% Check inputs and set defaults
%-------------------------------------------------------------------------------
if nargin < 1
    whatData = 'norm';
end
if nargin < 3
    numPCs = 10;
end
if nargin < 4
    doComputeAtMaxInSample = false;
end

%-------------------------------------------------------------------------------
%% Load data
%-------------------------------------------------------------------------------
[TS_DataMat,TimeSeries,Operations,whatDataFile] = TS_LoadData(whatData);

% Assign group labels (removing unlabeled data):
[TS_DataMat,TimeSeries] = FilterLabeledTimeSeries(TS_DataMat,TimeSeries);
[groupLabels,classLabels,groupLabelsInteger,numGroups] = TS_ExtractGroupLabels(TimeSeries);
TellMeAboutLabeling(TimeSeries);

if nargin < 2 || isempty(cfnParams)
    cfnParams = GiveMeDefaultClassificationParams(TimeSeries);
end
TellMeAboutClassification(cfnParams);

% Filter down to reduced features if specified/required:
[TS_DataMat,Operations] = FilterFeatures(TS_DataMat,Operations,cfnParams);
numFeatures = size(TS_DataMat,2);

if doComputeAtMaxInSample
    if ~cfnParams.computePerFold
        cfnParams.computePerFold = true;
        warning(['You must compute in-sample accuracies (per fold) for',...
                        'doComputeAtMaxInSample to work: setting to true.']);
    end
end

%-------------------------------------------------------------------------------
%% Check for NaNs in data matrix
%-------------------------------------------------------------------------------
if any(isnan(TS_DataMat(:)))
    warning('Consider re-running TS_Normalize to filter out NaNs from the feature matrix)')
end

%-------------------------------------------------------------------------------
%% Compute leading X PCs of the data matrix
%-------------------------------------------------------------------------------
% Use pca to compute the first two principal components:
% (project data into space of PC scores, Y)
if ~any(isnan(TS_DataMat))
    fprintf('Computing top %u PCs...',numPCs)
    if doComputeAtMaxInSample
        [pcCoeff,pcScore,~,~,percVar] = pca(zscore(TS_DataMat));
    else
        [pcCoeff,pcScore,~,~,percVar] = pca(zscore(TS_DataMat),'NumComponents',numPCs);
    end
else
    warning(['Data matrix contains %.2g%% NaNs. Estimating covariances on remaining data...\n' ...
                '(Could take some time...)'],100*mean(isnan(TS_DataMat(:))))
    if doComputeAtMaxInSample
        [pcCoeff,pcScore,~,~,percVar] = pca(BF_NormalizeMatrix(TS_DataMat,'zscore'),...
                            'Rows','pairwise');
    else
        [pcCoeff,pcScore,~,~,percVar] = pca(BF_NormalizeMatrix(TS_DataMat,'zscore'),...
                            'Rows','pairwise','NumComponents',numPCs);
    end
    % If this fails (covariance matrix not positive definite), can try the
    % (...,'algorithm','als') option in pca... (or toolbox for probabilistic PCA)
end
fprintf(' Done.\n')
numPCs = min(numPCs,size(pcScore,2)); % sometimes lower than the number attempted

%-------------------------------------------------------------------------------
%% Display some info about feature loading onto the reduced components
%-------------------------------------------------------------------------------
numTopLoadFeat = min(numFeatures,20); % display this many features loading onto each PC
LowDimDisplayTopLoadings(numTopLoadFeat,numPCs,pcCoeff,pcScore,TS_DataMat,Operations);

%-------------------------------------------------------------------------------
%% Compute performance with leading PCs of the feature space:
%-------------------------------------------------------------------------------
cfnRatePCs = zeros(numPCs,1);
stdAcc = zeros(numPCs,1);
if cfnParams.computePerFold
    inSampleStats = zeros(numPCs,2);
    allFoldPCs = cell(numPCs,1);
end
fprintf('Computing %s, keeping top 1-%u PCs...\n',cfnParams.whatLoss,numPCs)
cfnParams.suppressWarning = true;
for i = 1:numPCs
    topPCs = pcScore(:,1:i);
    if cfnParams.computePerFold
        [cfnRatePCs(i),stdAcc(i),inSampleStats(i,:),allFoldPCs{i}] = ComputeCVAccuracies(topPCs,TimeSeries.Group,cfnParams,true);
        fprintf(1,'%u PCs: [in-fold: %.1f%%] %.1f +/- %.1f%%\n',i,inSampleStats(i,1),cfnRatePCs(i),stdAcc(i));
    else
        [cfnRatePCs(i),stdAcc(i)] = ComputeCVAccuracies(topPCs,TimeSeries.Group,cfnParams);
        fprintf(1,'%u PCs: %.1f +/- %.1f%%\n',i,cfnRatePCs(i),stdAcc(i));
    end
end

%-------------------------------------------------------------------------------
%% Compute accuracy in low-dimensional space (first time hitting max in-sample accuracy)
%-------------------------------------------------------------------------------
if doComputeAtMaxInSample
    if any(inSampleStats(:,1)==100)
        theNumPCs = find(inSampleStats(:,1)==100,1,'first');
        accAtPerfectInSample = cfnRatePCs(theNumPCs);
        stdAtPerfectInSample = stdAcc(theNumPCs);
        allFoldsAtPerfectInSample = allFoldPCs{theNumPCs};
    else
        % Keep searching
        fprintf(1,'Let''s keep searching for perfect in-sample accuracy...\n');
        PC_inSampleMeanAcc = 0;
        i = numPCs;
        while PC_inSampleMeanAcc < 100
            i = i + 1;
            topPCs = pcScore(:,1:i);
            [PCacc_mean,PCAcc_std,PC_inSampleMeanAcc,PC_allFoldData] = ComputeCVAccuracies(topPCs,TimeSeries.Group,cfnParams,true);
            fprintf(1,'%u PCs: [%.1f%%] %.1f +/- %.1f%%\n',i,PC_inSampleMeanAcc(1),PCacc_mean,PCAcc_std);
        end
        theNumPCs = i;
        accAtPerfectInSample = PCacc_mean;
        stdAtPerfectInSample = PCAcc_std;
        allFoldsAtPerfectInSample = PC_allFoldData;
    end
end

%-------------------------------------------------------------------------------
%% Comparison to all features
%-------------------------------------------------------------------------------
fprintf('Now with all %u features for comparison...\n',numFeatures)
if cfnParams.computePerFold
    [cfnRateAll,stdAll,inSampleAll,allFoldData] = ComputeCVAccuracies(TS_DataMat,TimeSeries.Group,cfnParams,true);
else
    [cfnRateAll,stdAll] = ComputeCVAccuracies(TS_DataMat,TimeSeries.Group,cfnParams);
end

%-------------------------------------------------------------------------------
%% Simple null estimate
%-------------------------------------------------------------------------------
numNulls = 1000;
nullStatsAll = GiveMeSimpleNullStats(TimeSeries.Group,numNulls,cfnParams,false);
nullStatsFoldMeans = mean(nullStatsAll,2);

%-------------------------------------------------------------------------------
%% Plot
%-------------------------------------------------------------------------------
plotColors = {[0,114,178]/255,[213,94,0]/255,[0,158,115]/255,[204,121,167]/255,...
                [230,159,0]/255,[86,180,233]/255,[240,215,66]/255,[0,0,0]/255};
lineWidth = 2;
f = figure('color','w');
f.Position(3:4) = [506,324];
ax = subplot(1,4,1:2);
hold('on')

%-------------------------------------------------------------------------------
% All-feature out-of-sample:
l_allfeats = yline(cfnRateAll,'-','color',plotColors{4},'LineWidth',lineWidth,...
            'Label',sprintf('All %u features (%.1f +/- %.1f%%)',numFeatures,cfnRateAll,stdAll));
legendEntryAllFeatures = sprintf('All %u features (%.1f%%)',numFeatures,cfnRateAll);

% All-feature in-sample:
if cfnParams.computePerFold
    legendEntryAllFeatures_inSample = sprintf('All features (in-fold): %.1f%%',inSampleAll(1));
    yline(inSampleAll(1),'--','color',brighten(plotColors{3},-0.5),'LineWidth',lineWidth,...
                    'Label',legendEntryAllFeatures_inSample)

end

%-------------------------------------------------------------------------------
% Naive null estimate
l_naive_null = yline(mean(nullStatsFoldMeans),'-','color',ones(1,3)*0.5,'LineWidth',lineWidth,...
                        'Label','Naive null');
l_naive_null_over = yline(quantile(nullStatsFoldMeans,0.95),'--','color',ones(1,3)*0.5,'LineWidth',lineWidth,...
                        'Label','Naive null 95% quantile');

%-------------------------------------------------------------------------------
% Out-of-fold accuracy at perfect in-fold accuracy:
if doComputeAtMaxInSample
    yline(accAtPerfectInSample,'-','color','r','LineWidth',lineWidth,...
            'Label',sprintf('At perfect in-fold acc (%.1f +/- %.1f%%)',...
                        accAtPerfectInSample,stdAtPerfectInSample));
    if theNumPCs < numPCs
        xline(theNumPCs,'--r');
    end
end

% Leading PCA accuracies:
l_testSet = plot(1:numPCs,cfnRatePCs,'o-k','LineWidth',lineWidth);
legendEntryPCs = sprintf('PCs (%.1f-%.1f%%)',min(cfnRatePCs),max(cfnRatePCs));
if cfnParams.computePerFold
    l_trainSet = plot(1:numPCs,inSampleStats(:,1),'o-','Color',plotColors{2},'LineWidth',lineWidth);
    legendEntryPCs_inSample = sprintf('In-sample (%.1f-%.1f%%)',...
                                min(inSampleStats(:,1)),max(inSampleStats(:,1)));
end

% Error bars:
plot(1:numPCs,cfnRatePCs + stdAcc,'-','Color',0.5*ones(1,3),'LineWidth',lineWidth/2);
plot(1:numPCs,cfnRatePCs - stdAcc,'-','Color',0.5*ones(1,3),'LineWidth',lineWidth/2);

% All-feature accuracies:
% yline(cfnRateAll - stdAll,':','color',plotColors{4},'LineWidth',lineWidth)
% yline(cfnRateAll + stdAll,':','color',plotColors{4},'LineWidth',lineWidth)

legend([l_testSet,l_trainSet],{legendEntryPCs,legendEntryPCs_inSample},...
        'Location','SouthEast')

ax.XTick = 1:numPCs;
xlabel('Number of PCs');
ylabel('Classification accuracy (%)')

titleText = sprintf('Classification rate (%u-class) using %u-fold %s',...
            cfnParams.numClasses,cfnParams.numFolds,cfnParams.classifierText);
title(titleText,'interpreter','none')

%-------------------------------------------------------------------------------
% Violin plots of individual fold accuracies
%-------------------------------------------------------------------------------
title(sprintf('All (%u) individual folds',cfnParams.numRepeats*cfnParams.numFolds))
ax_bar_folds = subplot(1,4,4);

if doComputeAtMaxInSample
    allFeatures_allTrainFolds = squeeze(allFoldData(:,1,:));
    allFeatures_allTestFolds = squeeze(allFoldData(:,2,:));
    allFoldsAtPerfectInSample_allTrainFolds = squeeze(allFoldsAtPerfectInSample(:,1,:));
    allFoldsAtPerfectInSample_allTestFolds = squeeze(allFoldsAtPerfectInSample(:,2,:));
    nullStatsAllFolds = nullStatsAll(:);
    accData = {nullStatsAllFolds,allFeatures_allTrainFolds(:),allFeatures_allTestFolds(:),...
            allFoldsAtPerfectInSample_allTrainFolds(:),allFoldsAtPerfectInSample_allTestFolds(:)};
    extraParams = struct();
    plotColors{1} = ones(1,3)*0.5;
    extraParams.theColors = plotColors;
    BF_ViolinPlot(accData,[],[],false,extraParams);

    ax_bar_folds.XTick = [1:5+0.5];
    xTickLabels = {'Naive-shuffle null',sprintf('all %u features train folds',numFeatures),...
                    'all features test folds',...
                    sprintf('perfect-in-fold (%u PCs) train folds',theNumPCs),...
                    sprintf('perfect-in-fold (%u PCs) test folds',theNumPCs)};

    % Bar + error bars:
    % hold('on')
    % b = bar(accData,'FaceColor','flat');
    % b.CData(1,:) = plotColors{4};
    % b.CData(2,:) = brighten(plotColors{3},-0.5);
    % b.CData(3,:) = [1,0,0];
    % stdData = [stdAll,0,stdAtPerfectInSample];
    % er = errorbar(1:3,accData,stdData);
    % er.Color = [0 0 0];
    % er.LineStyle = 'none';
else
    hold('on')
    b = bar([cfnRateAll,inSampleAll(1)],'FaceColor','flat');
    b.CData(1,:) = plotColors{4};
    b.CData(2,:) = brighten(plotColors{3},-0.5);
    ax_bar_folds.XTick = 1:2;
    xTickLabels = {'all','all (in-fold)'};
end
ax_bar_folds.XTickLabel = xTickLabels;
ax_bar_folds.YLim = ax.YLim;
linkaxes([ax,ax_bar_folds],'y')

%-------------------------------------------------------------------------------
% Violin plots of fold-average accuracies
%-------------------------------------------------------------------------------
title(sprintf('%u repeats of %u-fold averages',cfnParams.numRepeats,cfnParams.numFolds))
ax_bar = subplot(1,4,3);
hold('on')
allFeatures_allTrainFoldMeans = mean(squeeze(allFoldData(:,1,:)),1);
allFeatures_allTestFoldMeans = mean(squeeze(allFoldData(:,2,:)),1);
allFoldsAtPerfectInSample_allTrainFoldMeans = mean(squeeze(allFoldsAtPerfectInSample(:,1,:)),1);
allFoldsAtPerfectInSample_allTestFoldMeans = mean(squeeze(allFoldsAtPerfectInSample(:,2,:)),1);
accData = {nullStatsFoldMeans,allFeatures_allTrainFoldMeans,allFeatures_allTestFoldMeans,...
        allFoldsAtPerfectInSample_allTrainFoldMeans,allFoldsAtPerfectInSample_allTestFoldMeans};
extraParams = struct();
plotColors{1} = ones(1,3)*0.5;
extraParams.theColors = plotColors;
BF_ViolinPlot(accData,[],[],false,extraParams);
l_naive_null_over = yline(quantile(nullStatsFoldMeans,0.95),'--','color',ones(1,3)*0.5,'LineWidth',lineWidth,...
                        'Label','Naive null 95% quantile');

ax_bar.XTick = [1:5+0.5];
xTickLabels = {'Naive-shuffle null',sprintf('all %u features train-fold-means',numFeatures),...
                'all features test-fold-means',...
                sprintf('perfect-in-fold (%u PCs) train-fold-means',theNumPCs),...
                sprintf('perfect-in-fold (%u PCs) test-fold-means',theNumPCs)};
ax_bar.XTickLabel = xTickLabels;
ax_bar.YLim = ax.YLim;
linkaxes([ax_bar,ax_bar_folds],'y')

end


function [ raw ] = RawFrom4Channels( fourchan )
%RAWTO4CHANNELS Summary of this function goes here
%   Detailed explanation goes here

[h,w,~] = size(fourchan);

raw = zeros(h*2,w*2);

idx = [1,1; 1,2; 2,1; 2,2];

for c=1:4
    raw(idx(c,1):2:end, idx(c,2):2:end) = fourchan(:,:,c);
end

end


% test for computations of nn

name = 'swissroll';
n = 3000;
options.sampling = 'rand';
[X,col] = load_points_set( name, n, options );
nbr_nn = 50;

options.use_nntools = 0;
[D1,nn_list1] = compute_nn_distance(X,nbr_nn, options);
options.use_nntools = 1;
[D2,nn_list2] = compute_nn_distance(X,nbr_nn, options);

disp( ['Error (should be 0)=' num2str(norm(D1-D2) + norm(nn_list1-nn_list2),2) '.'] );

L = nn_list1(1,:);
col(L) = 2;

plot_scattered(X,col);


%RANDINIT Reset random number generator
%
% RANDINIT resets the defaul random number stream.  For example:
%
%         >> rand
%         ans =
%            0.8147
%         >> rand
%         ans =
%             0.9058
%         >> rand
%         ans =
%             0.1270
%         >> randinit
%         >> rand
%         ans =
%             0.8147

%
% See also RandStream.

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math

function randinit(seed)

    stream = RandStream.getGlobalStream;
    stream.reset()


function [hog_data, valid_inds, vid_id] = Read_HOG_files(users, vid_ids, hog_data_dir)

    hog_data = [];
    vid_id = {};

    feats_filled = 0;

    for i=1:numel(users)

        hog_file = [hog_data_dir, '/train/' users{i} '.hog'];
        if(~exist(hog_file, 'file'))
            hog_file = [hog_data_dir, '/devel/' users{i} '.hog'];
        end

        f = fopen(hog_file, 'r');

        num_cols = fread(f, 1, 'int32');
        if(isempty(num_cols))
            break;
        end

        num_rows = fread(f, 1, 'int32');
        num_chan = fread(f, 1, 'int32');
        num_feats = num_rows * num_cols * num_chan + 1;

        % go to the beginning
        fseek(f, 0, 'bof');

        % Read only the relevant bits

        % Skip to the right start element (1 indexed)
        fseek(f, 4*(4+num_rows*num_rows*num_chan)*(vid_ids(i,1)-1), 'bof');

        feature_vec = fread(f, [4 + num_rows * num_cols * num_chan, vid_ids(i,2) - vid_ids(i,1)], 'float32');
        fclose(f);

        curr_data = feature_vec(4:end,:)';
        curr_ind = size(curr_data,1);

        vid_id_curr = cell(size(curr_data,1),1);
        vid_id_curr(:) = users(i);

        vid_id = cat(1, vid_id, vid_id_curr);

        % Assume same number of frames per video
        if(i==1)
            hog_data = zeros(sum(vid_ids(:,2)-vid_ids(:,1)), num_feats);
        end

        hog_data(feats_filled+1:feats_filled+curr_ind,:) = curr_data;

        feats_filled = feats_filled + curr_ind;

    end
    valid_inds = hog_data(:,1) > 0;
    hog_data = hog_data(:,2:end);
end


% load 'RSS_data.mat';%original rss signals
% load 'B.mat';% smoothing rss signals
[action_size,sample_size]=size(RSS_data);
for i=1:action_size
    for j=1:1
               rss_sample=RSS_data{i,j};
%                rss_smooth_sample_medfilt=medfilt1(RSS_data{i,j},5);
%                rss_smooth_sample =B{i,j};
    end
    subplot(4,4,i);
%     plot(rss_sample,'r');
%     hold on;
    plot(rss_sample,'k','LineWidth',2,'MarkerSize',12);
      xlabel('Packets');
     ylabel('RSS','fontsize',8);
%      legend('Original Signals','Smooth Signals','location','best');

end
figure;

for i=1:16
    for j=1:1
               csi_sample=CSI_data{i,j};
%                rss_smooth_sample_medfilt=medfilt1(RSS_data{i,j},5);
%                rss_smooth_sample =B{i,j};
              csi_sample_re=permute(csi_sample,[2,1,3]);
    end
    subplot(4,4,i);
%     plot(rss_sample,'r');
%     hold on;
    plot(csi_sample_re(:,10,1),'k','LineWidth',2,'MarkerSize',12);
      xlabel('Packets');
     ylabel('CSI','fontsize',8);
%      legend('Original Signals','Smooth Signals','location','best');

end


function varargout = drawCurve(varargin)
%DRAWCURVE draw a curve specified by a list of points
%
%   drawCurve(COORD);
%   packs coordinates in a single [N*2] array.
%
%   drawCurve(PX, PY);
%   specifies coordinates in separate arrays. PX and PY must be column
%   vectors with the same length.
%
%   drawCurve(..., TYPE);
%   where TYPE is either 'closed' or 'open', specifies if last point must
%   be connected to the first one ('closed') or not ('open').
%   Default is 'open'.
%
%   drawCurve(..., PARAM, VALUE);
%   specify plot options as described for plot command.
%
%   H = drawCurve(...) also return a handle to the list of line objects.
%
%   Example:
%   % Draw a curve representing an ellipse
%   t = linspace(0, 2*pi, 100)';
%   px = 10*cos(t); py = 5*sin(t);
%   drawCurve([px py], 'closed');
%   axis equal;
%
%   % The same, with different drawing options
%   drawCurve([px py], 'closed', 'lineWidth', 2, 'lineStyle', '--');
%
%   See Also:
%   polygons2d, drawPolyline
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 06/04/2004.
%

%   HISTORY
%   03/01/2007 better processing of input, and update doc (drawing
%       options and CLOSE option)
%   03/03/2010 add deprecation warning

% deprecation warning
warning('geom2d:deprecated', ...
    '''drawCurve'' is deprecated, use ''drawPolyline'' instead');

% default values
closed = false;

% If first argument is a cell array, draw each curve individually,
% and eventually returns handle of each plot.
var = varargin{1};
if iscell(var)
    h = [];
    for i=1:length(var(:))
        h = [h ; drawCurve(var{i}, varargin{2:end})]; %#ok<AGROW>
    end
    if nargout>0
        varargout{1}=h;
    end
    return;
end

% extract curve coordinate
if size(var, 2)==1
    % first argument contains x coord, second argument contains y coord
    px = var;
    if length(varargin)==1
        error('Wrong number of arguments in drawCurve');
    end
    py = varargin{2};
    varargin = varargin(3:end);
else
    % first argument contains both coordinate
    px = var(:, 1);
    py = var(:, 2);
    varargin = varargin(2:end);
end

% check if curve is closed or open
if ~isempty(varargin)
    var = varargin{1};
    if strncmpi(var, 'close', 5)
        closed = true;
        varargin = varargin(2:end);
    elseif strncmpi(var, 'open', 4)
        closed = false;
        varargin = varargin(2:end);
    end
end

% add first point at the end to close the curve
if closed
    px = [px; px(1)];
    py = [py; py(1)];
end

% plot the curve, with eventually optional parameters
h = plot(px, py, varargin{:});

% format output arguments
if nargout>0
    varargout{1}=h;
end


classdef TestDrawDetectedDiamonds
    %TestDrawDetectedDiamonds

    methods (Static)
        function test_1
            [img, diamond] = get_image_markers();
            [corners, ids] = cv.detectMarkers(img, diamond.dict);
            [diamondCorners, diamondIds] = cv.detectCharucoDiamond(img, ...
                corners, ids, diamond.slen / diamond.mlen);
            out = cv.drawDetectedDiamonds(img, diamondCorners, ...
                'IDs',diamondIds, 'BorderColor',[0 0 255]);
            validateattributes(out, {class(img)}, {'size',size(img)});
        end

        function test_error_argnum
            try
                cv.drawDetectedDiamonds();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end

function [img, d] = get_image_markers()
    % 4 markers in a diamond
    d = struct();
    d.dict = '6x6_50';
    d.ids = randi([0 49], [1 4]);
    d.slen = 80;
    d.mlen = 50;
    img = cv.drawCharucoDiamond(d.dict, d.ids, d.slen, d.mlen);
    img = repmat(img, [1 1 3]);
end


% Test file for @chebfun/sum.m.

function pass = test_sum(pref)

% Obtain preferences.
if ( nargin == 0 )
    pref = chebfunpref();
end

% Generate a few random points in [-1 1] to use as test values.
seedRNG(7681);
xr = 2 * rand(1000, 1) - 1;

%% SCALAR-VALUED

% Check the empty case.
pass(1) = sum(chebfun()) == 0;

% Check operation in the general case.
f = chebfun({@(x) exp(4*pi*1i*x), @exp, @exp}, [-1 0 0.5 1], pref);
pass(2) = abs(sum(f) - (exp(1) - 1)) < 10*vscale(f)*eps;

% Check operation for row chebfuns.
ft = f.';
pass(3) = abs(sum(ft) - (exp(1) - 1)) < 10*vscale(ft)*eps;

% Check sum over a subdomain.
pass(4) = abs(sum(f, [-1 1]) - (exp(1) - 1)) < 10*vscale(f)*eps;
pass(5) = abs(sum(f, [-1 0])) < 10*vscale(f)*eps;
pass(6) = abs(sum(f, [0 1]) - (exp(1) - 1)) < 10*vscale(f)*eps;

% Check sum between chebfun limits.
f = chebfun(@exp, [-1 -0.5 0 0.5 1], pref);
a = chebfun(@(x) x.^2 - 1, [-1 1]);
b = chebfun(@(x) -x.^2 + 1, [-1 1]);

F1 = sum(f, a, 1);
F1_exact = @(x) exp(1) - exp(x.^2 - 1);
pass(7) = norm(feval(F1, xr) - F1_exact(xr), inf) < 10*vscale(F1)*eps;

F2 = sum(f, -1, b);
F2_exact = @(x) exp(-x.^2 + 1) - exp(-1);
pass(8) = norm(feval(F2, xr) - F2_exact(xr), inf) < 10*vscale(F2)*eps;

F3 = sum(f, a, b);
F3_exact = @(x) exp(-x.^2 + 1) - exp(x.^2 - 1);
pass(9) = norm(feval(F3, xr) - F3_exact(xr), inf) < 10*vscale(F3)*eps;

%% ARRAY-VALUED
f = chebfun(@(x) [sin(x) cos(x) exp(x)], [-1 -0.5 0 0.5 1]);
pass(10) = norm(sum(f) - [0 2*sin(1) (exp(1) - exp(-1))], inf) < ...
    10*vscale(f)*eps;

ft = f.';
pass(11) = norm(sum(ft) - [0 2*sin(1) (exp(1) - exp(-1))].', inf) < ...
    10*vscale(ft)*eps;

pass(12) = norm(sum(f, [-1 1]) - [0 2*sin(1) (exp(1) - exp(-1))], inf) < ...
    10*vscale(f)*eps;

pass(13) = norm(sum(f, [-1 0]) - [(cos(-1) - 1) sin(1) (1 - exp(-1))], inf) ...
    < 10*vscale(f)*eps;

F1 = sum(f, a, 1);
F1_col1_exact = @(x) cos(x.^2 - 1) - cos(1);
F1_col2_exact = @(x) sin(1) - sin(x.^2 - 1);
F1_col3_exact = @(x) exp(1) - exp(x.^2 - 1);
F1_exact = @(x) [F1_col1_exact(x) F1_col2_exact(x) F1_col3_exact(x)];
err = feval(F1, xr) - F1_exact(xr);
pass(14) = norm(err(:), inf) < 10*vscale(F1)*eps;

F2 = sum(f, -1, b);
F2_col1_exact = @(x) cos(-1) - cos(-x.^2 + 1);
F2_col2_exact = @(x) sin(-x.^2 + 1) - sin(-1);
F2_col3_exact = @(x) exp(-x.^2 + 1) - exp(-1);
F2_exact = @(x) [F2_col1_exact(x) F2_col2_exact(x) F2_col3_exact(x)];
err = feval(F2, xr) - F2_exact(xr);
pass(15) = norm(err(:), inf) < 10*vscale(F2)*eps;

F3 = sum(f, a, b);
F3_col1_exact = @(x) -cos(-x.^2 + 1) + cos(x.^2 - 1);
F3_col2_exact = @(x) sin(-x.^2 + 1) - sin(x.^2 - 1);
F3_col3_exact = @(x) exp(-x.^2 + 1) - exp(x.^2 - 1);
F3_exact = @(x) [F3_col1_exact(x) F3_col2_exact(x) F3_col3_exact(x)];
err = feval(F3, xr) - F3_exact(xr);
pass(16) = norm(err(:), inf) < 1e2*vscale(F3)*eps;

%% Check dim argument.
g = sum(f, 2);
g_exact = @(x) sin(x) + cos(x) + exp(x);
pass(17) = norm(feval(g, xr) - g_exact(xr), inf) < 10*vscale(g)*eps;

g = sum(ft, 1);
g_exact = @(x) (sin(x) + cos(x) + exp(x)).';
pass(18) = norm(feval(g, xr) - g_exact(xr), inf) < 10*vscale(g)*eps;

%% Check error conditions.
try
    s = sum(f, -2, 2);
    pass(19) = false;
catch ME
    pass(19) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:sum:sumSubDom:ab');
end

try
    s = sum(f, -2, b);
    pass(20) = false;
catch ME
    pass(20) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:sum:sumSubDom:a');
end

try
    s = sum(f, a, 2);
    pass(21) = false;
catch ME
    pass(21) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:sum:sumSubDom:b');
end

%% QUASIMATRICES:

f = quasimatrix(@(x) [sin(x) cos(x) exp(x)], [-1 -0.5 0 0.5 1]);
pass(22) = norm(sum(f) - [0 2*sin(1) (exp(1) - exp(-1))], inf) < ...
    10*vscale(f)*eps;

ft = f.';
pass(23) = norm(sum(ft) - [0 2*sin(1) (exp(1) - exp(-1))].', inf) < ...
    10*vscale(ft)*eps;

pass(24) = norm(sum(f, [-1 1]) - [0 2*sin(1) (exp(1) - exp(-1))], inf) < ...
    10*vscale(f)*eps;

pass(25) = norm(sum(f, [-1 0]) - [(cos(-1) - 1) sin(1) (1 - exp(-1))], inf) ...
    < 10*vscale(f)*eps;

%% Test on singular function: piecewise smooth chebfun - splitting on.

% Set a domain
dom = [-2 7];

pow = -0.5;
op = @(x) (x - dom(1)).^pow.*sin(100*x);
f = chebfun(op, dom, 'exps', [pow 0], 'splitting', 'on');
I = sum(f);
I_exact = 0.17330750941063138;
pass(26) = ( abs(I-I_exact) < 1e3*eps*abs(I_exact) );

%% Test for functions defined on unbounded domain:

% Functions on [-inf inf]:

% Set the domain:
dom = [-Inf 2 Inf];

op1 = @(x) x.^2.*exp(-x.^2);
op2 = @(x) (1-exp(-x.^2))./x.^2;
f = chebfun({op1 op2}, dom);
I = sum(f);
% The exact solution is obtained using Matlab symbolic toolbox:
IExact = 1.364971769155161;
err = abs(I - IExact);
pass(27) = err < 1e7*eps*get(f,'vscale');

% Functions on [0 inf]:

% Set the domain:
dom = [0 Inf];

op1 = @(x) x.*exp(-x);
f = chebfun(op1, dom);
I1 = sum(f);

IExact = 1;
err1 = abs(I1 - IExact);
pass(28) = err1 < 2e5*eps*get(f,'vscale');

x = chebfun('x', dom);
g = x.*exp(-x);
warning('off', 'CHEBFUN:UNBNDFUN:sum:slowDecay');
I2 = sum(g);
warning('on', 'CHEBFUN:UNBNDFUN:sum:slowdDecay');
err2 = abs(I2 - IExact);
tol = 2e10*eps*get(f,'vscale');
pass(29) = err2 < tol;

% Function on [-Inf Inf]:
f = chebfun('exp(-x.^2/16).*(1+.2*cos(10*x))',[-inf,inf]);

% Suppress expected warnings which may occur on certain machines:
warning('off','CHEBFUN:UNBNDFUN:sum:slowDecay');
I = sum(f);
% Re-enable warnings:
warning('on','CHEBFUN:UNBNDFUN:sum:slowDecay');
IExact = 7.0898154036220641;
err = abs(I - IExact);
pass(30) = err < 1e9*eps*get(f,'vscale');

% #496
k = 3;
op = @(t) exp(-t)./(1+k*t);
f = chebfun(op, [0 inf]);
I = sum(f);
% The following exact result is obtained using Matlab symbolic toolbox:
I_exact = 0.385602012136694;
err = abs(I-I_exact);
pass(31) = err < 2e1*eps*get(f,'vscale');

% #920: sum of array-valued chebfun defined on unbounded domain:
% (same function which decays fast enough to be integrable):
f = chebfun(@(x) exp(-[x x].^2), [0 Inf]);
I = sum(f);
% The following exact value is obtained by Mathematica.
I_exact = 0.886226925452758*ones(1, 2);
pass(32) = norm(I-I_exact, inf) < 1e1*eps*get(f,'vscale');

% #920: sum of array-valued chebfun defined on unbounded domain:
% (different functions but both decay fast so that integrable)
f = chebfun(@(x)[exp(-x.^2) 1./(x.^3)], [1 Inf]);
I = sum(f);
% The following exact value is obtained by Mathematica.
I_exact = [0.139402792640331 0.5];
pass(33) = norm(I-I_exact, inf) < 1e6*eps*get(f,'vscale');

% #920: sum of array-valued chebfun defined on unbounded domain:
% (two different functions one of which is integrable and the other one is not):
f = chebfun(@(x)[exp(-x.^2) 0*x+1], [1 Inf]);
I = sum(f);
% The following exact value is obtained by Mathematica.
I_exact = 0.139402792640331;
pass(34) = abs(I(1)-I_exact) < 1e1*eps*get(f,'vscale') && ...
    isinf(I(2));

%%

% Complex limits are not supported
f = chebfun(@sin);
try
    sum(f, [0 1i]);
    pass(35) = false;
catch
    pass(35) = true;
end

% Invalid subdomains in reverse order
t = chebfun(@(t) t, [0 1]);
try
    sum(t, 0, -1)
    pass(36) = false;
catch
    pass(36) = true;
end

end


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

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym csch (@var{x})
%% Symbolic csch function.
%%
%% Example:
%% @example
%% @group
%% syms x
%% y = csch (x)
%%   @result{} y = (sym) csch(x)
%% @end group
%% @end example
%%
%% Note: this file is autogenerated: if you want to edit it, you might
%% want to make changes to 'generate_functions.py' instead.
%%
%% @end defmethod

function y = csch(x)
  if (nargin ~= 1)
    print_usage ();
  end
  y = elementwise_op ('csch', x);
end

%!error csch (sym(1), 2)
%!assert (isequaln (csch (sym(nan)), sym(nan)))

%!shared x, d
%! d = 1;
%! x = sym('1');

%!test
%! f1 = csch(x);
%! f2 = csch(d);
%! assert( abs(double(f1) - f2) < 1e-15 )

%!test
%! D = [d d; d d];
%! A = [x x; x x];
%! f1 = csch(A);
%! f2 = csch(D);
%! assert( all(all( abs(double(f1) - f2) < 1e-15 )))

%!test
%! % round trip
%! y = sym('y');
%! A = csch (d);
%! f = csch (y);
%! h = function_handle (f);
%! B = h (d);
%! assert (A, B, -eps)


function [ shape2D, global_params, local_params, final_lhood, landmark_lhoods, view_used ] = Fitting_from_bb( Image, DepthImage, bounding_box, PDM, patchExperts, clmParams, varargin)
%FITTING Summary of this function goes here
%   Detailed explanation goes here

    % the bounding box format is [minX, minY, maxX, maxY];

    % the mean model shape
    M = PDM.M;

    num_points = numel(M) / 3;

    if(any(strcmp(varargin,'orientation')))
        orientation = varargin{find(strcmp(varargin, 'orientation'))+1};
        rot = Euler2Rot(orientation);
    else
        rot = eye(3);
        orientation = [0;0;0];
    end

    rot_m = rot * reshape(M, num_points, 3)';
    width_model = max(rot_m(1,:)) - min(rot_m(1,:));
    height_model = max(rot_m(2,:)) - min(rot_m(2,:));

    a = (((bounding_box(3) - bounding_box(1)) / width_model) + ((bounding_box(4) - bounding_box(2))/ height_model)) / 2;

    tx = (bounding_box(3) + bounding_box(1))/2;
    ty = (bounding_box(4) + bounding_box(2))/2;

    % correct it so that the bounding box is just around the minimum
    % and maximum point in the initialised face
    tx = tx - a*(min(rot_m(1,:)) + max(rot_m(1,:)))/2;
    ty = ty - a*(min(rot_m(2,:)) + max(rot_m(2,:)))/2;

    % visualisation of the initial state
    %hold off;imshow(Image);hold on;plot(a*rot_m(1,:)+tx, a*rot_m(2,:)+ty,'.r');hold on;rectangle('Position', [bounding_box(1), bounding_box(2), bounding_box(3)-bounding_box(1), bounding_box(4)-bounding_box(2)]);
    global_params = [a, 0, 0, 0, tx, ty]';
    global_params(2:4) = orientation;

    local_params = zeros(numel(PDM.E), 1);

    if(any(strcmp(varargin,'gparam')))
        global_params = varargin{find(strcmp(varargin, 'gparam'))+1};
    end

    if(any(strcmp(varargin,'lparam')))
        local_params = varargin{find(strcmp(varargin, 'lparam'))+1};
    end

    scale = clmParams.startScale;

    if(size(Image, 3) == 1)
        GrayImage = Image;
    else
        GrayImage = rgb2gray(Image);
    end

    [heightImg, widthImg] = size(GrayImage);

    % Some predefinitions for faster patch extraction
    [xi, yi] = meshgrid(0:widthImg-1,0:heightImg-1);
    xi = double(xi);
    yi = double(yi);

    GrayImageDb = double(GrayImage);

    clmParams_old = clmParams;

    % In case there are no iterations initialize thes to empty
    view = GetView(patchExperts(scale).centers, global_params(2:4));
    final_lhood = 0;
    [shape2D] = GetShapeOrtho(M, PDM.V, local_params, global_params);
    landmark_lhoods = zeros(size(shape2D,1),1);

    % multi iteration refinement using NU-RLMS in each one
    for i=1:clmParams.numPatchIters

        current_patch_scaling = patchExperts(scale).trainingScale;
        visibilities = patchExperts(scale).visibilities;

        view = GetView(patchExperts(scale).centers, global_params(2:4));

        % The shape fitting is performed in the reference frame of the
        % patch training scale
        refGlobal = [current_patch_scaling, 0, 0, 0, 0, 0]';

        % the reference shape
        refShape = GetShapeOrtho(M, PDM.V, local_params, refGlobal);

        % shape around which the patch experts will be evaluated in the original image
        [shape2D] = GetShapeOrtho(M, PDM.V, local_params, global_params);
        shape2D_img = shape2D(:,1:2);

        % Create transform using a slightly modified version of Kabsch that
        % takes scaling into account as well, in essence we get a
        % similarity transform from current estimate to reference shape
        [A_img2ref, T_img2ref, ~, ~] = AlignShapesWithScale(shape2D_img(:,1:2),refShape(:,1:2));

        % Create a transform, from shape in image to reference shape
        T = maketform('affine', [A_img2ref;T_img2ref]);

        shape_2D_ref = tformfwd(T, shape2D_img);

        % transform the current shape to the reference one, so we can
        % interpolate
        shape2D_in_ref = (A_img2ref * shape2D_img')';

        sideSizeX = (clmParams.window_size(i,1) - 1)/2;
        sideSizeY = (clmParams.window_size(i,2) - 1)/2;

        patches = zeros(size(shape2D_in_ref,1), clmParams.window_size(i,1) * clmParams.window_size(i,2));

        Ainv = inv(A_img2ref);

        % extract patches on which patch experts will be evaluted
        for l=1:size(shape2D_in_ref,1)
            if(visibilities(view,l))

                xs = (shape2D_in_ref(l,1)-sideSizeX):(shape2D_in_ref(l,1)+sideSizeX);
                ys = (shape2D_in_ref(l,2)-sideSizeY):(shape2D_in_ref(l,2)+sideSizeY);

                [xs, ys] = meshgrid(xs, ys);

                pairs = [xs(:), ys(:)];

                actualLocs = (Ainv * pairs')';

                actualLocs(actualLocs(:,1) < 0,1) = 0;
                actualLocs(actualLocs(:,2) < 0,2) = 0;
                actualLocs(actualLocs(:,1) > widthImg - 1,1) = widthImg - 1;
                actualLocs(actualLocs(:,2) > heightImg - 1,2) = heightImg - 1;

                [t_patch] = interp2_mine(xi, yi, GrayImageDb, actualLocs(:,1), actualLocs(:,2), 'bilinear');
                t_patch = reshape(t_patch, size(xs));

                patches(l,:) = t_patch(:);

            end
        end

        % Calculate patch responses, either SVR or CCNF
        if(strcmp(patchExperts(scale).type, 'SVR'))
            responses = PatchResponseSVM_multi_modal( patches, patchExperts(scale).patch_experts(view,:), visibilities(view,:), patchExperts(scale).normalisationOptionsCol, clmParams, clmParams.window_size(i,:));
        elseif(strcmp(patchExperts(scale).type, 'CCNF'))
            responses = PatchResponseCCNF( patches, patchExperts(scale).patch_experts(view,:), visibilities(view,:), patchExperts(scale), clmParams.window_size(i,:));
        elseif(strcmp(patchExperts(scale).type, 'CEN'))

            % responses_o = PatchResponseCEN( patches, patchExperts(scale).patch_experts(view,:), visibilities(view,:), patchExperts(scale), clmParams.window_size(i,:));

            % A faster (and very slightly less accurate) version of patch
            % response computation
            responses = PatchResponseCEN_sparse( patches, patchExperts(scale).patch_experts(view,:), visibilities(view,:), clmParams.window_size(i,:), patchExperts(scale).normalisationOptionsCol.patchSize);
        end

        % If a depth image is provided compute patch experts around it as
        % well (unless it's the final iteration)
        if(~isempty(DepthImage) && (i ~= clmParams.numPatchIters))

            % Extracting the depth patches here
            patches_depth = zeros(size(shape2D_in_ref,1), clmParams.window_size(i,1) * clmParams.window_size(i,2));

            % extract patches on which patch experts will be evaluted
            for l=1:size(shape2D_in_ref,1)
                if(visibilities(view,l))

                    xs = (shape2D_in_ref(l,1)-sideSizeX):(shape2D_in_ref(l,1)+sideSizeX);
                    ys = (shape2D_in_ref(l,2)-sideSizeY):(shape2D_in_ref(l,2)+sideSizeY);

                    [xs, ys] = meshgrid(xs, ys);

                    pairs = [xs(:), ys(:)];

                    actualLocs = (Ainv * pairs')';

                    actualLocs(actualLocs(:,1) < 1,1) = 1;
                    actualLocs(actualLocs(:,2) < 1,2) = 1;
                    actualLocs(actualLocs(:,1) > widthImg,1) = widthImg;
                    actualLocs(actualLocs(:,2) > heightImg,2) = heightImg;

                    % use nearest neighbour interpolation as bilinear would
                    % produce artefacts in depth image (when missing data
                    % is there)
                    [t_patch] = interp2_mine(xi, yi, DepthImage, actualLocs(:,1), actualLocs(:,2), 'nearest');
                    t_patch = reshape(t_patch, size(xs));

                    patches_depth(l,:) = t_patch(:);

                end
            end

            old_mm = clmParams.use_multi_modal;
            clmParams.use_multi_modal = 0;
            responses_depth = PatchResponseSVM_multi_modal( patches_depth, patchExperts(scale).patch_experts_depth(view,:), visibilities(view,:), patchExperts(scale).normalisationOptionsDepth, clmParams, clmParams.window_size(i,:));
            clmParams.use_multi_modal = old_mm;

            % Combining the patch responses from different channels here
            for l=1:size(shape2D_in_ref,1)
                responses{l} = responses{l} + responses_depth{l};
            end
        end

        % the better the correlation in training the more reliable the feature
        % the reliabilities are independent for every modality in SVR, so
        % combine them (also correlation is inverse to variance)
        if(strcmp(patchExperts(scale).type, 'SVR'))
            if(clmParams.use_multi_modal)
                reliabilities = patchExperts(scale).patch_experts{1,1}(end).correlations{1};
            else
                reliabilities = patchExperts(scale).patch_experts{1,1}(1).correlations{1};
            end
        else
            % for CCNF the modalities work together
            reliabilities = patchExperts(scale).correlations;
        end
        reliabilities = reliabilities(view,:);

        % deal with the fact that params might be different for different
        % scales
        if(numel(clmParams_old.regFactor) > 1)
            clmParams.regFactor = clmParams_old.regFactor(scale);
        end
        if(numel(clmParams_old.sigmaMeanShift) > 1)
            clmParams.sigmaMeanShift = clmParams_old.sigmaMeanShift(scale);
        end
        if(numel(clmParams_old.tikhonov_factor) > 1)
            clmParams.tikhonov_factor = clmParams_old.tikhonov_factor(scale);
        end

        % The actual NU-RLMS step

        % first the rigid transform
        [global_params, local_params] = NU_RLMS(global_params, local_params, PDM, responses, visibilities, view, reliabilities, shape2D_img, T, true, clmParams, []);

        % second the combined transform
        [global_params, local_params, final_lhood, landmark_lhoods] = ...
            NU_RLMS(global_params, local_params, PDM, responses, visibilities, view, reliabilities, shape2D_img, T, false, clmParams, []);

        % Clamp orientation and make sure it doesn't get out of hand
        orientation = global_params(2:4);
        orientation(orientation < -pi/2) = -pi/2;
        orientation(orientation > pi/2) = pi/2;
        global_params(2:4) = orientation;

        % move up a scale if possible
        if(clmParams.useMultiScale && scale ~= numel(patchExperts))

            % only go up a scale if we don't need to upsample
            if(0.9 * patchExperts(scale+1).trainingScale < global_params(1))
                scale = scale + 1;
            else
                break;
            end
        end
    end

    % the view in last iteration
    view_used = view;

    % See how good the tracking was in the end
    [shape2D] = GetShapeOrtho(M, PDM.V, local_params, global_params);

    % Moving to matlab format
    shape2D = shape2D(:,1:2) + 1;

end

function [id] = GetView(centers, rotation)

    [~,id] = min(sum((centers * pi/180 - repmat(rotation', size(centers,1), 1)).^2,2));

end


function [expr morder] = sim_ex_Mullen_2011_Seizure(varargin)
% Simulation:  Epileptic Seizure
%
% Description:
%
%   13-variate VAR[6] system of stocastically-forced, damped coupled oscillators with time-varying (non-stationary) coupling.
%
%   This simulation creates a simulated "seizure" with time-varying coupling between clusters of sources which switches between 4 different stages.
%
%   The simulation is designed to be single-trial.
%
%   Recommended Settings:
%   Number of trials: 1
%   Sampling Rate:    100
%
%   The directed graph for this model can be viewed by executing the following command:
%
%   >>hlp_viewGraphicsResource('/sim/Mullen_2011_Seizure.jpg');
%
%
% Author Credits:
%
%   Tim Mullen, 2011
%
% References and Code:
%
%   Mullen, 2011. Unpublished
%
% ------------------------------------------------------------------------

% Cluster 1
f1   = .2;      % central frequency
tau1 = .2;      % damping time (larger-->highest variance)
% Cluster 2
f2   = .20;
tau2 = 3;
% Cluster 3
f3   = .10;
tau3 = 7;
% Cluster 4
f4   = .10;
tau4 = 6;

% start of seizure (1 minute)
S0 = 6000;
% set the approximate durations of each stage
S1_duration = 500;  % 5 sec at 100 Hz
S2_duration = 500;
S3_duration = 500;
S4_duration = 500;
% set seizure stage mid-points
S1_center = S0+S1_duration/2;                      % center of stage 1
S2_center = S1_center+S1_duration;                 % center of stage 2
S3_center = S2_center;                             % center of stage 3
S4_center = S3_center+S3_duration;                 % center of stage 4

Offset = 0;
SamplingRate = 1;

% specify the default system of equations
expr_def = { ...
          sprintf('x1(t) = {2*exp(-1/(%f+normpdfg(t,%f,%f,%f,100)))*cos(2*pi*%f/1)}*x1(t-1) + {-exp(-2/(%f+normpdfg(t,%f,%f,%f,100)))}*x1(t-2) + e1(t)',tau1,S1_duration/2,8,Offset+S1_center, f1, tau1,S1_duration,8,Offset+S1_center) ...                    % Ictal driver
          sprintf('x2(t) = %s + {normpdfg(t,%f,%f,%f,0.01)}*x3(t-2) + {normpdfg(t,%f,%f,%f,0.01)}*x4(t-2)  + {normpdfg(t,%f,%f,%f,1.3)}*x1(t-6)    + e2(t)',sim_dampedOscillator(f2,tau2,SamplingRate,2),  S1_duration/2,8,Offset+S1_center,   S1_duration/2,8,Offset+S1_center,  S1_duration/2,8,Offset+S1_center) ...                     % CLUSTER 2
          sprintf('x3(t) = %s +  {normpdfg(t,%f,%f,%f,0.7)}*x2(t-2) + {normpdfg(t,%f,%f,%f,0.1)}*x4(t-2)  + {normpdfg(t,%f,%f,%f,0.3)}*x5(t-3)    + e3(t)',sim_dampedOscillator(f2-1,tau2,SamplingRate,3),  S1_duration/2,8,Offset+S1_center,   S1_duration/2,8,Offset+S1_center,   (S2_duration+S3_duration)/2,10,Offset+S3_center) ...       % CLUSTER 2
          sprintf('x4(t) = %s +  {normpdfg(t,%f,%f,%f,0.7)}*x2(t-2) + {normpdfg(t,%f,%f,%f,0.1)}*x3(t-2)                                          + e4(t)',sim_dampedOscillator(f2+1,tau2,SamplingRate,4),  S1_duration/2,8,Offset+S1_center,  S1_duration/2,8,Offset+S1_center) ... % CLUSTER 2
          sprintf('x5(t) = %s +  {normpdfg(t,%f,%f,%f,0.001)}*x6(t-2) + {normpdfg(t,%f,%f,%f,0.5)}*x3(t-3)        + e5(t)'  ,sim_dampedOscillator(f3,tau3,SamplingRate,5), S3_duration/2,8,Offset+S3_center , S3_duration/2,8,Offset+S3_center) ...  % CLUSTER 3
          sprintf('x6(t) = %s +  {normpdfg(t,%f,%f,%f,0.001)}*x5(t-2)                                             + e6(t)'  ,sim_dampedOscillator(f3,tau3,SamplingRate,6), S3_duration/2,8,Offset+S3_center) ...                                  % CLUSTER 3
          sprintf('x7(t) = %s + {normpdfg(t,%f,%f,%f,1.3)}*x4(t-6) + {normpdfg(t,%f,%f,%f,0.01)}*x9(t-2) + {normpdfg(t,%f,%f,%f,0.01)}*x8(t-2) + {normpdfg(t,%f,%f,%f,0.01)}*x10(t-2)    + e7(t)' ,sim_dampedOscillator(f4,tau4,SamplingRate,7),     S4_duration/2,8,Offset+S4_center,     S4_duration/2,8,Offset+S4_center,     S4_duration/2,8,Offset+S4_center,     S4_duration/2,8,Offset+S4_center) ...  % CLUSTER 4
          sprintf('x8(t) = %s + {normpdfg(t,%f,%f,%f,0.6)}*x7(t-2)         + e8(t)' ,sim_dampedOscillator(f4,tau4,SamplingRate,8),     S4_duration/2,8,Offset+S4_center) ... % CLUSTER 4
          sprintf('x9(t) = %s + {normpdfg(t,%f,%f,%f,0.6)}*x7(t-2)         + e9(t)' ,sim_dampedOscillator(f4+1,tau4,SamplingRate,9),   S4_duration/2,8,Offset+S4_center) ... % CLUSTER 4
          sprintf('x10(t)= %s + {normpdfg(t,%f,%f,%f,0.6)}*x7(t-2)         + e10(t)',sim_dampedOscillator(f4-1,tau4,SamplingRate,10),  S4_duration/2,8,Offset+S4_center) ... % CLUSTER 4
          sprintf('x11(t)= 1.3*x11(t-1) + -0.8*x11(t-2)                             + e11(t)') ...    % Decoupled
          sprintf('x12(t)= 1.2*x12(t-1) + -0.4*x12(t-2)                             + e12(t)') ...    % Decoupled
          sprintf('x13(t)= 0.8*x13(t-1) + -0.4*x13(t-2) + -0.1*x13(t-4)             + e13(t)') ...    % Decoupled
          };

% set up argument definitions
arg_define(varargin, ...
    arg({'expr','DynamicalEquations'},expr_def,[],'System of equations'), ...
    arg({'morder','ModelOrder'},6,[1 Inf],'Model order. This is mandatory'), ...
    arg_subtoggle({'setDynamics','SetDynamics'},'off', ...
        {...
            arg_sub({'c1','Cluster1'},{},...
            { ...
                arg({'f0','Freq'},f1,[0 Inf],'Central frequency (Hz). This is in normalized Hz (i.e. f0=0.5=SamplingRate/2).'), ...
                arg({'tau','DampingTime'},tau1,[0 Inf],'Damping time') ...
            },'Cluster 1 Settings'), ...
            arg_sub({'c2','Cluster2'},{},...
            { ...
                arg({'f0','Freq'},f2,[0 Inf],'Central frequency (Hz). This is in normalized Hz (i.e. f0=0.5=SamplingRate/2).'), ...
                arg({'tau','DampingTime'},tau2,[0 Inf],'Damping time') ...
            },'Cluster 2 Settings'), ...
            arg_sub({'c3','Cluster3'},{},...
            { ...
                arg({'f0','Freq'},f3,[0 Inf],'Central frequency (Hz). This is in normalized Hz (i.e. f0=0.5=SamplingRate/2).'), ...
                arg({'tau','DampingTime'},tau3,[0 Inf],'Damping time') ...
            },'Cluster 3 Settings'), ...
            arg_sub({'c4','Cluster4'},{},...
            { ...
                arg({'f0','Freq'},f4,[0 Inf],'Central frequency (Hz). This is in normalized Hz (i.e. f0=0.5=SamplingRate/2).'), ...
                arg({'tau','DampingTime'},tau4,[0 Inf],'Damping time') ...
            },'Cluster 4 Settings'), ...
            arg_sub({'s1','Stage1'},{},...
            { ...
                arg({'duration','Duration'},S1_duration,[0 Inf],'Duration (samples)'), ...
                arg({'center','Center'},S1_center,[0 Inf],'Midpoint of stage (samples)'), ...
            },'Stage 1 Settings'), ...
            arg_sub({'s2','Stage2'},{},...
            { ...
                arg({'duration','Duration'},S2_duration,[0 Inf],'Duration (samples)'), ...
                arg({'center','Center'},S2_center,[0 Inf],'Midpoint of stage (samples)'), ...
            },'Stage 2 Settings'), ...
            arg_sub({'s3','Stage3'},{},...
            { ...
                arg({'duration','Duration'},S3_duration,[0 Inf],'Duration (samples)'), ...
                arg({'center','Center'},S3_center,[0 Inf],'Midpoint of stage (samples)'), ...
            },'Stage 3 Settings'), ...
            arg_sub({'s4','Stage4'},{},...
            { ...
                arg({'duration','Duration'},S4_duration,[0 Inf],'Duration (samples)'), ...
                arg({'center','Center'},S4_center,[0 Inf],'Midpoint of stage (samples)'), ...
            },'Stage 4 Settings'), ...
            arg({'szStart','SeizureStart'},S0,[0 Inf],'Start of seizure (samples)'), ...
        },'Override default system dynamics'));

if isempty(morder)
    error('SIFT:sim_examples:badParam','ModelOrder must be specified');
end

if g.setDynamics.arg_selection
    % re-evaluate expression with user-defined settings

    % set central frequency and damping times
    % Cluster 1
    f1   = g.setDynamics.c1.f0;      % central frequency
    tau1 = g.setDyamics.c1.tau;      % damping time (larger-->highest variance)
    % Cluster 2
    f2   = g.setDynamics.c2.f0;
    tau2 = g.setDyamics.c2.tau;
    % Cluster 3
    f3   = g.setDyamics.c3.f0;
    tau3 = g.setDyamics.c3.tau;
    % Cluster 4
    f4   = g.setDyamics.c4.f0;
    tau4 = g.setDyamics.c4.tau;

    % set start of seizure
    S0 = g.setDynamics.szStart;

    % set the approximate durations of each stage
    S1_duration = g.setDynamics.s1.duration;
    S2_duration = g.setDynamics.s2.duration;
    S3_duration = g.setDynamics.s3.duration;
    S4_duration = g.setDynamics.s4.duration;

    % set seizure stage mid-points
    if ~isempty(g.setDynamics.s1.center)
        S1_center = g.setDynamics.s1.center;
    else
        S1_center = S0+S1_duration/2;
    end
    if ~isempty(g.setDynamics.s2.center)
        S2_center = g.setDynamics.s2.center;
    else
        S2_center = S1_center+S1_duration;
    end
    if ~isempty(g.setDynamics.s3.center)
        S3_center = g.setDynamics.s3.center;
    else
         S3_center = S2_center;
    end
    if ~isempty(g.setDynamics.s4.center)
        S4_center = g.setDynamics.s4.center;
    else
        S4_center = S3_center+S3_duration;
    end

    % generate system of equations
    expr = { ...
          sprintf('x1(t) = {2*exp(-1/(%f+normpdfg(t,%f,%f,%f,100)))*cos(2*pi*%f/1)}*x1(t-1) + {-exp(-2/(%f+normpdfg(t,%f,%f,%f,100)))}*x1(t-2) + e1(t)',tau1,S1_duration/2,8,Offset+S1_center, f1, tau1,S1_duration,8,Offset+S1_center) ...                    % Ictal driver
          sprintf('x2(t) = %s + {normpdfg(t,%f,%f,%f,0.01)}*x3(t-2) + {normpdfg(t,%f,%f,%f,0.01)}*x4(t-2)  + {normpdfg(t,%f,%f,%f,1.3)}*x1(t-6)    + e2(t)',sim_dampedOscillator(f2,tau2,SamplingRate,2),  S1_duration/2,8,Offset+S1_center,   S1_duration/2,8,Offset+S1_center,  S1_duration/2,8,Offset+S1_center) ...                     % CLUSTER 2
          sprintf('x3(t) = %s +  {normpdfg(t,%f,%f,%f,0.7)}*x2(t-2) + {normpdfg(t,%f,%f,%f,0.1)}*x4(t-2)  + {normpdfg(t,%f,%f,%f,0.3)}*x5(t-3)    + e3(t)',sim_dampedOscillator(f2-1,tau2,SamplingRate,3),  S1_duration/2,8,Offset+S1_center,   S1_duration/2,8,Offset+S1_center,   (S2_duration+S3_duration)/2,10,Offset+S3_center) ...       % CLUSTER 2
          sprintf('x4(t) = %s +  {normpdfg(t,%f,%f,%f,0.7)}*x2(t-2) + {normpdfg(t,%f,%f,%f,0.1)}*x3(t-2)                                          + e4(t)',sim_dampedOscillator(f2+1,tau2,SamplingRate,4),  S1_duration/2,8,Offset+S1_center,  S1_duration/2,8,Offset+S1_center) ... % CLUSTER 2
          sprintf('x5(t) = %s +  {normpdfg(t,%f,%f,%f,0.001)}*x6(t-2) + {normpdfg(t,%f,%f,%f,0.5)}*x3(t-3)        + e5(t)'  ,sim_dampedOscillator(f3,tau3,SamplingRate,5), S3_duration/2,8,Offset+S3_center , S3_duration/2,8,Offset+S3_center) ...  % CLUSTER 3
          sprintf('x6(t) = %s +  {normpdfg(t,%f,%f,%f,0.001)}*x5(t-2)                                             + e6(t)'  ,sim_dampedOscillator(f3,tau3,SamplingRate,6), S3_duration/2,8,Offset+S3_center) ...                                  % CLUSTER 3
          sprintf('x7(t) = %s + {normpdfg(t,%f,%f,%f,1.3)}*x4(t-6) + {normpdfg(t,%f,%f,%f,0.01)}*x9(t-2) + {normpdfg(t,%f,%f,%f,0.01)}*x8(t-2) + {normpdfg(t,%f,%f,%f,0.01)}*x10(t-2)    + e7(t)' ,sim_dampedOscillator(f4,tau4,SamplingRate,7),     S4_duration/2,8,Offset+S4_center,     S4_duration/2,8,Offset+S4_center,     S4_duration/2,8,Offset+S4_center,     S4_duration/2,8,Offset+S4_center) ...  % CLUSTER 4
          sprintf('x8(t) = %s + {normpdfg(t,%f,%f,%f,0.6)}*x7(t-2)         + e8(t)' ,sim_dampedOscillator(f4,tau4,SamplingRate,8),     S4_duration/2,8,Offset+S4_center) ... % CLUSTER 4
          sprintf('x9(t) = %s + {normpdfg(t,%f,%f,%f,0.6)}*x7(t-2)         + e9(t)' ,sim_dampedOscillator(f4+1,tau4,SamplingRate,9),   S4_duration/2,8,Offset+S4_center) ... % CLUSTER 4
          sprintf('x10(t)= %s + {normpdfg(t,%f,%f,%f,0.6)}*x7(t-2)         + e10(t)',sim_dampedOscillator(f4-1,tau4,SamplingRate,10),  S4_duration/2,8,Offset+S4_center) ... % CLUSTER 4
          sprintf('x11(t)= 1.3*x11(t-1) + -0.8*x11(t-2)                             + e11(t)') ...    % Decoupled
          sprintf('x12(t)= 1.2*x12(t-1) + -0.4*x12(t-2)                             + e12(t)') ...    % Decoupled
          sprintf('x13(t)= 0.8*x13(t-1) + -0.4*x13(t-2) + -0.1*x13(t-4)             + e13(t)') ...    % Decoupled
          };
end


function [C,h,Ph,F,Fa,Fc,Eh,Ch,hE,hC,Q] = spm_reml_sc(YY,X,Q,N,hE,hC,V)
% ReML estimation of covariance components from y*y' - proper components
% FORMAT [C,h,Ph,F,Fa,Fc,Eh,Ch,hE,hC,Q] = spm_reml_sc(YY,X,Q,N,[hE,hC,V])
%
% YY  - (m x m) sample covariance matrix Y*Y'  {Y = (m x N) data matrix}
% X   - (m x p) design matrix
% Q   - {1 x q} covariance components
% N   - number of samples
%
% hE  - hyperprior expectation in log-space [default = -32]
% hC  - hyperprior covariance  in log-space [default = 256]
% V   - fixed covariance component
%
% C   - (m x m) estimated errors = h(1)*Q{1} + h(2)*Q{2} + ...
% h   - (q x 1) ReML hyperparameters h
% Ph  - (q x q) conditional precision of log(h)
%
% hE  - prior expectation of log scale parameters
% hC  - prior covariances of log scale parameters
% Eh  - posterior expectation of log scale parameters
% Ch  - posterior covariances of log scale parameters
%
% Q   - scaled covariance components
%
% F   - [-ve] free energy F = log evidence = p(Y|X,Q) = ReML objective
%
% Fa  - accuracy
% Fc  - complexity (F = Fa - Fc)
%
% Performs a Fisher-Scoring ascent on F to find MAP variance parameter
% estimates.  NB: uses weakly informative log-normal hyperpriors.
% See also spm_reml for an unconstrained version that allows for negative
% hyperparameters.
%
%__________________________________________________________________________
%
% SPM ReML routines:
%
%      spm_reml:    no positivity constraints on covariance parameters
%      spm_reml_sc: positivity constraints on covariance parameters
%      spm_sp_reml: for sparse patterns (c.f., ARD)
%
%__________________________________________________________________________
% Copyright (C) 2007-2017 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_reml_sc.m 7305 2018-05-07 13:35:06Z karl $

% assume a single sample if not specified
%--------------------------------------------------------------------------
try, N; catch, N  = 1;  end
try, V; catch, V  = 0;  end

% initialise h
%--------------------------------------------------------------------------
n     = length(Q{1});
m     = length(Q);
h     = zeros(m,1);
dFdh  = zeros(m,1);
dFdhh = zeros(m,m);
Inn   = speye(n,n);

[PQ{1:m}] = deal(zeros(n,n));

% ortho-normalise X
%--------------------------------------------------------------------------
if isempty(X)
    X = sparse(n,0);
    R = Inn;
else
    X = spm_svd(X,0);
    R = Inn - X*X';
end

% check fixed component
%--------------------------------------------------------------------------
if length(V) == 1
    V = V*Inn;
end

% initialise and specify hyperpriors
%==========================================================================

% scale Q and YY
%--------------------------------------------------------------------------
sY = spm_trace(R,YY)/(N*n);
YY = YY/sY;
V  = V/sY;
for i = 1:m
    sh(i,1) = spm_trace(R,Q{i})/n;
    Q{i}    = Q{i}/sh(i);
end

% hyperpriors
%--------------------------------------------------------------------------
try, hE = hE(:);        catch, hE = -32;   end
try, hP = spm_inv(hC);  catch, hP = 1/256; end

% check sise
%--------------------------------------------------------------------------
if length(hE) < m, hE = hE(1)*ones(m,1);   end
if length(hP) < m, hP = hP(1)*speye(m,m);  end

% intialise h: so that sum(exp(h)) = 1
%--------------------------------------------------------------------------
if any(diag(hP) > exp(16))
    h = hE;
end

% ReML (EM/VB)
%--------------------------------------------------------------------------
dF    = Inf;
as    = 1:m;
t     = 4;
for k = 1:32

    % compute current estimate of covariance
    %----------------------------------------------------------------------
    C     = V;
    for i = as
        C = C + Q{i}*exp(h(i));
    end
    iC    = spm_inv(C);

    % E-step: conditional covariance cov(B|y) {Cq}
    %======================================================================
    iCX    = iC*X;
    if ~isempty(X)
        Cq = inv(X'*iCX);
    else
        Cq = sparse(0);
    end

    % M-step: ReML estimate of hyperparameters
    %======================================================================

    % Gradient dF/dh (first derivatives)
    %----------------------------------------------------------------------
    P     = iC  - iCX*Cq*iCX';
    U     = Inn - P*YY/N;
    for i = as

        % dF/dh = -trace(dF/diC*iC*Q{i}*iC)
        %------------------------------------------------------------------
        PQ{i}   = P*Q{i};
        dFdh(i) = -spm_trace(PQ{i},U)*N/2;

    end

    % Expected curvature E{dF/dhh} (second derivatives)
    %----------------------------------------------------------------------
    for i = as
        for j = as

            % dF/dhh = -trace{P*Q{i}*P*Q{j}}
            %--------------------------------------------------------------
            dFdhh(i,j) = -spm_trace(PQ{i},PQ{j})*N/2;
            dFdhh(j,i) =  dFdhh(i,j);

        end
    end

    % modulate
    %----------------------------------------------------------------------
    dFdh  = dFdh.*exp(h);
    dFdhh = dFdhh.*(exp(h)*exp(h)');

    % add hyperpriors
    %----------------------------------------------------------------------
    e     = h     - hE;
    dFdh  = dFdh  - hP*e;
    dFdhh = dFdhh - hP;

    % Fisher scoring: update dh = -inv(ddF/dhh)*dF/dh
    %----------------------------------------------------------------------
    dh    = spm_dx(dFdhh(as,as),dFdh(as),{t});
    h(as) = h(as) + dh;

    % predicted change in F - increase regularisation if increasing
    %----------------------------------------------------------------------
    pF    = dFdh(as)'*dh;
    if pF > dF
        t = t - 1;
    else
        t = t + 1/8;
    end
    dF    = pF;

    % convergence
    %----------------------------------------------------------------------
    fprintf('%s %-23d: %10s%e [%+3.2f]\n','  ReML Iteration',k,'...',full(dF),t);
    if dF < 1e-2
        break
    else
        % eliminate redundant components (automatic selection)
        %------------------------------------------------------------------
        as  = find(h > hE);
        as  = as(:)';
    end
end

% log evidence = ln p(y|X,Q) = ReML objective = F = trace(R'*iC*R*YY)/2 ...
%--------------------------------------------------------------------------
Ph    = -dFdhh;
if nargout > 3

    % tr(hP*inv(Ph)) - nh (complexity KL cost of parameters = 0)
    %----------------------------------------------------------------------
    Ft = trace(hP/Ph) - length(Ph);

    % complexity - KL(Ph,hP)
    %----------------------------------------------------------------------
    Fc = Ft/2 + e'*hP*e/2 + spm_logdet(Ph/hP)/2;

    % Accuracy - ln p(Y|h)
    %----------------------------------------------------------------------
    Fa = Ft/2 - spm_trace(C*P,YY*P)/2 - N*n*log(2*pi)/2  - N*spm_logdet(C)/2;

    % Free-energy
    %----------------------------------------------------------------------
    F  = Fa - Fc - N*n*log(sY)/2;

end

% priors and posteriors of log parameters (with scaling)
%--------------------------------------------------------------------------
if nargout > 7

    hE = hE + log(sY) - log(sh);
    hC = spm_inv(hP);
    Eh = h  + log(sY) - log(sh);
    Ch = spm_inv(Ph);

end

% return exp(h) hyperpriors and rescale
%--------------------------------------------------------------------------
h  = sY*exp(h)./sh;
C  = sY*C;


function value = year_length_gregorian ( y )

%*****************************************************************************80
%
%% YEAR_LENGTH_GREGORIAN returns the number of days in a Gregorian year.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year to be checked.
%
%    Output, integer VALUE, the number of
%    days in the year.
%
  if ( y == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'YEAR_LENGTH_GREGORIAN - Fatal error!\n' );
    fprintf ( 1, '  Illegal Y = 0.\n' );
    error ( 'YEAR_LENGTH_GREGORIAN - Fatal error!' );
  end

  if ( year_is_leap_gregorian ( y ) )
    value = 366;
  else
    value = 365;
  end

  return
end


function model = spm_mvb(X,Y,X0,U,V,nG,sG)
% Bayesian optimisation of a multivariate linear model with a greedy search
% FORMAT model = spm_mvb(X,Y,X0,U,V,nG,sG)
%
% X      - contrast or target vector
% Y      - date feature matrix
% X0     - confounds
% U      - patterns
% V      - observation noise covariance
% nG     - number of Greedy iterations (nG = 1 => uniform hyperpriors)
%        - if not specified, the search will terminate when F falls
% sG     - size of successive subdivisions [default is 1/2)
%
% returns model:
%                F: log-evidence [F(0), F(1),...]
%                G: covariance partition indices
%                h: covariance hyperparameters
%                U: ordered patterns
%                M: MAP projector: qE = M*X
%               qE: conditional expectation of voxel weights
%               qC: conditional variance of voxel weights
%               Cp: empirical prior covariance (ordered  pattern space)
%               cp: empirical prior covariance (original pattern space)
%__________________________________________________________________________
%
% model: X = Y*P + X0*Q + R
%        P = U*E;
%   cov(E) = h1*diag(G(:,1)) + h2*diag(G(:,2)) + ...
%
% This routine uses a multivariate Bayesian (MVB) scheme to decode or
% recognise brain states from neuroimages. It resolves the ill-posed
% many-to-one mapping, from voxel values or data features to a target
% variable, using a parametric empirical or hierarchical Bayesian model.
% This model is inverted using standard variational techniques, in this
% case Variational Laplace, to furnish the model evidence and the
% conditional density of the model's parameters. This allows one to compare
% different models or hypotheses about the mapping from functional or
% structural anatomy to perceptual and behavioural consequences (or their
% deficits). The aim of MVB is not to predict (because the outcomes are
% known) but to enable inference on different models of structure-function
% mappings; such as distributed and sparse representations. This allows one
% to optimise the model itself and produce predictions that outperform
% standard pattern classification approaches, like support vector machines.
% Technically, the model inversion and inference uses the same empirical
% Bayesian procedures developed for ill-posed inverse problems (e.g.,
% source reconstruction in EEG).
%
% CAUTION: MVB should not be used to establish a significant mapping
% between brain states and some classification or contrast vector. Its use
% is limited to comparison of different models under the assumption
% (hyperprior) that this mapping exists. To ensure the mapping exists, use
% CVA or related approaches.
%
% See spm_mvb_ui and:
%
% Bayesian decoding of brain images.
% Friston K, Chu C, Mourão-Miranda J, Hulme O, Rees G, Penny W, Ashburner J.
% Neuroimage. 2008 Jan 1;39(1):181-205
%
% Multiple sparse priors for the M/EEG inverse problem.
% Friston K, Harrison L, Daunizeau J, Kiebel S, Phillips C, Trujillo-Barreto
% N, Henson R, Flandin G, Mattout J.
% Neuroimage. 2008 Feb 1;39(3):1104-20.
%
% Characterizing dynamic brain responses with fMRI: a multivariate approach.
% Friston KJ, Frith CD, Frackowiak RS, Turner R.
% Neuroimage. 1995 Jun;2(2):166-72.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_mvb.m 7679 2019-10-24 15:54:07Z spm $

% defaults (use splits +/- one standard deviation by default)
%--------------------------------------------------------------------------
try, V;          catch, V  = [];             end
try, nG; aG = 0; catch, nG = 8; aG = 1;      end
try, sG;         catch, sG = 1/2;            end

% get orders
%--------------------------------------------------------------------------
ns     = size(Y,1);                 % number of samples
nv     = size(Y,2);                 % number of voxels
np     = size(U,2);                 % number of patterns or parameters

% confounds
%--------------------------------------------------------------------------
if isempty(X0), X0 = zeros(ns,1);  end
if isempty(U),  U  = zeros(nv,0);  end
if isempty(V),  V  = speye(ns,ns); end

% number of error components
%--------------------------------------------------------------------------
if iscell(V)
    nh = length(V);
else
    nh = 1;
end

% null model
%--------------------------------------------------------------------------
model  = spm_mvb_G(X,zeros(ns,0),X0,[],V);
if ~np, return, end

% initialise G and greedy search
%==========================================================================
F  = model.F;
L  = Y*U;
G  = ones(np,1);
for  i = 1:nG

    % invert
    %----------------------------------------------------------------------
    M  = spm_mvb_G(X,L,X0,G,V);

    % record conditional estimates (and terminate automatically if aG)
    %----------------------------------------------------------------------
    if i == 1
        save_model = 1;
    else
        save_model = M.F > max(F);
    end
    if  save_model
        model      = M;
    elseif aG
        break
    end

    % record free-energy
    %----------------------------------------------------------------------
    F(i + 1)     = M.F;
    lnh          = log(M.h');

    disp('log evidence & hyperparameters:')
    fprintf('% 8.2f',F-F(1)),fprintf('\n')
    fprintf('% 8.2f',full(lnh)),fprintf('\n\n')

    % eliminate redundant components
    %----------------------------------------------------------------------
    lnh          = lnh((nh + 1):end) - lnh(end);
    j            = find(lnh < -16);
    G(:,j)       = [];

    % create new spatial support
    %----------------------------------------------------------------------
    g            = find(G(:,end));
    ng           = ceil(length(g)*sG);
    [q,j]        = sort(-sum(M.qE(g,:).^2,2));
    q            = g(j(1:ng));
    G(q,end + 1) = 1;

    % break if cluster is one
    %----------------------------------------------------------------------
    if ng < 1/(1 - sG), break, end

end

% project pattern weights to feature (voxel) weights
%==========================================================================

% remove some patterns if there are too many
%--------------------------------------------------------------------------
clear M X Y
qE       = sum(model.qE.^2,2);
[i,j]    = sort(-qE);
try
    i    = j(1:2^11);
catch
    i    = j;
end
L        = L(:,i);
U        = U(:,i);
cp       = model.Cp;
Cp       = cp(i,i);
MAP      = U*model.MAP(i,:);

% try to save conditional expectations (if there is enough memory)
%--------------------------------------------------------------------------
try
    qE   = U*model.qE(i,:);
catch
    qE   = [];
end

% remove confounds from L = Y*U
%--------------------------------------------------------------------------
L        = L - X0*pinv(full(X0))*L;

% conditional covariance in voxel space: qC  = U*( Cp - Cp*L'*iC*L*Cp )*U';
%--------------------------------------------------------------------------
UCp      = U*Cp;
qC       = sum(UCp.*U,2) - sum((UCp*L').*MAP,2);

model.F  = F;
model.U  = U;
model.M  = MAP;                             % MAP projector
model.qE = qE;                              % conditional expectation
model.Cp = Cp;                              % prior covariance (ordered)
model.cp = cp;                              % prior covariance (original)
model.qC = max(qC,exp(-16));                % conditional variance


function correct_rate = test_speaker_identification
    % Test speaker identification with TIDIGITS
    USE_CACHE = false;

    USE_GMM = true;
    USE_BINARYSVM = false;
    USE_MULTISVM = false;
    USE_DTW = false;

    db = gendb('tidigits_gyro');
    db = filterdb(db, 'age', '1-Adults');

    if USE_CACHE
        load('speaker_identification_features', 'features', 'labels');
    else
        [features, labels] = ...
            get_speaker_features_and_labels(db, 'speaker');
        savesave('speaker_identification_features', 'features', 'labels');
    end

    db = filterdb(db, 'type', 'WOMAN');
%     db = filterdb(db, 'device', '0094e779d7d1841f');
    db = filterdb(db, 'device', '00a697fa469633a5');
    speakers = get_speakers(db);

    % choose random speakers
    SPEAKERS_NUM = 5;
    TOTAL_NUM = length(speakers);
    speaker_ids = randperm(TOTAL_NUM);
    speaker_ids = sort(speaker_ids(1:SPEAKERS_NUM));
    % set mask
    mask = false(size(labels));
    for i = 1:length(speaker_ids)
       mask(labels == speaker_ids(i)) = true;
    end
    % filter speakers
    features = features(:, mask);
    labels = labels(mask);

    % Choose train and test sets
    cp = cvpartition(labels, 'holdout', 0.1);
    train_labels = labels(cp.training);
    test_labels = labels(cp.test);

    if USE_GMM || USE_MULTISVM || USE_BINARYSVM
        train_features = cell2mat(features(cp.training));
        test_features = cell2mat(features(cp.test));
    end

    if USE_GMM
        NUM_OF_GAUSSIANS = 8;
        NUM_OF_ITERATIONS = 20;
        [mu_train, sigma_train, c_train] = ...
            GMM.gmm_training(train_features, train_labels, NUM_OF_GAUSSIANS, ...
                             NUM_OF_ITERATIONS);
        class = GMM.gmm_classification(test_features, mu_train, sigma_train, c_train);
    end

    if USE_MULTISVM
        class = multisvm(train_features', train_labels, test_features', ...
            'tolkkt', 1e-2, 'kktviolationlevel', 0.1);
    end

    if USE_BINARYSVM
        s = svmtrain(train_features', train_labels, ...
            'tolkkt', 1e-2, 'kktviolationlevel', 0.1);
        class = svmclassify(s, test_features');
    end

    if USE_DTW
        train_features = features(cp.training);
        test_features = features(cp.test);

        class = zeros(size(test_labels));
        display('Classifying...');
        progressbar;
        for i = 1:length(test_labels)
            sample = test_features{i};
            class(i) = dtw_classify_sample(sample, train_features, train_labels);
            progressbar(i/length(test_labels));
        end
    end

    if ~USE_DTW
        u = unique(train_labels);
        class = u(class);
    end

    correct = class == test_labels;
    correct_rate = sum(correct)/length(correct);
    display(correct_rate);
end


function SE = functionPowerOptimization_prodSINR(signal,interference,Pmax,prelogFactor)
%Compute DL power allocation that solves the max product SINR problem,
%using the algorithm in Theorem 7.2.
%
%This function require additional software packages to be used, which
%need to be downloaded and installed separately. These packages are
%developed independently and are delivered with separate licenses.
%The implementation uses CVX (http://cvxr.com/cvx) and has been tested
%using CVX version 2.1. We recommend the use of the Mosek solver (we
%have tested using version 7.1.0.12).
%
%INPUT:
%signal       = K x L matrix where element (k,j) is a_jk in (7.2)
%interference = K x L x K x L matrix where (l,i,j,k) is b_lijk in (7.3)
%Pmax         = Maximum transmit power per BS
%prelogFactor = Prelog factor
%
%OUTPUT:
%SE = K x L matrix where element (k,j) is the downlink SE of UE k in cell j
%     using the max product power allocation solution
%
%
%This Matlab function was developed to generate simulation results to:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.1 (Last edited: 2018-06-08)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Extract number of UEs
K = size(signal,1);

%Extract number of cells
L = size(signal,2);

%% Solve geometric program in (7.8) using CVX
cvx_begin gp
cvx_quiet(true); % This suppresses screen output from the solver

variable rho(K,L);
variable c(K,L);

maximize prod(prod(c))

subject to

for j = 1:L

    for k = 1:K

        if signal(k,j)>0
            %SINR constraints of UE k in cell j
            c(k,j)*(sum(sum(rho.*interference(:,:,k,j))) + 1) <= (rho(k,j)*signal(k,j));

            rho(k,j) >= 0;

        else
            %This applies if UE k in cell j is inactive
            c(k,j) == 1;
            rho(k,j) >= 0;

        end

    end

    sum(rho(:,j)) <= Pmax;

end

cvx_end

%% Analyze the CVX output and prepare the output variables
if isempty(strfind(cvx_status,'Solved')) %The problem was not solved by CVX, for some reason, and we then consider equal power allocation
    rhoSolution = (Pmax/K)*ones(K,L);
else %The problem was solved by CVX
    rhoSolution = rho;
end

%Refine the solution obtained from CVX using the Matlab command fmincon.
%This is particularly important in case CVX fails to solve the problem
A = kron(eye(L),ones(1,K));
B = Pmax*ones(L,1);
options = optimoptions('fmincon','Algorithm','interior-point','MaxFunEvals',50000,'MaxIter',5000);
xend = fmincon(@(x) -functionComputesumSE_DL_poweralloc(x,signal,interference,prelogFactor),rhoSolution(:),A,B,[],[],zeros(K*L,1),[],[],options);
rhoBest = reshape(xend,[K L]);

%Compute the SEs using Theorem 4.6
SE = functionComputeSE_DL_poweralloc(rhoBest,signal,interference,prelogFactor);

function sumSE = functionComputesumSE_DL_poweralloc(rho,signal,interference,prelogFactor)

%Extract number of UEs
K = size(signal,1);

%Extract number of cells
L = size(signal,2);

%Reshape power variable since fmincon optimizes vectors
rho = reshape(rho,[K L]);

%Prepare to save results
SE = zeros(K,L);

for j = 1:L

    for k = 1:K

        if signal(k,j) > 0 %Check if the UE k in cell j is active

            %Compute the SE in Theorem 4.6 using the formulation in (7.1)
            SE(k,j) = prelogFactor*log2((rho(k,j)*signal(k,j)) / (sum(sum(rho.*interference(:,:,k,j))) + 1));

        else %If the UE is inactive

            SE(k,j) = 0;

        end

    end

end

%Compute the sum SE of all cells
sumSE = sum(SE(:));


function tc=TurnningCurver_power(angle,amplitute,binwidth,sampleTime,smoothfactor)

% input:
% amplitute: the calcium signal in each frame;
% angle: the angle in each frame;
% the binwidth to make the histogrom;

% amplitute must have the same length as angle

angle = mod(angle, 360); % Make sure 0 <= theta <= 2*pi
numBins = ceil(360 / binwidth);
x = (0:numBins-1) * 360/numBins + 180./numBins;
tc(:, 1)= x;
%     edges = sort(mod([(x(2:end) + x(1:end-1))/2 (x(end) + x(1) + twoPi)/2], twoPi));
%     edges = [edges edges(1) + twoPi];

[timeperbin, edge] = histcounts(angle(:,1), numBins(:,1), 'binlimits', [0 360]);
Amplituteperbin=zeros(numBins,1);

for i=1:1:size(angle,1)
    if ceil(angle(i)/binwidth)==0
        Amplituteperbin(1)=Amplituteperbin(1)+amplitute(i);
    else
        Amplituteperbin(ceil(angle(i)/binwidth))=Amplituteperbin(ceil(angle(i)/binwidth))+amplitute(i);
    end
end
timeperbin=timeperbin*sampleTime;
tc(:, 2)=Amplituteperbin./(timeperbin'+eps);
tc(:, 3)=timeperbin';
tc(:, 2)=general.smooth(tc(:, 2), smoothfactor);

end


function [angle, check] = WardSimpleRotAux(img1, img2, rect)
%
%
%       rot = WardSimpleRotAux(img1, img2, rect)
%
%       This function computes the Ward's MTB.
%
%       Input:
%           -img1: the target image
%           -img2: the image that needs to be aligned to img1
%
%       Output:
%           -rot: rotation angle (degree) for aligning img2 into img1.
%
%     Copyright (C) 2013-15 Francesco Banterle. A big thank to Greg J. Ward
%     for help during the implementation.
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

[r, c, ~] = size(img1);

maxDivergence = 0.005;

%First block
r_img1  = img1(rect(1):rect(2),rect(3):rect(4),:);
r_img2  = img2(rect(1):rect(2),rect(3):rect(4),:);
r1_shift = WardGetExpShift(r_img1, r_img2);

%Mirror block
rect_mirror(1) = r - rect(2) + 1;
rect_mirror(2) = r - rect(1) + 1;
rect_mirror(3) = c - rect(4) + 1;
rect_mirror(4) = c - rect(3) + 1;

r_img1  = img1(rect_mirror(1):rect_mirror(2),rect_mirror(3):rect_mirror(4),:);
r_img2  = img2(rect_mirror(1):rect_mirror(2),rect_mirror(3):rect_mirror(4),:);
r2_shift = WardGetExpShift(r_img1, r_img2);

dx = rect_mirror(3) - rect(3);
dy = rect_mirror(1) - rect(1);

dxr = dx + 0.5 * (r2_shift(1) - r1_shift(1));
dyr = dy + 0.5 * (r2_shift(2) - r1_shift(2));

value = abs(sqrt((dxr * dxr + dyr * dyr) / (dx * dx + dy * dy)) - 1.0);

if(value <= maxDivergence)
    angle = atan2(dyr, dxr) - atan2(dy, dx);
    angle = (angle * 180.0) / pi;
    check = 1;
else
    angle = 0.0;
    check = 0;
end

end


function result = MTrick_enterFunc(alpha,beta,numCluster,maxIter,inputPath)
    %% ======================================================================
    %%STEP 1: load the data
    fprintf('start load the data...\n');
%     TrainData = load('MTrick/Train.data');
%     TrainData = spconvert(TrainData);
%     TrainLabel = load('MTrick/Train.label');
%     TrainLabel = TrainLabel';
%     TestData = load('MTrick/Test.data');
%     TestData = spconvert(TestData);
%     TestLabel = load('MTrick/Test.label');
%     TestLabel = TestLabel';
    [TrainData, TestData, TrainLabel] = MTrick_loadData(inputPath);
    TrainData = double(TrainData);
    TestData = double(TestData);
    TrainLabel = double(TrainLabel);
%
%     TrainData = sparse(TrainData);
%     TestData = sparse(TestData);
%
%     mex -largeArrayDims mex_Pw_d.c
%     mex -largeArrayDims mex_EMstep.c
%     mex -largeArrayDims mex_logL.c
    TrainData = sparse(TrainData);
    TestData = sparse(TestData);

    result = MTrick1(TrainData,TrainLabel,TestData,alpha,beta,numCluster,maxIter);
end


classdef SparseEA2 < ALGORITHM
% <multi> <real/integer/binary> <large/none> <constrained/none> <sparse>
% Improved SparseEA

%------------------------------- Reference --------------------------------
% Y. Zhang, Y. Tian, and X. Zhang, Improved SparseEA for sparse large-scale
% multi-objective optimization problems, Complex & Intelligent Systems,
% 2021.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Population initialization
            % Calculate the fitness of each decision variable
            TDec    = [];
            TMask   = [];
            TempPop = [];
            Fitness = zeros(1,Problem.D);
            for i = 1 : 1+4*any(Problem.encoding~=4)
                Dec = unifrnd(repmat(Problem.lower,Problem.D,1),repmat(Problem.upper,Problem.D,1));
                Dec(:,Problem.encoding==4) = 1;
                Mask       = eye(Problem.D);
                Population = Problem.Evaluation(Dec.*Mask);
                TDec       = [TDec;Dec];
                TMask      = [TMask;Mask];
                TempPop    = [TempPop,Population];
                Fitness    = Fitness + NDSort([Population.objs,Population.cons],inf);
            end
            % Generate initial population
            Dec = unifrnd(repmat(Problem.lower,Problem.N,1),repmat(Problem.upper,Problem.N,1));
            Dec(:,Problem.encoding==4) = 1;
            Mask = false(Problem.N,Problem.D);
            for i = 1 : Problem.N
                Mask(i,TournamentSelection(2,ceil(rand*Problem.D),Fitness)) = 1;
            end
            Population = Problem.Evaluation(Dec.*Mask);
            [Population,Dec,Mask,FrontNo,CrowdDis] = EnvironmentalSelection([Population,TempPop],[Dec;TDec],[Mask;TMask],Problem.N);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool       = TournamentSelection(2,2*Problem.N,FrontNo,-CrowdDis);
                [OffDec,OffMask] = Operator(Problem,Dec(MatingPool,:),Mask(MatingPool,:),Fitness);
                Offspring        = Problem.Evaluation(OffDec.*OffMask);
                [Population,Dec,Mask,FrontNo,CrowdDis] = EnvironmentalSelection([Population,Offspring],[Dec;OffDec],[Mask;OffMask],Problem.N);
            end
        end
    end
end