function  varargout = formulate_data(source_1, source_2, eI, mode)
% Copyright (c) 2014-present University of Illinois at Urbana-Champaign
% All rights reserved.
%
% Developed by:     Po-Sen Huang, Paris Smaragdis
%                   Department of Electrical and Computer Engineering
%                   Department of Computer Science
%
% eI.winSize: Size of window
% eI.seqLen: unique lengths (in ascending order)
%             files are chopped by these lengths. (ex: [1, 10, 100])
% eI.targetWhiten: Specify the path to the whitening data.
% data_ag: noisy data. cell array of different training lengths.
% target_ag: clean data. cell array of different training lengths.

% mode:
%     0: Training (noisy data and clean data)
%     1: Testing (just noisy data)
%     2: Error testing (noisy data and clean data, both loaded without chunking)

%% Testing code
input_fnames = {};
unique_lengths = [];

%% Set up. During testing, dont know the lengths so cant pre-allocate
eI.fs = 16000;

if mode,
    data_ag = {};
    target_ag = {};  % returns empty targets
    mixture_ag = {};
    item=0;
else % training -- chunk
    seqLenSizes = zeros(1,length(eI.seqLen));

    assert(numel(source_1) == numel(source_2));

    for ifile=1:numel(source_1)
%         if (strcmp(train_files(ifile).name,'.') || strcmp(train_files(ifile).name,'..')), continue; end

%         [train_wav,fs]=wavread([eI.DataPath,'train',filesep,train_files(ifile).name]);
%         eI.fs=fs;
%         train1 = train_wav(:,2); % singing
%         train2 = train_wav(:,1); % music
        train1 = source_1{ifile};
        train2 = source_2{ifile};

%         maxLength=max([length(train1), length(train2)]);
%         train1(end+1:maxLength)=eps;
%         train2(end+1:maxLength)=eps;

%         train1=train1./sqrt(sum(train1.^2));
%         train2=train2./sqrt(sum(train2.^2));

        shift_size=min(eI.circular_step, numel(train2)-1);
        for ioffset=1:shift_size:numel(train2)-shift_size % circle shift
            train2_shift = [train2(ioffset: end), train2(1: ioffset-1)];
            fprintf('%d %d, %d %d, %d\n', ioffset, numel(train2), 1, ioffset-1, numel(train2_shift));
            dmix = train1+ train2_shift;

            % input feature calculate
            [DATA, mixture_spectrum, eI]=compute_features_stft2(dmix, eI);

            [T, innfeat] = size(DATA');
            nfeat=size(mixture_spectrum,1);

            remainder = T;
            for i=length(eI.seqLen):-1:1
              num = floor(remainder/eI.seqLen(i));
              remainder = mod(remainder,eI.seqLen(i));
              seqLenSizes(i) = seqLenSizes(i)+num;
            end
        end

        data_ag = cell(1,length(eI.seqLen));
        target_ag = cell(1,length(eI.seqLen));
        mixture_ag = cell(1,length(eI.seqLen));
        for i=length(eI.seqLen):-1:1
            data_ag{i} = zeros(eI.inputDim*eI.seqLen(i),seqLenSizes(i));
            mixture_ag{i} = zeros(nfeat*eI.seqLen(i),seqLenSizes(i));
            if eI.cleanonly==1,
                target_ag{i} = zeros(nfeat*eI.seqLen(i),seqLenSizes(i));
            else
                target_ag{i} = zeros(2*nfeat*eI.seqLen(i),seqLenSizes(i));
            end

        end

    end
end

seqLenPositions = ones(1,length(eI.seqLen));
% winsize=eI.winsize;
nFFT= eI.nFFT; hop= eI.hop; scf=eI.scf;
% windows=sin(0:pi/winsize:pi-pi/winsize);
wn = sqrt( hann( eI.nFFT, 'periodic')); % hann window

 for ifile=1:numel(source_1)
%     if (strcmp(train_files(ifile).name,'.') || strcmp(train_files(ifile).name,'..')), continue; end

%     [train_wav,fs]=wavread([eI.DataPath,'train',filesep,train_files(ifile).name]);
%     eI.fs=fs;
%     train1 = train_wav(:,2); % singing
%     train2 = train_wav(:,1); % music
    train1 = source_1{ifile};
    train2 = source_2{ifile};

%     maxLength=max([length(train1), length(train2)]);
%     train1(end+1:maxLength)=eps;
%     train2(end+1:maxLength)=eps;

%     train1=train1./sqrt(sum(train1.^2));
%     train2=train2./sqrt(sum(train2.^2));

    shift_step=min(eI.circular_step,numel(train2)-1);
    for ioffset=1:shift_step:numel(train2)-shift_step % circle shift
        train2_shift = [train2(ioffset: end), train2(1: ioffset-1)];
        fprintf('%d %d, %d %d, %d\n', ioffset, numel(train2), 1, ioffset-1, numel(train2_shift));
        dmix = train1+ train2_shift;
        [DATA, mixture_spectrum, eI]=compute_features_stft2(dmix, eI);

%         spectrum.signal =scf* stft(train1, nFFT ,windows, hop, eI.fs);
%         spectrum.noise =scf* stft(train2_shift, nFFT ,windows, hop, eI.fs);

        if eI.MFCCorlogMelorSpectrum==2 || eI.MFCCorlogMelorSpectrum==3
            spectrum.signal = scf* stft2(train1, nFFT ,hop, 0, wn);
            spectrum.noise = scf* stft2(train2_shift, nFFT ,hop, 0, wn);
        else
            spectrum.signal = scf * stft3(train1, nFFT, hop, 0, wn, [], 0);
            spectrum.noise = scf* stft3(train2_shift, nFFT ,hop, 0, wn, [], 0);
        end

        multi_data = DATA;
        [nFeat,T] = size(multi_data);

        %% input normalize
        if eI.inputL1==1, % DATA (NUMCOFS x nSamp)
            % apply CMVN to targets (normalize such that the freq bean equal to zero mean var 1)
            cur_mean = mean(multi_data, 2);
            cur_std = std(multi_data, 0, 2);
            multi_data = bsxfun(@minus, multi_data, cur_mean);
            multi_data = bsxfun(@rdivide, multi_data, cur_std);
        elseif eI.inputL1==2,  % at each time frame, freq sum to 1
            l1norm = sum(multi_data,1)+eps;
            multi_data = bsxfun(@rdivide, multi_data, l1norm);
        end

        %% output normalize
         if eI.outputL1==1
            if eI.cleanonly==1,
                l1norm = sum(abs(spectrum.signal),1)+eps;
                spectrum.signal = bsxfun(@rdivide, spectrum.signal, l1norm);
            else
                l1norm = sum([abs(spectrum.noise); abs(spectrum.signal)],1)+eps;
                spectrum.noise = bsxfun(@rdivide, spectrum.noise, l1norm);
                spectrum.signal = bsxfun(@rdivide, spectrum.signal, l1norm);
            end
         end

         if eI.cleanonly==1,
            clean_data=abs(spectrum.signal);
         else
            clean_data=[abs(spectrum.noise); abs(spectrum.signal)];
         end
        %% zero pad
        if eI.num_contextwin > 1
            % winSize must be odd for padding to work
            if mod(eI.num_contextwin,2) ~= 1
                fprintf(1,'error! winSize must be odd!');
                return
            end;
            % pad with repeated frames on both sides so im2col data
            % aligns with output data
            nP = (eI.num_contextwin-1)/2;
            multi_data = [repmat(multi_data(:,1),1,nP), multi_data, ...
                repmat(multi_data(:,end),1,nP)];
        end;
        %% im2col puts winSize frames in each column
        multi_data_slid = im2col(multi_data,[nFeat, eI.num_contextwin],'sliding');
        % concatenate noise estimate to each input
         if mode == 1, % Testing
            c = find(unique_lengths == T);
            if isempty(c)
                % add new unique length if necessary
                data_ag = [data_ag, multi_data_slid(:)];
                unique_lengths = [unique_lengths, T];
            else
                data_ag{c} = [data_ag{c}, multi_data_slid(:)];
            end;
        elseif mode == 2, % Error analysis.
            c = find(unique_lengths == T);
            if isempty(c)
                % add new unique length if necessary
                data_ag = [data_ag, multi_data_slid(:)];
                target_ag = [target_ag, clean_data(:)];
                mixture_ag = [mixture_ag, mixture_spectrum(:)];
                unique_lengths = [unique_lengths, T];
            else
                data_ag{c} = [data_ag{c}, multi_data_slid(:)];
                target_ag{c} = [target_ag{c}, clean_data(:) ];
                mixture_ag{c} = [mixture_ag{c}, mixture_spectrum(:) ];
            end;
         elseif mode == 3 % formulate one data per cell
            item =item+1;
            % feadim x nframes
            data_ag{item} = multi_data_slid;
            target_ag{item} = clean_data;
            mixture_ag{item} = mixture_spectrum;
         else % training (mode == 0)
            %% put it in the correct cell area.
            while T > 0
                % assumes length in ascending order.
                % Finds longest length shorter than utterance
                c = find(eI.seqLen <= T, 1,'last');

                binLen = eI.seqLen(c);
                assert(~isempty(c),'could not find length bin for %d',T);
                % copy data for this chunk
                data_ag{c}(:,seqLenPositions(c))=reshape(multi_data_slid(:,1:binLen),[],1);
                target_ag{c}(:,seqLenPositions(c))=reshape(clean_data(:,1:binLen),[],1);
                mixture_ag{c}(:,seqLenPositions(c))=reshape(mixture_spectrum(:,1:binLen),[],1);

                seqLenPositions(c) = seqLenPositions(c)+1;
                % trim for next iteration
                T = T-binLen;
                if T > 0
                    multi_data_slid = multi_data_slid(:,(binLen+1):end);
                    clean_data = clean_data(:,(binLen+1):end);
                    mixture_spectrum = mixture_spectrum(:,(binLen+1):end);
                end;
            end;
        end;
    end;
 end

theoutputs = {data_ag, target_ag, mixture_ag};
varargout = theoutputs(1:nargout);

return;

%% Unit test
% % (TODO) add
% eI.DataPath=['.',filesep];
%
% eI.MFCCorlogMelorSpectrum=2; % 0- mfcc, 1- logmel, 2- spectrum
% eI.winsize = 1024;    eI.nFFT = 1024;    eI.hop =eI.winsize/2;    eI.scf=1;%scf = 2/3;
% eI.featDim =513;
% eI.num_contextwin=3;
% eI.inputDim = eI.featDim * eI.num_contextwin;
%
% train_files= dir( [eI.DataPath, 'train',filesep,'*wav']);
%
% eI.seqLen = [1 50 100];
% eI.inputL1=0;eI.outputL1=0;
% eI.circular_step=100000; %eI.hop = eI.winsize/2
% formulate_data(train_files, eI, 0)

%% Unit test
eI.DataPath=['.',filesep];

eI.MFCCorlogMelorSpectrum=2; % 0- mfcc, 1- logmel, 2- spectrum
eI.winsize = 1024;    eI.nFFT = 1024;    eI.hop =eI.winsize/2;    eI.scf=1;%scf = 2/3;
eI.featDim =513;
eI.num_contextwin=3;
eI.inputDim = eI.featDim * eI.num_contextwin;

train_files= dir( [eI.DataPath, 'train',filesep,'*wav']);

eI.seqLen = [1 50 100];
eI.inputL1=0;eI.outputL1=0;
eI.circular_step=100000; %eI.hop = eI.winsize/2
formulate_data(source_1, source_2, eI, 0)
end


classdef CaffePooling < dagnn.Filter
%CAFFEPOOLING - This layer implements Caffe-style pooling, which applies
% the pooling kernel to the padding, as well as to the input (unlike matconvnet
% which does not include the padding itself in the kernel computation).
% To mimic this effect, we "pre-pad" the input using an empty convolution, then
% pass to the standard pooling layer.

  properties
    method = 'max'
    poolSize = [1 1]
    opts = {'cuDNN'}
  end

  methods
    function outputs = forward(self, inputs, params)
      outputs{1} = vl_nncaffepool(inputs{1}, self.poolSize, ...
                                  'pad', self.pad, ...
                                  'method', self.method, ...
                                  'stride', self.stride, ...
                                  'extraArgs', self.opts) ;
    end

    function [derInputs, derParams] = backward(self, inputs, params, derOutputs)
      derInputs{1} = vl_nncaffepool(inputs{1}, self.poolSize, derOutputs{1}, ...
                                  'pad', self.pad, ...
                                  'method', self.method, ...
                                  'stride', self.stride, ...
                                  'extraArgs', self.opts) ;
      derParams = {} ;
    end

    function kernelSize = getKernelSize(obj)
      kernelSize = obj.poolSize ;
    end

    function outputSizes = getOutputSizes(obj, inputSizes)
      outputSizes = getOutputSizes@dagnn.Filter(obj, inputSizes) ;
      outputSizes{1}(3) = inputSizes{1}(3) ;
    end

    function obj = CaffePooling(varargin)
      obj.load(varargin) ;
    end
  end
end


function [A, timePoints, nWindows] = bst_henv(X, Y, Time, OPTIONS)
% BST_HENV Compute the time-varying Coherence and Envelope measures
%
% INPUTS:
%   - X            : Input signals [Nsignals x Ntimes]
%   - Y            : Input signals [Nsignals x Ntimes]
%   - Time         : Time values for all the samples of the input signal
%   - OPTIONS:
%     - SampleRate : Sampling frequency
%     - CohMeasure : Desired measure of connectivity
%                    'coh'  - coherence
%                    'lcoh' - lagged coherence
%                    'penv' - plain envelope correlation (No Orthogonalization)
%                    'oenv' - orthogonalized envelope correlation
%     - WinLength  : window size in second for dynamic networks
%     - WinOverlap : overlap between windows in percentage
%     - HStatDyn   : Time scale of the network ('dynamic' or 'static')
%     - tfSplit    : Number of blocks to split the raw signal for time-frequnecy analysis
%     - isParallel : Parallel Processing option (1 when it is enabled)
%     - tfMeasure  : Time-frequency transformation method (Hilbert/Morlet)
%
% OUTPUTS:
%   - A            : Four-dimensional connectivity matrix (nSignals x nSignals x nWindows x nfBins)
%   - timePoints   : Time vector corresponding to the connectivity matrix
%   - nWindows     : Number of estimator windows
%
% REFERENCES:
%   - Comparisons with previous AEC implementation is discussed on the forum: https://neuroimage.usc.edu/forums/t/30358
%
% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
% Author: Hossein Shahabi, 2020-2022

%% ~~~~~ Checking Input Variables
if (nargin < 4)
    error('Invalid call.');
end

% Signal properties
nX      = size(X,1);
nY      = size(Y,1);
N       = size(X,2);
% Check if the two inputs are the same
sameInp = isequal(X,Y);

% Options for connectivity analysis
Fs        = OPTIONS.SampleRate ;
winSize   = OPTIONS.WinLength * Fs ;
overLap   = OPTIONS.WinOverlap * winSize ;
numBlocks = OPTIONS.tfSplit ;
parMode   = OPTIONS.isParallel ;

% Time-frequency options
OPTIONS_tf.Method        = OPTIONS.tfMeasure;
OPTIONS_tf.Output        = 'all' ;
OPTIONS_tf.Comment       = [] ;
OPTIONS_tf.ListFiles     = [] ;
OPTIONS_tf.iTargetStudy  = [] ;
OPTIONS_tf.RowNames      = cell(nX,1);
OPTIONS_tf.Measure       = 'none' ;

%% ~~~~~~ Constant definition and pre-allocation
% Compute the parameters for window
hopSize    = ceil(winSize-overLap) ;
nWindows   = fix((N-winSize)/hopSize)+1 ;
if (nWindows == 0)
    error('No data to process: the estimator window is maybe larger than the data to process.');
end
timePoints = NaN(nWindows,1) ;                   % Pre-define the center of windows
nfBins     = size(OPTIONS.Freqrange,1) ;         % Number of Frequency bins
A          = zeros(nX,nY,nWindows,nfBins) ;  % Pre-define the connectivity matrix
% Data       = bst_bsxfun(@minus, Data, mean(Data,2)) ;  % Removing mean from the data

%% Block analysis for large files
% Consider a fixed 5 sec for transient (filter) effect (each side)
tranLen = 5 * Fs ;
if numBlocks>1
    [X_Data3D, blockLen3D, actLen] = dataDimTran(X,tranLen,numBlocks) ;
    if ~sameInp
        Y_Data3D = dataDimTran(Y,tranLen,numBlocks) ;
    end
end

%%
for f = 1:nfBins
    %% Time-frequency transformation
    switch OPTIONS.tfMeasure
        case 'hilbert'
            OPTIONS_tf.Freqs        = OPTIONS.Freqrange(f,:);
        case 'morlet'
            OPTIONS_tf.Freqs        = OPTIONS.Freqrange(f) ;
            OPTIONS_tf.MorletFc     = OPTIONS.MorletFc ;
            OPTIONS_tf.MorletFwhmTc = OPTIONS.MorletFwhmTc ;
    end

    % Predefine the complex frequency domain signal
    Xh = zeros(N,nX) ;
    if ~sameInp
        Yh = zeros(N,nY) ;
    end
    if numBlocks>1
        OPTIONS_tf.TimeVector = linspace(Time(1),blockLen3D/Fs,blockLen3D) ;
        for bl = 1:numBlocks
            % Compute Frequency Transform for X
            tfOut_tmp = bst_timefreq(X_Data3D(:,:,bl), OPTIONS_tf) ;
            tfOut_tmp = tfOut_tmp{1}.TF ;
            % We select the middle part of each block and drop "tranLen" on each side
            Xh((bl-1)*actLen+(1:actLen),:) = transpose(tfOut_tmp(:,(1:actLen)+tranLen)) ;
            if ~sameInp
                % Compute Frequency Transform for Y
                tfOut_tmp = bst_timefreq(Y_Data3D(:,:,bl), OPTIONS_tf) ;
                tfOut_tmp = tfOut_tmp{1}.TF ;
                % We select the middle part of each block and drop "tranLen" on each side
                Yh((bl-1)*actLen+(1:actLen),:) = transpose(tfOut_tmp(:,(1:actLen)+tranLen)) ;
            end
        end
    else
        OPTIONS_tf.TimeVector = Time;
        % Compute Frequency Transform for X
        tfOut_tmp = bst_timefreq(X, OPTIONS_tf) ;
        tfOut_tmp = tfOut_tmp{1}.TF ;
        Xh        = transpose(tfOut_tmp) ;
        if ~sameInp
            % Compute Frequency Transform for X
            tfOut_tmp = bst_timefreq(Y, OPTIONS_tf) ;
            tfOut_tmp = tfOut_tmp{1}.TF ;
            Yh        = transpose(tfOut_tmp) ;
        end
    end

    %% Connectivity computation
    for t = 1:nWindows
        % Display progress in command window
        strProgress = sprintf('HENV> Connectivity analysis: win%4d/%d, freq%4d/%d', t, nWindows, f, nfBins);
        if (t > 1) || (f > 1)
            strProgress = [repmat('\b', 1, length(strProgress)), strProgress];
        end
        fprintf(1, strProgress);
        % Selecting the appropriate time points
        tRange = (t-1)*hopSize+(1:winSize) ;
        % Center of the window
        timePoints(t) = median(tRange(1:end-1)) ;
        % Complex signals in the current window range
        tXh = Xh(tRange,:) ;
        tYh = tXh ;
        if ~sameInp
            tYh = Yh(tRange,:) ;
        end
        % Computing auto and cross Spectrums
        Sxy     = (transpose(tXh)*conj(tYh))/winSize ;
        Sxx     = real(diag((transpose(tXh)*conj(tXh))/winSize)) ;
        Syy     = real(diag((transpose(tYh)*conj(tYh))/winSize)) ;
        SxxSyy  = Sxx*Syy' ;
        % Pre-define the connectivity matrix for the current freq and window
        CorrMat = zeros(nX,nY) ;
        % Compute the desired measure
        switch OPTIONS.CohMeasure
            case 'coh'  % Coherence
                CorrMat = Sxy./sqrt(SxxSyy) ;
            case 'lcoh' % Lagged-Coherence
                CorrMat = (imag(Sxy)./sqrt(SxxSyy-(real(Sxy).^2))) ;
            case 'penv' % Envelope Correlation (Plain - No Orthogonalization)
                CorrMat = HMatCorr(abs(tXh),abs(tYh)) ;
            case 'oenv' % Envelope Correlation (Orthogonalized)
                if parMode     % Using parallel processing
                    parfor k = 1:nX
                        CorrMat(k,:) = HOrthCorr(tXh(:,k),tYh) ;
                    end
                else
                    for k = 1:nX
                        CorrMat(k,:) = HOrthCorr(tXh(:,k),tYh) ;
                    end
                end
        end
        if sameInp
            % No auto-correlation
%            CorrMat(logical(eye(nX))) = 0 ;
            % We assume all measures are real, non-negative, and symmetric
%             A(:,:,t,f) = (abs(CorrMat) + abs(CorrMat'))/2 ;
        else
        end
        A(:,:,t,f) = abs(CorrMat) ;

    end
end
fprintf(1, '\n');

%% Outputs
switch (OPTIONS.HStatDyn)
    case 'dynamic'  % Time-varying networks
        timePoints = timePoints/Fs ;
    case 'static'   % Average among all networks
        A = bst_nanmean(A,3);
        timePoints = median(timePoints/Fs) ;
end
end

function [Data3D,blockLen3D,actLen] = dataDimTran(Data2D,tranLen,numBlocks)
% Total Length of the Data
[nSig,N] = size(Data2D) ;
actLen   = ceil(N/numBlocks) ;
tfBlockRem = rem(N,numBlocks) ;

% If transition length is longer than block length, then the block #2 crashes in the for loop below
% Exclude this case asking the user to use less blocks (FT 11-NOV-2022)
% Reference: https://neuroimage.usc.edu/forums/t/error-using-envelope-correlation-2022/37624
if (actLen <= tranLen)
    error(['When splitting large data in multiple blocks, the function bst_henv.m adds' 10 ...
           'a hard-coded transition of 5s before and after each block.' 10 ...
           'Decrease the number of blocks to obtain individual blocks longer than 5s.']);
end

% Adding zero to the end of the data (few samples)
% WARNING (FT 11-NOV-2022): If tfBlockRem=1, then it adds a completely empty block.
% Shouldn't this last block be discarded instead?
if tfBlockRem ~= 0
    Data2D(:,end:end+(actLen-tfBlockRem)) = 0;
end

% Compute the length of each block
blockLen3D = actLen + 2*tranLen ;
% Pre-define the data in 3D format (Signals x Samples x Blcoks)
Data3D = zeros(nSig,blockLen3D,numBlocks) ;
% Divide data into a 3D structure with overlapping periods
for k = 1:numBlocks
    % First block
    if k == 1
        range2D = 1:(actLen + tranLen) ;
        range3D = (tranLen+1):blockLen3D ;
        % Last block
    elseif k == numBlocks
        range2D = (N-(actLen+tranLen)+1):N ;
        range3D = 1:(actLen + tranLen) ;
    else % Middle blocks
        range2D = ((k-1)*actLen - tranLen) + (1:blockLen3D) ;
        range3D = 1:blockLen3D ;
    end
    Data3D(:,range3D,k) = Data2D(:,range2D) ;
end
end

function ConVec = HOrthCorr(tXh,tYh)
Xh_p   = imag(bsxfun(@times, tXh, conj(tYh)./abs(tYh)));
Yh_p   = imag(bsxfun(@times, tYh, conj(tXh)./abs(tXh)));
r1     = HMatCorr(abs(tXh),abs(Yh_p)) ;
r2     = diag(HMatCorr(abs(Xh_p),abs(tYh)))' ;
ConVec = (abs(r1)+abs(r2))/2 ;
end

function At = HMatCorr(U,V)
% Implementation H.Shahabi (2021)
% % Number of channels
% n1 = size(U,2) ;
% n2 = size(V,2) ;
% At = zeros(n1,n2) ;
% for k = 1:n1
%     for m = 1:n2
%         tmp1    = corrcoef(U(:,k),V(:,m)) ;
%         At(k,m) = tmp1(1,2) ;
%     end
% end

% Equivalent implementation F.TADEL (14-11-2022)
Uc = U - mean(U,1);  % Remove mean
Vc = V - mean(V,1);  % Remove mean
At = (Uc' * Vc) ./ sqrt(sum(Uc.*Uc,1) .* sum(Vc.*Vc,1));

end


function [layers, net_output_info, dag_net]=gen_network_featnet_imgraw(featnet_config)

dag_net=dagnn.DagNN();
start_var_name='data_input';

layer_gen_info=[];
layer_gen_info.one_output_dim=3;
layer_gen_info.one_outputs={start_var_name};

layer_gen_info=do_add_img_input_scratch_path(dag_net, featnet_config, layer_gen_info);

net_output_info=[];
net_output_info.var_names=layer_gen_info.one_outputs;
net_output_info.var_dims=layer_gen_info.one_output_dim;

My_net_util.fix_padding_resnet(dag_net);

% input_vars=net.getInputs;
% dag_net.print({input_vars{1}, [400 400 3]}, 'Format', 'dot');

layers=cell(0);

one_layer=[];
one_layer.type='my_custom';
one_layer.custom_type='dagnn_wrapper';
one_layer.forward_fn=@cnn_layer_dagnn_wrapper_forward;
one_layer.backward_fn=@cnn_layer_dagnn_wrapper_backward;
one_layer.layer_update_fn=[];

one_layer.input_var_names=dag_net.getInputs();
one_layer.output_var_names=dag_net.getOutputs();

one_layer.use_single_output=true;

assert(length(one_layer.input_var_names)==1);
assert(length(one_layer.output_var_names)==1);
assert(all(ismember(net_output_info.var_names, one_layer.output_var_names)));

layers{end+1}=one_layer;

end

function layer_gen_info=do_add_img_input_scratch_path(dag_net, featnet_config, layer_gen_info)

layer_gen_info=add_scratch_img_input_block(dag_net, featnet_config, layer_gen_info);

conv_num=featnet_config.conv_num_one_block;
conv_block_num=featnet_config.conv_block_num;
for b_idx=1:conv_block_num

    one_output_dim=featnet_config.filter_num_blocks(b_idx);
    layer_name_prefix=sprintf('inputblock%d', b_idx);

    if b_idx>=2
        layer_gen_info=do_add_res_conv_block_downsample(dag_net, layer_gen_info, one_output_dim, conv_num, layer_name_prefix);
    else
        layer_gen_info=My_net_util.add_res_conv_block(dag_net, layer_gen_info, one_output_dim, conv_num, layer_name_prefix);
    end

end

output_var_name=layer_gen_info.one_outputs{1};
output_var_idx=dag_net.getVarIndex(output_var_name);
dag_net.vars(output_var_idx).fanout=0;

end

function layer_gen_info=add_scratch_img_input_block(dag_net, featnet_config, layer_gen_info)

input_dim=layer_gen_info.one_output_dim;
one_output_dim=featnet_config.filter_num_blocks(1);

filter_size=[3 3 input_dim one_output_dim];
one_inputs=layer_gen_info.one_outputs;
filter_name='imginput_c1';
one_outputs={[filter_name '_outvar']};
My_net_util.add_conv_layer_dagnn(dag_net, one_inputs, one_outputs, false, filter_size, filter_name, true);

tmp_l_idx=dag_net.getLayerIndex(filter_name);
dag_net.layers(tmp_l_idx).block.stride=2;

one_outputs=My_net_util.add_relu_dagnn(dag_net, filter_name, one_outputs);

layer_gen_info.one_output_dim=one_output_dim;
layer_gen_info.one_outputs=one_outputs;

filter_size=[3 3 one_output_dim one_output_dim];
one_inputs=layer_gen_info.one_outputs;
filter_name='imginput_c2';
one_outputs={[filter_name '_outvar']};
My_net_util.add_conv_layer_dagnn(dag_net, one_inputs, one_outputs, false, filter_size, filter_name, true);

layer_gen_info.one_outputs=one_outputs;

end

function layer_gen_info=do_add_res_conv_block_downsample(dag_net, layer_gen_info, output_dim, conv_num, layer_name_prefix)

one_output_dim=layer_gen_info.one_output_dim;
feat_dim_before=one_output_dim;
feat_dim_after=output_dim;
one_outputs_dimred=layer_gen_info.one_outputs;

[one_outputs_dimred, layer_name_dimred]=My_net_util.add_dim_reduce_layer(dag_net, one_outputs_dimred{1}, feat_dim_before, feat_dim_after);

layer_gen_info.one_output_dim=feat_dim_after;

tmp_l_idx=dag_net.getLayerIndex(layer_name_dimred);
dag_net.layers(tmp_l_idx).block.stride=2;

layer_gen_info.one_outputs=one_outputs_dimred;

layer_gen_info=My_net_util.add_res_conv_block(dag_net, layer_gen_info, output_dim, conv_num, layer_name_prefix);

end


function model = gpCovGradsTest(model)

% GPCOVGRADSTEST Test the gradients of the likelihood wrt the covariance.
% FORMAT
% DESC tests the gradients of the covariance to ensure they are
% correct.
% ARG model : the model to be tested.
% RETURN model : the model that was tested.
%
% SEEALSO : gpCreate, gpCovGrads
%
% COPYRIGHT : Neil D. Lawrence, 2006, 2009

% GP

% WARNING --- this isn't testing g_Lambda in gpCovGrads

changeVal = 1e-6;
switch model.approx
 case 'ftc'

 case {'dtc', 'dtcvar', 'fitc', 'pitc'}
  for i =1 :size(model.K_uu, 1)
    for j=1:i
      origK = model.K_uu(i, j);
      model.K_uu(i, j) = origK + changeVal;
      model.K_uu(j, i) = model.K_uu(i, j);
      [model.invK_uu, U] = pdinv(model.K_uu);
      model.logDetK_uu = logdet(model.K_uu, U);
      model = gpUpdateAD(model);
      objPlus = gpLogLikelihood(model);
      model.K_uu(i, j) = origK - changeVal;
      model.K_uu(j, i) = model.K_uu(i, j);
      [model.invK_uu, U] = pdinv(model.K_uu);
      model.logDetK_uu = logdet(model.K_uu, U);
      model = gpUpdateAD(model);
      objMinus = gpLogLikelihood(model);
      diffsK_uu(i, j) = (objPlus - objMinus)/(2*changeVal);
      diffsK_uu(j, i) = diffsK_uu(i, j);
      model.K_uu(i, j) = origK;
      model.K_uu(j, i) = origK;
      [model.invK_uu, U] = pdinv(model.K_uu);
      model.logDetK_uu = logdet(model.K_uu, U);
      model = gpUpdateAD(model);
    end
  end
  for i=1:size(model.K_uf, 1)
    for j=1:size(model.K_uf, 2)
      origK = model.K_uf(i, j);
      model.K_uf(i, j) = origK + changeVal;
      model = gpUpdateAD(model);
      objPlus = gpLogLikelihood(model);
      model.K_uf(i, j) = origK - changeVal;
      model = gpUpdateAD(model);
      objMinus = gpLogLikelihood(model);
      diffsK_uf(i, j) = (objPlus - objMinus)/(2*changeVal);
      model.K_uf(i, j) = origK;
      model = gpUpdateAD(model);
    end
  end

  [gK_uu, gK_uf, g_Lambda] = gpCovGrads(model, model.m);

  gK_uuMaxDiff = max(max(abs(2*(gK_uu-diag(diag(gK_uu))) ...
                             + diag(diag(gK_uu)) ...
                             - diffsK_uu)));
  gK_ufMaxDiff = max(max(abs(gK_uf - diffsK_uf)));

  fprintf('K_uu grad max diff %2.4f\n', gK_uuMaxDiff);
  if gK_uuMaxDiff > 1e-4
    disp(2*(gK_uu-diag(diag(gK_uu))) ...
         + diag(diag(gK_uu)) ...
         - diffsK_uu);
  end
  fprintf('K_uf grad max diff %2.4f\n', gK_ufMaxDiff);
  if gK_ufMaxDiff > 1e-4
    disp(gK_uf - diffsK_uf)
  end
end


function [fimu, imudata, previousImuData]=readimuheader(imufile, ...
    previousImuData, startTime, numPrevImuDataToKeep, imuFileType)
% input
% imuFileType
%   0: plain text 3dm gx3-35 data,
%   1: H764G 1 comprehensive csv
%   2: steval mki 062v2 from Yujia file
%   3: steval iNemo suite output data
%   4: microstrain csv
%   5: epson csv

% imufile: imu file name

% To load IMU data from the very beginning, set startTime <= 0.

% output:
% fimu the file pointer after opening the file
% imudata: the entry of imu data that has timestamp no less than startTime
% of format [time(sec), ax, ay, az(,/s^2), wx, wy, wz(rad/s)]
% previousImuData: stores numPrevImuDataToKeep inertial data that are just
% before the retrieved imu data

if(nargin<5)
    imuFileType=0;
end
switch(imuFileType)
    case 0% plain text 3dm gx3-35 data
        % make sure previousImuData is transferred correctly, no memory leaking
        fimu=fopen(imufile,'r');
        fgetl(fimu);% remove the header
        hstream= fgetl(fimu);
        mass=textscan(hstream,'%f','delimiter',',');
        imudata=mass{1};
        while(imudata(7,1)==0||imudata(1,1)<startTime)
            if(length(previousImuData)==numPrevImuDataToKeep)
                previousImuData(1) = [];
            end
            previousImuData{end + 1} = imudata(:,end);
            hstream= fgetl(fimu);
            mass=textscan(hstream,'%f','delimiter',',');
            imudata=mass{1};
        end
    case 1 % H764G-1 comprehensive csv data
        %open files
        fimu=fopen(imufile,'r');
        h= fgetl(fimu);
        while(true)
            if(contains(h,'RAW 01'))
                break;
            else h= fgetl(fimu);
            end
        end
        h= fgetl(fimu);
        mass=textscan(h,'%f','delimiter',',');
        imudata=mass{1};
        while(imudata(9,1)==0||imudata(2,1)<startTime)
            imudata=imudata(2:8,1);% gps time, xyz delta v, xyz delta theta
            imudata(2:4,1)=imudata(2:4,1)*.3048;% convert to metric unit meter
            if(length(previousImuData)==numPrevImuDataToKeep)
                previousImuData(1) = [];
            end
            previousImuData{end + 1} = imudata(:,end);
            h= fgetl(fimu);
            mass=textscan(h,'%f','delimiter',',');
            imudata=mass{1};
        end
        imudata=imudata(2:8,1);% gps time, xyz delta v, xyz delta theta
        imudata(2:4,1)=imudata(2:4,1)*.3048;% convert to metric unit meter
    case 2 % steval mki 062v2 from Yujia file
        % make sure previousImuData is transferred correctly, no memory leaking
        fimu=fopen(imufile,'r');
        hstream= fgetl(fimu);
        if(isempty(hstream))
            hstream= fgetl(fimu);
        end
        hstream=fgetl(fimu);% discard first two lines
        hstream=fgetl(fimu);
        mass=textscan(hstream,'%f','delimiter',' ');
        imudata=mass{1};
        hstream= fgetl(fimu);
        imu_scalefactor = 9.78;
        while(imudata(1,1)<startTime)
            imudata(2:4,1) = imudata(2:4,1)/1000.0 * imu_scalefactor;
            imudata(5:7,1) = imudata(5:7,1)*pi/180;
            imudata(6,1) = -imudata(6,1); % wrong data format
            if(length(previousImuData)==numPrevImuDataToKeep)
                previousImuData(1) = [];
            end
            previousImuData{end + 1} = imudata(:,end);
            hstream= fgetl(fimu);
            mass=textscan(hstream,'%f','delimiter',' ');
            imudata=mass{1};
            hstream= fgetl(fimu);
        end
        imudata(2:4,1) = imudata(2:4,1)/1000.0 * imu_scalefactor;
        imudata(5:7,1) = imudata(5:7,1)*pi/180;
        imudata(6,1) = -imudata(6,1);
    case 3 % steval iNemo suite output data
        fimu=fopen(imufile,'r');
        fgetl(fimu); % discard first line
        hstream=fgetl(fimu);
        mass=textscan(hstream,'%f','delimiter',',');
        imudata=mass{1};
        imudata(1)=imudata(1)/1000;
        imu_scalefactor = 9.8;
        while(imudata(1,1)<startTime)
            imudata(2:4,1) = imudata(2:4,1)/1000.0 * imu_scalefactor;
            imudata(5:7,1) = imudata(5:7,1)*pi/180;
            imudata(6,1) = -imudata(6,1); % wrong data format
            if(length(previousImuData)==numPrevImuDataToKeep)
                previousImuData(1) = [];
            end
            previousImuData{end + 1} = imudata(:,end);
            hstream= fgetl(fimu);
            mass=textscan(hstream,'%f','delimiter',',');
            imudata=mass{1};
            imudata(1)=imudata(1)/1000;
        end
        imudata(2:4,1) = imudata(2:4,1)/1000.0 * imu_scalefactor;
        imudata(5:7,1) = imudata(5:7,1)*pi/180;
        imudata(6,1) = -imudata(6,1);
    case 4 % microstrain csv data
        fimu=fopen(imufile,'r');
        % remove first 16 lines
        headerLines=16;
        for i=1:headerLines
            fgetl(fimu); % discard first lines
        end
        hstream=fgetl(fimu);
        mass=textscan(hstream,'%f','delimiter',',');
        imudata=mass{1};
        imudata=imudata([3,8:13]);

        while(imudata(1)<startTime || sum(isnan(imudata)))
            if(~sum(isnan(imudata)))
                imudata(2:4)=imudata(2:4)*9.80665;
                if(length(previousImuData)==numPrevImuDataToKeep)
                    previousImuData(1) = [];
                end
                previousImuData{end + 1} = imudata(:,end);
            end
            hstream=fgetl(fimu);
            if (~ischar(hstream))
                imudata=[];
                return;
            end
            mass=textscan(hstream,'%f','delimiter',',');
            imudata=mass{1};
            imudata=imudata([3,8:13]);
        end
        imudata(2:4)=imudata(2:4)*9.80665;
    case 5 % read the header of epson csv data
        fimu=fopen(imufile,'r');
        % remove first 4 lines
        headerLines=4;
        for i=1:headerLines
            fgetl(fimu); % discard header
        end
        hstream=fgetl(fimu);
        mass=textscan(hstream,'%f','delimiter',',');
        imudata=mass{1};
        imudata=imudata([2,[8:10, 5:7]]);
        while(imudata(1)<startTime)
            imudata(2:4)=imudata(2:4)*9.8/1000;
            imudata(5:7)=imudata(5:7)*pi/180;
            if(length(previousImuData)==numPrevImuDataToKeep)
                previousImuData(1) = [];
            end
            previousImuData{end + 1} = imudata(:,end);
            hstream=fgetl(fimu);
            if (~ischar(hstream))
                imudata=[];
                return;
            end
            mass=textscan(hstream,'%f','delimiter',',');
            imudata=mass{1};
            imudata=imudata([2,[8:10, 5:7]]);
        end
        imudata(2:4)=imudata(2:4)*9.80665/1000;
        imudata(5:7)=imudata(5:7)*pi/180;
    otherwise
        fprintf('IMU file type %d is unsupported!\n', imuFileType);
        imudata = [];
end


function rsamobj = medfilt1(rsamobj, nsamples_to_average_over)
% MEDFILT1 Median filter for RSAM objects
% For removing outliers due to transient events, or telemetry spikes
    for c = 1:numel(rsamobj)
        if nargin==1
            nsamples_to_average_over = 3;
        end
        rsamobj(c).data = medfilt1(rsamobj(c).data, nsamples_to_average_over);
    end
end


function [ labels, valid_ids, filenames  ] = extract_FERA2011_labels( FERA2011_dir, recs, aus )
%EXTRACT_SEMAINE_LABELS Summary of this function goes here
%   Detailed explanation goes here

    num_files = numel(recs);

    % speech invalidates lower face AUs
    labels = cell(num_files, 1);
    valid_ids = cell(num_files, 1);
    filenames = cell(num_files, 1);

    file_id = 1;

    for i=1:numel(recs)

        file = [FERA2011_dir, '/', recs{i}, '/', recs{i}, '-au.dat'];

        [~, filename,~] = fileparts(file);
        filenames{file_id} = filename;

        data = dlmread(file); %import annotations for one video file

        speech = data(:,end);

        labels{file_id} = data(:, aus);

        % Finding the invalid regions
        if(aus(1) >= 10)
            valid = speech == 0;
        else
            valid = true(size(speech,1), 1);
        end

        % all indices in SEMAINE are valid
        valid_ids{file_id} = valid;

        file_id = file_id + 1;
    end

    labels = labels(1:file_id-1);
    valid_ids = valid_ids(1:file_id-1);
    filenames = filenames(1:file_id-1);

end


function [heatMaps, prediction] = applyModel_human3d(test_image, param, rectangle, modelID)

caffe.reset_all();

%% check to use click mode or not
click = param.click;

%% select model, 1 for pose estimation, 2 for person detection
model = param.model;
model = model(modelID);
boxsize = model.boxsize;
np = model.np;

%%
oriImg = imread(test_image);
makeFigure = 0;
octave = param.octave;
starting_range = param.starting_range;
ending_range = param.ending_range;
assert(starting_range <= ending_range, 'starting ratio should <= ending ratio');
assert(octave>=1, 'octave should >= 1');

starting_scale = boxsize/(size(oriImg,1)*ending_range);
ending_scale = boxsize/(size(oriImg,1)*starting_range);
multiplier = 2.^(log2(starting_scale):(1/octave):log2(ending_scale));

% set the center and roughly scale range (overwrite the config!) according to the rectangle
    x_start = max(rectangle(1), 1);
    x_end = min(rectangle(1)+rectangle(3), size(oriImg,2));
    y_start = max(rectangle(2), 1);
    y_end = min(rectangle(2)+rectangle(4), size(oriImg,1));

    middle_range = (y_end - y_start) / size(oriImg,1);
    starting_range = middle_range - 0.25;
    ending_range = middle_range + 0.25;

    center = [(x_start + x_end)/2, (y_start + y_end)/2];

    starting_scale = boxsize/(size(oriImg,1)*ending_range);
    ending_scale = boxsize/(size(oriImg,1)*starting_range);
    multiplier = 2.^(log2(starting_scale):(1/octave):log2(ending_scale));

% data container for each scale
score = cell(1);;
peakValue = zeros(length(multiplier), np+1);
pad = cell(1, length(multiplier));
ori_size = cell(1, length(multiplier));

for m = 1
    scale = multiplier(m);
    imageToTest = imresize(oriImg, scale);
    ori_size{m} = size(imageToTest);
    center_s = center * scale;
    [imageToTest, pad{m}] = padAround(imageToTest, boxsize, center_s, model.padValue);
    imageToTest = preprocess(imageToTest, 0.5);
    if(m==1 || ~click)
        caffe.reset_all();
        system(sprintf('sed -i "4s/.*/input_dim: %d/" %s', size(imageToTest,2), model.deployFile));
        system(sprintf('sed -i "5s/.*/input_dim: %d/" %s', size(imageToTest,1), model.deployFile));
        net = caffe.Net(model.deployFile, model.caffemodel, 'test');
    end

    score{m} = applyDNN(imageToTest, net);

    pool_time = size(imageToTest,1) / size(score{m},1);
    score{m} = imresize(score{m}, pool_time);
    score{m} = resizeIntoScaledImg(score{m}, pad{m});
    score{m} = imresize(score{m}, [size(oriImg,2) size(oriImg,1)]);

    if(makeFigure)
        title(sprintf('Current Scale: %f, TOTAL: %f', multiplier(m), sum(peakValue(m,1:np))));
    end
end

%% make heatmaps into the size of scaled image according to pad

    final_score = zeros(size(score{1,1}));
    for m = 1:size(score,2)
        final_score = final_score + score{m};
    end
    heatMaps = permute(final_score, [2 1 3]);

    % generate prediction
    prediction = zeros(np,2);
    for j = 1:np
        [prediction(j,1), prediction(j,2)] = findMaximum(final_score(:,:,j));
    end

function img_out = preprocess(img, mean)
    img_out = double(img)/256;
    img_out = double(img_out) - mean;
    img_out = permute(img_out, [2 1 3]);
    img_out = img_out(:,:,[3 2 1]);
    boxsize = 368;
    centerMapCell = produceCenterLabelMap([boxsize boxsize], boxsize/2, boxsize/2);
    img_out(:,:,4) = centerMapCell{1};

function scores = applyDNN(images, net)
    input_data = {single(images)};
    % do forward pass to get scores
    % scores are now Width x Height x Channels x Num
    s_vec = net.forward(input_data);
    scores = s_vec{1}; % note this score is transposed

function [img_padded, pad] = padAround(img, boxsize, center, padValue)
    center = round(center);
    h = size(img, 1);
    w = size(img, 2);
    pad(1) = boxsize/2 - center(2); % up
    pad(3) = boxsize/2 - (h-center(2)); % down
    pad(2) = boxsize/2 - center(1); % left
    pad(4) = boxsize/2 - (w-center(1)); % right

    pad_up = repmat(img(1,:,:), [pad(1) 1 1])*0 + padValue;
    img_padded = [pad_up; img];
    pad_left = repmat(img_padded(:,1,:), [1 pad(2) 1])*0 + padValue;
    img_padded = [pad_left img_padded];
    pad_down = repmat(img_padded(end,:,:), [pad(3) 1 1])*0 + padValue;
    img_padded = [img_padded; pad_down];
    pad_right = repmat(img_padded(:,end,:), [1 pad(4) 1])*0 + padValue;
    img_padded = [img_padded pad_right];

    center = center + [max(0,pad(2)) max(0,pad(1))];

    img_padded = img_padded(center(2)-(boxsize/2-1):center(2)+boxsize/2, center(1)-(boxsize/2-1):center(1)+boxsize/2, :); %cropping if needed

function [img_padded, pad] = padRightDownCorner(img, padValue)
    h = size(img, 1);
    w = size(img, 2);

    pad(1) = 76; % up
    pad(2) = 76; % left
    pad(3) = 76 + 8 - mod((152+h), 8); % down
    pad(4) = 76 + 8 - mod((152+w), 8); % right

    img_padded = img;
    pad_up = repmat(img_padded(1,:,:), [pad(1) 1 1])*0 + padValue;
    img_padded = [pad_up; img_padded];
    pad_left = repmat(img_padded(:,1,:), [1 pad(2) 1])*0 + padValue;
    img_padded = [pad_left img_padded];
    pad_down = repmat(img_padded(end,:,:), [pad(3) 1 1])*0 + padValue;
    img_padded = [img_padded; pad_down];
    pad_right = repmat(img_padded(:,end,:), [1 pad(4) 1])*0 + padValue;
    img_padded = [img_padded pad_right];

function [x,y] = findMaximum(map)
    [~,i] = max(map(:));
    [x,y] = ind2sub(size(map), i);

function score = resizeIntoScaledImg(score, pad)
    np = size(score,3)-1;
    score = permute(score, [2 1 3]);
    if(pad(1) < 0)
        padup = cat(3, zeros(-pad(1), size(score,2), np), zeros(-pad(1), size(score,2), 1));
        score = [padup; score]; % pad up
    else
        score(1:pad(1),:,:) = []; % crop up
    end

    if(pad(2) < 0)
        padleft = cat(3, zeros(size(score,1), -pad(2), np), zeros(size(score,1), -pad(2), 1));
        score = [padleft score]; % pad left
    else
        score(:,1:pad(2),:) = []; % crop left
    end

    if(pad(3) < 0)
        paddown = cat(3, zeros(-pad(3), size(score,2), np), zeros(-pad(3), size(score,2), 1));
        score = [score; paddown]; % pad down
    else
        score(end-pad(3)+1:end, :, :) = []; % crop down
    end

    if(pad(4) < 0)
        padright = cat(3, zeros(size(score,1), -pad(4), np), zeros(size(score,1), -pad(4), 1));
        score = [score padright]; % pad right
    else
        score(:,end-pad(4)+1:end, :) = []; % crop right
    end
    score = permute(score, [2 1 3]);

function label = produceCenterLabelMap(im_size, x, y) %this function is only for center map in testing
    sigma = 21;
    %label{1} = zeros(im_size(1), im_size(2));
    [X,Y] = meshgrid(1:im_size(1), 1:im_size(2));
    X = X - x;
    Y = Y - y;
    D2 = X.^2 + Y.^2;
    Exponent = D2 ./ 2.0 ./ sigma ./ sigma;
    label{1} = exp(-Exponent);


classdef LaunchVehicleAeroState < matlab.mixin.SetGet & matlab.mixin.Copyable
    %LaunchVehicleAeroState Summary of this class goes here
    %   Detailed explanation goes here

    properties
        %drag
        dragCoeffModel DragCoeffModel

        %lift
        liftCoeffModel LiftCoeffModel
    end

    %old properties
    properties(Access=private)
        %drag
        Cd(1,1) double = 0.3;

        %drag
        area(1,1) double = 1; %m^2
        CdInterp %should be (1,1) griddedInterpolant
        CdIndepVar (1,1) AeroIndepVar = AeroIndepVar.Altitude;
        CdInterpMethod (1,1) GriddedInterpolantMethodEnum = GriddedInterpolantMethodEnum.Linear;
        CdInterpPts(1,1) GriddedInterpolantPointSet

        %lift
        useLift(1,1) logical = false;
        areaLift(1,1) double = 16.2;
        Cl_0(1,1) double = 0.731;
        bodyLiftVect(3,1) double = [0;0;-1];
    end

    methods
        function obj = LaunchVehicleAeroState()
            obj.dragCoeffModel = DragCoeffModel();
            obj.liftCoeffModel = LiftCoeffModel();
        end

        function CdA = getDragCoeff(obj, ut, rVect, vVect, bodyInfo, mass, altitude, pressure, density, vVectECEFMag, totalAoA, aoa, sideslip)
            arguments
                obj(1,1) LaunchVehicleAeroState
                ut(1,1) double
                rVect(3,1) double
                vVect(3,1) double
                bodyInfo(1,1) KSPTOT_BodyInfo
                mass(1,1) double
                altitude(1,1) double
                pressure(1,1) double %kPa
                density(1,1) double
                vVectECEFMag(1,1) double
                totalAoA(1,1) double = 0;
                aoa(1,1) double = 0;
                sideslip(1,1) double = 0;
            end

            CdA = obj.dragCoeffModel.getDragCoeff(ut, rVect, vVect, bodyInfo, mass, altitude, pressure, density, vVectECEFMag, totalAoA, aoa, sideslip);
        end

        function [ClS, liftUnitVectInertial] = getLiftCoeffAndDir(obj, ut, rVect, vVect, bodyInfo, mass, altitude, pressure, density, vVectECEF, attState)
            arguments
                obj(1,1) LaunchVehicleAeroState
                ut(1,1) double
                rVect(3,1) double
                vVect(3,1) double
                bodyInfo(1,1) KSPTOT_BodyInfo
                mass(1,1) double
                altitude(1,1) double
                pressure(1,1) double
                density(1,1) double
                vVectECEF(3,1) double
                attState(1,1) LaunchVehicleAttitudeState
            end

            [ClS, liftUnitVectInertial] = obj.liftCoeffModel.getLiftCoeffAndDir(ut, rVect, vVect, bodyInfo, mass, altitude, pressure, density, vVectECEF, attState);
        end

        function newAeroState = deepCopy(obj)
            newAeroState = obj.copy();
        end
    end

    methods(Static)
        function obj = loadobj(obj)
            arguments
                obj(1,1) LaunchVehicleAeroState
            end

            if(isempty(obj.CdInterp))
                pointSet = GriddedInterpolantPointSet();
                obj.CdInterpMethod = GriddedInterpolantMethodEnum.Linear;

                pointSet.addPoint(GriddedInterpolantPoint(0,obj.Cd));
                pointSet.addPoint(GriddedInterpolantPoint(1,obj.Cd));
                obj.CdInterp = pointSet.getGriddedInterpFromPoints(obj.CdInterpMethod, GriddedInterpolantMethodEnum.Nearest);
                obj.CdInterpPts = pointSet;
            end

            if(isempty(obj.dragCoeffModel))
                obj.dragCoeffModel = DragCoeffModel.createFromOldDragInputs(obj.area, obj.CdInterp, obj.CdIndepVar, obj.CdInterpMethod, obj.CdInterpPts);
            end

            if(isempty(obj.liftCoeffModel))
                obj.liftCoeffModel = LiftCoeffModel();
            end
        end
    end
end


function params = tSeriesParamsDefault;
% Default signal-processing parameters for time series fMRI data.
%
% params = tSeriesParamsDefault;
%
% This is a rough adaptation of the mrVista 1.0 params. But,
% I expect this to be updated significantly. It should, add least,
% include more explicit specification of desired high- and
% low-pass frequency filtering cutoffs. (Currently this is only
% achieved by setting detrend = 1 and setting the detrend frames
% to 1/[desired high-pass cutoff] for an approximate high-pass detrend
% using multiple boxcar smoothing.)
%
% Conversely, if the preprocessing done by mrDetrend is satisfactory,
% then we may want to ditch the whole idea of having separate tSeries
% methods, and assume all mr objects w/ time series data have already
% been pre-processed.
%
% ras, 08/05

% Detrend options: integer w/ following values
% 0: Don't Detrend
% 1: Detrend w/ multiple boxcar smoothing
% 2: Linear Trend Removal
% 3: Quartic Trend Removal
params.detrend = 1;

% frames to use as period for boxcar detrending
params.detrendFrames = 20;

% Options for how to compensate for distance from the coil, depending
% on the value of inhomoCorrection
%   0 do nothing
%   1 divide by the mean, independently at each voxel
%   2 divide by null condition
%   3 divide by anything you like, e.g., robust estimate of intensity
%   inhomogeneity
params.inhomoCorrect = 1;

% temporal normalization:
% if 1, normalize each frame to have same mean intensity
params.temporalNormalization = 0;

% subtract the mean:
% if 1, subtract the mean, setting the overall mean to 0,
% if 0, don't do this
params.subtractMean = 1;

return


function test_issue1564

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_artifact_clip ft_artifact_threshold ft_artifact_jump ft_rejectartifact

global ft_default
ft_default.representation = 'table';

%%

ntrials = 10;
nchans = 3;
fsample = 1000;

data = [];
data.label = arrayfun(@num2str, 1:nchans, 'UniformOutput', false);
for i=1:ntrials
  data.time{i} = (1:fsample)/fsample;
  data.trial{i} = randn(nchans, fsample);
  data.sampleinfo(i,1) = (i-1)*fsample + 1 + (i*100); % create some gaps between the trials
  data.sampleinfo(i,2) = data.sampleinfo(i,1) + fsample - 1;
end

if strcmp(ft_default.representation, 'numeric')
  data.trialinfo = [1:ntrials]';
else
  trialnum = arrayfun(@num2str, 1:ntrials, 'UniformOutput', false)';
  data.trialinfo = table(trialnum);
end

%%

cfg = [];
cfg.continuous = 'no';
ft_databrowser(cfg, data);

%%

cfg = [];
cfg.continuous = 'yes';
ft_databrowser(cfg, data);

%%

cfg = [];
cfg.continuous = [];
ft_databrowser(cfg, data);

%%
% close all

data_clip = data;
data_clip.trial{1}(1,201:700) =  2;
data_clip.trial{2}(2,301:400) = -2;
data_clip.trial{2}(2,501:600) = -2;

cfg = [];
cfg.artfctdef.clip.amplthreshold = 0;
[cfg, artifact] = ft_artifact_clip(cfg, data_clip);

% cfg = [];
% cfg.artfctdef.clip.amplthreshold = '0%';
% [cfg, artifact] = ft_artifact_clip(cfg, data_clip)

% cfg = [];
% cfg.artfctdef.clip.amplthreshold = '100%';
% [cfg, artifact] = ft_artifact_clip(cfg, data_clip)

% explore the detected artifacts in the databrowser
ft_databrowser(cfg, data_clip);

%%

cfg.artfctdef.reject = 'partial';
cfg.artfctdef.minaccepttim = 0;
cfg.artfctdef.feedback = 'yes';
data_rej = ft_rejectartifact(cfg, data_clip);
if ~istable(data.trialinfo)
  assert(isequal(data_rej.trialinfo, [1 1 2 2 2 3 4 5 6 7 8 9 10]'));
end

cfg.artfctdef.reject = 'complete';
data_rej = ft_rejectartifact(cfg, data_clip);
if ~istable(data.trialinfo)
  assert(isequal(data_rej.trialinfo, [3 4 5 6 7 8 9 10]'));
end

cfg.artfctdef.reject = 'nan';
data_rej = ft_rejectartifact(cfg, data_clip);
if ~istable(data.trialinfo)
  assert(isequal(data_rej.trialinfo, [1 2 3 4 5 6 7 8 9 10]'));
end

%%
close all

data_thresh = data;
data_thresh.trial{1}(1,201:700) = data_thresh.trial{1}(1,201:700) + 11;
data_thresh.trial{2}(2,301:400) = data_thresh.trial{2}(2,301:400) + 12;
data_thresh.trial{2}(3,501:600) = data_thresh.trial{2}(3,501:600) - 13;

cfg = [];
cfg.artfctdef.threshold.channel = 'all';
cfg.artfctdef.threshold.bpfilter = 'no';
cfg.artfctdef.threshold.min = -8;
cfg.artfctdef.threshold.max =  8;

[cfg, artifact] = ft_artifact_threshold(cfg, data_thresh);

% explore the detected artifacts in the databrowser
ft_databrowser(cfg, data_thresh);

cfg.artfctdef.reject = 'partial';
cfg.artfctdef.minaccepttim = 0;
cfg.artfctdef.feedback = 'yes';
data_rej = ft_rejectartifact(cfg, data_thresh);
if ~istable(data.trialinfo)
  assert(isequal(data_rej.trialinfo, [1 1 2 2 2 3 4 5 6 7 8 9 10]'));
end

cfg.artfctdef.reject = 'complete';
data_rej = ft_rejectartifact(cfg, data_thresh);
if ~istable(data.trialinfo)
  assert(isequal(data_rej.trialinfo, [3 4 5 6 7 8 9 10]'));
end

cfg.artfctdef.reject = 'nan';
data_rej = ft_rejectartifact(cfg, data_thresh);
if ~istable(data.trialinfo)
  assert(isequal(data_rej.trialinfo, [1 2 3 4 5 6 7 8 9 10]'));
end

%%
% Although I identified in issue 1564 that the detected jump artifacts could also
% have their channel in the outpout table, in the end I did not implement it. It
% would be a lot of work and not really worth it.

close all

data_jump = data;
data_jump.trial{1}(1,401:end) = data_jump.trial{1}(1,401:end) + 15;
data_jump.trial{2}(2,601:end) = data_jump.trial{2}(2,601:end) - 15;

cfg = [];
cfg.artfctdef.jump.channel = 'all';
cfg.artfctdef.jump.trlpadding = 0;
cfg.artfctdef.jump.fltpadding = 0;
cfg.artfctdef.jump.artpadding = 0.1;
cfg.artfctdef.jump.cutoff = 20;
cfg.artfctdef.jump.interactive = 'no';

[cfg, artifact] = ft_artifact_jump(cfg, data_jump);

% explore the detected artifacts in the databrowser
ft_databrowser(cfg, data_jump);


function [lags,use_ascend,dc,sc,E] = gc_lags(points,options)

% GC_LAGS       Run lag extraction using a binary graph cut
%
%   Run lag extraction on a reordered raster plot (2D Image) using a binary graph cut
%
%   SYNTAX
%       [LAGS,USE_ASCEND,DC,SC] = GC_LAGS(POINTS,OPTIONS)
%
%   DC is the data cost
%   SC is the smoothness cost
%

% $Id: gc_lags.m 4 2009-08-15 21:10:35Z gramfort $
% $LastChangedBy: gramfort $
% $LastChangedDate: 2009-08-15 17:10:35 -0400 (Sam, 15 aoû 2009) $
% $Revision: 4 $

if nargin<2
    options.null = 0;
end

if ~isfield(options, 'alpha')
    options.alpha = 0.01;
end
alpha = options.alpha;

if ~isfield(options, 'verbose')
    options.verbose = true;
end
verbose = options.verbose;

if ~isfield(options, 'use_lcurve')
    options.use_lcurve = false;
end
use_lcurve = options.use_lcurve;

if ~isfield(options, 'maxit') % maximum number of iterations for alpha search in l-curve
    options.maxit = 40;
end
maxit = options.maxit;

if ~isfield(options, 'dc_tol') % tolerance on data cost for alpha search in l-curve
    options.dc_tol = 1e-25;
end
dc_tol = options.dc_tol;

if isfield(options, 'null')
    options = rmfield(options,'null');
end

%%%%%%%% END OPTIONS %%%%%%%%%

points = - points; % min cut through maximum of points values
points = points - min(points(:)); % only positive capacities for the maxflow

npoints = size(points,1);

if options.disp_log
    disp(['Nb trials : ',num2str(npoints)]);
    disp(['Nb time samples : ',num2str(size(points,2))]);
    disp(['Lambda : ',num2str(alpha)]);
end

if use_lcurve
    disp('Computing best alpha with L-curve');
    verbose = false;
    sc = Inf;
    ht = java.util.Hashtable;

    % Add alpha = 0 to ht
    [lags0,use_ascend,dc0,sc0] = gc_lags_aux(0.0);

    % Find the biggest alpha
    while sc > 0
        fprintf('.');
        [lags,use_ascend,dc,sc] = gc_lags_aux(alpha);
        ht.put(alpha,[dc sc]);
        alpha = 3*alpha;
        % alpha = 2.5*alpha;
        % alpha = 5*alpha;
        % alpha = 2*alpha;
        % alpha = 1.5*alpha;
    end
    fprintf('\n Biggest alpha : %f\n',alpha);
    % Try to find best alpha iteratively
    goon = true;
    niter = 1;
    while goon
        [alphas,dcs,scs] = ht_to_array();

        % Make sure dcs and scs values are unique
        [tmp,dcs_idx] = unique(dcs,'first');
        [tmp,scs_idx] = unique(scs,'first');
        good_idx = sort(intersect(dcs_idx,scs_idx));
        dcs = dcs(good_idx);
        scs = scs(good_idx);
        alphas = alphas(good_idx);
        clear good_idx

        scs_intermediate = diff(dcs+alphas.*scs) ./ diff(alphas);
        dcs_intermediate = (dcs(1:end-1)+alphas(1:end-1).*scs(1:end-1)) - alphas(1:end-1) .* scs_intermediate;
        dcs_interp = linspace(min(dcs),max(dcs),500);
        scs_interp = interp1([dcs;dcs_intermediate],[scs;scs_intermediate],dcs_interp,'linear');

        l1 = 1./alphas(1:end-1);
        l2 = 1./alphas(2:end);
        angle_intermediate = acos((1+l1.*l2)./(sqrt(1+l1.^2).*sqrt(1+l2.^2))); % angle between neighboring segments

        angle_intermediate(1) = 0;
        angle_intermediate(end) = 0;
        midx = localmax(angle_intermediate);

        [tmp,best_midx] = max(angle_intermediate);

        best_dcs = dcs_intermediate(best_midx);
        best_scs = scs_intermediate(best_midx);

        best_dcs_all = best_dcs;
        best_scs_all = best_scs;

        % get alpha by computing harmonic mean of neighboring alphas (better than arithmetic mean)
        new_alphas = alphas(midx) .* alphas(midx+1) ./ (alphas(midx)+alphas(midx+1)) * 2;

        % Sort alphas using order of angle differences
        [tmp,I] = sort(angle_intermediate(midx));
        new_alphas = new_alphas(I);

        precision = 0;

        needs_update = false;
        for ii = 1:length(new_alphas)
            alpha = new_alphas(ii); % take in between alpha

            if isempty(ht.get(alpha)) % Check if solution with this alpha has already between computed
                disp(['    Computing solution for alpha : ',num2str(alpha)]);
                [lags,use_ascend,dc,sc] = gc_lags_aux(alpha);
                ht.put(alpha,[dc sc]);
                needs_update = true; % Say if at least one new alpha is added
            else
                sc_dc = ht.get(alpha);
                dc = sc_dc(1);
                sc = sc_dc(2);
            end
            precision = max(precision,min(abs(dcs-dc)));
        end

        if ~needs_update
            disp('Stopping search (all alphas already computed)');
            goon = false;
        end

        disp([' Error : ',num2str(precision)]);
        if precision < dc_tol | niter >= maxit
            goon = false;
        end
        niter = niter+1;

        if 1 % set best final alpha with interpolation
            if ~goon
                [alpha,best_dcs,best_scs] = get_best_alpha();
            end
        end

        if 1 % show l-curve
            smart_figure('Lag extraction L-curve'); clf
            title('Lag extraction L-curve')
            if goon
                color = {'g','b'};
                linewidth = 1;
            else
                color = {'r','r'};
                linewidth = 3;
            end
            USE_LOG = false;
            % USE_LOG = true;
            if USE_LOG % plot log-log or not
                loglog(dcs_interp(dcs_interp>0),scs_interp(dcs_interp>0),color{2});
                hold on
                loglog(dcs(dcs>0),scs(dcs>0),[color{2},'x']);
            else
                plot(dcs_interp,scs_interp,color{2},'LineWidth',linewidth);
                hold on
                plot(dcs,scs,[color{2},'x']);
            end

            for k=1:length(dcs)
                if alphas(k) > 0
                    flow = dcs(k)+alphas(k)*scs(k);
                    line([0 , flow],[flow/alphas(k), 0],'Color','k');
                end
            end

            axis tight
            line([min(dcs_interp)/100,best_dcs],[best_scs,best_scs], ...
                 'LineStyle','--','Color',color{1},'LineWidth',linewidth);
            line([best_dcs,best_dcs],[min(scs_interp)/100,best_scs], ...
                 'LineStyle','--','Color',color{1},'LineWidth',linewidth);
            hold off;
            xlabel('Data term')
            ylabel('Smoothness term')

            if 1 % show max curvature points
                hold on
                if USE_LOG
                    loglog(best_dcs_all,best_scs_all,'bo');
                else
                    plot(best_dcs_all,best_scs_all,'bo');
                end
                hold off
            end
        end
    end
    disp(['L-curve done with final alpha : ',num2str(alpha)]);
    disp(['         tol   : ',num2str(precision)]);
    disp(['         niter : ',num2str(niter)]);
else
    [lags,use_ascend,dc,sc] = gc_lags_aux(alpha);
end

E = dc+alpha*sc;

if options.disp_log
    disp('E = DC + alpha * SC');
    disp(sprintf('%f = %f + %f * %f',dc+alpha*sc,dc,alpha,sc));
end

% ****************************************************************************** %
function [best_alpha,best_dcs,best_scs] = get_best_alpha()

[local_alphas,local_dcs,local_scs] = ht_to_array();
offset = 1; % don't consider extremities of the curve
local_dcs = local_dcs(1+offset:end-offset);
local_scs = local_scs(1+offset:end-offset);
local_alphas = local_alphas(1+offset:end-offset);

local_dcs_interp = linspace(min(local_dcs),max(local_dcs),100);
local_scs_interp = interp_cubic_herm(local_dcs,local_scs,-1./local_alphas,local_dcs_interp);
local_curvature = curvature_2D_diff(local_dcs_interp,local_scs_interp);
local_curvature = abs(local_curvature); % use absolute value
local_curvature(1:5) = NaN;
local_curvature(end-5:end) = NaN;

% [tmp,idx] = max(local_curvature);
[tmp,idx] = min(local_curvature);
best_dcs = local_dcs_interp(idx);
best_scs = local_scs_interp(idx);

[tmp,local_alpha_idx] = min(abs(local_dcs-best_dcs));

best_alpha = local_alphas(local_alpha_idx);

if 0
    smart_figure('Lag extraction L-curve'); hold on
    plot(local_dcs_interp,local_scs_interp,'g')
end

if 0
    smart_figure('SC vs alpha');
    plot(local_alphas,local_scs)
    xlabel('alpha')
    ylabel('SC')
    hold on
    plot(best_alpha,best_scs,'ro');
    hold off
end

if verbose
    smart_figure('Curve interpolation');
    plot(local_dcs_interp,local_scs_interp);
    hold on
    plot(best_dcs,best_scs,'ro');
    hold off
    title('Curve interpolation');
end

end % end get_best_alpha function

% ****************************************************************************** %
function [alphas,dcs,scs] = ht_to_array()
    alphas = zeros(ht.size(),1);
    dcs = zeros(ht.size(),1);
    scs = zeros(ht.size(),1);
    keys = ht.keys();
    cnt = 1;
    while keys.hasNext()
       alpha = keys.next();
       dc_sc = ht.get(alpha);
       dc = dc_sc(1);
       sc = dc_sc(2);
       alphas(cnt) = alpha;
       dcs(cnt) = dc;
       scs(cnt) = sc;
       cnt = cnt+1;
    end;
    [tmp,I] = sort(alphas);
    alphas = alphas(I);
    dcs = dcs(I);
    scs = scs(I);

    dcs = dcs - dc0; % remove offset
end

% ****************************************************************************** %
function idx = localmax(x)
% Find local maxima in an array

idx = find( diff( sign( diff([0; x(:); 0]) ) ) < 0 );

end % end function

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

function [lags,use_ascend,dc,sc] = gc_lags_aux(alpha)

if options.disp_log
    disp('Testing ascend')
end

order = [npoints:-1:1];
[lags_ascend,flow_ascend,labels_ascend] = gc_aux_mex(points(order,:),alpha);
lags_ascend = lags_ascend(order);
labels_ascend = labels_ascend(order,:);

% Compute manually resulting data cost and smoothness cost
%
% lag_ind = sub2ind(size(points(order,:)),[1:size(points,1)]',lags_ascend(:));
% dc = sum(points(lag_ind))
% sc = sum(abs(diff(lags_ascend)))
% flow_ascend
% fl = dc + alpha * sc

if options.disp_log
    disp('Testing descend')
end
[lags_descend,flow_descend,labels_descend] = gc_aux_mex(points,alpha);

if flow_ascend < flow_descend
    lags = lags_ascend;
    use_ascend = true;
    flow = flow_ascend;
else
    lags = lags_descend;
    use_ascend = false;
    flow = flow_descend;
end

sc = sum(abs(diff(lags))); % Smoothness cost
dc = flow - alpha * sc; % Data cost

end %  function

end


function [maxVal] = maxIdx(a, idx)
%
% Copyright Aditya Khosla http://mit.edu/khosla
%
% Please cite this paper if you use this code in your publication:
%   A. Khosla, J. Xiao, A. Torralba, A. Oliva
%   Memorability of Image Regions
%   Advances in Neural Information Processing Systems (NIPS) 2012
%

maxVal = max(a(idx, :), [], 1);


function plot_transform(~, eventObj)

hFig = gcf;
idxPC = eventObj.Source.Parent.Parent.UserData.idxPC; % index of PC(s)

% Get new value
prompt = {'UTM Zone:'};
dlgTitle = '';
noLines = 1;
defaultAnswer = {'33n'};
answer = inputdlg(prompt, dlgTitle, noLines, defaultAnswer);
if ~isempty(answer)
    UTMZone = answer{1};
else
    return;
end

% Transform
for i = 1:numel(idxPC)

    if i == 1, logLevelOrig = msg('O', 'GetLogLevel'); end
    msg('O', 'SetLogLevel', 'off');

    % mstruct
    mstruct       = defaultm('utm');
    mstruct.zone  = UTMZone;
    mstruct.geoid = referenceEllipsoid('GRS 80');
    mstruct       = defaultm(mstruct);

    % Transform!
    hFig.UserData.PC{idxPC(i)}.ecef2mapTrafo(mstruct);

    msg('O', 'SetLogLevel', logLevelOrig);

end

% Update plot!
for i = 1:numel(idxPC)
    plot_setPrmAndPlot(hFig, idxPC(i));
end

end


function [energyV,entropyV,sumAvgV,corrV,invDiffMomV,contrastV,...
    clustShadeV,clustProminV] = haralickTextureAllDir(scanArray3M, nL, offsetsM, flagv, hWait)
% [energyV,entropyV,sumAvgV,corrV,invDiffMomV,contrastV,...
%     clustShadeV,clustProminV] = haralickTextureAllDir(scanArray3M, nL, offsetsM, flagv, hWait)
%
% Haralick texture with cooccurrence matrix combined from all directions.
%
% APA, 05/12/2016

% Generate flags
if ~exist('flagv','var')
    flagv = ones(1,8);
elseif exist('flagv','var') && isempty(flagv)
    flagv = ones(1,8);
end

% % Flag to draw waitbar
% waitbarFlag = 0;
% if exist('hWait','var') && ishandle(hWait)
%     waitbarFlag = 1;
% end

% % Grid resolution
% deltaX = deltaXYZv(1);
% deltaY = deltaXYZv(2);
% deltaZ = deltaXYZv(3);
%
% % Get Block size to process
% slcWindow = floor(2*patchSizeV(3)/deltaZ);
% rowWindow = floor(2*patchSizeV(1)/deltaY);
% colWindow = floor(2*patchSizeV(2)/deltaX);
%
% % Make sure that the window is of odd size
% if mod(slcWindow,2) == 0
%     slcWindow = slcWindow + 1;
% end
% if mod(rowWindow,2) == 0
%     rowWindow = rowWindow + 1;
% end
% if mod(colWindow,2) == 0
%     colWindow = colWindow + 1;
% end

slcWindow = size(scanArray3M,3);
rowWindow = size(scanArray3M,1);
colWindow = size(scanArray3M,2);

% Build distance matrices
numColsPad = 1;
numRowsPad = 1;
numSlcsPad = 1;

% Get number of voxels per slice
[numRows, numCols, numSlices] = size(scanArray3M);
numVoxels = numRows*numCols;

minQ = min(scanArray3M(:));
maxQ = max(scanArray3M(:));
dq = (maxQ-minQ)/nL/4;
levels = linspace(minQ,maxQ,nL);
levels = linspace(dq,maxQ-dq,nL);
q1 = imquantize_cerr(scanArray3M,nL);
q2 = imquantize(scanArray3M, levels);
%levels = multithresh(scanArray3M, nL);
%q3 = imquantize(scanArray3M, levels);

% Pad doseArray2 so that sliding window works also for the edge voxels
%scanArrayTmp3M = padarray(scanArray3M,[numRowsPad numColsPad
%numSlcsPad],NaN,'both'); % aa commented
if exist('padarray.m','file')
    q = padarray(q1,[numRowsPad numColsPad numSlcsPad],NaN,'both');
else
    q = padarray_oct(q1,[numRowsPad numColsPad numSlcsPad],NaN,'both');
end

% Quantize the image
%nL = 16;
% if any(isnan(q)) % aa commented
%     nL = nL-1; % aa commented
% end % aa commented
%q = imquantize_cerr(scanArrayTmp3M,nL); % aa commented
%clear scanArrayTmp3M; % aa commented
qmax = max(q(:));
nanFlag = 0;
if any(isnan(q(:)))
    nanFlag = 1;
    q(isnan(q))=qmax+1;
end
qs=sort(unique(q));
lq=length(qs);

for k = 1:length(qs)
    q(q==qs(k)) = k;
end

q = uint16(q); % q is the quantized image
%numSlcsWithPadding = size(q,3);

% % Create indices for 2D blocks
% [m,n,~] = size(q);
% m = uint32(m);
% n = uint32(n);
% colWindow = uint32(colWindow);
% rowWindow = uint32(rowWindow);
% slcWindow = uint32(slcWindow);
%
% % Index calculation adapted from
% % http://stackoverflow.com/questions/25449279/efficient-implementation-of-im2col-and-col2im
%
% %// Start indices for each block
% start_ind = reshape(bsxfun(@plus,[1:m-rowWindow+1]',[0:n-colWindow]*m),[],1); %//'
%
% %// Row indices
% lin_row = permute(bsxfun(@plus,start_ind,[0:rowWindow-1])',[1 3 2]);  %//'
%
% %// Get linear indices based on row and col indices and get desired output
% % imTmpM = A(reshape(bsxfun(@plus,lin_row,[0:ncols-1]*m),nrows*ncols,[]));
% indM = reshape(bsxfun(@plus,lin_row,(0:colWindow-1)*m),rowWindow*colWindow,[]);

% Create indices list
%indCooccurM = reshape(1:nL*nL,nL,nL);

% Number of offsets
numOffsets = size(offsetsM,1);

% Indices of last level to filter out
nanIndV = false([lq*lq,1]);
nanIndV([lq:lq:lq*lq-lq, lq*lq-lq:lq*lq]) = true;

% Build levels vector for mu, sig
levRowV = repmat(1:lq,[1 lq]);
levColV = repmat(1:lq,[lq 1]);
levColV = levColV(:)';

% Build list of indices for px and contrast calculation
for n=0:lq-1
    % indices for p(x-y), contrast
    indCtrstV = false(lq*lq,1);
    indCtrst1V = 1:lq-n;
    indCtrst2V = 1+n:lq;
    indCtrstTmpV = indCtrst1V + (indCtrst2V-1)*lq;
    indCtrstTmpV = [indCtrstTmpV indCtrst2V + (indCtrst1V-1)*lq];
    indCtrstV(indCtrstTmpV) = 1;
    indCtrstV(nanIndV) = [];
    indCtrstC{n+1} = indCtrstV;

    % indices for p(x+y)
    indPxPlusYv = false(lq*lq,1);
    indPxPlusYv(levRowV + levColV == n+2) = 1;
    indPxPlusYv(nanIndV) = [];
    indPxPlusYc{n+1} = indPxPlusYv;

    % indices for px
    indPxV = false(lq*lq,1);
    indPxV(lq*n+1:lq*(n+1)) = true;
    indPxV(nanIndV) = [];
    indPxC{n+1} = indPxV;
end

% Filter NaN indices from row/col levels
levRowV(nanIndV) = [];
levColV(nanIndV) = [];

% Build linear indices column/row-wise for Symmetry
indRowV = zeros(1,lq*lq);
for i=1:lq
    indRowV((i-1)*lq+1:(i-1)*lq+lq) = i:lq:lq*lq;
end

% Initialize
energyV = [];
entropyV = [];
sumAvgV = [];
corrcorrV = [];
invDiffMomV = [];
contrastV = [];
clustShadeV = [];
clustProminV = [];

tic
% Initialize cooccurrence matrix (vectorized for speed)
cooccurV = zeros(lq*lq,1,'single');
for off = 1:numOffsets

    offset = offsetsM(off,:);
    slc1M = q(numRowsPad+(1:numRows),numColsPad+(1:numCols),...
        numSlcsPad+(1:numSlices));
    slc2M = circshift(q,offset);
    slc2M = slc2M(numRowsPad+(1:numRows),numColsPad+(1:numCols),numSlcsPad+(1:numSlices))...
        + (slc1M-1)*lq;
    cooccurV = cooccurV + accumarray(slc2M(:),1, [lq*lq,1]); % patch-wise cooccurance

end

cooccurV = cooccurV + cooccurV(indRowV,:); % for symmetry
cooccurV(nanIndV,:) = [];
cooccurV = cooccurV./sum(cooccurV);

% Calculate scalar texture for this offset
% Angular Second Moment (Energy)
if flagv(1)
    energyV = sum(cooccurV.^2);
end
% Entropy
if flagv(2)
    entropyV = -sum(cooccurV.*log2(cooccurV+1e-10));
end
% Contrast, inverse Difference Moment
contrastV = 0;
invDiffMomV = 0;
for n=0:nL-1
    % px
    px(n+1) = sum(cooccurV(indPxC{n+1},:),1);
    % p(x+y)
    pXplusY = cooccurV(indCtrstC{n+1},:);
    % p(x-y)
    pXminusY = cooccurV(indCtrstC{n+1},:);
    % contrast
    if flagv(6)
        contrastV = contrastV + ...
            sum(n^2*cooccurV(indCtrstC{n+1},:));
    end
    % inv diff moment
    if flagv(5)
        invDiffMomV = invDiffMomV + ...
            sum((1/(1+n^2))*cooccurV(indCtrstC{n+1},:));
    end
end
% weighted pixel average (mu), weighted pixel variance (sig)
mu = (1:nL) * px';
sig = ((1:nL)-mu).^2 * px';
%px = sum(cooccurV(indCooccurM),2);
muX = mean(px);
sigX = std(px);
%         for colNum = 1:numCalcVoxs
clstrV = (levRowV + levColV - 2*mu);
% Cluster Shade
if flagv(7)
    clustShadeV = clstrV.*clstrV.*clstrV * cooccurV(:,1);
end
% Cluster Prominence
%             clustProminTmpV(colNum) = (levRowV + levColV - 2*mu(colNum)).^4 ...
%                 * cooccurPatchM(:,colNum);
if flagv(8)
    clustProminV = clstrV.*clstrV.*clstrV.*clstrV ...
        * cooccurV(:,1);
end
% Sum Avg
if flagv(3)
    sumAvgV = (levRowV + levColV) * cooccurV(:,1);
end
% Correlation
if flagv(4)
    corrV = (levRowV .* levColV * cooccurV(:,1) - ...
        muX*muX) / sigX^2; %var(1); % check this
end

%     % Average texture from all directions
%     energyV = energyV / numOffsets;
%     entropyV = entropyV / numOffsets;
%     contrastV = contrastV / numOffsets;
%     invDiffMomV = invDiffMomV / numOffsets;
%     clustShadeV = clustShadeV / numOffsets;
%     clustProminV = clustProminV / numOffsets;

%     if flagv(1)
%         energyV = energyV / numOffsets;
%     end
%     if flagv(2)
%         entropyV = entropyV / numOffsets;
%     end
%     if flagv(3)
%         sumAvgV = sumAvgV / numOffsets;
%     end
%     if flagv(4)
%         corrV = corrV / numOffsets;
%     end
%
%     if flagv(6)
%         contrastV = contrastV / numOffsets;
%     end
%     if flagv(5)
%         invDiffMomV = invDiffMomV / numOffsets;
%     end
%     if flagv(7)
%         clustShadeV = clustShadeV / numOffsets;
%     end
%     if flagv(8)
%         clustProminV = clustProminV / numOffsets;
%     end

%     if waitbarFlag
%         set(hWait, 'Vertices', [[0 0 slcNum/numSlices slcNum/numSlices]' [0 1 1 0]']);
%         drawnow;
%     end

% end
toc


function out = isequal( f, g )
%ISEQUAL Equality test for SEPARABLEAPPROX.
%
% BOL = ISEQUAL(F, G) returns 0 or 1. If returns 1 then F and G are the same
% SEPARABLEAPPROX, up to relative machine precision. If returns 0 then F and G are
% not the same up to relative machine precision.

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

if ( isempty( f ) )
    if ( isempty( g ) )
        out = true;
    else
        out = false;
    end
    return
end

% Get the low rank representation for f.
fcols = f.cols;
frows = f.rows;
fpiv = f.pivotValues;

% Get the low rank representation for g.
gcols = g.cols;
grows = g.rows;
gpiv = g.pivotValues;

% Test every part:
out = ( isequal(fcols, gcols) & isequal(frows, grows) & isequal(fpiv, gpiv) );

end


function [net1, avgImg] = initVGG16Net( )
%VGG16
opts.gpus=1;
cold=true;
prepareGPUs(opts,cold);

net2=load('./exp/model/imagenet-vgg-verydeep-16.mat');
net2.layers(31:end) = [];

avgImg=net2.meta.normalization.averageImage;

net1=dagnn.DagNN();

net1.addLayer('conv1_1', dagnn.Conv('size', [3,3,3,64],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'input', 'conv_11', {'conv11_f', 'conv11_b'});
net1.addLayer('relu1_1', dagnn.ReLU(),'conv_11','relu_11');

f = net1.getParamIndex('conv11_f') ;
net1.params(f).value=net2.layers{1}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv11_b') ;
net1.params(f).value=net2.layers{1}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

net1.addLayer('conv1_2', dagnn.Conv('size', [3,3,64,64],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_11', 'conv_12', {'conv12_f', 'conv12_b'});
net1.addLayer('relu1_2', dagnn.ReLU(),'conv_12','relu_12');
net1.addLayer('pool1', dagnn.Pooling('poolSize', [2,2], 'pad',...
    [0,1,0,1], 'stride', [2,2]),'relu_12','pool_1');

f = net1.getParamIndex('conv12_f') ;
net1.params(f).value=net2.layers{3}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv12_b') ;
net1.params(f).value=net2.layers{3}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

%-------------------------covn 2--------------------
net1.addLayer('conv2_1', dagnn.Conv('size', [3,3,64,128],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'pool_1', 'conv_21', {'conv21_f', 'conv21_b'});
net1.addLayer('relu2_1', dagnn.ReLU(),'conv_21','relu_21');

f = net1.getParamIndex('conv21_f') ;
net1.params(f).value=net2.layers{6}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv21_b') ;
net1.params(f).value=net2.layers{6}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

net1.addLayer('conv2_2', dagnn.Conv('size', [3,3,128,128],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_21', 'conv_22', {'conv22_f', 'conv22_b'});
net1.addLayer('relu2_2', dagnn.ReLU(),'conv_22','relu_22');
net1.addLayer('pool2', dagnn.Pooling('poolSize', [2,2], 'pad',...
    [0,1,0,1], 'stride', [2,2]),'relu_22','pool_2');

f = net1.getParamIndex('conv22_f') ;
net1.params(f).value=net2.layers{8}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv22_b') ;
net1.params(f).value=net2.layers{8}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

%-------------------------covn 3--------------------
net1.addLayer('conv3_1', dagnn.Conv('size', [3,3,128,256],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'pool_2', 'conv_31', {'conv31_f', 'conv31_b'});
net1.addLayer('relu3_1', dagnn.ReLU(),'conv_31','relu_31');

f = net1.getParamIndex('conv31_f') ;
net1.params(f).value=net2.layers{11}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv31_b') ;
net1.params(f).value=net2.layers{11}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

net1.addLayer('conv3_2', dagnn.Conv('size', [3,3,256,256],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_31', 'conv_32', {'conv32_f', 'conv32_b'});
net1.addLayer('relu3_2', dagnn.ReLU(),'conv_32','relu_32');

f = net1.getParamIndex('conv32_f') ;
net1.params(f).value=net2.layers{13}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv32_b') ;
net1.params(f).value=net2.layers{13}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

net1.addLayer('conv3_3', dagnn.Conv('size', [3,3,256,256],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_32', 'conv_33', {'conv33_f', 'conv33_b'});
net1.addLayer('relu3_3', dagnn.ReLU(),'conv_33','relu_33');

f = net1.getParamIndex('conv33_f') ;
net1.params(f).value=net2.layers{15}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv33_b') ;
net1.params(f).value=net2.layers{15}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

%-------------------------covn 4--------------------
net1.addLayer('conv4_1', dagnn.Conv('size', [3,3,256,512],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_33', 'conv_41', {'conv41_f', 'conv41_b'});
net1.addLayer('relu4_1', dagnn.ReLU(),'conv_41','relu_41');

f = net1.getParamIndex('conv41_f') ;
net1.params(f).value=net2.layers{18}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv41_b') ;
net1.params(f).value=net2.layers{18}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

net1.addLayer('conv4_2', dagnn.Conv('size', [3,3,512,512],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_41', 'conv_42', {'conv42_f', 'conv42_b'});
net1.addLayer('relu4_2', dagnn.ReLU(),'conv_42','relu_42');

f = net1.getParamIndex('conv42_f') ;
net1.params(f).value=net2.layers{20}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv42_b') ;
net1.params(f).value=net2.layers{20}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

net1.addLayer('conv4_3', dagnn.Conv('size', [3,3,512,512],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_42', 'conv_43', {'conv43_f', 'conv43_b'});
net1.addLayer('relu4_3', dagnn.ReLU(),'conv_43','relu_43');

f = net1.getParamIndex('conv43_f') ;
net1.params(f).value=net2.layers{22}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv43_b') ;
net1.params(f).value=net2.layers{22}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

% net1.addLayer('pool2_show', dagnn.Crop('crop',[1,1]),...
%     {'pool_2','pool_2'}, 'pool2show');
% net1.addLayer('relu33_show', dagnn.Crop('crop',[1,1]),...
%     {'relu_33','relu_33'}, 'relu33_show');

net1.move('gpu');
clear net2;
end

% -------------------------------------------------------------------------
function clearMex()
% -------------------------------------------------------------------------
clear vl_tflow vl_imreadjpeg ;
end

% -------------------------------------------------------------------------
function prepareGPUs(opts, cold)
% -------------------------------------------------------------------------
numGpus = numel(opts.gpus) ;
if numGpus > 1
  % check parallel pool integrity as it could have timed out
  pool = gcp('nocreate') ;
  if ~isempty(pool) && pool.NumWorkers ~= numGpus
    delete(pool) ;
  end
  pool = gcp('nocreate') ;
  if isempty(pool)
    parpool('local', numGpus) ;
    cold = true ;
  end

end
if numGpus >= 1 && cold
  fprintf('%s: resetting GPU\n', mfilename)
  clearMex() ;
  if numGpus == 1
    gpuDevice(opts.gpus)
  else
    spmd
      clearMex() ;
      gpuDevice(opts.gpus(labindex))
    end
  end
end
end


% Copyright Mohammad SAFEEA, 17th-Aug-2017

% Simulation of controlling KUKA iiwa robot using game pad.
% In this script, the code is testsed, a graph showing the joints angles is
% showed to confirm the correctness of the code

clear all;
close all;
clc;
% Start the joystick, make sure that the joystick is connected
instrreset;
ID=1;
joy=vrjoystick(ID);
% Initial configuration
jPos={pi / 180 * 30, pi / 180 * 30, 0, -pi / 180 * 60, 0,...
                        pi / 180 * 90, 0};
% add the KST directory
cDir = pwd;
cDir=getTheKSTDirectory(cDir);
addpath(cDir);
% IK solver parameters
numberOfIterations=10;
lambda=0.1;
TefTool=eye(4);

qin=zeros(7,1);
for i=1:7
    qin(i)=jPos{i};
end

[Tt,j]=directKinematics(qin,TefTool); % EEF frame transformation amtrix

% Draw the interface
%[figureHandle,anglesTextHandlesCellArray,labelTextHandlesCellArray]=constructInterface();
% max angular velocity
w=5*pi/180; % rad/sec
% max linear velocity
v=0.05; % m/sec
% Joint space control
firstExecution=0;

% setBackGroundColor(labelTextHandlesCellArray, 1 );
figure();
bufferSize=100;

tempVec=[1:bufferSize];
qVec=zeros(7,bufferSize);

pause(0.1);
% frameUpdateCount=0;
% maxDisp=1*pi/180;
while true
    joyStatus=read(joy);
    % Remove the bias in the analog signal
    analogPrecission=1/20;
    for i=1:4
        if abs( joyStatus(i))<analogPrecission
            joyStatus(i)=0;
        end
    end

    % About the variable (joyStatus)
    % joyStatus: is a 4x1 vector, the first and the second elements of this
    % vector correspond to the analog values of the left analog-stick
    % psoition. the third and the fourth elements of this
    % vector correspond to the analog values of the right analog-stick
    % psoition.
    %
    % Only the seconds and the third elements of this vector are used for
    % controlling the joints motion of the robot one at a time.
    %
    % The 2d element is used to select the robot axes, this element corresponds
    % to the (up,down) directions of the left analog stick of the gamepad.
    %
    % The 3rd element is used to control the motion of the selcted axes,
    % this element corresponds to the (right, left) directions of the right
    % analog stick of the gamepad.

    % Calculate the elapsed time
    if(firstExecution==0)
        firstExecution=1;
        a=datevec(now);
        timeNow=a(6)+a(5)*60+a(4)*60*60; % calculate time at this instant
        dt=0; % elapsed is zero at first excution
        time0=timeNow;
    else
        a=datevec(now);
        timeNow=a(6)+a(5)*60+a(4)*60*60; % calculate time at this instant
        dt=timeNow-time0; % calculate elapsed time interval
        time0=timeNow;
    end
 % Construct motion control command
     n=[joyStatus(3);
         joyStatus(4);
         0];
     k=[0;0;1];
     w_control=w*cross(n,k);
     v_control=v*[joyStatus(1);
         joyStatus(2);
         0];
% Calculate target transform using command, and current transform
     Tt=updateTransform(Tt,w_control,v_control,dt);

     qt=kukaDLSSolver( qin, Tt, TefTool,numberOfIterations,lambda );

     flag=jointLimitReached(qt);
     if flag==true;
         qt=qin;
     end

     % A condition to break the loop, when x button of the gamepad is
     % pressed
     b=button(joy,1);
     if(b==1)
         break;
     end

        qVec(:,1)=[];
        qVec=[qVec,columnVec(qt)];
        plot(tempVec,qVec);
        pause(0.001)
end


function [ output_args ] = gmm_gradient_descent(X,dlog_pi,dlog_mu,dlog_Sigmas,K,D)
%GMM_GRADIENT_DESCENT
%
%   Maximise the log-likelihood of the data via gradient descent
%
%    input ----------------------------------------------------------------
%
%       o X : (D x N), set of Data
%
%       o dlog_pi : function handle, returns derivatives of the priors
%
%       o dlog_mu : function handle, returns derivaties of the Mu's
%
%       o dlog_Sigma : function handle, returns derivatives of the sCovariance.
%
%

end


function [uOutput] = import_california_fm(nFunction, sFilename)
  % import NCEDC NCSN-FPFIT catalog from the UCB, CA, and the Hauksson relocated catalog for southern California.
% [uOutput] = import_california_fm(nFunction, sFilename)
%
% See:
% http://quake.geo.berkeley.edu
% http://www.data.scec.org/ftp/catalogs/hauksson/Socal_focal/
%
% updated: 17.05.2005, Danijel Schorlemmer
% schorlemmer@sed.ethz.ch

% Counter for double events
nCount = 0;

% Filter function switchyard
if nFunction == FilterOp.getDescription
  uOutput = 'California focal mechanism catalogs (NCEDC NCSN-FPFIT/SCSN Hauksson) raw format/multiple solutions eliminated';
elseif nFunction == FilterOp.importCatalog
  % Read formatted data
  mData = textread(sFilename, '%s', 'delimiter', '\n', 'whitespace', '');
  % Create empty catalog
  uOutput = zeros(length(mData), 24);
  % Loop thru all lines of catalog and convert them
  for i = 1:length(mData)
    if rem(i,100) == 0
      % Notify the user every 100 processed events
      disp([num2str(i) ' of ' num2str(length(mData)) ' events processed.']);
    end
    try
      % Replace whitespace in first column with '0'
      l = find(mData{i} == ' ');
      mData{i}(l) = '0';
      % Read values into catalog
      uOutput(i,1) = -str2num(mData{i}(29:32)) - str2num(mData{i}(34:38))/60 ;  % Longitude
      uOutput(i,2) =  str2num(mData{i}(20:22)) + str2num(mData{i}(24:28))/60 ;  % Latitude
      uOutput(i,3) = str2num(mData{i}(1:4));      % Year
      uOutput(i,4) = str2num(mData{i}(5:6));      % Month
      uOutput(i,5) = str2num(mData{i}(7:8));      % Day
      uOutput(i,6) = str2num(mData{i}(48:52));    % Magnitude
      uOutput(i,7) = str2num(mData{i}(39:45));    % Depth
      uOutput(i,8) = str2num(mData{i}(10:11));    % Hour
      uOutput(i,9) = str2num(mData{i}(12:13));    % Minute
      uOutput(i,10) = str2num(mData{i}(15:19));   % Second
      uOutput(i,11) = nan;                        % Reserved for cross-section values
      uOutput(i,13) = str2num(mData{i}(84:86));   % Dip direction (Plane 1)
      uOutput(i,12) = uOutput(i,13) - 90;         % Strike (Plane 1)
      uOutput(i,12) = mod(uOutput(i,12) + 360, 360);
      uOutput(i,14) = str2num(mData{i}(88:89));   % Dip (Plane 1)
      uOutput(i,15) = str2num(mData{i}(90:93));   % Rake (Plane 1)
      % Second nodal plane
      % uOutput(i,16) = Strike (Plane 2)
      % uOutput(i,17) = Dip direction (Plane 2)
      % uOutput(i,18) = Dip (Plane 2)
      % uOutput(i,19) = Rake (Plane 2)
      [uOutput(i,16), uOutput(i,18), uOutput(i,19), uOutput(i,17)] ...
        = ComputeSecondPlane(uOutput(i,12), uOutput(i,14), uOutput(i,15));
      uOutput(i,20) = str2num(mData{i}(72:74));   % Horizontal location error [km]
      uOutput(i,21) = str2num(mData{i}(77:79));   % Vertical location error [km]
      uOutput(i,22) = str2num(mData{i}(96:99));   % Solution misfit value. 0=perfect fit, 1=perfect misfit (never exceeds 0.5 in reality).
      uOutput(i,23) = str2num(mData{i}(132:141)); % Event ID # found in Hypoinverse archive file
      uOutput(i,24) = str2num(mData{i}(111:114)); % (STDR) Station distribution ratio (0-1). Lower numbers indicate data lie near nodal planes.
      uOutput(i,25) = str2num(mData{i}(101:103)); % Number of first motion observations used in solution
      uOutput(i,26) = str2num(mData{i}(122:123)); % Maximum half-width of 90% confidence range of strike
      uOutput(i,27) = str2num(mData{i}(125:126)); % Maximum half-width of 90% confidence range of dip
      uOutput(i,28) = str2num(mData{i}(128:129)); % Maximum half-width of 90% confidence range of rake
      % In case of multiple solutions, take only the best solution
      try
        if uOutput(i,23) == uOutput(i-1,23)       % Same event?
            nCount=nCount+1;
          if uOutput(i,24) > uOutput(i-1,24)      % Higher STDR?
            uOutput(i-1,1) = nan;                 % Invalidate the former event
          elseif uOutput(i,24) == uOutput(i-1,24) % Same STDR?
            if uOutput(i,22) < uOutput(i-1,22)    % Better solution misfit?
              uOutput(i-1,1) = nan;               % Invalidate the former event
            else                                  % Worse or equal solution misfit?
              uOutput(i,:) = uOutput(i-1,:);      % Copy the former event into current one
              uOutput(i-1,1) = nan;               % Invalidate the former event
            end
          else                                    % Lower STDR?
            uOutput(i,:) = uOutput(i-1,:);        % Copy the former event into current one
            uOutput(i-1,1) = nan;                 % Invalidate the former event
          end
        end
      catch
      end
      %Create decimal year
      uOutput(i,3) = decyear([uOutput(i,3) uOutput(i,4) uOutput(i,5) uOutput(i,8) uOutput(i,9) uOutput(i,10)]);
    catch
      msg.dbfprintf('Import: Problem in line %d of %s. Line ignored.\n',i, sFilename);
      uOutput(i,:) = nan;
    end
  end
  % Delete all invalidated events from catalog
  vSel = isnan(uOutput(:,1));
  uOutput(vSel,:) = [];
end

% --- Helper function

function [fStrike2, fDip2, fRake2, fDipDir2] = ComputeSecondPlane(fStrike1, fDip1, fRake1)

% Do not modify strike (Dip and rake need to be modified in order to
% minimize computational errors)
fDip1 = fDip1 + 0.000001;
fRake1 = mod((fRake1 + 0.000001) + 360, 360);
[fStrike2, fDip2, fRake2, fDipDir2, nError] = focal_pl2pl(fStrike1, fDip1, fRake1);


 function g = gibbsperiodicKernDiagGradient(kern, x, covDiag)

% GIBBSPERIODICKERNDIAGGRADIENT Compute the gradient of the GIBBSPERIODIC kernel's diagonal wrt parameters.
% FORMAT
% DESC computes the gradient of functions of the diagonal of the
% Gibbs-kernel derived periodic kernel matrix with respect to the parameters of the kernel. The
% parameters' gradients are returned in the order given by the
% gibbsperiodicKernExtractParam command.
% ARG kern : the kernel structure for which the gradients are
% computed.
% ARG x : the input data for which the gradient is being computed.
% ARG factors : partial derivatives of the function of interest with
% respect to the diagonal elements of the kernel.
% RETURN g : gradients of the relevant function with respect to each
% of the parameters. Ordering should match the ordering given in
% gibbsperiodicKernExtractParam.
%
% SEEALSO : gibbsperiodicKernParamInit, kernDiagGradient, gibbsperiodicKernExtractParam, gibbsperiodicKernGradient
%
% COPYRIGHT : Neil D. Lawrence, 2007

% KERN

g = zeros(1, kern.nParams);
g(end) = sum(covDiag);