function [energyinband_array,sqi_array,hr_array] = ComputeMVM(ecg,ann_ecg,Fs,segment_size,normalize)

% [energyinband_array,sqi_array] = ComputeMVM(ecg,ann,Fs,segment_size,normalize)
%   OVERVIEW:   This function returns the QRS MVM measured in non overlapping
%   windows of size 'segment_size' in minutes. It also returns the
%   signal quality and mean heart rate for each window.
%
%   INPUT:      MANDATORY:
%               ecg             : a single row of ECG data in samples. Must be sampled at 1 KHz.
%
%               ann             : annotation for the ecg passed as input. The ann variable
%                                 is a struct with Q,R,S fiduical points in sample number as fields
%                                 for the struct. i.e. ann.R, ann.Q, ...
%
%               Fs              : sampling frequency for the ecg signal (Hz)
%
%               segment_size        : the length of non overlapping windows
%                                 being analyzed in minutes. Default size
%                                 is 5 minutes
%
%               normalize       : boolean value (0 or 1). When set to 1
%                               will normalize each analysis window by the median R
%                               amplitude. Default is 1.
%
%
%   OUTPUT:
%               energyinband_array     : the energy for QRS complex morphological variability measured in
%                                        the every 2-7 beats region for each analysis window.
%
%               sqi_array        : the signal quality index measured for each analysis window.
%
%               hr_array          : the median heart rate measured for analysis
%                                 window
%
%   REF:
%   The script is based on the algorithm given in the following paper,
%   Liu Y, Syed Z, Scirica BM, Morrow DA, Guttag JV, Stultz CM.
%   ECG morphological variability in beat space for risk stratification after acute coronary syndrome.
%   J Am Heart Assoc. 2014;3(3):e000981. Published 2014 Jun 24. doi:10.1161/JAHA.114.000981
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Written by Ismail Sadiq
%	COPYRIGHT (C) 2019
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information. The license may be found in
%       the Documents folder of the Physionet-Cardiovascular-Signal-Toolbox.
%

% divide into 5 minute segments
% initialize variables
segment_size_samples = Fs*60*segment_size; % size of analysis window in samples
Nsegments = floor(length(ecg)/segment_size_samples); % number of segments analyzed
energyinband_array = NaN(1, Nsegments); % array for storing the MVM energy measured in each analysis window
sqi_array = NaN(1, Nsegments); % array of storing the signal quality for each window analyzed
hr_array = NaN(1,Nsegments); % array for storing the heart rate estimate for each window analyzed
lengthflag = 0;

sqi_threshold = 0.7;

% convert ecg to row vector
if (size(ecg,1) > size(ecg,2))
   ecg = ecg';
end

if (Nsegments == 0)   % length of data < segment_size_samples, analyze available data
    Nsegments = 1; lengthflag = 1;
end

for segmentidx = 1:Nsegments

    try

        % isolate segment
        if(lengthflag)
            % if the length of signal is < segment_size in samples,
            % determine annotations till the end of the record.
            fiveminsegment = ecg(segmentidx*segment_size_samples+1-segment_size_samples:end);
            ann.R = ann_ecg.R(ann_ecg.R < length(ecg));
            ann.R = ann.R(ann.R > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;% + 1;
            ann.Q = ann_ecg.Q(ann_ecg.Q < length(ecg));
            ann.Q = ann.Q(ann.Q > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;% + 1;
            ann.S = ann_ecg.S(ann_ecg.S < length(ecg));
            ann.S = ann.S(ann.S > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;% + 1;
            ann.QRSon = ann_ecg.QRSon(ann_ecg.QRSon < length(ecg));
            ann.QRSon = ann.QRSon(ann.QRSon > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;
            ann.QRSoff = ann_ecg.QRSoff(ann_ecg.QRSoff < length(ecg));
            ann.QRSoff = ann.QRSoff(ann.QRSoff > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;
            lengthflag = 0;
        else
            % get ecg signal and corresponding annotations in the current
            % window
            fiveminsegment = ecg(segmentidx*segment_size_samples+1-segment_size_samples:segmentidx*segment_size_samples);
            ann = ann_ecg;
            ann.R = ann_ecg.R(ann_ecg.R < segmentidx*segment_size_samples);
            ann.R = ann.R(ann.R > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;% + 1;
            ann.Q = ann_ecg.Q(ann_ecg.Q < segmentidx*segment_size_samples);
            ann.Q = ann.Q(ann.Q > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;% + 1;
            ann.S = ann_ecg.S(ann_ecg.S < segmentidx*segment_size_samples);
            ann.S = ann.S(ann.S > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;% + 1;
            ann.QRSon = ann_ecg.QRSon(ann_ecg.QRSon < segmentidx*segment_size_samples);
            ann.QRSon = ann.QRSon(ann.QRSon > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;
            ann.QRSoff = ann_ecg.QRSoff(ann_ecg.QRSoff < segmentidx*segment_size_samples);
            ann.QRSoff = ann.QRSoff(ann.QRSoff > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;
            %figure(2); plot(fiveminsegment); hold on; % Check annotations after loading
            %scatter(ann.R(~isnan(ann.R)), fiveminsegment(ann.R(~isnan(ann.R)))); hold off;
        end

        ecg_mv = fiveminsegment; clear fiveminsegment;

        % remove any nan values from the annotations
        R = round(ann.R);
        sum(isnan(R)); R = R(~isnan(R)); sum(isnan(R));
        Q = round(ann.Q);
        sum(isnan(Q)); Q = Q(~isnan(Q)); sum(isnan(Q));
        S = round(ann.S);
        sum(isnan(S)); S = S(~isnan(S)); sum(isnan(S));

        % compute the median Q,S and R amplitude, used to determine
        % normalization factor
        median_Q_amp = nanmedian(ecg_mv(1,Q));
        median_R_amp = nanmedian(ecg_mv(1,R));
        median_S_amp = nanmedian(ecg_mv(1,S));

        % compute normalization factor
        if (median_S_amp <= median_Q_amp || isnan(median_Q_amp))
            norm_factor = median_R_amp - median_S_amp;
        else
            if (~isempty(median_Q_amp))
                norm_factor = median_R_amp - median_Q_amp;
            else
                % if unable to determine, set to 1
                disp('unable to compute norm factor, setting to 1')
                norm_factor = 1;
            end
        end

        % normalize signal
        if (normalize)
            ecg_mv = ecg_mv / norm_factor;
        end

        % generate second set of annotations
        refqrs = ann.R;
        % mean impute missing val
        ecg_mv(isnan(ecg_mv)) = nanmean(ecg_mv);
        testqrs = wqrsm(ecg_mv, Fs);
        thres = 0.1; margin = 0; windowlen = 60*segment_size;
        % Determine signal quality
        refqrs = refqrs./Fs; testqrs = testqrs./Fs; % convert to time (s)
        [current_win_sqi,Se,PPV,Nb] = run_sqi(refqrs,testqrs,thres,margin,windowlen,Fs);
        sqi_array(segmentidx) = current_win_sqi;
        if (current_win_sqi > sqi_threshold)   % may need to be set higher

            % perform morphological variability on QRS

            % determine qrs onset and qrs offset annotations over 5 minute segment
            qrson = ann.QRSon;
            qrsoff = ann.QRSoff;
            if (qrsoff(1) < qrson(1))
                startidx = find(qrsoff < qrson(1));
                qrsoff = qrsoff(startidx+1:end);
                qrson = qrson(1:length(qrsoff));
            else
                qrson = qrson(1:length(qrsoff));
            end

            % make sure sufficient annotations to estimate median S
            % amplitude
            if (-1*median_S_amp > median_R_amp && (length(S) > 200))
                alignmentpoint = 'S';
            else
                alignmentpoint = 'R';
            end

            % re-compute R peak
            radj = zeros(1,length(qrson)); % r-adjusted, used in ectopic beat removal
            % setting max value between qrson and qrsoff as r-pk
            for qrsonidx = 1:length(qrson)
                %[pks,locs] = findpeaks(ecg(qrson(qrsonidx):qrsoff(qrsonidx)));
                [pks,locs] = findpeaks(ecg_mv(qrson(qrsonidx):qrsoff(qrsonidx)));
                maxpk = max(pks); maxloc = locs((maxpk == pks));
                if (isempty(maxloc))
                    radj(qrsonidx) = qrson(qrsonidx);
                else
                    radj(qrsonidx) = qrson(qrsonidx)-1+maxloc(1);   % 1 in case of duplicates
                end
            end
            % compute alignemnt point
            % find optimal alignment points max r-pk or s-valley
            alignpoint = zeros(1,length(qrson)); % used in qrs complex isolation
            for qrsonidx = 1:length(qrson)

                if (strcmp(alignmentpoint, 'R'))
                    [pks,locs] = findpeaks((ecg_mv(qrson(qrsonidx):qrsoff(qrsonidx))));
                else
                    [pks,locs] = findpeaks(-1*(ecg_mv(qrson(qrsonidx):qrsoff(qrsonidx))));
                end

                if (isempty(pks))   % if no positive peaks define mid-point between qrson and qrsoff as alignment point
                    locs = round(mean(qrson(qrsonidx), qrsoff(qrsonidx)));
                    pks = ecg_mv(locs);
                    alignpoint(qrsonidx) = locs; continue;
                end
                maxpk = max(pks); maxloc = locs((maxpk == pks));
                alignpoint(qrsonidx) = qrson(qrsonidx)-1+maxloc(1);   % 1 in case of duplicates
            end

            % detect pvc beats
            signal = ecg_mv; th = 0.1;
            [qrs_time pvc_output] = detectpvc2(signal',Fs,th);
            qrs_time = qrs_time(logical(pvc_output));

            % only use 5 minute signals with complete annotations to maintane validity
            % of fourier analysis (may want to replace with interpolation techhnique)
            if (sum(isnan(qrson)) || sum(isnan(qrsoff)))
                disp(['annotations incomplete ' num2str(fileidx)]);
                continue;
            end

            % ectopic beat removal
            qrsonectopicfree = qrson(1);
            rectopicfree = radj(1);
            qrsoffectopicfree = qrsoff(1);
            alignpointsectopicfree = alignpoint(1);
            for ridx = 2:length(radj)-1   % start from idx 2 and goto length(r)-1
                if (ridx <= length(radj)-40)
                    %meanRR = mean(diff(radj(ridx-1:ridx+38)));   % compute mean over 40 beat segment
                    meanRR = median(diff(radj(ridx-1:ridx+38)));   % compute median over 40 beat segment
                else
                    %meanRR = mean(diff(radj(end-39:end)));   % compute mean over last 40 beat segment
                    meanRR = median(diff(radj(end-39:end)));   % compute median over last 40 beat segment
                end
                preRR = radj(ridx) - radj(ridx-1);    % pre-RR interval
                postRR = radj(ridx+1) - radj(ridx);   % post-RR interval
                % if the pre-RR and post-RR vary from the mean RR by more than
                % 20% consider as ectopic
                if ((((meanRR-preRR) > 0.2*meanRR) && ((postRR-meanRR) > 0.2*meanRR)) || (((meanRR-preRR) > 0.2*meanRR) && ((meanRR-postRR) > 0.2*meanRR))) % && ((meanRR-postRR) > 0.2*meanRR)))  % 326, 367
                    continue;
                else % include the QRS complex, may also want to include t-wave onset/offset/amplitude/QT-interval
                    qrsonectopicfree = [qrsonectopicfree qrson(ridx)];
                    rectopicfree = [rectopicfree radj(ridx)];
                    qrsoffectopicfree = [qrsoffectopicfree qrsoff(ridx)];
                    alignpointsectopicfree = [alignpointsectopicfree alignpoint(ridx)];
                end
            end

            % remove ectopic beats
            [C,ia,ib] = intersect(rectopicfree, qrs_time);
            rectopicfree(ia) = [];
            qrsonectopicfree(ia) = [];
            qrsoffectopicfree(ia) = [];
            alignpointsectopicfree(ia) = [];

            % calculate NTWDseries and TWDseries
            % Isolate complexes
            [Complexes] = IsolateQRSComplexes(ecg_mv, qrsoffectopicfree, alignpointsectopicfree, qrsonectopicfree);

            % NTWD and TWD
            [NTWDseries TWDseries] = SquaredDiffComputation_beatbybeat(Complexes);

            % compute spectrum
            nfft = 256; % standardize the length of the fft to 256
            pxx = pwelch(NTWDseries,[],[],nfft);
            %             pxx = pwelch(TWDseries,[],[],nfft);
            N = nfft;
            stepsize = 0.5/length(pxx);

            dl = 2; du = 7;
            indexeverytwobeats = round(N/dl, 0);    % corresponding to every 2 beats
            indexeverysevenbeats = floor(N/du); % corresponding to every 7 beats

            % compute energy in every 2-7 beats region
            energyinband = trapz((indexeverysevenbeats:indexeverytwobeats) * stepsize, pxx(indexeverysevenbeats:indexeverytwobeats));
            energyinband_array(segmentidx) = energyinband; disp(['MVM for current window is ' num2str(energyinband)]);

            % hr est
            hr_array(segmentidx) = 60/(nanmedian(diff(alignpointsectopicfree))/Fs);
        else
            disp('unclean segment')
        end

    catch
        disp('error in segment')
        energyinband_array(segmentidx) = -inf;
        sqi_array(segmentidx) = -inf;
        hr_array(segmentidx) = -inf;
    end

end

end


function [pC] = spm_dcm_symm(pV,pE)
% locks ECD orientations by introducing prior correlations
% FORMAT [pC] = spm_dcm_symm(pV,pE)
%__________________________________________________________________________
%
% pE   - prior expectation
% pV   - prior variance
% pC   - prior covariance
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_dcm_symm.m 5376 2013-04-02 09:59:01Z karl $

% Distance between homolgous sources (16mm)
%--------------------------------------------------------------------------
V     = 16;

% symmetry constraints (based on Euclidean distance from mirror image)
%==========================================================================

% diagonalise feilds
%--------------------------------------------------------------------------
feilds = fieldnames(pV);
for  i = 1:length(feilds)
    pF = getfield(pV,feilds{i});
    pV = setfield(pV,feilds{i},spm_diag(spm_vec(pF)));
end

% impose correlations between orientations (L)
%==========================================================================
n         = size(pE.Lpos,2);
Rpos      = pE.Lpos;
Rpos(1,:) = -Rpos(1,:);
D         = 128*ones(n);

% find symmetrical sources in each hemisphere
%--------------------------------------------------------------------------
for i = 1:n
    for j = 1:n
        if sign(pE.Lpos(1,i)) == sign(Rpos(1,j))
            D(i,j) = sqrt(sum(pE.Lpos(:,i) - Rpos(:,j)).^2);
        end
    end
end
D     = (D + D')/2;
DD    = zeros(n);
for i = 1:n
    [M, I] = min(D(i,:));
    if M < V
        DD(i,I) = 1;
    end
end

% reduce rank of prior covariance matrix of positions
%--------------------------------------------------------------------------
try
    pV.L = pV.L + kron(DD,diag(pV.L(1)*[-1 1 1]));
end

% and concatenate
%--------------------------------------------------------------------------
for  i = 1:length(feilds)
    pF      = getfield(pV,feilds{i});
    if ~isempty(pF)
        pC{i,i} = pF;
    end
end
pC    = spm_cat(pC);


function V = ReferenceVectorRegeneration(PopObj,V)
% Reference vector regeneration

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

    PopObj        = PopObj - repmat(min(PopObj,[],1),size(PopObj,1),1);
    [~,associate] = max(1-pdist2(PopObj,V,'cosine'),[],2);
    inValid       = setdiff(1:size(V,1),associate);
    V(inValid,:)  = rand(length(inValid),size(V,2)).*repmat(max(PopObj,[],1),length(inValid),1);
end


function [DEM] = spm_LAP(DEM)
% Laplacian model inversion (see also spm_LAPS)
% FORMAT DEM   = spm_LAP(DEM)
%
% DEM.M  - hierarchical model
% DEM.Y  - response variable, output or data
% DEM.U  - explanatory variables, inputs or prior expectation of causes
%__________________________________________________________________________
%
% generative model
%--------------------------------------------------------------------------
%   M(i).g  = v     =  g(x,v,P)   {inline function, string or m-file}
%   M(i).f  = dx/dt =  f(x,v,P)   {inline function, string or m-file}
%
%   M(i).ph = pi(v) = ph(x,v,h,M) {inline function, string or m-file}
%   M(i).pg = pi(x) = pg(x,v,g,M) {inline function, string or m-file}
%                                  (assumed to be linear in v and x)
%
%   pi(v,x) = vectors of log-precisions; (h,g) = precision parameters
%
%   M(i).pE = prior expectation of p model-parameters
%   M(i).pC = prior covariances of p model-parameters
%   M(i).hE = prior expectation of h log-precision (cause noise)
%   M(i).hC = prior covariances of h log-precision (cause noise)
%   M(i).gE = prior expectation of g log-precision (state noise)
%   M(i).gC = prior covariances of g log-precision (state noise)
%   M(i).xP = precision (states)
%   M(i).Q  = precision components (input noise)
%   M(i).R  = precision components (state noise)
%   M(i).V  = fixed precision (input noise)
%   M(i).W  = fixed precision (state noise)
%
%   M(i).P  = optional initial value for parameters (defaults to M(i).pE)
%
%   M(i).m  = number of inputs v(i + 1);
%   M(i).n  = number of states x(i);
%   M(i).l  = number of output v(i);
%
% conditional moments of model-states - q(u)
%--------------------------------------------------------------------------
%   qU.x    = Conditional expectation of hidden states
%   qU.v    = Conditional expectation of causal states
%   qU.w    = Conditional prediction error (states)
%   qU.z    = Conditional prediction error (causes)
%   qU.C    = Conditional covariance: cov(v)
%   qU.S    = Conditional covariance: cov(x)
%
% conditional moments of model-parameters - q(p)
%--------------------------------------------------------------------------
%   qP.P    = Conditional expectation
%   qP.C    = Conditional covariance
%
% conditional moments of hyper-parameters (log-transformed) - q(h)
%--------------------------------------------------------------------------
%   qH.h    = Conditional expectation (cause noise)
%   qH.g    = Conditional expectation (state noise)
%   qH.C    = Conditional covariance
%
% F         = log-evidence = log-marginal likelihood = negative free-energy
%
%__________________________________________________________________________
% Accelerated methods: To accelerate computations one can specify the
% nature of the model equations using:
%
% M(1).E.linear = 0: full        - evaluates 1st and 2nd derivatives
% M(1).E.linear = 1: linear      - equations are linear in x and v
% M(1).E.linear = 2: bilinear    - equations are linear in x, v & x*v
% M(1).E.linear = 3: nonlinear   - equations are linear in x, v, x*v, & x*x
% M(1).E.linear = 4: full linear - evaluates 1st derivatives (for GF)
%
% similarly, for evaluating precisions:
%
% M(1).E.method.h = 0,1  switch for precision parameters (hidden causes)
% M(1).E.method.g = 0,1  switch for precision parameters (hidden states)
% M(1).E.method.v = 0,1  switch for precision (hidden causes)
% M(1).E.method.x = 0,1  switch for precision (hidden states)
%__________________________________________________________________________
%
% spm_LAP implements a variational scheme under the Laplace
% approximation to the conditional joint density q on states u, parameters
% p and hyperparameters (h,g) of an analytic nonlinear hierarchical dynamic
% model, with additive Gaussian innovations.
%
%            q(u,p,h,g) = max <L(t)>q
%
% L is the ln p(y,u,p,h,g|M) under the model M. The conditional covariances
% obtain analytically from the curvature of L with respect to the unknowns.
%__________________________________________________________________________
% Copyright (C) 2010-2013 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_LAP.m 6508 2015-07-25 15:23:25Z karl $

% find or create a DEM figure
%--------------------------------------------------------------------------
try
    DEM.M(1).nograph;
catch
    DEM.M(1).nograph = 0;
end
if ~DEM.M(1).nograph
    Fdem = spm_figure('GetWin','DEM');
end

% check model, data and priors
%==========================================================================
[M,Y,U] = spm_DEM_set(DEM);

% set regularisation
%--------------------------------------------------------------------------
try
    dt = DEM.M(1).E.v;
catch
    dt = 0;
    DEM.M(1).E.v = dt;
end

% number of iterations
%--------------------------------------------------------------------------
try, nD = M(1).E.nD; catch, nD = 1;  end
try, nN = M(1).E.nN; catch, nN = 8;  end

% ensure integration scheme evaluates gradients at each time-step
%--------------------------------------------------------------------------
M(1).E.linear = 4;

% assume precisions are a function of, and only of, hyperparameters
%--------------------------------------------------------------------------
try
    method = M(1).E.method;
catch
    method.h = 1;
    method.g = 1;
    method.x = 0;
    method.v = 0;
end
try method.h; catch, method.h = 0; end
try method.g; catch, method.g = 0; end
try method.x; catch, method.x = 0; end
try method.v; catch, method.v = 0; end

% assume precisions are a function of, and only of, hyperparameters
%--------------------------------------------------------------------------
try
    form = M(1).E.form;
catch
    form = 'Gaussian';
end

% checks for Laplace models (precision functions; ph and pg)
%--------------------------------------------------------------------------
for i  = 1:length(M)
    try
        feval(M(i).ph,M(i).x,M(i + 1).v,M(i).hE,M(i)); method.v = 1;
    catch
        M(i).ph = inline('spm_LAP_ph(x,v,h,M)','x','v','h','M');
    end
    try
        feval(M(i).pg,M(i).x,M(i + 1).v,M(i).gE,M(i)); method.x = 1;
    catch
        M(i).pg = inline('spm_LAP_pg(x,v,h,M)','x','v','h','M');
    end
end

M(1).E.method = method;

% order parameters (d = n = 1 for static models) and checks
%==========================================================================
d   = M(1).E.d + 1;                          % embedding order of q(v)
n   = M(1).E.n + 1;                          % embedding order of q(x)

% number of states and parameters
%--------------------------------------------------------------------------
ns  = size(Y,2);                             % number of samples
nl  = size(M,2);                             % number of levels
nv  = sum(spm_vec(M.m));                     % number of v (casual states)
nx  = sum(spm_vec(M.n));                     % number of x (hidden states)
ny  = M(1).l;                                % number of y (inputs)
nc  = M(end).l;                              % number of c (prior causes)
nu  = nv*d + nx*n;                           % number of generalised states
ne  = nv*n + nx*n + ny*n;                    % number of generalised errors

% precision (R) of generalised errors and null matrices for concatenation
%==========================================================================
s     = M(1).E.s;
Rh    = spm_DEM_R(n,s,form);
Rg    = spm_DEM_R(n,s,form);

if ~nx, Rg = sparse(0,0); end

W     = sparse(nx*n,nx*n);
V     = sparse((ny + nv)*n,(ny + nv)*n);

% fixed priors on states (u)
%--------------------------------------------------------------------------
Px    = kron(spm_DEM_R(n,2),spm_cat(spm_diag({M(1:end).xP})));
Pv    = kron(spm_DEM_R(d,2),spm_cat(spm_diag({M(2:end).vP})));
Pu    = spm_cat(spm_diag({Px Pv}));

% hyperpriors
%--------------------------------------------------------------------------
ph.h  = spm_vec({M.hE M.gE});                % prior expectation of h,g
ph.c  = spm_cat(spm_diag({M.hC M.gC}));      % prior covariances of h,g
Ph    = spm_inv(ph.c);                       % prior precision of h,g

qh.h  = {M.hE};                              % conditional expectation h
qh.g  = {M.gE};                              % conditional expectation g
nh    = length(spm_vec(qh.h));               % number of hyperparameters h
ng    = length(spm_vec(qh.g));               % number of hyperparameters g
nb    = nh + ng;                             % number of hyperparameters

% priors on parameters (in reduced parameter space)
%==========================================================================
pp.c  = cell(nl,nl);
qp.p  = cell(nl,1);
for i = 1:(nl - 1)

    % eigenvector reduction: p <- pE + qp.u*qp.p
    %----------------------------------------------------------------------
    qp.u{i}   = spm_svd(M(i).pC,0);          % basis for parameters
    M(i).p    = size(qp.u{i},2);             % number of qp.p
    qp.p{i}   = sparse(M(i).p,1);            % initial deviates
    pp.c{i,i} = qp.u{i}'*M(i).pC*qp.u{i};    % prior covariance

end
Up    = spm_cat(spm_diag(qp.u));

% priors on parameters
%--------------------------------------------------------------------------
pp.p  = spm_vec(M.pE);
pp.c  = spm_cat(pp.c);
Pp    = spm_inv(pp.c);

% initialise conditional density q(p)
%--------------------------------------------------------------------------
for i = 1:(nl - 1)
    try
        qp.p{i} = qp.p{i} + qp.u{i}'*(spm_vec(M(i).P) - spm_vec(M(i).pE));
    end
end
np    = size(Up,2);

% initialise cell arrays for D-Step; e{i + 1} = (d/dt)^i[e] = e[i]
%==========================================================================
qu.x      = cell(n,1);
qu.v      = cell(n,1);
qu.y      = cell(n,1);
qu.u      = cell(n,1);
[qu.x{:}] = deal(sparse(nx,1));
[qu.v{:}] = deal(sparse(nv,1));
[qu.y{:}] = deal(sparse(ny,1));
[qu.u{:}] = deal(sparse(nc,1));

% initialise cell arrays for hierarchical structure of x[0] and v[0]
%--------------------------------------------------------------------------
x         = {M(1:end - 1).x};
v         = {M(1 + 1:end).v};
qu.x{1}   = spm_vec(x);
qu.v{1}   = spm_vec(v);

% derivatives for Jacobian of D-step
%--------------------------------------------------------------------------
Dx     = kron(spm_speye(n,n,1),spm_speye(nx,nx));
Dv     = kron(spm_speye(d,d,1),spm_speye(nv,nv));
Dy     = kron(spm_speye(n,n,1),spm_speye(ny,ny));
Dc     = kron(spm_speye(d,d,1),spm_speye(nc,nc));
Du     = spm_cat(spm_diag({Dx,Dv}));
Ib     = spm_speye(np + nb,np + nb);
dbdt   = sparse(np + nb,1);

% gradients of generalised weighted errors
%--------------------------------------------------------------------------
dedh   = sparse(nh,ne);
dedg   = sparse(ng,ne);
dedv   = sparse(nv,ne);
dedx   = sparse(nx,ne);
dedhh  = sparse(nh,nh);
dedgg  = sparse(ng,ng);

% curvatures of Gibb's energy w.r.t. hyperparameters
%--------------------------------------------------------------------------
dHdh   = sparse(nh,1);
dHdg   = sparse(ng,1);
dHdp   = sparse(np,1);
dHdu   = sparse(nu,1);

% preclude unnecessary iterations and set switches
%--------------------------------------------------------------------------
if ~np && ~nh && ~ng, nN = 1; end
mnx    = nx*~~method.x;
mnv    = nv*~~method.v;

% preclude very precise states from entering free-energy/action
%--------------------------------------------------------------------------
p      = spm_LAP_eval(M,qu,qh);
ih     = p.h < 8;
ig     = p.g < 8;
ie     = kron(ones(n,1),ih);
ix     = kron(ones(n,1),ig);
iv     = kron(ones(d,1),ih((1:nv) + ny));
je     = find([ie; ix]); ix(1:nx) = 1;
ju     = find([ix; iv]);

% and other useful indices
%--------------------------------------------------------------------------
ix     = (1:nx);
ih     = (1:nb);
iv     = (1:nv) + nx*n;
ip     = (1:np) + nu;

% number of iterations for convergence
%--------------------------------------------------------------------------
convergence = -4;

% Iterate Laplace scheme
%==========================================================================
F      = -Inf;
for iN = 1:nN

    % get time and clear persistent variables in evaluation routines
    %----------------------------------------------------------------------
    tic; clear spm_DEM_eval

    % [re-]set states & their derivatives
    %----------------------------------------------------------------------
    try, qu = Q(1).u; end

    % D-Step: (nD D-Steps for each sample)
    %======================================================================
    for is = 1:ns

        % D-Step: until convergence for static systems
        %==================================================================
        for iD = 1:nD

            % sampling time
            %--------------------------------------------------------------
            ts = is + (iD - 1)/nD;

            % derivatives of responses and inputs
            %--------------------------------------------------------------
            try
                qu.y(1:n) = spm_DEM_embed(Y,n,ts,1,M(1).delays);
                qu.u(1:d) = spm_DEM_embed(U,d,ts);
            catch
                qu.y(1:n) = spm_DEM_embed(Y,n,ts);
                qu.u(1:d) = spm_DEM_embed(U,d,ts);
            end

            % evaluate functions and derivatives
            %==============================================================

            % prediction errors (E) and precision vectors (p)
            %--------------------------------------------------------------
            [E,dE] = spm_DEM_eval(M,qu,qp);
            [p,dp] = spm_LAP_eval(M,qu,qh);

            % gradients of log(det(iS)) dDd...
            %==============================================================

            % get precision matrices
            %--------------------------------------------------------------
            iSh   = diag(exp(p.h));
            iSg   = diag(exp(p.g));
            iS    = blkdiag(kron(Rh,iSh),kron(Rg,iSg));

            % gradients of trace(diag(p)) = sum(p); p = precision vector
            %--------------------------------------------------------------
            dpdx  = n*sum(spm_cat({dp.h.dx; dp.g.dx}));
            dpdv  = n*sum(spm_cat({dp.h.dv; dp.g.dv}));
            dpdh  = n*sum(dp.h.dh);
            dpdg  = n*sum(dp.g.dg);
            dpdx  = kron(sparse(1,1,1,1,n),dpdx);
            dpdv  = kron(sparse(1,1,1,1,d),dpdv);
            dDdu  = [dpdx dpdv]';
            dDdh  = [dpdh dpdg]';

            % gradients precision-weighted generalised error dSd..
            %==============================================================

            % gradients w.r.t. hyperparameters
            %--------------------------------------------------------------
            for i = 1:nh
                diS       = diag(dp.h.dh(:,i).*exp(p.h));
                diSdh{i}  = blkdiag(kron(Rh,diS),W);
                dedh(i,:) = E'*diSdh{i};
            end
            for i = 1:ng
                diS       = diag(dp.g.dg(:,i).*exp(p.g));
                diSdg{i}  = blkdiag(V,kron(Rg,diS));
                dedg(i,:) = E'*diSdg{i};
            end

            % gradients w.r.t. hidden states
            %--------------------------------------------------------------
            for i = 1:mnx
                diV       = diag(dp.h.dx(:,i).*exp(p.h));
                diW       = diag(dp.g.dx(:,i).*exp(p.g));
                diSdx{i}  = blkdiag(kron(Rh,diV),kron(Rg,diW));
                dedx(i,:) = E'*diSdx{i};
            end

            % gradients w.r.t. causal states
            %--------------------------------------------------------------
            for i = 1:mnv
                diV       = diag(dp.h.dv(:,i).*exp(p.h));
                diW       = diag(dp.g.dv(:,i).*exp(p.g));
                diSdv{i}  = blkdiag(kron(Rh,diV),kron(Rg,diW));
                dedv(i,:) = E'*diSdv{i};
            end

            dSdx  = kron(sparse(1,1,1,n,1),dedx);
            dSdv  = kron(sparse(1,1,1,d,1),dedv);
            dSdu  = [dSdx; dSdv];
            dEdh  = [dedh; dedg];
            dEdp  = dE.dp'*iS;
            dEdu  = dE.du'*iS;

            % curvatures w.r.t. hyperparameters
            %--------------------------------------------------------------
            for i = 1:nh
                for j = i:nh
                    diS        = diag(dp.h.dh(:,i).*dp.h.dh(:,j).*exp(p.h));
                    diS        = blkdiag(kron(Rh,diS),W);
                    dedhh(i,j) = E'*diS*E;
                    dedhh(j,i) = dedhh(i,j);
                end
            end
            for i = 1:ng
                for j = i:ng
                    diS        = diag(dp.g.dg(:,i).*dp.g.dg(:,j).*exp(p.g));
                    diS        = blkdiag(V,kron(Rg,diS));
                    dedgg(i,j) = E'*diS*E;
                    dedgg(j,i) = dedgg(i,j);
                end
            end

            % combined curvature
            %--------------------------------------------------------------
            dSdhh = spm_cat({dedhh  [] ;
                             [] dedgg});

            % errors (from prior expectations) (NB pp.p = 0)
            %--------------------------------------------------------------
            Eu    = spm_vec(qu.x(1:n),qu.v(1:d));
            Ep    = spm_vec(qp.p);
            Eh    = spm_vec(qh.h,qh.g) - ph.h;

            % first-order derivatives of Gibb's Energy
            %==============================================================
            dLdu  = dEdu*E + dSdu*E/2 - dDdu/2 + Pu*Eu;
            dLdh  = dEdh*E/2          - dDdh/2 + Ph*Eh;
            dLdp  = dEdp*E                     + Pp*Ep;

            % and second-order derivatives of Gibb's Energy
            %--------------------------------------------------------------
            dLduu = dEdu*dE.du + Pu;
            dLdpp = dEdp*dE.dp + Pp;
            dLdhh = dSdhh/2    + Ph;
            dLduy = dEdu*dE.dy;
            dLduc = dEdu*dE.dc;
            dLdup = dEdu*dE.dp;
            dLdhp = dEdh*dE.dp;
            dLdpu = dLdup';
            dLdph = dLdhp';

            % precision and covariances for entropy
            %--------------------------------------------------------------
            dLdaa = spm_cat({dLduu dLdup  ;
                             dLdpu dLdpp});
            dLdbb = spm_cat({dLdpp dLdph  ;
                             dLdhp dLdhh});

            Cup   = spm_inv(dLdaa);
            Chh   = spm_inv(dLdhh);

            % first-order derivatives of Entropy term
            %==============================================================

            % log-precision
            %--------------------------------------------------------------
            for i = 1:nh
                Luub    = dE.du'*diSdh{i}*dE.du;
                Lpub    = dE.dp'*diSdh{i}*dE.du;
                Lppb    = dE.dp'*diSdh{i}*dE.dp;
                diCdh   = spm_cat({Luub Lpub';
                                   Lpub Lppb});
                dHdh(i) = spm_trace(diCdh,Cup)/2;
            end
            for i = 1:ng
                Luub    = dE.du'*diSdg{i}*dE.du;
                Lpub    = dE.dp'*diSdg{i}*dE.du;
                Lppb    = dE.dp'*diSdg{i}*dE.dp;
                diCdg   = spm_cat({Luub Lpub';
                                   Lpub Lppb});
                dHdg(i) = spm_trace(diCdg,Cup)/2;
            end

            % parameters
            %--------------------------------------------------------------
            for i = 1:np
                Luup    = dE.dup{i}'*dEdu';
                Lpup    = dEdp*dE.dup{i};
                Luup    = Luup + Luup';
                diCdp   = spm_cat({Luup Lpup';
                                   Lpup [] });
                dHdp(i) = spm_trace(diCdp,Cup)/2;
            end

            % hidden states and causes (disabled for stability)
            %--------------------------------------------------------------
            for i = 1:(nu - nu)
                Lppu    = dE.dpu{i}'*dEdp';
                Lupu    = dEdu*dE.dpu{i};
                Lppu    = Lppu + Lppu';
                diCdu   = spm_cat({[]    Lupu;
                                   Lupu' Lppu});
                dHdu(i) = spm_trace(diCdu,Cup)/2;
            end

            % and concatenate
            %--------------------------------------------------------------
            dHdb  = [dHdh; dHdg];
            dHdb  = [dHdp; dHdb];
            dLdb  = [dLdp; dLdh];

            % save conditional moments (and prediction error) at Q{t}
            %==============================================================
            if iD == 1 || ns == 1

                % save means
                %----------------------------------------------------------
                Q(is).e   = E;
                Q(is).E   = diag(diag(iS))*E;
                Q(is).u   = qu;
                Q(is).p   = qp;
                Q(is).h   = qh;

                % and conditional covariances
                %----------------------------------------------------------
                Q(is).u.s = Cup(ix,ix);
                Q(is).u.c = Cup(iv,iv);
                Q(is).p.c = Cup(ip,ip);
                Q(is).h.c = Chh(ih,ih);

                % Free-energy (components)
                %----------------------------------------------------------
                Fc(is,1)  = - E(je)'*iS(je,je)*E(je)/2;
                Fc(is,2)  = - Eu(ju)'*Pu(ju,ju)*Eu(ju)/2;
                Fc(is,3)  = - n*ny*log(2*pi)/2;
                Fc(is,4)  = spm_logdet(iS(je,je))/2;
                Fc(is,5)  = spm_logdet(Pu(ju,ju)*Cup(ju,ju))/2;

                % Free-action (states and parameters)
                %----------------------------------------------------------
                AC(is)    = sum(Fc(is,:))    ...
                          - Ep'*Pp*Ep/2      ...
                          - Eh'*Ph*Eh/2      ...
                          + spm_logdet(Pp)/2 ...
                          + spm_logdet(Ph)/2 ...
                          - spm_logdet(dLdbb)/2;

            end

            % update conditional moments
            %==============================================================

            % prior precision of fluctuations on [hyper] parameters
            %--------------------------------------------------------------
            Kb    = ns*Ib;

            % accumulate curvatures of [hyper] parameters
            %--------------------------------------------------------------
            try
                dLdBB = dLdBB*(1 - 1/ns) + dLdbb/ns;
            catch
                dLdBB = dLdbb + Ib*32;
            end

            % whiten gradient (and curvatures) with regularised precision
            %--------------------------------------------------------------
            Cb    = spm_inv(dLdBB + diag(diag(dLdBB))*exp(dt));
            dLdb  = Cb*dLdb;
            dHdb  = Cb*dHdb;

            % assemble conditional means
            %--------------------------------------------------------------
            q.y  = qu.y(1:n);
            q.x  = qu.x(1:n);
            q.v  = qu.v(1:d);
            q.c  = qu.u(1:d);
            q.p  = qp.p;
            q.h  = qh.h;
            q.g  = qh.g;
            q.d  = dbdt;

            % flow
            %--------------------------------------------------------------
            f.y  =  Dy*spm_vec(q.y)                  ;
            f.u  =  Du*spm_vec(q.x,q.v) - dLdu - dHdu;
            f.c  =  Dc*spm_vec(q.c)                  ;
            f.b  =     spm_vec(q.d)                  ;
            f.d  = -Kb*spm_vec(q.d)     - dLdb - dHdb;

            % and Jacobian
            %--------------------------------------------------------------
            dfdq = {Dy     []        []     []    [] ;
                   -dLduy  Du-dLduu -dLduc  []    [] ;
                    []     []        Dc     []    [] ;
                    []     []        []     []    Ib ;
                    []     []        []    -Ib   -Kb};

            dfdq = spm_cat(dfdq);

            % get eigenvalues of Jacobian (on hidden states)
            %--------------------------------------------------------------
            try DEM.options.eigenvalues;
                DEM.E(:,is) = eig(full(Du - dLduu));
            end

            % update conditional modes of states
            %==============================================================
            dq   = spm_dx(dfdq,spm_vec(f),1/nD);
            q    = spm_unvec(spm_vec(q) + dq,q);

            % unpack conditional means
            %--------------------------------------------------------------
            qu.x(1:n) = q.x;
            qu.v(1:d) = q.v;
            qp.p      = q.p;
            qh.h      = q.h;
            qh.g      = q.g;
            dbdt      = q.d;

        end % D-Step

    end % sequence (ns)

    % Bayesian parameter averaging
    %======================================================================

    % Conditional moments of time-averaged parameters
    %----------------------------------------------------------------------
    Ep    = 0;
    Qp    = 0;
    for i = 1:ns
        P   = spm_inv(Q(i).p.c);
        Ep  = Ep  + P*spm_vec(Q(i).p.p);
        Qp  = Qp + P;
    end
    Ep    = spm_inv(Qp)*Ep;
    Cp    = spm_inv(Qp + (1 - ns)*Pp);

    % conditional moments of hyper-parameters
    %----------------------------------------------------------------------
    Eh    = 0;
    Qh    = 0;
    for i = 1:ns
        P   = spm_inv(Q(i).h.c);
        Eh  = Eh  + P*spm_vec(Q(i).h.h,Q(i).h.g);
        Qh  = Qh + P;
    end
    Eh    = spm_inv(Qh)*Eh - ph.h;
    Ch    = spm_inv(Qh + (1 - ns)*Ph);

    % Free-action of states plus free-energy of parameters
    %======================================================================
    FT    = sum(Fc,2);          %   instantaneous Free energy (of states)
    FC(1) = sum(Fc(:,1));       % - E'*iS*E/2;
    FC(2) = sum(Fc(:,2));       % - Eu'*Pu*Eu/2;
    FC(3) = sum(Fc(:,3));       % - n*ny*log(2*pi)/2;
    FC(4) = sum(Fc(:,4));       %   spm_logdet(iS)/2;
    FC(5) = sum(Fc(:,5));       %   spm_logdet(Pu*Cu)/2;
    FC(6) = -Ep'*Pp*Ep/2;
    FC(7) = -Eh'*Ph*Eh/2;
    FC(8) = spm_logdet(Pp*Cp)/2;
    FC(9) = spm_logdet(Ph*Ch)/2;

    CC(iN,:) = FC;
    S(iN)    = sum(AC);
    Fe       = sum(FC);

    % if F is decreasing, revert [hyper] parameters and slow down
    %----------------------------------------------------------------------
    if Fe < F(iN) && iN > 4

        % save free-energy
        %------------------------------------------------------------------
        F(iN + 1) = F(iN);

        % load current MAP estimates
        %------------------------------------------------------------------
        qp = PQ.qp;
        qh = PQ.qh;

        % decrease update time
        %------------------------------------------------------------------
        dt = max(dt + 2,2);

        % convergence
        %------------------------------------------------------------------
        if dt > 6; convergence = 1; end

    else

        % convergence
        %------------------------------------------------------------------
        if Fe - F(iN) < 1, convergence = convergence + 1; end

        % save free-energy
        %------------------------------------------------------------------
        F(iN)     = Fe;
        F(iN + 1) = Fe;

        % save current MAP estimates
        %------------------------------------------------------------------
        PQ.qp = qp;
        PQ.qh = qh;

        % increase update time
        %------------------------------------------------------------------
        dt    = max(dt - 1,-8);

    end

    % Convergence
    %======================================================================
    if convergence > 0; break, end

    % otherwise save conditional moments (for each time point)
    %======================================================================
    for t = 1:length(Q)

        % states and predictions
        %------------------------------------------------------------------
        v     = spm_unvec(Q(t).u.v{1},v);
        x     = spm_unvec(Q(t).u.x{1},x);
        z     = spm_unvec(Q(t).e(1:(ny + nv)),{M.v});
        Z     = spm_unvec(Q(t).E(1:(ny + nv)),{M.v});
        w     = spm_unvec(Q(t).e((1:nx) + (ny + nv)*n),{M.x});
        X     = spm_unvec(Q(t).E((1:nx) + (ny + nv)*n),{M.x});
        for i = 1:(nl - 1)
            if M(i).m, qU.v{i + 1}(:,t) = spm_vec(v{i});  end
            if M(i).n, qU.x{i}(:,t)     = spm_vec(x{i});  end
            if M(i).n, qU.w{i}(:,t)     = spm_vec(w{i});  end
            if M(i).l, qU.z{i}(:,t)     = spm_vec(z{i});  end
            if M(i).n, qU.W{i}(:,t)     = spm_vec(X{i});  end
            if M(i).l, qU.Z{i}(:,t)     = spm_vec(Z{i});  end
        end
        if    M(nl).l, qU.z{nl}(:,t)    = spm_vec(z{nl}); end
        if    M(nl).l, qU.Z{nl}(:,t)    = spm_vec(Z{nl}); end

        qU.v{1}(:,t)  = spm_vec(Q(t).u.y{1}) - spm_vec(z{1});

        % and conditional covariances
        %------------------------------------------------------------------
        qU.S{t} = Q(t).u.s;
        qU.C{t} = Q(t).u.c;

        % parameters
        %------------------------------------------------------------------
        qP.p{t} = spm_vec(Q(t).p.p);
        qP.c{t} = Q(t).p.c;

        % hyperparameters
        %------------------------------------------------------------------
        qH.p{t} = spm_vec(Q(t).h.h,Q(t).h.g);
        qH.c{t} = Q(t).h.c;

    end

    % graphics (states)
    %----------------------------------------------------------------------
    if exist('Fdem','var')
        spm_figure('Select', Fdem)
        spm_DEM_qU(qU)

        % graphics (parameters and log-precisions)
        %------------------------------------------------------------------
        if np && nb
            subplot(2*nl,2,4*nl - 2)
            plot(1:ns,full(spm_cat(qP.p)))
            set(gca,'XLim',[1 ns])
            title('parameters (modes)','FontSize',16)

            subplot(2*nl,2,4*nl)
            plot(1:ns,full(spm_cat(qH.p)))
            set(gca,'XLim',[1 ns])
            title('log-precision','FontSize',16)

        elseif nb
            subplot(nl,2,2*nl)
            plot(1:ns,full(spm_cat(qH.p)))
            set(gca,'XLim',[1 ns])
            title('log-precision','FontSize',16)

        elseif np
            subplot(nl,2,2*nl)
            plot(1:ns,full(spm_cat(qP.p)))
            set(gca,'XLim',[1 ns])
            title('parameters (modes)','FontSize',16)

        end
        drawnow
    end

    % report (EM-Steps)
    %----------------------------------------------------------------------
    try
        dF = F(iN) - F(iN - 1);
    catch
        dF = 0;
    end
    str{1} = sprintf('LAP: %i (%i)', iN,iD);
    if iN == 1
        str{2} = sprintf('  F0:%.4e', full(F(iN)));
    else
        str{2} = sprintf('F-F0:%.4e', full(F(iN) - F(1)));
    end
    str{3} = sprintf('dF:%.2e',      full(dF));
    str{4} = sprintf('(%.2e sec)',   full(toc));
    fprintf('%-16s%-20s%-14s%-16s\n',str{:})

end
fprintf('%-19sF:%.4e\n', 'LAP: Converged', F(end));
if exist('Fdem','var')
    spm_figure('Focus', Fdem)
end

% Place Bayesian parameter averages in output arguments
%==========================================================================

% Conditional moments of time-averaged parameters
%--------------------------------------------------------------------------
Qp = 0;
Ep = 0;
for i = 1:ns

    % weight in proportion to precisions
    %----------------------------------------------------------------------
    P  = spm_inv(qP.c{i});
    Ep = Ep + P*qP.p{i};
    Qp = Qp + P;

end
Ep     = spm_inv(Qp)*Ep;
Cp     = spm_inv(Qp + (1 - ns)*Pp);
qP.P   = spm_unvec(Up*Ep + pp.p,{M.pE});
qP.C   = Up*Cp*Up';
qP.V   = spm_unvec(diag(qP.C),{M.pE});
qP.U   = Up;

% conditional moments of hyper-parameters
%--------------------------------------------------------------------------
Qh = 0;
Eh = 0;
for i = 1:ns

    % weight in proportion to precisions
    %----------------------------------------------------------------------
    P  = spm_inv(qH.c{i});
    Eh = Eh + P*qH.p{i};
    Qh = Qh + P;

end
Eh     = spm_inv(Qh)*Eh;
Ch     = spm_inv(Qh + (1 - ns)*Ph);
P      = spm_unvec(Eh,{qh.h,qh.g});
qH.h   = P{1};
qH.g   = P{2};
qH.C   = Ch;
P      = spm_unvec(diag(qH.C),P);
qH.V   = P{1};
qH.W   = P{2};

% assign output variables
%--------------------------------------------------------------------------
DEM.M  = M;                   % model
DEM.U  = U;                   % causes

DEM.qU = qU;                  % conditional moments of model-states
DEM.qP = qP;                  % conditional moments of model-parameters
DEM.qH = qH;                  % conditional moments of hyper-parameters

DEM.F  = F(1:iN);             % [-ve] Free-energy
DEM.S  = S(1:iN);             % [-ve] Free-action
DEM.FC = FC;                  % Free-energy components
DEM.CC = CC;                  % over iterations
DEM.FT = FT;                  % over time


filename='Arch_hexahedra_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'MMA'; incrementFactor = 1;
filterType = 'P1';

nsteps = 1;
Vfrac_final = 0.1;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


%TIME_7
%
%   The purpose of this test is to compare the FB routines.
%
%   fb_1 is the original, using two passes through memory and no integer
%   optimizations.
%
%   fb_2 use only one pass and integer optimizations.

Lr=[480000*sf^2,480000*sf^2,262144*sf^2,262144*sf^2,900*sf^2];
ar=[     600*sf,     600*sf,        512,        512,       2];
Mr=[     800*sf,     800*sf,       1024,       1024,  600*sf];
gr=[     800*sf,  40*600*sf,       1024,     40*512,  600*sf];

for ii=1:length(Lr)

  L=Lr(ii);

  M=Mr(ii);
  a=ar(ii);

  gl=gr(ii);

  g=rand(L,1);
  f=rand(L,1);
  gfir=rand(gl,1);

  [L, a, M, gl]

  c1=mex_dgt_fb_1(f,gl,a,M,1);
  c2=mex_dgt_fb_2(f,gl,a,M,1);

end;


classdef ShFunc_Chomog_EnforceCh < ShFunc_Chomog

    properties (Access = public)
        ChTarget
    end

    properties (Access = private)
       C0
       weights
       difC
       difCp
    end

    properties (Access = protected)
        pNorm
    end

    methods (Access = public)

      function computeGradientValue(obj)
            obj.computeChDerivative();
            nElem  = obj.getnElem();
            nGaus  = obj.getnGaus();
            nStres = obj.getnStre();
            nComp  = obj.computeNcomp(nStres);
            grad = zeros(nElem,nGaus,nComp);
            dCh = permute(obj.dCh,[3,4,1,2]);
            for iStres = 1:nStres
                for jStres = 1:nStres
                    iv     = obj.vector2Voigt(iStres,jStres);
                    difCij = obj.difC(iStres,jStres);
                    C0ij   = obj.C0(iStres,jStres);
                    wij    = obj.weights(iv);
                    p      = obj.pNorm;
                    dChij   = squeeze(dCh(:,:,iStres,jStres));
                    dCostIv = (sum(obj.difCp))^(1/p-1)*wij*(difCij/C0ij)^(p-1)*(dChij/C0ij);
                    grad(:,:,iv) = dCostIv + grad(:,:,iv);
                end
            end
            obj.gradient = sum(grad,3);
        end

        function computeFunctionValue(obj)
            obj.difC  = obj.computeDifC();
            obj.difCp = obj.computeNormP(obj.difC);
            p = obj.pNorm;
            obj.value = sum(obj.difCp)^(1/p);
        end

        function v = getVariablesToPlot(obj)
            ChTargetP = obj.computeNormP(obj.ChTarget);
            p = obj.pNorm;
            normChTarget = sum(ChTargetP)^(1/p);
            v{1} = (obj.value*obj.value0)/normChTarget;
        end

    end

    methods (Access = protected)

        function difCp = computeNormP(obj,difC)
            nStres = obj.getnStre();
            nComp  = obj.computeNcomp(nStres);
            dCp = zeros(nComp,1);
            for iStres = 1:nStres
                for jStres = 1:nStres
                    iv = obj.vector2Voigt(iStres,jStres);
                    difCij = difC(iStres,jStres);
                    C0ij   = obj.C0(iStres,jStres);
                    wij    = obj.weights(iv);
                    p      = obj.pNorm;
                    costij = wij*(difCij/C0ij)^p;
                    dCp(iv) = costij;
                end
            end
            difCp = dCp;
        end

        function difC = computeDifC(obj)
            difC = (obj.Chomog - obj.ChTarget);
        end

        function computeChTarget(obj,cParams)
            obj.ChTarget = ChTargetFactory.create(cParams);
        end

        function computeC0(obj)
            C = ones(3,3);
            C(1,1) = obj.ChTarget(1,1);
%             C(2,2) = obj.ChTarget(2,2);
%             C(3,3) = obj.ChTarget(3,3);
             C(1,2) = 0.01;
             C(2,1) = 0.01;
            obj.C0 = C;
        end

        function computeWeights(obj)
            f = 1;%0000;
            obj.weights = [1,1,1,1,1,f]';
        end

    end

    methods (Access = private, Static)

        function n = computeNcomp(nStre)
           n = (nStre+1)*nStre/2;
        end

        function [iv] = vector2Voigt(iStre,jStre)
            T = [1 6 5; 6 2 4; 5 4 3];
            iv = T(iStre,jStre);
        end

    end
end


function [Csig,Cbak,Covl]= composite(cleanFile, enhancedFile);
% ----------------------------------------------------------------------
%          Composite Objective Speech Quality Measure
%
%   This function implements the composite objective measure proposed in
%   [1].
%
%   Usage:  [sig,bak,ovl]=composite(cleanFile.wav, enhancedFile.wav)
%
%         cleanFile.wav - clean input file in .wav format
%         enhancedFile  - enhanced output file in .wav format
%         sig           - predicted rating [1-5] of speech distortion
%         bak           - predicted rating [1-5] of noise distortion
%         ovl           - predicted rating [1-5] of overall quality
%
%       In addition to the above ratings (sig, bak, & ovl) it returns
%       the individual values of the LLR, SNRseg, WSS and PESQ measures.
%
%  Example call:  [sig,bak,ovl] =composite('sp04.wav','enhanced.wav')
%
%
%  References:
%
%     [1]   Hu, Y. and Loizou, P. (2006). Evaluation of objective measures
%           for speech enhancement. Proc. Interspeech, Pittsburg, PA.
%
%   Authors: Yi Hu and Philipos C. Loizou
%   (the LLR, SNRseg and WSS measures were based on Bryan Pellom and John
%     Hansen's implementations)
%
% Copyright (c) 2006 by Philipos C. Loizou
% $Revision: 0.0 $  $Date: 10/09/2006 $

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

if nargin~=2
    fprintf('USAGE: [sig,bak,ovl]=composite(cleanFile.wav, enhancedFile.wav)\n');
    fprintf('For more help, type: help composite\n\n');
    return;
end

alpha= 0.95;

[data1, Srate1, Nbits1]= wavread(cleanFile);
[data2, Srate2, Nbits2]= wavread(enhancedFile);
if ( Srate1~= Srate2) | ( Nbits1~= Nbits2)
    error( 'The two files do not match!\n');
end

len= min( length( data1), length( data2));
data1= data1( 1: len)+eps;
data2= data2( 1: len)+eps;

% -- compute the WSS measure ---
%
wss_dist_vec= wss( data1, data2,Srate1);
wss_dist_vec= sort( wss_dist_vec);
wss_dist= mean( wss_dist_vec( 1: round( length( wss_dist_vec)*alpha)));

% --- compute the LLR measure ---------
%
LLR_dist= llr( data1, data2,Srate1);
LLRs= sort(LLR_dist);
LLR_len= round( length(LLR_dist)* alpha);
llr_mean= mean( LLRs( 1: LLR_len));

% --- compute the SNRseg ----------------
%
[snr_dist, segsnr_dist]= snr( data1, data2,Srate1);
snr_mean= snr_dist;
segSNR= mean( segsnr_dist);

% -- compute the pesq ----
%
% if     Srate1==8000,    mode='nb';
% elseif Srate1 == 16000, mode='wb';
% else,
%      error ('Sampling freq in PESQ needs to be 8 kHz or 16 kHz');
% end

 [pesq_mos_scores]= pesq(cleanFile, enhancedFile);

 if length(pesq_mos_scores)==2
     pesq_mos=pesq_mos_scores(1); % take the raw PESQ value instead of the
                                  % MOS-mapped value (this composite
                                  % measure was only validated with the raw
                                  % PESQ value)
 else
     pesq_mos=pesq_mos_scores;
 end

% --- now compute the composite measures ------------------
%
Csig = 3.093 - 1.029*llr_mean + 0.603*pesq_mos-0.009*wss_dist;
  Csig = max(1,Csig);  Csig=min(5, Csig); % limit values to [1, 5]
Cbak = 1.634 + 0.478 *pesq_mos - 0.007*wss_dist + 0.063*segSNR;
  Cbak = max(1, Cbak); Cbak=min(5,Cbak); % limit values to [1, 5]
Covl = 1.594 + 0.805*pesq_mos - 0.512*llr_mean - 0.007*wss_dist;
  Covl = max(1, Covl); Covl=min(5, Covl); % limit values to [1, 5]

fprintf('\n LLR=%f   SNRseg=%f   WSS=%f   PESQ=%f\n',llr_mean,segSNR,wss_dist,pesq_mos);

return; %=================================================================

function distortion = wss(clean_speech, processed_speech,sample_rate)

% ----------------------------------------------------------------------
% Check the length of the clean and processed speech.  Must be the same.
% ----------------------------------------------------------------------

clean_length      = length(clean_speech);
processed_length  = length(processed_speech);

if (clean_length ~= processed_length)
  disp('Error: Files  musthave same length.');
  return
end

% ----------------------------------------------------------------------
% Global Variables
% ----------------------------------------------------------------------

winlength   = round(30*sample_rate/1000); %240;		   % window length in samples
skiprate    = floor(winlength/4);		   % window skip in samples
max_freq    = sample_rate/2;	   % maximum bandwidth
num_crit    = 25;		   % number of critical bands

USE_FFT_SPECTRUM = 1;		   % defaults to 10th order LP spectrum
n_fft       = 2^nextpow2(2*winlength);
n_fftby2    = n_fft/2;		   % FFT size/2
Kmax        = 20;		   % value suggested by Klatt, pg 1280
Klocmax     = 1;		   % value suggested by Klatt, pg 1280

% ----------------------------------------------------------------------
% Critical Band Filter Definitions (Center Frequency and Bandwidths in Hz)
% ----------------------------------------------------------------------

cent_freq(1)  = 50.0000;   bandwidth(1)  = 70.0000;
cent_freq(2)  = 120.000;   bandwidth(2)  = 70.0000;
cent_freq(3)  = 190.000;   bandwidth(3)  = 70.0000;
cent_freq(4)  = 260.000;   bandwidth(4)  = 70.0000;
cent_freq(5)  = 330.000;   bandwidth(5)  = 70.0000;
cent_freq(6)  = 400.000;   bandwidth(6)  = 70.0000;
cent_freq(7)  = 470.000;   bandwidth(7)  = 70.0000;
cent_freq(8)  = 540.000;   bandwidth(8)  = 77.3724;
cent_freq(9)  = 617.372;   bandwidth(9)  = 86.0056;
cent_freq(10) = 703.378;   bandwidth(10) = 95.3398;
cent_freq(11) = 798.717;   bandwidth(11) = 105.411;
cent_freq(12) = 904.128;   bandwidth(12) = 116.256;
cent_freq(13) = 1020.38;   bandwidth(13) = 127.914;
cent_freq(14) = 1148.30;   bandwidth(14) = 140.423;
cent_freq(15) = 1288.72;   bandwidth(15) = 153.823;
cent_freq(16) = 1442.54;   bandwidth(16) = 168.154;
cent_freq(17) = 1610.70;   bandwidth(17) = 183.457;
cent_freq(18) = 1794.16;   bandwidth(18) = 199.776;
cent_freq(19) = 1993.93;   bandwidth(19) = 217.153;
cent_freq(20) = 2211.08;   bandwidth(20) = 235.631;
cent_freq(21) = 2446.71;   bandwidth(21) = 255.255;
cent_freq(22) = 2701.97;   bandwidth(22) = 276.072;
cent_freq(23) = 2978.04;   bandwidth(23) = 298.126;
cent_freq(24) = 3276.17;   bandwidth(24) = 321.465;
cent_freq(25) = 3597.63;   bandwidth(25) = 346.136;

bw_min      = bandwidth (1);	   % minimum critical bandwidth

% ----------------------------------------------------------------------
% Set up the critical band filters.  Note here that Gaussianly shaped
% filters are used.  Also, the sum of the filter weights are equivalent
% for each critical band filter.  Filter less than -30 dB and set to
% zero.
% ----------------------------------------------------------------------

min_factor = exp (-30.0 / (2.0 * 2.303));       % -30 dB point of filter

for i = 1:num_crit
  f0 = (cent_freq (i) / max_freq) * (n_fftby2);
  all_f0(i) = floor(f0);
  bw = (bandwidth (i) / max_freq) * (n_fftby2);
  norm_factor = log(bw_min) - log(bandwidth(i));
  j = 0:1:n_fftby2-1;
  crit_filter(i,:) = exp (-11 *(((j - floor(f0)) ./bw).^2) + norm_factor);
  crit_filter(i,:) = crit_filter(i,:).*(crit_filter(i,:) > min_factor);
end

% ----------------------------------------------------------------------
% For each frame of input speech, calculate the Weighted Spectral
% Slope Measure
% ----------------------------------------------------------------------

num_frames = clean_length/skiprate-(winlength/skiprate); % number of frames
start      = 1;					% starting sample
window     = 0.5*(1 - cos(2*pi*(1:winlength)'/(winlength+1)));

for frame_count = 1:num_frames

   % ----------------------------------------------------------
   % (1) Get the Frames for the test and reference speech.
   %     Multiply by Hanning Window.
   % ----------------------------------------------------------

   clean_frame = clean_speech(start:start+winlength-1);
   processed_frame = processed_speech(start:start+winlength-1);
   clean_frame = clean_frame.*window;
   processed_frame = processed_frame.*window;

   % ----------------------------------------------------------
   % (2) Compute the Power Spectrum of Clean and Processed
   % ----------------------------------------------------------

    if (USE_FFT_SPECTRUM)
       clean_spec     = (abs(fft(clean_frame,n_fft)).^2);
       processed_spec = (abs(fft(processed_frame,n_fft)).^2);
    else
       a_vec = zeros(1,n_fft);
       a_vec(1:11) = lpc(clean_frame,10);
       clean_spec     = 1.0/(abs(fft(a_vec,n_fft)).^2)';

       a_vec = zeros(1,n_fft);
       a_vec(1:11) = lpc(processed_frame,10);
       processed_spec = 1.0/(abs(fft(a_vec,n_fft)).^2)';
    end

   % ----------------------------------------------------------
   % (3) Compute Filterbank Output Energies (in dB scale)
   % ----------------------------------------------------------

   for i = 1:num_crit
      clean_energy(i) = sum(clean_spec(1:n_fftby2) ...
		            .*crit_filter(i,:)');
      processed_energy(i) = sum(processed_spec(1:n_fftby2) ...
			        .*crit_filter(i,:)');
   end
   clean_energy = 10*log10(max(clean_energy,1E-10));
   processed_energy = 10*log10(max(processed_energy,1E-10));

   % ----------------------------------------------------------
   % (4) Compute Spectral Slope (dB[i+1]-dB[i])
   % ----------------------------------------------------------

   clean_slope     = clean_energy(2:num_crit) - ...
		     clean_energy(1:num_crit-1);
   processed_slope = processed_energy(2:num_crit) - ...
		     processed_energy(1:num_crit-1);

   % ----------------------------------------------------------
   % (5) Find the nearest peak locations in the spectra to
   %     each critical band.  If the slope is negative, we
   %     search to the left.  If positive, we search to the
   %     right.
   % ----------------------------------------------------------

   for i = 1:num_crit-1

       % find the peaks in the clean speech signal

       if (clean_slope(i)>0) 		% search to the right
	  n = i;
          while ((n<num_crit) & (clean_slope(n) > 0))
	     n = n+1;
 	  end
	  clean_loc_peak(i) = clean_energy(n-1);
       else				% search to the left
          n = i;
	  while ((n>0) & (clean_slope(n) <= 0))
	     n = n-1;
 	  end
	  clean_loc_peak(i) = clean_energy(n+1);
       end

       % find the peaks in the processed speech signal

       if (processed_slope(i)>0) 	% search to the right
	  n = i;
          while ((n<num_crit) & (processed_slope(n) > 0))
	     n = n+1;
	  end
	  processed_loc_peak(i) = processed_energy(n-1);
       else				% search to the left
          n = i;
	  while ((n>0) & (processed_slope(n) <= 0))
	     n = n-1;
 	  end
	  processed_loc_peak(i) = processed_energy(n+1);
       end

   end

   % ----------------------------------------------------------
   %  (6) Compute the WSS Measure for this frame.  This
   %      includes determination of the weighting function.
   % ----------------------------------------------------------

   dBMax_clean       = max(clean_energy);
   dBMax_processed   = max(processed_energy);

   % The weights are calculated by averaging individual
   % weighting factors from the clean and processed frame.
   % These weights W_clean and W_processed should range
   % from 0 to 1 and place more emphasis on spectral
   % peaks and less emphasis on slope differences in spectral
   % valleys.  This procedure is described on page 1280 of
   % Klatt's 1982 ICASSP paper.

   Wmax_clean        = Kmax ./ (Kmax + dBMax_clean - ...
		 	    clean_energy(1:num_crit-1));
   Wlocmax_clean     = Klocmax ./ ( Klocmax + clean_loc_peak - ...
				clean_energy(1:num_crit-1));
   W_clean           = Wmax_clean .* Wlocmax_clean;

   Wmax_processed    = Kmax ./ (Kmax + dBMax_processed - ...
			        processed_energy(1:num_crit-1));
   Wlocmax_processed = Klocmax ./ ( Klocmax + processed_loc_peak - ...
			            processed_energy(1:num_crit-1));
   W_processed       = Wmax_processed .* Wlocmax_processed;

   W = (W_clean + W_processed)./2.0;

   distortion(frame_count) = sum(W.*(clean_slope(1:num_crit-1) - ...
		       processed_slope(1:num_crit-1)).^2);

   % this normalization is not part of Klatt's paper, but helps
   % to normalize the measure.  Here we scale the measure by the
   % sum of the weights.

   distortion(frame_count) = distortion(frame_count)/sum(W);

   start = start + skiprate;

end

%-----------------------------------------------
function distortion = llr(clean_speech, processed_speech,sample_rate)

% ----------------------------------------------------------------------
% Check the length of the clean and processed speech.  Must be the same.
% ----------------------------------------------------------------------

clean_length      = length(clean_speech);
processed_length  = length(processed_speech);

if (clean_length ~= processed_length)
  disp('Error: Both Speech Files must be same length.');
  return
end

% ----------------------------------------------------------------------
% Global Variables
% ----------------------------------------------------------------------

winlength   = round(30*sample_rate/1000); %  window length in samples
skiprate    = floor(winlength/4);		   % window skip in samples
if sample_rate<10000
   P           = 10;		   % LPC Analysis Order
else
    P=16;     % this could vary depending on sampling frequency.
end

% ----------------------------------------------------------------------
% For each frame of input speech, calculate the Log Likelihood Ratio
% ----------------------------------------------------------------------

num_frames = clean_length/skiprate-(winlength/skiprate); % number of frames
start      = 1;					% starting sample
window     = 0.5*(1 - cos(2*pi*(1:winlength)'/(winlength+1)));

for frame_count = 1:num_frames

   % ----------------------------------------------------------
   % (1) Get the Frames for the test and reference speech.
   %     Multiply by Hanning Window.
   % ----------------------------------------------------------

   clean_frame = clean_speech(start:start+winlength-1);
   processed_frame = processed_speech(start:start+winlength-1);
   clean_frame = clean_frame.*window;
   processed_frame = processed_frame.*window;

   % ----------------------------------------------------------
   % (2) Get the autocorrelation lags and LPC parameters used
   %     to compute the LLR measure.
   % ----------------------------------------------------------

   [R_clean, Ref_clean, A_clean] = ...
      lpcoeff(clean_frame, P);
   [R_processed, Ref_processed, A_processed] = ...
      lpcoeff(processed_frame, P);

   % ----------------------------------------------------------
   % (3) Compute the LLR measure
   % ----------------------------------------------------------

   numerator   = A_processed*toeplitz(R_clean)*A_processed';
   denominator = A_clean*toeplitz(R_clean)*A_clean';
   distortion(frame_count) = log(numerator/denominator);
   start = start + skiprate;

end

%---------------------------------------------
function [acorr, refcoeff, lpparams] = lpcoeff(speech_frame, model_order)

   % ----------------------------------------------------------
   % (1) Compute Autocorrelation Lags
   % ----------------------------------------------------------

   winlength = max(size(speech_frame));
   for k=1:model_order+1
      R(k) = sum(speech_frame(1:winlength-k+1) ...
		     .*speech_frame(k:winlength));
   end

   % ----------------------------------------------------------
   % (2) Levinson-Durbin
   % ----------------------------------------------------------

   a = ones(1,model_order);
   E(1)=R(1);
   for i=1:model_order
      a_past(1:i-1) = a(1:i-1);
      sum_term = sum(a_past(1:i-1).*R(i:-1:2));
      rcoeff(i)=(R(i+1) - sum_term) / E(i);
      a(i)=rcoeff(i);
      a(1:i-1) = a_past(1:i-1) - rcoeff(i).*a_past(i-1:-1:1);
      E(i+1)=(1-rcoeff(i)*rcoeff(i))*E(i);
   end

   acorr    = R;
   refcoeff = rcoeff;
   lpparams = [1 -a];

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

function [overall_snr, segmental_snr] = snr(clean_speech, processed_speech,sample_rate)

% ----------------------------------------------------------------------
% Check the length of the clean and processed speech.  Must be the same.
% ----------------------------------------------------------------------

clean_length      = length(clean_speech);
processed_length  = length(processed_speech);

if (clean_length ~= processed_length)
  disp('Error: Both Speech Files must be same length.');
  return
end

% ----------------------------------------------------------------------
% Scale both clean speech and processed speech to have same dynamic
% range.  Also remove DC component from each signal
% ----------------------------------------------------------------------

%clean_speech     = clean_speech     - mean(clean_speech);
%processed_speech = processed_speech - mean(processed_speech);

%processed_speech = processed_speech.*(max(abs(clean_speech))/ max(abs(processed_speech)));

overall_snr = 10* log10( sum(clean_speech.^2)/sum((clean_speech-processed_speech).^2));

% ----------------------------------------------------------------------
% Global Variables
% ----------------------------------------------------------------------

winlength   = round(30*sample_rate/1000); %240;		   % window length in samples
skiprate    = floor(winlength/4);		   % window skip in samples
MIN_SNR     = -10;		   % minimum SNR in dB
MAX_SNR     =  35;		   % maximum SNR in dB

% ----------------------------------------------------------------------
% For each frame of input speech, calculate the Segmental SNR
% ----------------------------------------------------------------------

num_frames = clean_length/skiprate-(winlength/skiprate); % number of frames
start      = 1;					% starting sample
window     = 0.5*(1 - cos(2*pi*(1:winlength)'/(winlength+1)));

for frame_count = 1: num_frames

   % ----------------------------------------------------------
   % (1) Get the Frames for the test and reference speech.
   %     Multiply by Hanning Window.
   % ----------------------------------------------------------

   clean_frame = clean_speech(start:start+winlength-1);
   processed_frame = processed_speech(start:start+winlength-1);
   clean_frame = clean_frame.*window;
   processed_frame = processed_frame.*window;

   % ----------------------------------------------------------
   % (2) Compute the Segmental SNR
   % ----------------------------------------------------------

   signal_energy = sum(clean_frame.^2);
   noise_energy  = sum((clean_frame-processed_frame).^2);
   segmental_snr(frame_count) = 10*log10(signal_energy/(noise_energy+eps)+eps);
   segmental_snr(frame_count) = max(segmental_snr(frame_count),MIN_SNR);
   segmental_snr(frame_count) = min(segmental_snr(frame_count),MAX_SNR);

   start = start + skiprate;

end


function outputImage = minip(this, applicationDimension)
% Computes minimum intensity projection image along specified dimension,
% uses Matlab min function
%
%
%   Y = MrImage()
%   Y.minip(applicationDimension)
%
% This is a method of class MrImage.
%
% IN
%   applicationDimension    image dimension along which operation is
%                           performed (e.g. 4 = time, 3 = slices)
%                           default: The last dimension with more than one
%                           value is chosen
%                           (i.e. 3 for 3D image, 4 for 4D image)
%
% OUT
%   outputImage
%
% EXAMPLE
%   minip
%
%   See also MrImage MrImage.perform_unary_operation min MrImage.maxip

% Author:   Saskia Klein & Lars Kasper
% Created:  2014-11-25
% Copyright (C) 2014 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public Licence (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.

if nargin < 2
    applicationDimension = this.dimInfo.nDims;
else
    applicationDimension = this.dimInfo.convert_application_dimensions(...
        applicationDimension);
end

outputImage = this.perform_unary_operation(@(x) min(x,[],applicationDimension));


function [traj] = updateTraj(traj,Rot,v,x,omega_b,a_b,i)
%record trajectory
angles = rotm2eul(Rot);
traj.Rot(:,:,i) = Rot;
traj.phi(i) = angles(3);
traj.theta(i) = angles(2);
traj.psi(i) = angles(1);
traj.v(:,i) = v;
traj.x(:,i) = x;
traj.omega_b(:,i) = omega_b;
traj.a_b(:,i) = a_b;
end


classdef NSGAIIDTI < ALGORITHM
% <multi> <real/integer/label/binary/permutation> <constrained/none> <robust>
% NSGA-II of Deb's type I robust version

%------------------------------- Reference --------------------------------
% K. Deb and H. Gupta, Introducing robustness in multi-objective
% optimization, Evolutionary Computation, 2006, 14(4): 463-494.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

	methods
       function main(Algorithm,Problem)
            %% Generate random population
            Population = Problem.Initialization();
            [PopObjV,PopConV]    = MeanEffective(Problem,Population);
            [~,FrontNo,CrowdDis] = EnvironmentalSelection(Population,Problem.N,PopObjV,PopConV);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool = TournamentSelection(2,Problem.N,FrontNo,-CrowdDis);
                Offspring  = OperatorGA(Problem,Population(MatingPool),{1,10,1,50});
                [OffObjV,OffConV] = MeanEffective(Problem,Offspring);
                [Population,FrontNo,CrowdDis,PopObjV,PopConV] = EnvironmentalSelection([Population,Offspring],Problem.N,[PopObjV;OffObjV],[PopConV;OffConV]);
            end
        end
    end
end


classdef BatchNorm < dagnn.ElementWise
  properties
    numChannels
    epsilon = 1e-5
    opts = {'NoCuDNN'} % ours seems slightly faster
    noise_param_idx
    noise_cache_size
    bnorm_moment_type_trn
    bnorm_moment_type_tst
  end

  properties (Transient)
    moments
  end

  methods
    function outputs = forward(obj, inputs, params)

	  if strcmp(obj.net.mode, 'test')
        outputs{1} = vl_nnbnorm(inputs{1}, params{1}, params{2}, ...
                                'moments', params{3}, ...
                                'epsilon', obj.epsilon, ...
                                obj.opts{:}) ;
      else

      	% Guosheng: changed for RefineNet
      	bnorm_moment_type=obj.bnorm_moment_type_trn;

        if strcmp(bnorm_moment_type, 'batch')

	        [outputs{1},obj.moments] = ...
	            vl_nnbnorm(inputs{1}, params{1}, params{2}, ...
	                       'epsilon', obj.epsilon, ...
	                       obj.opts{:}) ;

		elseif strcmp(bnorm_moment_type, 'global')
			outputs{1} = vl_nnbnorm(inputs{1}, params{1}, params{2}, ...
                                'moments', params{3}, ...
                                'epsilon', obj.epsilon, ...
                                obj.opts{:}) ;
		else
            error('not support!');
        end

      end

    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)

		% Guosheng: changed for RefineNet

    	bnorm_moment_type=obj.bnorm_moment_type_trn;

    	if strcmp(bnorm_moment_type, 'batch')

	      [derInputs{1}, derParams{1}, derParams{2}, derParams{3}] = ...
	        vl_nnbnorm(inputs{1}, params{1}, params{2}, derOutputs{1}, ...
	                   'epsilon', obj.epsilon, ...
	                   'moments', obj.moments, ...
	                   obj.opts{:}) ;

  		elseif strcmp(bnorm_moment_type, 'global')

  			[derInputs{1}, derParams{1}, derParams{2}, derParams{3}] = ...
	        	vl_nnbnorm(inputs{1}, params{1}, params{2}, derOutputs{1}, ...
	        		'moments', params{3}, ...
	                   'epsilon', obj.epsilon, ...
	                   obj.opts{:}) ;

	    else
            error('not support!');
  		end

  		obj.moments = [] ;

      % multiply the moments update by the number of images in the batch
      % this is required to make the update additive for subbatches
      % and will eventually be normalized away
      derParams{3} = derParams{3} * size(inputs{1},4) ;
    end

    % ---------------------------------------------------------------------
    function obj = BatchNorm(varargin)
      obj.load(varargin{:}) ;
    end

    function params = initParams(obj)
      params{1} = ones(obj.numChannels,1,'single') ;
      params{2} = zeros(obj.numChannels,1,'single') ;
      params{3} = zeros(obj.numChannels,2,'single') ;
    end

    function attach(obj, net, index)
      attach@dagnn.ElementWise(obj, net, index) ;
      p = net.getParamIndex(net.layers(index).params{3}) ;
      net.params(p).trainMethod = 'average' ;
      net.params(p).learningRate = 0.1 ;
    end
  end
end