function Population = WOF_NSGAIIIEnvironmentalSelection(Population,N,Z,Zmin)
% -----------------------------------------------------------------------
%  Copyright (C) 2020 Heiner Zille
%
%  This work is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0
%  International License. (CC BY-NC-SA 4.0). To view a copy of this license,
%  visit http://creativecommons.org/licenses/by-nc-sa/4.0/ or see the
%  pdf-file "License-CC-BY-NC-SA-4.0.pdf" that came with this code.
%
%  You are free to:
%  * Share ? copy and redistribute the material in any medium or format
%  * Adapt ? remix, transform, and build upon the material
%  Under the following terms:
%  * Attribution ? You must give appropriate credit, provide a link to the
%     license, and indicate if changes were made. You may do so in any reasonable
%     manner, but not in any way that suggests the licensor endorses you or your use.
%  * NonCommercial ? You may not use the material for commercial purposes.
%  * ShareAlike ? If you remix, transform, or build upon the material, you must
%    distribute your contributions under the same license as the original.
%  * No additional restrictions ? You may not apply legal terms or technological
%    measures that legally restrict others from doing anything the license permits.
%
%  Author of this Code:
%   Heiner Zille <heiner.zille@ovgu.de> or <heiner.zille@gmail.com>
%
%  This code is based on the following publications:
%
%  1) Heiner Zille
%     "Large-scale Multi-objective Optimisation: New Approaches and a Classification of the State-of-the-Art"
%     PhD Thesis, Otto von Guericke University Magdeburg, 2019
%     http://dx.doi.org/10.25673/32063
%
%  2) Heiner Zille and Sanaz Mostaghim
%     "Comparison Study of Large-scale Optimisation Techniques on the LSMOP Benchmark Functions"
%     IEEE Symposium Series on Computational Intelligence (SSCI), IEEE, Honolulu, Hawaii, November 2017
%     https://ieeexplore.ieee.org/document/8280974
%
%  3) Heiner Zille, Hisao Ishibuchi, Sanaz Mostaghim and Yusuke Nojima
%     "A Framework for Large-scale Multi-objective Optimization based on Problem Transformation"
%     IEEE Transactions on Evolutionary Computation, Vol. 22, Issue 2, pp. 260-275, April 2018.
%     http://ieeexplore.ieee.org/document/7929324
%
%  4) Heiner Zille, Hisao Ishibuchi, Sanaz Mostaghim and Yusuke Nojima
%     "Weighted Optimization Framework for Large-scale Mullti-objective Optimization"
%     Genetic and Evolutionary Computation Conference (GECCO), ACM, Denver, USA, July 2016
%     http://dl.acm.org/citation.cfm?id=2908979
%
%  This file is intended to work with the PlatEMO framework version 2.5.
%  Date of publication of this code: 06.04.2020
%  Last Update of this code: 06.04.2020
%  A newer version of this algorithm may be available. Please contact the author
%  or see http://www.ci.ovgu.de/Research/Codes.html.
%
% The files may have been modified in Feb 2021 by the authors of the Platemo framework to work with the Platemo 3.0 release.
% -----------------------------------------------------------------------
% This file is derived from its original version containied in the PlatEMO
% framework. The original copyright disclaimer can be found below.
% -----------------------------------------------------------------------

% The environmental selection of NSGA-III

    if isempty(Zmin)
        Zmin = ones(1,size(Z,2));
    end

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

    %% Select the solutions in the last front
    Last   = find(FrontNo==MaxFNo);
    Choose = LastSelection(Population(Next).objs,Population(Last).objs,N-sum(Next),Z,Zmin);
    Next(Last(Choose)) = true;
    % Population for next generation
    Population = Population(Next);
end

function Choose = LastSelection(PopObj1,PopObj2,K,Z,Zmin)
% Select part of the solutions in the last front

    PopObj = [PopObj1;PopObj2] - repmat(Zmin,size(PopObj1,1)+size(PopObj2,1),1);
    [N,M]  = size(PopObj);
    N1     = size(PopObj1,1);
    N2     = size(PopObj2,1);
    NZ     = size(Z,1);

    %% Normalization
    % Detect the extreme points
    Extreme = zeros(1,M);
    w       = zeros(M)+1e-6+eye(M);
    for i = 1 : M
        [~,Extreme(i)] = min(max(PopObj./repmat(w(i,:),N,1),[],2));
    end
    % Calculate the intercepts of the hyperplane constructed by the extreme
    % points and the axes
    Hyperplane = PopObj(Extreme,:)\ones(M,1);
    a = 1./Hyperplane;
    if any(isnan(a))
        a = max(PopObj,[],1)';
    end
    % Normalization
    PopObj = PopObj./repmat(a',N,1);

    %% Associate each solution with one reference point
    % Calculate the distance of each solution to each reference vector
    Cosine   = 1 - pdist2(PopObj,Z,'cosine');
    Distance = repmat(sqrt(sum(PopObj.^2,2)),1,NZ).*sqrt(1-Cosine.^2);
    % Associate each solution with its nearest reference point
    [d,pi] = min(Distance',[],1);

    %% Calculate the number of associated solutions except for the last front of each reference point
    rho = hist(pi(1:N1),1:NZ);

    %% Environmental selection
    Choose  = false(1,N2);
    Zchoose = true(1,NZ);
    % Select K solutions one by one
    while sum(Choose) < K
        % Select the least crowded reference point
        Temp = find(Zchoose);
        Jmin = find(rho(Temp)==min(rho(Temp)));
        j    = Temp(Jmin(randi(length(Jmin))));
        I    = find(Choose==0 & pi(N1+1:end)==j);
        % Then select one solution associated with this reference point
        if ~isempty(I)
            if rho(j) == 0
                [~,s] = min(d(N1+I));
            else
                s = randi(length(I));
            end
            Choose(I(s)) = true;
            rho(j) = rho(j) + 1;
        else
            Zchoose(j) = false;
        end
    end
end


function [sCodebook, settings] = grlvq(sCodebook, sData, varargin)

%GRLVQ trains a codebook using the Generalized Relevance LVQ (GRLVQ)
%algorithm.
%
% sM = grlvq(sM, sD, [argID, value, ...])
%
%  sM = grlvq(sM, sD)
%  sM = grlvq(sM, sD, 'learningRatePrototypes', [0.01 0.001], 'relevanceStart', 10);
%
% Required Input Arguments:
%   sM                     (struct) map struct containing the labels as the
%                                   first column of .labels
%   sD                     (struct) data struct containing the labels as
%                                   the first column of .labels
%
% Optional Input Arguments:
%   PrototypesPerClass     (vector) number of prototypes per class used;
%                                   a number or a vector with a number
%                                   for each class (default=1)
%   initialRelevances      (vector) initial Relevances
%   regularization         (scalar) regularization for relevances
%   testSet                (matrix) a test set to compute the test error;
%                                   the last column should be the labels
%   comparable             (scalar) a flag which resets the random
%                                   generator to produce comparable results
%                                   if set to 1
%   optimization           (string) choice for the optimization technique:
%                                   sgd or fminlbfgs (default=fminlbfgs)
%  Parameter for the stochastic gradient descent sgd:
%   nb_epochs              (scalar) the number of epochs (default=100)
%   learningRatePrototypes (vector) learning rate for the prototypes; could
%                                   be the start and end value or a vector
%                                   of length nb_epochs
%   learningRateRelevances (vector) learning rate for the relevances; could
%                                   be the start and end value or a vector
%                                   of length nb_epochs
%   relevanceStart         (scalar) epoch to start the relevance learning
%                                   (default=1)
%  Parameter for the build-in function fminlbfgs:
%   threshstop             (scalar) the training error for early stopping
%                                   (default=0)
%   useEarlyStopping       (scalar) use early stopping based on threshstop
%                                   (default=1)
%   Display                (string) the optimization output 'iter' or 'off'
%                                   (default='off')
%   GradObj                (string) turn the usage of gradient information
%                                   on or off (default='on')
%   HessUpdate             (string) the update can be 'lbfgs', 'bfgs' or
%                                   'steepdesc' (default='lbfgs')
%   TolFun                 (scalar) the tolerance (default=1e-6)
%   MaxIter                (scalar) the maximal number of iterations
%                                   (default=2500)
%   MaxFunEvals                     the maximal number of function
%                                   evaluations (default=1000000)
%   TolX                   (scalar) tolerance on the minimum
%                                   (default=1e-10)
%   DiffMinChange          (scalar) minimal change (default=1e-10)
%
% Output Arguments:
%   sM                     (struct) map struct containing the optimized
%                                   prototypes and the relevances lambda
%   settings               (struct) information on the settings of the
%                                   algorithm
%
% NOTE: does not take the vector mask into account but trains a relevance
%       for each dimension.
%
% For more help, try 'type grlvq', or check out the online documentation.
% See also GMLVQ, GRLVQ_CORE, LVQ3, LVQ1.

%%%%%%%%%%%%% DETAILED DESCRIPTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% grlvq
%
% PURPOSE
%
% Trains a codebook and a global metric with the Generalized Relevance LVQ
% (GRLVQ) algorithm.
%
% SYNTAX
%
%   sM = grlvq(sM, sD)
%   sM = grlvq(sM, sD, 'learningRatePrototypes', [0.01 0.001], 'relevanceStart', 10);
%   [sM, settings] = grlvq(sM, sD, argID, value, ...);
%
% DESCRIPTION
%
% GRLVQ constitutes an enhancement of the LVQ algorithm by minimizing an
% explicit error function and optimizing the metric of the space where the
% computation is performed. Distances are calculated by
%   d(x,y) = (x - y) * diag(lambda) * (x - y)'.
% The vector lambda is optimized by the algorithm.
%
% Two optimization techniques are implemented:
%     - stochastic gradient descent (sgd)
%     - limited memory Quasi Newton Broyden-Fletcher-Goldfarb-Shanno
%       (L-BFGS)
%
% Classification is performed similarly to standard LVQ, with the only
% difference that for the computation of distances the above specified
% formula is applied.
%
% This function provides an interface in the style of the som-toolbox and
% is basically a wrapper for the method grlvq_core. It deals with structs
% rather then directly with data matrices.
%
% REFERENCES
%
%   Barbara Hammer, Thomas Villmann: Generalized relevance learning vector
%   quantization. Neural Networks 15(8-9): 1059-1068 (2002).
%
%   P. Schneider, K. Bunte, B. Hammer and M. Biehl: Regularization in
%   Matrix Relevance Learning, IEEE Transactions on Neural Networks, vol.
%   21, nb. 5, pp. 831-840, 2010.
%
% REQUIRED INPUT ARGUMENTS
%   sM                     (struct) map struct containing the initialized
%                                   prototype vectors as rows of the
%                                   .codebooks matrix and the labels as the
%                                   first column of .labels
%   sD                     (struct) data struct containing the data vectors
%                                   as rows of .data and the labels as the
%                                   first column of .labels
%
% OPTIONAL INPUT ARGUMENTS
%   PrototypesPerClass     (vector) number of prototypes per class used;
%                                   a number or a vector with a number
%                                   for each class (default=1)
%   initialRelevances      (vector) initial Relevances
%                                   (default: vector of ones)
%   regularization         (scalar) regularization for relevances
%                                   (detault=0)
%   comparable             (scalar) a flag which resets the random
%                                   generator to produce comparable results
%                                   if set to 1 (default=0)
%   optimization           (string) choice for the optimization technique:
%                                   sgd (stochastic gradient descent) or
%                                   fminlbfgs (Limited memory Quasi Newton
%                                   Broyden-Fletcher-Goldfarb-Shanno)
%                                   (default=fminlbfgs)
%  Parameter for the stochastic gradient descent sgd:
%   nb_epochs              (scalar) the number of epochs (default=100)
%   learningRatePrototypes (vector) learning rate for the prototypes; could
%                                   be the start and end value or a vector
%                                   of length nb_epochs
%   learningRateRelevances (vector) learning rate for the relevances; could
%                                   be the start and end value or a vector
%                                   of length nb_epochs
%   relevanceStart         (scalar) epoch to start the relevance learning
%                                   (default=1)
%  Parameter for the build-in function fminlbfgs:
%   threshstop             (scalar) the training error for early stopping
%                                   (default=0)
%   useEarlyStopping       (scalar) use early stopping based on threshstop
%                                   (default=1)
%   Display                (string) the optimization output 'iter' or 'off'
%                                   (default='off')
%   GradObj                (string) turn the usage of gradient information
%                                   on or off (default='on')
%   HessUpdate             (string) the update can be 'lbfgs', 'bfgs' or
%                                   'steepdesc' (default=lbfgs)
%   TolFun                 (scalar) the tolerance (default=1e-6)
%   MaxIter                (scalar) the maximal number of iterations
%                                   (default=2500)
%   MaxFunEvals                     the maximal number of function
%                                   evaluations (default=1000000)
%   TolX                   (scalar) tolerance on the minimum
%                                   (default=1e-10)
%   DiffMinChange          (scalar) minimal change (default=1e-10)
%
% OUTPUT ARGUMENTS
%   sM                     (struct) map struct containing the optimized
%                                   prototypes as well as the relevances
%                                   for each dimensionlambda
%   settings               (struct) information on the settings of the
%                                   algorithm
%
% EXAMPLES
%
%   lab   = unique(sD.labels(:,1));           % different classes
%   nProt = length(lab)*5;                    % 5 prototypes for each
%   sM = som_randinit(sD,'msize',[nProt 1]);  % initial prototypes
%   sM.labels = [lab;lab;lab;lab;lab];        % and their classes
%   sM = grlvq(sM,sD);                        % use GRLVQ to adjust the
%                                             % prototypes and the metric
%
% SEE ALSO
%
%   gmlvq         Use the GMLVQ algorithm for training.
%   grlvq_core    Access the GRLVQ functionality without using structs.
%   lvq3          Use LVQ3 algorithm for training.
%   lvq1          Use LVQ1 algorithm for training.

% Contributed to SOM Toolbox vs2, December 3rd, 2012 by Alexander Schulz
% Copyright (c) Alexander Schulz
% http://www.cis.hut.fi/projects/somtoolbox/

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

cods      = sCodebook.codebook;
codLabels = sCodebook.labels;
data      = sData.data;
labels    = sData.labels;

% convert labels from cell to a vector
labelsV    = som_label2num(labels);
codLabelsV = som_label2num(codLabels);

% calculate the number of prototypes per class
n_prot_per_class = zeros(1, max(codLabelsV));
for i=1:max(codLabelsV)
    n_prot_per_class(i) = sum(codLabelsV == i);
end

% call the main function
[model, settings]  = grlvq_core(data, labelsV, 'initialPrototypes', ...
    [cods, codLabelsV], 'PrototypesPerClass', n_prot_per_class, varargin{:});

% write the results in the output struct
sCodebook.codebook = model.w;
sCodebook.lambda   = model.lambda;

if strcmp(settings.optimization, 'sgd')
    trainlen   = settings.nb_epochs;
    alpha_ini  = settings.learningRatePrototypes(1);
    alpha_type = 'power';
else
    trainlen   = NaN;
    alpha_ini  = NaN;
    alpha_type = '';
end

sTrain = som_set('som_train','algorithm','GRLVQ',...
		 'data_name',sData.name,...
		 'neigh','',...
		 'mask',ones(size(model.w,2),1),...
		 'radius_ini',NaN,...
		 'radius_fin',NaN,...
		 'alpha_ini',alpha_ini,...
		 'alpha_type',alpha_type,...
		 'trainlen',trainlen,...
		 'time',datestr(now,0));
sCodebook.trainhist(end+1) = sTrain;


classdef ReasenbergDeclusterClass < ZmapFunction
    % Reasenberg Declustering codes
    % made from code originally from A.Allman that has chopped up and rebuilt

    properties
        taumin  duration    = days(1)   % look ahead time for not clustered events
        taumax  duration    = days(10)  % maximum look ahead time for clustered events
        P                   = 0.95      % confidence level that this is next event in sequence
        xk                  = 0.5       % is the factor used in xmeff

        % effective lower magnitude cutoff for catalog.
        % During clusteres, it is raised by a factor xk*cmag1
        xmeff               = 1.5

        rfact               = 10        % factor for interaction radius for dependent events
        err                 = 1.5       % epicenter error
        derr                = 2         % depth error, km
        %declustRoutine      = "ReasenbergDeclus"
        declusteredCatalog   ZmapCatalog
        replaceSequenceWithEquivMainshock   logical = false

         % if empty, clustering details will not be saved to workspace
        clusterDetailsVariableName          char    = 'cluster_details'

        % if empty, catalog will not be saved to workspace
        declusteredCatalogVariableName      char    = 'declustered_catalog'
        memorizeOriginalCatalog             logical = true
    end

    properties(Constant)
        PlotTag = "ReasenbergDecluster"

        ParameterableProperties = ["taumin", "taumax", "P",...
            "xk","xmeff","rfact","err","derr",..."declustRoutine",...
            "replaceSequenceWithEquivMainshock",...
            "clusterDetailsVariableName",...
            "declusteredCatalogVariableName",...
            "memorizeOriginalCatalog"];

        References = ['Paul Reasenberg (1985) ',...
            '"Second -order Moment of Central California Seismicity"',...
            ', JGR, Vol 90, P. 5479-5495.'];

        % interaction formula bundles assumptions about stress drop into a mag-dependent formula
        InteractFormula = struct('Reasenberg1985', @(m) 0.011 .* 10.^ (0.4 .* m),...
            'WellsCoppersmith1994', @(m) 0.01 * 10 .^ (0.5 * m))
    end

    methods
        function obj = ReasenbergDeclusterClass(catalog, varargin)
            % ReasenbergDeclusterClass

            obj@ZmapFunction(catalog);

            report_this_filefun();
            obj.parseParameters(varargin);
            obj.clusterDetailsVariableName = matlab.lang.makeValidName(obj.clusterDetailsVariableName);
            obj.declusteredCatalogVariableName = matlab.lang.makeValidName(obj.declusteredCatalogVariableName);
            obj.StartProcess();
        end

        function InteractiveSetup(obj)

            % make the interface

            zdlg = ZmapDialog();
            zdlg.AddHeader('Reasenberg Declustering parameters','FontSize',12);
            zdlg.AddHeader('look-ahead times');
            zdlg.AddDurationEdit('taumin', '(min) for UNclustered events' ,obj.taumin, 'TauMin look ahead time for not clustered events',@days);
            zdlg.AddDurationEdit('taumax', '(max) for   clustered events', obj.taumax,  'TauMax maximum look ahead time for clustered events',@days);
            zdlg.AddHeader('');
            zdlg.AddEdit('P',       'Confidence Level',             obj.P,          'P1 Confidence level : observing the next event in the sequence');
            zdlg.AddEdit('xk',      'XK factor',                    obj.xk,         'XK factor used in xmeff');
            zdlg.AddEdit('xmeff',   'Effective min mag cutoff',     obj.xmeff,   'XMEFF "effective" lower magnitude cutoff for catalog, during clusters, it is xmeff^{xk*cmag1}');
            zdlg.AddEdit('rfact',   'Interation radius factor:',    obj.rfact,      'RFACT>/b>factor for interaction radius for dependent events');
            zdlg.AddEdit('err',     'Epicenter error',              obj.err,        'Epicenter error');
            zdlg.AddEdit('derr',    'Depth error',                  obj.derr,       'derrDepth error');
            %zdlg.AddHeader('Output')
            zdlg.AddCheckbox('replaceSequenceWithEquivMainshock','Replace clusters with equivalent event',...
                obj.replaceSequenceWithEquivMainshock, {},...
                'Will replace each set of cluster earthquakes with a single event of equivalent Magnitude');
            zdlg.AddEdit('clusterDetailsVariableName',      'Save Clusters to workspace as', ...
                obj.clusterDetailsVariableName, 'if empty, then nothing will be separately saved');
            zdlg.AddEdit('declusteredCatalogVariableName',  'Save Declustered catalog to workspace as', ...
                obj.declusteredCatalogVariableName,'if empty, then nothing will be separately saved');
            zdlg.AddCheckbox('memorizeOriginalCatalog',     'Memorize Original catalog after sucessful decluster:', ...
                obj.memorizeOriginalCatalog, {}, 'Memorize original catalog prior to declustering');

            zdlg.Create('Name', 'Reasenberg Declustering','WriteToObj',obj,'OkFcn',@obj.Calculate);

        end

        function Results = Calculate(obj)
            calcFn = @obj.declus;
            [obj.declusteredCatalog, misc] = calcFn(obj);
            if nargout == 1
                Results = obj.declusteredCatalog;
            end

            obj.CalcFinishedFcn();
        end

        %{
        function [clustnum, eqs_in_clust] = decluster_from_python(obj)
              obj.verbose = config.get('verbose', obj.verbose);
            % Get relevant parameters
            neq = catalog.get_number_events()  % Number of earthquakes

            min_lookahead_days = obj.taumin;
            max_lookahead_days = obj.taumax;

            % Get elapsed days
            elapsed = days_from_first_event(catalog);

            assert(all(elapsed(2:end) >= elapsed(1:end-1)), "catalog needs to be in ascending date order")

            % easy-access variables
            dmethod='p2p';
            switch dmethod
                case 'gc'
                    surf_pos = [catalog.latitude, catalog.longitude];
                    event_distance = event_gc_distance;
                case'p2p'
                    surf_pos = geodetic_to_ecef(catalog.latitude, catalog.longitude);  % assumes at surface
                    event_distance = event_p2p_distance;
                otherwise
                    except(ValueError("unknown configuration dmethod. it should be 'gc' or 'p2p'"))
            end

            mags = catalog.magnitude;
            deps = catalog.depth;

            if isempty(obj.err)
                horiz_error = catalog.data.get('horizError', 0);
            else
                horiz_error = obj.err;
            end
            if isempty(obj.derr)
                depth_error = catalog.data.get('depthError', 0);
            else
                depth_error = obj.derr;
            end
            % Pre-allocate cluster index vectors
            vcl = zeros(neq, 1);

            % set the interaction zones, in km
            % Reasenberg 1987 or alternate version: Wells & Coppersmith 1994 / Helmstetter (SRL) 2007
            zone_noclust, zone_clust = obj.get_zone_distances_per_mag(mags, obj.rfact,...
                obj.interaction_formulas.(obj.interaction_formula), obj.taumax)

            k = 0  % clusterindex

            % variable to store information whether earthquake is already clustered
            clusmaxmag = ones(neq,1) * -inf;
            clus_biggest_idx = zeros(neq,1);

            % for every earthquake in catalog, main loop
            for i = 0 : neq-1 % in range(neq - 1)
                my_mag = mags(i);

                % variable needed for distance and timediff
                my_cluster = vcl(i);
                not_classified = my_cluster == 0;

                % attach interaction time

                if not_classified
                    obj.debug_print(i, ' is not in a cluster')
                    % this event is not associated with a cluster, yet
                    look_ahead_days = min_lookahead_days;

                elseif my_mag >= clusmaxmag(my_cluster)
                    % note, if this is now the biggest, then the cluster range collapses into its radius
                    printf('%d is the biggest event of cluster M=%g\n', i, my_mag);
                    % this is the biggest event  in this cluster, so far (or equal to it).
                    clusmaxmag(my_cluster) = my_mag;
                    clus_biggest_idx(my_cluster) = i;
                    look_ahead_days = min_lookahead_days;
                else
                    printf('%d is already in cluster, but not biggest', i);
                    % this event is already tied to a cluster, but is not the largest
                    idx_biggest = clus_biggest_idx(my_cluster);
                    days_since_biggest = elapsed(i) - elapsed(idx_biggest);
                    look_ahead_days = obj.clust_look_ahead_time(clusmaxmag(my_cluster),...
                        days_since_biggest, obj.xk, obj.xmeff, obj.P);

                    look_ahead_days(look_ahead_days<min_lookahead_days) = min_lookahead_days;
                    look_ahead_days(look_ahead_days>max_lookahead_days) = max_lookahead_days;
                end
                % extract eqs that fit interaction time window --------------

                max_elapsed = elapsed(i) + look_ahead_days;
                next_event = i + 1;
                last_event = bisect_left(elapsed, max_elapsed, next_event);
                temporal_evs = np.arange(next_event, last_event)
                if my_cluster ~= 0
                    temporal_evs = temporal_evs(vcl(temporal_evs) ~= my_cluster);
                end
                if len(temporal_evs) == 0
                    continue
                end
                % ------------------------------------
                % one or more events have now passed the time window test. Now compare
                % this subcatalog in space to A) most recent and B) largest event in cluster
                % ------------------------------------

                obj.debug_print('temporal_evs:', temporal_evs)
                my_biggest_idx = clus_biggest_idx(my_cluster)
                if not_classified
                    bg_ev_for_dist = i;
                else
                    bg_ev_for_dist = my_biggest_idx;
                end

                obj.debug_print('bg_ev_for_dist:', bg_ev_for_dist)
                % noinspection PyTypeChecker
                dist_to_recent = event_distance(surf_pos, deps, i, temporal_evs, horiz_error, depth_error)
                dist_to_biggest = event_distance(surf_pos, deps, bg_ev_for_dist, temporal_evs, horiz_error, depth_error)
                printf('dist_to_recent', dist_to_recent)
                printf('dist_to_biggest', dist_to_biggest)
                % extract eqs that fit the spatial interaction
                if look_ahead_days == min_lookahead_days
                    l_big = dist_to_biggest == 0;  % all false
                    l_recent = dist_to_recent <= zone_noclust(my_mag);
                    printf('Connecting those near to this event [dist <= {zone_noclust[my_mag]}]')
                else
                    l_big = dist_to_biggest <= zone_clust(clusmaxmag(my_cluster))
                    l_recent = dist_to_recent <= zone_clust(clusmaxmag(my_cluster))
                    printf('Connecting those near to this OR largest event [dist <= {zone_clust[clusmaxmag(my_cluster)]}]')
                end
                spatial_evs = l_recent | l_big;

                if ~any(spatial_evs)
                    continue
                end
                % ------------------------------------
                % one or more events have now passed both spatial and temporal window tests
                %
                % if there are events in this cluster that are already related to another
                % cluster, figure out the smallest cluster number. Then, assign all events
                % (both previously clustered and unclustered) to this new cluster number.
                % ------------------------------------

                % spatial events only include events AFTER i, not i itself
                % so vcl(events_in_any_cluster) is independent from vcl(i)

                candidates = temporal_evs(spatial_evs) ; % eqs that fit spatial and temporal criterion
                events_in_any_cluster = candidates(vcl(candidates) ~= 0);  % eqs which are already related with a cluster
                events_in_no_cluster = candidates(vcl(candidates) == 0);  % eqs that are not already in a cluster

                % if this cluster overlaps with any other cluster, then merge them
                % assign every event in all related clusters to the same (lowest) cluster number
                % set this cluster's maximum magnitude "clusmaxmag" to the largest magnitude of all combined events
                % set this cluster's clus_biggest_idx to the most recent largest event of all combined events

                if len(events_in_any_cluster) > 0
                    if not_classified
                        related_clust_nums = unique(vcl(events_in_any_cluster));
                    else
                        % include this cluster number in the reckoning
                        related_clust_nums = unique(np.hstack((vcl(events_in_any_cluster), my_cluster,)))
                    end
                    % associate all related events with my cluster
                    my_cluster = related_clust_nums(0);
                    vcl(i) = my_cluster;
                    vcl(candidates) = my_cluster;

                    for clustnum  = related_clust_nums
                        vcl(vcl == clustnum) = my_cluster;
                    end
                    events_in_my_cluster = vcl == my_cluster;
                    biggest_mag = np.max(mags(events_in_my_cluster));
                    biggest_mag_idx = find(mags == biggest_mag & events_in_my_cluster, 1, 'last');

                    % reset values for other clusters
                    clusmaxmag(related_clust_nums) = -inf;
                    clus_biggest_idx(related_clust_nums) = 0;

                    % assign values for this cluster
                    clusmaxmag(my_cluster) = biggest_mag;
                    clus_biggest_idx(my_cluster) = biggest_mag_idx;

                elseif my_cluster == 0
                    k = k + 1;
                    vcl(i) = k;
                    my_cluster = k;
                    clusmaxmag(my_cluster) = my_mag;
                    clus_biggest_idx(my_cluster) = i;
                else
                    pass  % no events found, and attached to existing cluster
                end
                % attach clustnumber to catalog yet unrelated to a cluster
                vcl(events_in_no_cluster) = my_cluster;

            end
            clustnum = vcl;
            eqs_in_clust = vcl > 0;
        end
        %}

        function [outputcatalog, details] = declus(obj, vals)
            % DECLUS main decluster algorithm
            % A.Allmann
            % main decluster algorithm
            % modified version, uses two different circles for already related events
            % works on catalog
            % different clusters stored with respective numbers in clus
            % Program is based on Raesenberg paper JGR;Vol90;Pages5479-5495;06/10/85
            %
            %basic variables used in the program
            %
            % interactzone_main_km  interaction zone for not clustered events
            % interactzone_in_clust_km     interaction zone for clustered events
            % rtest  radius in which the program looks for clusters
            % tau    look ahead time
            % tdiff  time difference between jth event and biggest eq
            % mbg    index of earthquake with biggest magnitude in a cluster
            % k      index of the cluster
            % k1     working index for cluster
            %
            % modified by Celso Reyes, 2017

            max_mag_in_cluster=[];
            idx_biggest_event_in_cluster=[];

            %% calculate interaction_zone  (1 value per event)

            interactzone_main_km = obj.InteractFormula.Reasenberg1985(obj.RawCatalog.Magnitude); %interaction zone for mainshock
            interactzone_in_clust_km = obj.rfact * interactzone_main_km;      %interaction zone if included in a cluster

            tau_min = days(obj.taumin);
            tau_max = days(obj.taumax);

            %calculation of the eq-time relative to 1902
            eqtime = days( obj.RawCatalog.Date - min(obj.RawCatalog.Date) );

            %variable to store information whether earthquake is already clustered
            clus = zeros(1,obj.RawCatalog.Count);

            k = 0;                                %clusterindex

            wai = waitbar(0,' Please Wait ...  Declustering the catalog');
            set(wai,'NumberTitle','off','Name','Declustering Progress');
            drawnow
            declustering_start = tic;
            %for every earthquake in catalog, main loop
            confine_value = @(value, min_val, max_val) max( min( value, max_val), min_val);
            for i = 1: (obj.RawCatalog.Count-1)

                % "myXXX" refers to the XXX for this event

                my_mag = obj.RawCatalog.Magnitude(i);

                if rem(i,50)==0
                    waitbar(i/(obj.RawCatalog.Count-1));
                end

                % variable needed for distance and timediff
                my_cluster = clus(i);
                alreadyInCluster = my_cluster~=0;
                not_classified = my_cluster==0;
                assert(not_classified~=alreadyInCluster)

                % attach interaction time
                if not_classified
                    look_ahead_days = tau_min;
                else
                    if my_mag >= max_mag_in_cluster(my_cluster)
                        max_mag_in_cluster(my_cluster) = my_mag;
                        idx_biggest_event_in_cluster(my_cluster) = i;
                        look_ahead_days = tau_min;
                    else
                        bgdiff = eqtime(i) - eqtime(idx_biggest_event_in_cluster(my_cluster));
                        look_ahead_days = clustLookAheadTime(obj.xk, max_mag_in_cluster(my_cluster), obj.xmeff, bgdiff, obj.P);
                        look_ahead_days = confine_value(look_ahead_days, tau_min, tau_max);
                    end
                end

                %extract eqs that fit interation time window
                temporal_evs = timediff(i, look_ahead_days, clus, eqtime); %local version

                if isempty(temporal_evs)
                    continue;
                end

                % ---------------------------
                % only continue if events passed the time test
                % ---------------------------

                rtest1 = interactzone_in_clust_km(i);
                if look_ahead_days == obj.taumin
                    rtest2 = 0;
                else
                    rtest2 = interactzone_main_km(idx_biggest_event_in_cluster(my_cluster));
                end

                if alreadyInCluster                % if i is already related with a cluster
                    tm1 = clus(temporal_evs) ~= my_cluster;  % eqs with a clustnumber different than i
                    if any(tm1)
                        temporal_evs = temporal_evs(tm1);
                    end
                    bg_ev_for_dist = idx_biggest_event_in_cluster(my_cluster);
                else
                    bg_ev_for_dist = i;
                end

                %calculate distances from the epicenter of biggest and most recent eq
                [dist1,dist2]=distance2(i,bg_ev_for_dist,temporal_evs, obj.RawCatalog);

                %extract eqs that fit the spatial interaction time
                sl0 = dist1<= rtest1 | dist2<= rtest2;

                if ~any(sl0)
                    continue
                end

                % ----------
                % only continue if events passed the distance test
                % ----------

                ll = temporal_evs(sl0);            %eqs that fit spatial and temporal criterion
                lla = ll(clus(ll)~=0);   %eqs which are already related with a cluster
                llb = ll(clus(ll)==0);   %eqs that are not already in a cluster
                if ~isempty(lla)         %find smallest clustnumber in the case several
                    sl1 = min(clus(lla));     %numbers are possible
                    if alreadyInCluster
                        my_cluster = min([sl1, my_cluster]);
                    else
                        my_cluster = sl1;
                    end
                    if clus(i)==0
                        clus(i) = my_cluster;
                    end
                    % merge related clusters together into cluster with the smallest number
                    sl2 = lla(clus(lla) ~= my_cluster);
                    if clus(i) ~= my_cluster
                        clus(clus==clus(i)) = my_cluster;
                    end

                    for j1 = sl2
                        if clus(j1) ~= my_cluster
                            clus(clus==clus(i)) = my_cluster;
                        end
                    end
                end

                if my_cluster==0   %if there was neither an event in the interaction zone nor i, already related to cluster
                    k = k+1;                         %
                    my_cluster = k;
                    clus(i) = my_cluster;
                    max_mag_in_cluster(my_cluster) = my_mag;
                    idx_biggest_event_in_cluster(my_cluster) = i;
                end

                if size(llb)>0     % attach clustnumber to events yet unrelated to a cluster
                    clus(llb) = my_cluster;  %
                end

            end

            close(wai);
            msg.dbfprintf('Declustering complete. It took %g seconds\n',toc(declustering_start));

            %% this table contains all we need to know about the clusters. maybe.
            details = table;
            details.Properties.UserData = struct;
            for j = 1 : numel(obj.ParameterableProperties)
                details.Properties.UserData.(obj.ParameterableProperties(j)) = obj.(obj.ParameterableProperties(j));
            end

            details.Properties.Description  = 'Details for cluster, from reasenberg declustering';
            details.eventNumber             = (1:obj.RawCatalog.Count)';
            details.clusterNumber           = clus(:);
            details.clusterNumber(details.clusterNumber==0) = missing;
            details.isBiggest               = false(size(details.clusterNumber));
            details.isBiggest(idx_biggest_event_in_cluster) = true;

            details.Latitude                = obj.RawCatalog.Y;
            details.Properties.VariableUnits(width(details)) = {'degrees'};

            details.Longitude               = obj.RawCatalog.X;
            details.Properties.VariableUnits(width(details)) = {'degrees'};

            details.Depth                   = obj.RawCatalog.Z;
            details.Properties.VariableUnits(width(details)) = {'kilometers'};

            details.Magnitude               = obj.RawCatalog.Magnitude;

            details.MagnitudeType           = obj.RawCatalog.MagnitudeType;

            details.Date                    = obj.RawCatalog.Date;

            details.InteractionZoneIfMain   = interactzone_main_km;
            details.Properties.VariableUnits(width(details)) = {'kilometers'};

            details.InteractionZoneIfInClust = interactzone_in_clust_km;
            details.Properties.VariableUnits(width(details)) = {'kilometers'};

            clusterFreeCatalog = obj.RawCatalog.subset(ismissing(details.clusterNumber));
            %biggest_events_in_cluster = obj.RawCatalog.subset(details.isBiggest);

            outputcatalog = clusterFreeCatalog;

            if ~any(clus)
                return
            end

            %build a matrix clust that stored clusters
            [~, biggest_events_in_cluster, max_mag_in_cluster,~,~] = funBuildclu(obj.RawCatalog,idx_biggest_event_in_cluster,clus,max_mag_in_cluster);

            % replace cluster sequences with summary events
            if obj.replaceSequenceWithEquivMainshock
                equi = obj.equevent(details(~ismissing(details.clusterNumber),:));  % calculates equivalent events
                if isempty(equi)
                    disp('No clusters in the catalog with this input parameters');
                    return;
                end
                tmpcat = cat(clusterFreeCatalog, equi);  %new, unsorted catalog

            else
                tmpcat = cat(clusterFreeCatalog, biggest_events_in_cluster); % builds catalog with biggest events instead
                disp('Original mainshocks kept');
            end

            tmpcat.sort('Date');

            tmpcat.Name = string(tmpcat.Name) + " (declust)";

            % save clustering details to workspace
            if ~isempty(obj.clusterDetailsVariableName)
                assignin('base',matlab.lang.makeValidName(obj.clusterDetailsVariableName),details);
            end

            if obj.memorizeOriginalCatalog
                mm = MemorizedCatalogManager();
                mm.memorize(obj.RawCatalog,'predeclust')
            end

            ZG              = ZmapGlobal.Data;
            ZG.original     = obj.RawCatalog;    %save catalog in variable original
            ZG.cluscat      = ZG.original.subset(clus(clus~=0));

            % save declustered catalog to workspace
            if ~isempty(obj.declusteredCatalogVariableName)
                assignin('base', obj.declusteredCatalogVariableName, tmpcat);
            end

            st1 = sprintf([' The declustering found %d clusters of earthquakes, a total of %d'...
                ' events (out of %d). The map window now displays the declustered catalog containing %d events.'], ...
                biggest_events_in_cluster.Count, ZG.cluscat.Count, ZG.original.Count , ZG.primeCatalog.Count);

            msgbox(st1,'Declustering Information')

            watchoff
            outputcatalog = tmpcat;

            obj.Result(1).values.cluster_details  = details;

        end

        function plot(obj, varargin)
            f = figure('Name','Reasenberg Deslustering Results');
            ax = subplot(2,2,1);
            ZG = ZmapGlobal.Data;
            biggest = obj.Result.values.cluster_details(obj.Result.values.cluster_details.isBiggest,:);
            non_cluster = obj.Result.values.cluster_details(ismissing(obj.Result.values.cluster_details.clusterNumber),:);
            msf = str2func(ZG.MainEventOpts.MarkerSizeFcn);
            scatter3(biggest.Longitude,biggest.Latitude,biggest.Depth,msf(biggest.Magnitude));
            ax.ZDir='reverse';
            title(ax,'Biggest events in each cluster');
            hold on
            ax.XLabel.String = 'Longitude';
            ax.YLabel.String = 'Latitude';
            ax.ZLabel.String = 'Depth [km]';
            feats = findobj(allchild(findobj('Tag','mainmap_ax')),'-regexp','Tag','mainmap_.+');
            copyobj(feats,ax); %copy features

            ax = subplot(2,2,2);

            ax = subplot(2,1,2);

            isInClust = ~ismissing(obj.Result.values.cluster_details.clusterNumber);
            isNotBig = ~obj.Result.values.cluster_details.isBiggest;
            clust = obj.Result.values.cluster_details(isInClust&isNotBig, :);
            scatter(clust.Date,clust.Depth,[],[.8 .8 .8],'Marker','.','DisplayName','other events in each cluster');
            ax.YDir='reverse';
            hold on;
            scatter(biggest.Date,biggest.Depth,msf(biggest.Magnitude),categorical(biggest.clusterNumber),'DisplayName','primary events in each cluster');
            cb = colorbar;
            cb.Label.String = 'Cluster #';
            ax.YLabel.String = 'Depth [km]';
            ax.XLabel.String = 'Date';
            title(ax,'Clusters through time');
            legend('show')
        end
    end

    methods(Static)
        function tau = clust_look_ahead_time(mag_big, dt_big, xk, xmeff, P)
            % CLUSTLOOKAHEAD calculate look ahead time for clustered events (days)
            deltam = (1-xk) .* mag_big - xmeff;
            if deltam < 0
                deltam = 0;
            end
            denom = 10.0 .^ ((deltam - 1) * (2/3));
            top = log(1 - P) * dt_big;
            tau = top / denom;
        end

        function h = AddMenuItem(parent, catalog, varargin)
            % create a menu item
            label = 'Reasenberg Decluster';
            h = uimenu(parent, 'Label', label,...
                'MenuSelectedFcn', @(~,~)ReasenbergDeclusterClass(catalog),...
                varargin{:});
        end

        function equi = equevent(tb)
            % equevent calc equivalent event to cluster
            % equi = equevent(catalog, cluster, bg)
            %   catalog : earthquake catalog
            %   cluster :
            %   bg : index of a big event (?)
            %  equevent.m                        A.Allmann
            % calculates equivalent event to a cluster
            % weight according to seismic moment
            % time for equivalent event is time of first biggest event
            %
            report_this_filefun();

            equi = ZmapCatalog;
            equi.Name='clusters';

            if isempty(tb)
                return
            end
            j = 0;
            nClusts = max(tb.clusterNumber);
            [elat, elon, edep, emag] = deal(nan(nClusts,1));
            edate(nClusts,1) = datetime(missing);
            emagtype(nClusts,1) = categorical(missing);

            for n = 1 : max(tb.clusterNumber)
                clust_events = tb(tb.clusterNumber==n,:);
                if isempty(clust_events)
                    continue;
                end
                j = j + 1;

                eqmoment = 10.^(clust_events.Magnitude .* 1.2);
                emoment = sum(eqmoment);         %moment

                weights = eqmoment./emoment;      %weightfactor
                elat(j)     = sum(clust_events.Latitude .* weights);
                elon(j)     = sum(clust_events.Longitude .* weights); %longitude
                edep(j)     = sum(clust_events.Depth .* weights); %depth
                emag(j)     = (log10(emoment))/1.2;
                theBiggest  = find(clust_events.isBiggest,1,'first');
                edate(j)    = clust_events.Date(theBiggest);
                emagtype(j) = clust_events.MagnitudeType(theBiggest);

            end

            %equivalent events for each cluster
            equi.Latitude = elat(1:j);
            equi.Longitude = elon(1:j);
            equi.Date = edate(1:j);
            equi.Magnitude = emag(1:j);
            equi.MagnitudeType = emagtype(1:j);
            equi.Depth = edep(1:j);
            [equi.Dip, equi.DipDirection, equi.Rake]=deal(nan(size(equi.Date)));
        end
    end

end

%% helper
function ac  = timediff(clus_idx, look_ahead_time, clus, eqtimes)
    % TIMEDIFF calculates the time difference between the ith and jth event
    % works with variable eqtime from function CLUSTIME
    % gives the indices ac of the eqs not already related to cluster k1
    % eqtimes should be sorted!
    %
    % clus_idx : ith cluster (ci)
    % look_ahead : look-ahead time (tau)
    % clus: clusters (length of catalog)
    % eqtimes: datetimes for event catalog, in days  [did not use duration because of overhead]
    %
    % tdiff: is time between jth event and eqtimes(clus_idx)
    % ac: index of each event within the cluster

    %assert(clus_idx <100, 'testing. remove me')

    comparetime = eqtimes(clus_idx);

    first_event = clus_idx + 1; % in cluster
    last_event = numel(eqtimes);
    max_elapsed = comparetime + look_ahead_time;

    if eqtimes(end) >= max_elapsed
        last_event = find(eqtimes(first_event : last_event) < max_elapsed, 1, 'last') + clus_idx;
    end

    if first_event == last_event
        % no additional events were found.
        ac = [];
        return
    end

    this_clusternum = clus(clus_idx);

    range = first_event : last_event;

    if this_clusternum == 0
        ac = range;
    else
        % indices of eqs not already related to this cluster
        ac = (find(clus(range) ~= this_clusternum)) + clus_idx;
    end
    ac = ac(:);
end


function a = lt( x, y )

%   Disciplined convex programming information for LT (<):
%      The left-hand side of a less-than constraint must be convex. The
%      right-hand side must be concave. Of course, real constant and
%      affine expressions are both convex and concave and can be used on
%      either side as well.
%
%   Disciplined geometric programming information for LT (<):
%      The left-hand side of a less-than constraint must be log-convex,
%      including positive constants, monomials, posynomials, generalized
%      posynomials, and products thereof. The right-hand side must be
%      log-concave---including positive constants, monomials,
%      reciprocals of log-convex expressions, and products thereof.
%
%   Note that CVX does not distinguish between strict less-than (<) and
%   less-than-or-equal (<=) constraints; they are treated identically.
%   Feasible interior-point solvers tend to return points which satisfy
%   strict inequality, but not all solvers do.

b = newcnstr( evalin( 'caller', 'cvx_problem', '[]' ), x, y, '<' );
if nargout, a = b; end

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [bbox_pred, bbox_pred_in_region, bbox_pred_in_region_quantized] = decode_loc_probs_to_bbox_targets(...
    bbox_in, class_indices, loc_prob_vectors, conf)
% decode_loc_probs_to_bbox_targets: given the input bounding boxes (bbox_in),
% the category ids of each bounding box (class_indices), and the predicted
% probability vectors (loc_prob_vectors) of each input bounding box it
% returns the location of the predicted bounding boxes.
%
% INPUTS:
% 1) bbox_in: a N x 4 array with the input bounding box coordinates in the
% form of [xi0,yi0,xi1,yi1] where (xi0,yi0) is the tot-left corner and
% (xi1,yi1) is the bottom-right corner. N is the number of bounding boxes.
% 2) class_indices: a N x 1 array with the category id of each input
% bounding box.
% 3) loc_prob_vectors: a M x K x C x N array with the predicted probability
% vectors of each input bounding box; N is the number of bounding boxes, C
% is the number of categoris, M is the resolution of the target
% probabilities specified by the input parameter conf.resolution and K is
% the number of target probability vectors per bounding box and per
% category. For the InOut model K = 2 (1 vector for each axis), for the
% Border model K = 4 (2 vectors for each axis; e.g. for the x axis we have
% 1 probability vector for the left border and 1 probability vector for the
% right bordr), and for the Combined model K = 6 (3 vectors for each axis;
% e.g. for the x axis we have 1 probability vector for the in-out elements,
% 1 probability vector for the left border, and 1 probability vector
% for the right border).
% 4) conf: struct with the configuration parameters of LocNet:
%   4.1) conf.scale_ratio: (scalar value) the scaling factor of the search region
%        w.r.t. the input bounding boxes. Specifically, given an input
%        bounding boxes, in order for LocNet to localize the target
%        bounding box it "look" in a search region that is obtained by scaling
%        the input bounding box by a scaling factor. The scaling factor is
%        given by the scale_ratio parameter.
%   4.2) conf.resolution: scalar value; In order for the LocNet to localize
%   the target bounding boxes inside the search region, it considers a division
%   of the search region in M horizontal stripes (rows) and M vertical stripes
%   (columns). The value of M is given by the parameter conf.resolution.
%   4.3) conf.num_classes: (scalar value) number of categories
%   4.4) conf.loc_type: string wiht the type of the localization model that
%   the LocNet implements. The possible options are: 'inout','borders', or 'combined'
%
% OUTPUTS:
% 1) bbox_pred: a N x 5 array with the predicted bounding box coordinates in the
% form of [xt0,yt0,xt1,yt1,c] where (xt0,yt0) is the tot-left corner, (xt1,yt1)
% is the bottom-right corner, and c is the category id of the bounding box.
% 3) bbox_pred_in_region: a N x 5 array with the predicted bounding box
% coordinates in the form of [xtr0,ytr0,xtr1,ytr1] where (xtr0,ytr0) is
% the tot-left corner, (xtr1,ytr1) is the bottom-right corner, and c is the
% category id of the bounding box. The coordinates are w.r.t. the
% corresponding search regions.
% 2) bbox_target_quantized: a N x 5 array with the predicted bounding box
% coordinates in the form of [xtq0,ytq0,xtq1,ytq1] where (xtq0,ytq0) is
% the tot-left corner, (xtq1,ytq1) is the bottom-right corner, and c is the
% category id of the bounding box. The coordinates are w.r.t. the
% corresponding search regions and quantized in discrite values between 1
% and resolution.
%
% This file is part of the code that implements the following paper:
% Title      : "LocNet: Improving Localization Accuracy for Object Detection"
% Authors    : Spyros Gidaris, Nikos Komodakis
% Institution: Universite Paris Est, Ecole des Ponts ParisTech
% ArXiv link : http://arxiv.org/abs/1511.07763
% code       : https://github.com/gidariss/LocNet
%
% AUTORIGHTS
% --------------------------------------------------------
% Copyright (c) 2016 Spyros Gidaris
%
% Title     : "LocNet: Improving Localization Accuracy for Object Detection"
% ArXiv link: http://arxiv.org/abs/1511.07763
% Licensed under The MIT License [see LICENSE for details]
% ---------------------------------------------------------

resolution  = conf.resolution;
scale_ratio = conf.scale_ratio;
loc_type    = conf.loc_type;

assert(size(loc_prob_vectors,1) == conf.resolution);

% get the coordinates of the search regions
bbox_search_region = scale_bboxes(bbox_in, scale_ratio);

% given the predicted probability vectors and the category ids, estimate
% the most likely bounding box location of the objects of interest; the
% predicted bounding boxes locations are w.r.t. the search region
% bbox_search_region and quantized in discrite values between 1 and
% resolution (in the paper the resolution parameter is referred as M)
switch loc_type
    case 'inout'
        bbox_pred_in_region_quantized = decode_in_out_probabilities(loc_prob_vectors, class_indices);
    case 'borders'
        bbox_pred_in_region_quantized = decode_borders_probabilities(loc_prob_vectors, class_indices);
    case 'combined'
        bbox_pred_in_region_quantized = decode_combined_probabilities(loc_prob_vectors, class_indices);
    otherwise
        error('Invalid localization type %s',loc_type)
end

% get the coordinates of the predicted bounding boxes w.r.t. the original
% image and in pixel units.
[bbox_pred, bbox_pred_in_region] = decode_quantized_location(...
    bbox_pred_in_region_quantized, resolution, bbox_search_region);
bbox_pred = [bbox_pred, class_indices];
end

function [real_coord, real_coord_region] = decode_quantized_location(quantized, resolution, bbox_search_region)
search_width  = bbox_search_region(:,3)-bbox_search_region(:,1)+1;
search_height = bbox_search_region(:,4)-bbox_search_region(:,2)+1;

% transform the coordinates of the predicted bounding boxes in pixel units
real_coord_region      = quantized;
real_coord_region(:,1) = (quantized(:,1)-1).*(search_width /resolution) + 0.5;
real_coord_region(:,2) = (quantized(:,2)-1).*(search_height/resolution) + 0.5;
real_coord_region(:,3) = (quantized(:,3)-1).*(search_width /resolution) + 0.5;
real_coord_region(:,4) = (quantized(:,4)-1).*(search_height/resolution) + 0.5;

% get the coordinates of the predicted bounding boxes w.r.t. the image
real_coord      = real_coord_region;
real_coord(:,1) = real_coord_region(:,1)+bbox_search_region(:,1);
real_coord(:,3) = real_coord_region(:,3)+bbox_search_region(:,1);
real_coord(:,2) = real_coord_region(:,2)+bbox_search_region(:,2);
real_coord(:,4) = real_coord_region(:,4)+bbox_search_region(:,2);
end

function best_location = decode_in_out_probabilities(loc_maps_per_cls, class_indices)

% keep from each input bounding box the probability vectors that correspond
% to its category id
loc_maps_size = size(loc_maps_per_cls);
assert(max(class_indices) <= loc_maps_size(3));
loc_maps_size(3) = 1;
loc_prob_vectors = zeros(loc_maps_size,'single');
unique_class_indices = unique(class_indices);
for i = 1:length(unique_class_indices)
    class_idx   = unique_class_indices(i);
    is_this_cls = class_idx == class_indices;
    loc_prob_vectors(:,:,1,is_this_cls) = loc_maps_per_cls(:,:,class_idx,is_this_cls);
end
loc_prob_vectors = squeeze(loc_prob_vectors);

% given the predicted probability vectors, for each input bounding box,
% estimate the most likely location (by minimizing the negative log likelihood)
% of bounding box of the actual object independently for the x and y axis.
[best_locationsX] = minimizeNegLogLikelihoodInOut(loc_prob_vectors(:,1,:)); % x axis
[best_locationsY] = minimizeNegLogLikelihoodInOut(loc_prob_vectors(:,2,:)); % y axis
best_location = single([best_locationsX(:,1),best_locationsY(:,1),best_locationsX(:,2),best_locationsY(:,2)]);
end

function best_location = decode_combined_probabilities(loc_maps_per_cls, class_indices)

% keep from each input bounding box the probability vectors that correspond
% to its category id
loc_maps_size = size(loc_maps_per_cls);
assert(max(class_indices) <= loc_maps_size(3));
loc_maps_size(3) = 1;
loc_prob_vectors = zeros(loc_maps_size,'single');
unique_class_indices = unique(class_indices);
for i = 1:length(unique_class_indices)
    class_idx   = unique_class_indices(i);
    is_this_cls = class_idx == class_indices;
    loc_prob_vectors(:,:,1,is_this_cls) = loc_maps_per_cls(:,:,class_idx,is_this_cls);
end
loc_prob_vectors = squeeze(loc_prob_vectors);

% given the predicted probability vectors, for each input bounding box,
% estimate the most likely location (by minimizing the negative log likelihood)
% of bounding box of the actual object independently for the x and y axis.
[best_locationsX] = minimizeNegLogLikelihoodCombined(loc_prob_vectors(:,1:3,:)); % x axis
[best_locationsY] = minimizeNegLogLikelihoodCombined(loc_prob_vectors(:,4:6,:)); % y axis
best_location = single([best_locationsX(:,1),best_locationsY(:,1),best_locationsX(:,2),best_locationsY(:,2)]);
end

function best_location = decode_borders_probabilities(loc_maps_per_cls, class_indices)

% keep from each input bounding box the probability vectors that correspond
% to its category id
loc_maps_size = size(loc_maps_per_cls);
assert(max(class_indices) <= loc_maps_size(3));
loc_maps_size(3) = 1;
loc_prob_vectors = zeros(loc_maps_size,'single');
unique_class_indices = unique(class_indices);
for i = 1:length(unique_class_indices)
    class_idx   = unique_class_indices(i);
    is_this_cls = class_idx == class_indices;
    loc_prob_vectors(:,:,1,is_this_cls) = loc_maps_per_cls(:,:,class_idx,is_this_cls);
end
loc_prob_vectors = squeeze(loc_prob_vectors);

% given the predicted probability vectors, for each input bounding box,
% estimate the most likely location (by minimizing the negative log likelihood)
% of bounding box of the actual object independently for the x and y axis.
[best_locationsX] = minimizeNegLogLikelihoodBorders(loc_prob_vectors(:,1:2,:)); % x axis
[best_locationsY] = minimizeNegLogLikelihoodBorders(loc_prob_vectors(:,3:4,:)); % y axis
best_location = single([best_locationsX(:,1),best_locationsY(:,1),best_locationsX(:,2),best_locationsY(:,2)]);
end

function [best_location, MinNegLogLikelihood, NegLogLikelihoodPerLocation, all_locations] = minimizeNegLogLikelihoodCombined(Probs)
% Given the predicted Combined probability vectors for a single dimension
% (either the x or y axis), estimate the most likely location of the actual
% bounding box in the corresponding axis by minimizing the negative
% log-likelihood. For the mimization we use exaustive search.

min_prob         = 0.0001;
PositiveProbs = max(  Probs,min_prob);
NegativeProbs = max(1-Probs,min_prob);

LogInsideProbs   = -log(squeeze(PositiveProbs(:,1,:)));
LogBordersProbs0 = -log(squeeze(PositiveProbs(:,2,:)))';
LogBordersProbs1 = -log(squeeze(PositiveProbs(:,3,:)))';

LogOutsideProbs     = -log(squeeze(NegativeProbs(:,1,:)));
LogNonBordersProbs0 = -log(squeeze(NegativeProbs(:,2,:)))';
LogNonBordersProbs1 = -log(squeeze(NegativeProbs(:,3,:)))';

resolution            = size(LogInsideProbs,1);
num_elems             = size(LogOutsideProbs,2);
LogInsideProbsCumSum  = cumsum([zeros(1,num_elems,'single');LogInsideProbs], 1)';
LogOutsideProbsCumSum = cumsum([zeros(1,num_elems,'single');LogOutsideProbs],1)';

NegLogLikelihoodPerLocation = zeros([num_elems, resolution*resolution],'single');
all_locations = zeros([resolution*resolution, 2],'single');
c = 0;
% compute the negative log-likelihood of each possible location of the
% bounding box
for a = 1:(resolution-1) % the double loop iterates over all possible (discrete) locations of the bounding box
    for b = (a+1):resolution
        c = c + 1;
        % NegLogLikelihoodPerLocation(:,c) is the negative log-likelihood of
        % the bounding box with coordines on [a, b] in the x (or y) axis.
        % For instance, in the x axis, a is the location of the left border
        % of the bounding box and b is the location of the right border of
        % the bounding box.
        NegLogLikelihoodPerLocation(:,c) = (LogInsideProbsCumSum(:,b+1)-LogInsideProbsCumSum(:,a))-...
            (LogOutsideProbsCumSum(:,b+1)-LogOutsideProbsCumSum(:,a)) + ...
            (LogBordersProbs0(:,a) + LogBordersProbs1(:,b)) - ...
            (LogNonBordersProbs0(:,a) + LogNonBordersProbs1(:,b));
        all_locations(c,1) = a;
        all_locations(c,2) = b;
    end
end
all_locations = all_locations(1:c,:);
NegLogLikelihoodPerLocation = NegLogLikelihoodPerLocation(:,1:c);

% pick the location with the minimum negative log-likelihood
[MinNegLogLikelihood, min_idx] = min(NegLogLikelihoodPerLocation,[],2);
best_location = all_locations(min_idx,:);
end

function [best_location, MinNegLogLikelihood, NegLogLikelihoodPerLocation, all_locations] = minimizeNegLogLikelihoodInOut(Probs)
% Given the predicted InOut probability vectors for a single dimension
% (either the x or y axis), estimate the most likely location of the actual
% bounding box in the corresponding axis by minimizing the negative
% log-likelihood. For the mimization we use exaustive search.

min_prob         = 0.0001;
InsideProbs      = max(  Probs,min_prob);
OutsideProbs     = max(1-Probs,min_prob);
LogInsideProbs   = -log(squeeze(InsideProbs(:,1,:)));
LogOutsideProbs  = -log(squeeze(OutsideProbs(:,1,:)));

resolution            = size(LogInsideProbs,1);
num_elems             = size(LogOutsideProbs,2);
LogInsideProbsCumSum  = cumsum([zeros(1,num_elems,'single');LogInsideProbs], 1)';
LogOutsideProbsCumSum = cumsum([zeros(1,num_elems,'single');LogOutsideProbs],1)';

NegLogLikelihoodPerLocation = zeros([num_elems, resolution*resolution],'single');
all_locations = zeros([resolution*resolution, 2],'single');
c = 0;
% compute the negative log-likelihood of each possible location of the
% bounding box
for a = 1:(resolution-1) % the double loop iterates over all possible (discrete) locations of the bounding box
    for b = (a+1):resolution
        c = c + 1;
        % NegLogLikelihoodPerLocation(:,c) is the negative log-likelihood of
        % the bounding box with coordines on [a, b] in the x (or y) axis.
        % For instance, in the x axis, a is the location of the left border
        % of the bounding box and b is the location of the right border of
        % the bounding box.
        NegLogLikelihoodPerLocation(:,c) = (LogInsideProbsCumSum(:,b+1)-LogInsideProbsCumSum(:,a))-...
            (LogOutsideProbsCumSum(:,b+1)-LogOutsideProbsCumSum(:,a));
        all_locations(c,1) = a;
        all_locations(c,2) = b;
    end
end
all_locations = all_locations(1:c,:);
NegLogLikelihoodPerLocation = NegLogLikelihoodPerLocation(:,1:c);

% pick the location with the minimum negative log-likelihood
[MinNegLogLikelihood, min_idx] = min(NegLogLikelihoodPerLocation,[],2);
best_location = all_locations(min_idx,:);
end

function [best_location, MinNegLogLikelihood, NegLogLikelihoodPerLocation, all_locations] = minimizeNegLogLikelihoodBorders(Probs)
% Given the predicted Borders probability vectors for a single dimension
% (either the x or y axis), estimate the most likely location of the actual
% bounding box in the corresponding axis by minimizing the negative
% log-likelihood. For the mimization we use exaustive search.

min_prob         = 0.0001;
PositiveProbs    = max(  Probs,min_prob);
LogBordersProbs0 = -log(squeeze(PositiveProbs(:,1,:)))';
LogBordersProbs1 = -log(squeeze(PositiveProbs(:,2,:)))';

resolution = size(LogBordersProbs0,2);
num_elems = size(LogBordersProbs0,1);

NegLogLikelihoodPerLocation = zeros([num_elems, resolution*resolution],'single');
all_locations = zeros([resolution*resolution, 2],'single');
c = 0;
% compute the negative log-likelihood of each possible location of the
% bounding box
for a = 1:(resolution-1) % the double loop iterates over all possible (discrete) locations of the bounding box
    for b = (a+1):resolution
        c = c + 1;
        % NegLogLikelihoodPerLocation(:,c) is the negative log-likelihood of
        % the bounding box with coordines on [a, b] in the x (or y) axis.
        % For instance, in the x axis, a is the location of the left border
        % of the bounding box and b is the location of the right border of
        % the bounding box.
        NegLogLikelihoodPerLocation(:,c) = (LogBordersProbs0(:,a) + LogBordersProbs1(:,b));
        all_locations(c,1) = a;
        all_locations(c,2) = b;
    end
end
all_locations = all_locations(1:c,:);
NegLogLikelihoodPerLocation = NegLogLikelihoodPerLocation(:,1:c);

% pick the location with the minimum negative log-likelihood
[MinNegLogLikelihood, min_idx] = min(NegLogLikelihoodPerLocation,[],2);
best_location = all_locations(min_idx,:);
end


function [X]=tt_minres_selfprec2(A,  eps, varargin)
%Computation of the approximate TT-matrix inverse using self-prec method
%   [X]=TT_MINRES_SELFPREC2(A,EPS,OPTIONS) Computation of the approximate
%   TT-matrix inverse using Saad self-prec method. Options are provided
%   in form 'PropertyName1',PropertyValue1,'PropertyName2',PropertyValue2
%   and so on. The parameters are set to default (in brackets in the
%   following) The list of option names and default values are:
%       o matvec - type of the local matvec [ {mm+compr} | mmk2 ]
%       o max_rank - maximal TT-rank bound [1000]
%       o prec_type - left or right inversion [ {left} | right ]
%       o maxit - maximal number of iterations [10]
%       o tol - the requested inversion tolerance [EPS]
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

% matvec='mmk2';
matvec='mm+compr';
max_rank=1000;
prec_type='left';
tol=eps;
maxit=10;

for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'matvec'
            matvec=varargin{i+1};
        case 'max_rank'
            max_rank=lower(varargin{i+1});
        case 'prec_type'
            prec_type=varargin{i+1};
        case 'maxit'
            maxit=varargin{i+1};
        otherwise
            error('Unrecognized option: %s\n',varargin{i});
    end
end

%d=max(size(A));
d=ndims(A);
ns=A.n;
%ns=tt_size(A);

I=tt_eye(ns, d); I=tt_matrix(I);
if (strcmp(prec_type, 'left'))
   % X=tt_scal(tt_transp(A), 1e-15);
   X=(1e-15)*conj(A');
else
  %  X=tt_scal(I, 1e-150);
   X=1e-150*I;
end;
normf=norm(I);
%normf=sqrt(tt_dot(tt_mat_to_vec(I), tt_mat_to_vec(I)));
err=1;
err_old=2;
sp=0;

for iout=1:maxit
    if (strcmp(matvec, 'mmk2'))
        resid=tt_mmk2(A, X, eps, max_rank);
    else
        %resid=tt_mm(A,X);
        resid=round(A*X,eps,max_rank*2);
        %resid=tt_mat_compr(resid, eps,max_rank*2);
    end;
    %resid=ttm_add(I, tt_scal(resid, -1));
    resid=I-resid;

    if (iout>1)||(strcmp(prec_type, 'left'))
        if (strcmp(matvec, 'mmk2'))
            Xresid=tt_mmk2(X, resid, eps,max_rank);
        else
            %Xresid=tt_mm(X,resid);
            Xresid=round(X*resid,eps,max_rank*2);
            %Xresid=tt_mat_compr(Xresid, eps,max_rank*2);

        end;
    else
        Xresid=resid;
    end;

    max_xrrank=max(rank(Xresid));

    if (strcmp(prec_type, 'left')) % we use left preconditioner (igmres)
        if (strcmp(matvec, 'mmk2'))
            w=tt_mmk2(A, Xresid, eps,max_rank);
            w=tt_mmk2(X, w, eps,max_rank);
        else
            w=round(A*Xresid,eps,max_rank);
            w=round(X*w,eps);
            %w=tt_mm(A,Xresid);
            %w=tt_mat_compr(w, eps,max_rank);
            %w=tt_mm(X,w);
%             w=tt_mat_compr(w, eps);
        end;
        %w = tt_mat_to_vec(w);
        %beta=sqrt(tt_dot(tt_mat_to_vec(Xresid),tt_mat_to_vec(Xresid)));
        beta=norm(Xresid);
        wr=dot(w,Xresid);
        %wr = tt_dot(w, tt_mat_to_vec(Xresid));
        ww=dot(w,w);

        H=zeros(2,1);
        H(1,1)=wr/(beta^2);
        H(2,1)=sqrt(ww/(beta^2)-wr^2/(beta^4));
        rhs=H(1,1)*beta;

        y = rhs/(H'*H);
        err = err*norm(H*y-[beta; 0])/beta;
        y=y/beta;

%         y = tt_dot(w,w);
%         if (y~=0)
%             y = tt_dot(w, tt_mat_to_vec(Xresid))/y;
%         else
%             y=1;
%         end;
    else % Use right preconditioner and fgmres
        if (strcmp(matvec, 'mmk2'))
            w=tt_mmk2(A, Xresid, eps,max_rank);
        else
            w=A*Xresid;
            w=round(w,eps);
%             w=tt_mat_compr(w, eps);
        end;
        w=tt_tensor(w);
        %w = tt_mat_to_vec(w);
        beta=norm(resid);
        %beta=sqrt(tt_dot(tt_mat_to_vec(resid),tt_mat_to_vec(resid)));
        wr=dot(w,tt_tensor(resid));
        ww=dot(w,w);
        %wr = tt_dot(w, tt_mat_to_vec(resid));
        %ww = tt_dot(w,w);

        H=zeros(2,1);
        H(1,1)=wr/(beta^2);
        H(2,1)=sqrt(ww/(beta^2)-wr^2/(beta^4));
        rhs=H(1,1)*beta;

        y = rhs/(H'*H);
        err = norm(H*y-[beta; 0])/normf;
        y=y/beta;

%         y2=ww;
%         if (y~=0)
%             y2 = wr/y2;
%         else
%             y2=1;
%         end;
    end;

    max_wrank=max(rank(w));

    %Xresid=tt_mat_to_vec(Xresid);
    Xresid=tt_tensor(Xresid);
    X=tt_tensor(X);
    %X=tt_mat_to_vec(X);
    %if (err<err_old)
        %X = tt_axpy(1, X, y, Xresid, eps, max_rank);
        X=round(X+y*Xresid,eps,max_rank);

        %err_old=err;
    %else
    %    sp=sp+1;
    %end;
    %max_xrank=max(tt_ranks(X));
    max_xrank=max(rank(X));
    %X = tt_vec_to_mat(X, ns, ns);
    X=tt_matrix(X,ns,ns);
%     if (strcmp(matvec, 'mmk2'))
%         resid=tt_mmk2(A, X, eps, max_rank);
%     else
%         resid=tt_mm(A, X);
% %         resid=tt_mat_compr(resid, eps);
%     end;
%     resid=ttm_add(I, tt_scal(resid, -1));
%     resid=tt_mat_to_vec(resid);
%
%     err_real=sqrt(tt_dot(resid,resid))/normf;
    err_real=0;
    fprintf('iter [%d], res_real=%3.3e, x_rank=%d, Xr_rank=%d, w_rank=%d, err=%3.3e\n', iout, err_real, max_xrank, max_xrrank, max_wrank, err);
    if (err<tol)||(sp>0)
        break;
    end

end
%keyboard;


function DCM = spm_meta_model(DCM)
% Meta-modelling of Bayes-optimal responses (Newton's method)
% FORMAT DCM = spm_meta_model(DCM)
%
% store estimates in DCM
%--------------------------------------------------------------------------
% DCM.M   - meta-model specification
%      M: [1 x m struct]   - hierarchical inference model (cf DEM.M)
%      G: [1 x s struct]   - generative process (for spm_ADEM or spm_ALAP)
%      U: [n x N double]   - n prior beliefs over N samples
%     pE: [1 x 1 struct]   - prior expectation of meta-model parameters
%     pC: [1 x 1 struct]   - prior variance of meta-model parameters
%
% DCM.xY  - data structure
%      y: [N x p double]   - N samples of a p-variate response
%     X0: [N x q double]   - q-variate confounds
%     dt: [1 x 1 double]   - size of time bin for each sample
%      Q: {[N x N double]} - precision component[s]
%
% DCM.xU  - input structure
%      u: [r x N double]   - r-variate input (hidden causes G in DEM)
%
% Computes (and stores in DCM_MM_???)
%--------------------------------------------------------------------------
% DCM.DEM - Inference (with MAP parameters)
% DCM.Ep  - conditional expectation
% DCM.Cp  - conditional covariances
% DCM.Eh  - conditional log-precision
% DCM.Ey  - conditional response
% DCM.F   - log-evidence
%
% This routine illustrates Bayesian meta modelling - the Bayesian inversion
% of a model of a Bayesian observer. This requires the specification of two
% models: An inference model used by the subject (specified by a DEM
% structure) and a meta-model (specified by a DCM structure). The inference
% model is completed by a response model to furnish the meta-model; where
% the response model takes the output of the (active) inference scheme
% specified by the DEM and generates an observed (behavioural or
% neurophysiological) response. Crucially either the inference model or
% the response model or both can have free parameters - that are optimised
% using Bayesian nonlinear system identification in the usual way.
%
% Although this routine is a function, it is expected that people will fill
% in the model-specific parts in a local copy, before running it.  The
% current example uses a model of slow pursuit and generates synthetic data
% (responses) to illustrate how it works. To replace these simulated data
% with real data, simply specify the DCM.xY (and xU fields) with
% empirical values. If other fields do not exist, exemplar fields will be filled in.
%
% The conditional density of the parameters and F values (log-evidence) can
% be used in the usual way for inference on parameters or Bayesian model
% comparison (as for other DCMs)
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_meta_model.m 5788 2013-12-06 20:08:57Z karl $

% INFERENCE MODEL: DEM.M and DEM.G
%==========================================================================
try
    M = DCM.M;

catch

    % create illustrative (M,G) if not specified (EXAMPLE)
    %----------------------------------------------------------------------

    % hidden causes and states
    %======================================================================
    % x    - hidden states:
    %   x.o(1) - oculomotor angle
    %   x.o(2) - oculomotor velocity
    %   x.x(1) - target angle - extrinsic coordinates
    %
    % v    - causal states: force on target
    %
    % g    - sensations:
    %   g(1) - oculomotor angle (proprioception)
    %   g(2) - oculomotor velocity
    %   g(:) - visual input - intrinsic coordinates
    %----------------------------------------------------------------------

    % Set-up
    %======================================================================
    M(1).E.s = 1/2;                               % smoothness
    M(1).E.n = 4;                                 % order of
    M(1).E.d = 1;                                 % generalised motion

    % angular frequency of target motion (per time bin)
    %----------------------------------------------------------------------
    w  = 2*pi/32;

    % occlusion and visual mapping functions
    %----------------------------------------------------------------------
    occ = @(x) x < 1/2;
    vis = @(x) exp(-((-8:8)' - x.x(1) + x.o(1)).^2)*occ(x.x);

    % Generative model (M) (sinusoidal movement)
    %======================================================================
    % Endow the model with internal dynamics (a simple oscillator) so that is
    % recognises and remembers the trajectory to anticipate jumps in rectified
    % sinusoidal motion.

    % slow pursuit following with (second order) generative model
    %----------------------------------------------------------------------
    x.o = [0;0];                                  % motor angle & velocity
    x.x = 0;                                      % target location

    % level 1: Displacement dynamics and mapping to sensory/proprioception
    %----------------------------------------------------------------------
    M(1).f = @(x,v,P) [x.o(2); (v - x.o(1))/4 - x.o(2)/2; v - x.x];
    M(1).g = @(x,v,P) [x.o; vis(x)];
    M(1).x = x;                                   % hidden states
    M(1).V = exp(4);                              % error precision
    M(1).W = exp(4);                              % error precision

    % level 2: With hidden (memory) states
    %----------------------------------------------------------------------
    M(2).f  = @(x,v,P)[x(2); -x(1)]*v;
    M(2).g  = @(x,v,P) x(1);
    M(2).x  = [0; 0];                             % hidden states
    M(2).V  = exp(4);                             % error precision
    M(2).W  = exp(4);                             % error precision

    % level 3: Encoding frequency of memory states (U)
    %----------------------------------------------------------------------
    M(3).v = 0;
    M(3).V = exp(4);

    % generative model (G) (with no noise)
    %======================================================================

    % first level
    %----------------------------------------------------------------------
    G(1).f = @(x,v,a,P) [x.o(2); a/4 - x.o(2)/8; v - x.x];
    G(1).g = @(x,v,a,P) [x.o; vis(x)];
    G(1).x = x;                                  % hidden states
    G(1).U = sparse(1,[1 2],[1 1],1,19)*exp(4);  % motor gain

    % second level
    %----------------------------------------------------------------------
    G(2).v = 0;                                  % exogenous force
    G(2).a = 0;                                  % action force

end

% Check generative model
%--------------------------------------------------------------------------
M      = spm_DEM_M_set(M);

% Experimental input (C)
%==========================================================================
try
    xU    = DCM.xU;
    N     = size(xU.u,2);

catch

    % Sine wave cause (EXAMPLE)
    %----------------------------------------------------------------------
    N     = 64;                                 % length of data sequence
    xU.dt = 16;                                 % time step (ms)
    xU.u  = sin((1:N)*w).*((1:N) > 16);         % sinusoidal target motion
end

% Priors (U)
%==========================================================================
try
    U = DCM.U;

catch

    % (EXAMPLE) prior beliefs about target frequency (w)
    %----------------------------------------------------------------------
    U = zeros(1,N) + w;
end

% Data and confounds
%==========================================================================
try
    xY = DCM.xY;

catch

    % Generate simulated data (EXAMPLE)
    %----------------------------------------------------------------------

    % meta-model and TRUE parameters
    %----------------------------------------------------------------------
    MM.M    = M;
    MM.G    = G;
    MM.U    = U;

    P.G     = 2;
    P.W     = 4;
    [y DEM] = IS(P,MM,xU);

    % and simulate observation noise
    %----------------------------------------------------------------------
    xY.y    = y + spm_Q(1/2,N,1)*randn(N,1)/16;

    spm_figure('GetWin','Simulated respsone');
    spm_DEM_qU(DEM.qU,DEM.pU)

end

% DCT confounds
%==========================================================================
xY.X0  = spm_dctmtx(N,1);

% and [serial] correlations (precision components) AR model
%--------------------------------------------------------------------------
xY.Q   = {spm_Q(1/2,N,1)};

% META-MODEL parameters
%==========================================================================
% These parameterise the inference scheme function (IS) at the bottom of
% this script - this functions defines how the parameters are used and
% therefore optimised. Change this function to specify the meta-model

try
    pE    = DCM.pE;
    pC    = DCM.pC;

catch

    % prior expectations (EXAMPLE)
    %----------------------------------------------------------------------
    pE.G  = 1;
    pE.W  = 2;

    % prior covariance (EXAMPLE)
    %----------------------------------------------------------------------
    pC.G  = 1;
    pC.W  = 1;

end

% This completes the meta-model specification. The fields are now assembled
% and passed to spm_nlsi_GN (nonlinear system identification using Gauss-
% Newton-like gradient ascent).
%==========================================================================

% META-MODEL (MM)
%==========================================================================
MM.M   = M;
MM.G   = G;
MM.U   = U;

% hyperpriors (assuming about 99% signal to noise)
%--------------------------------------------------------------------------
hE     = 6 - log(var(spm_vec(xY.y)));
hC     = exp(-4);

% Meta-model
%--------------------------------------------------------------------------
MM.IS  = @IS;
MM.pE  = pE;
MM.pC  = pC;
MM.hE  = hE;
MM.hC  = hC;

% model inversion
%==========================================================================
MM.Nmax      = 16;
[Ep,Cp,Eh,F] = spm_nlsi_GN(MM,xU,xY);

% integrate (A)DEM scheme with MAP etimates
%--------------------------------------------------------------------------
[Ey,DEM] = IS(Ep,MM,xU);

% store estimates in DCM
%--------------------------------------------------------------------------
DCM.DEM = DEM;                  % Inference scheme (with MAP parameters)
DCM.M   = MM;                   % meta-model
DCM.xY  = xY;                   % data structure
DCM.xU  = xU;                   % input structure
DCM.Ep  = Ep;                   % conditional expectation
DCM.Cp  = Cp;                   % conditional covariances
DCM.Eh  = Eh;                   % conditional log-precision
DCM.Ey  = Ey;                   % conditional response
DCM.F   = F;                    % log-evidence

% save
%--------------------------------------------------------------------------
save(sprintf('DCM_MM_%s',date),'DCM', spm_get_defaults('mat.format'));

% and plot
%--------------------------------------------------------------------------
spm_figure('GetWin','Parameter estimates');
subplot(2,1,1)
spm_plot_ci(Ep,Cp), hold on
Pp       = spm_vec(P);
pE       = spm_vec(pE);
plot(Pp(1),Pp(2),'.r' ,'MarkerSize',32), hold on
plot(pE(1),pE(2),'.g','MarkerSize',32), hold off
axis([0 4 0 8])
xlabel('Gain','FontSize',12)
ylabel('Precision','FontSize',12)
title('Posterior (blue), prior (green) and true (red) value','FontSize',16)

return

% inference scheme x = IS(p,M,U)
%==========================================================================
function [y,DEM] = IS(P,M,U)

% parameterise inference model (EXAMPLE)
%--------------------------------------------------------------------------
M.M(2).W = exp(P.W);

% reset random number generator (for action schemes)
%--------------------------------------------------------------------------
rng('default')

% Bayesian inference (spm_DEM, spm_LAP, spm_ADEM or spm_ALAP)
%--------------------------------------------------------------------------
DEM.M  = M.M;
DEM.G  = M.G;
DEM.U  = M.U;
DEM.C  = U.u;
DEM    = spm_ADEM(DEM);

% (Hidden) behavioural or electrophysiological response (EXAMPLE)
%--------------------------------------------------------------------------
x      = DEM.pU.x{1}(1,:);
y      = (P.G*x)';

return


%MSTRAJ Multi-segment multi-axis trajectory
%
% TRAJ = MSTRAJ(WP, QDMAX, TSEG, Q0, DT, TACC, OPTIONS) is a trajectory
% (KxN) for N axes moving simultaneously through M segment.  Each segment
% is linear motion and polynomial blends connect the segments.  The axes
% start at Q0 (1xN) and pass through M-1 via points defined by the rows of
% the matrix WP (MxN), and finish at the point defined by the last row of WP.
% The  trajectory matrix has one row per time step, and one column per
% axis.  The number of steps in the trajectory K is a function of the
% number of via points and the time or velocity limits that apply.
%
% - WP (MxN) is a matrix of via points, 1 row per via point, one column
%   per axis.  The last via point is the destination.
% - QDMAX (1xN) are axis speed limits which cannot be exceeded,
% - TSEG (1xM) are the durations for each of the K segments
% - Q0 (1xN) are the initial axis coordinates
% - DT is the time step
% - TACC (1x1) is the acceleration time used for all segment transitions
% - TACC (1xM) is the acceleration time per segment, TACC(i) is the acceleration
%   time for the transition from segment i to segment i+1.  TACC(1) is also
%   the acceleration time at the start of segment 1.
%
% TRAJ = MSTRAJ(WP, QDMAX, TSEG, [], DT, TACC, OPTIONS) as above but the
% initial coordinates are taken from the first row of WP.
%
% TRAJ = MSTRAJ(WP, QDMAX, Q0, DT, TACC, QD0, QDF, OPTIONS) as above
% but additionally specifies the initial and final axis velocities (1xN).
%
% Options::
% 'verbose'    Show details.
%
% Notes::
% - Only one of QDMAX or TSEG can be specified, the other is set to [].
% - If no output arguments are specified the trajectory is plotted.
% - The path length K is a function of the number of via points, Q0, DT
%   and TACC.
% - The final via point P(end,:) is the destination.
% - The motion has M segments from Q0 to P(1,:) to P(2,:) ... to P(end,:).
% - All axes reach their via points at the same time.
% - Can be used to create joint space trajectories where each axis is a joint
%   coordinate.
% - Can be used to create Cartesian trajectories where the "axes"
%   correspond to translation and orientation in RPY or Euler angle form.
% - If qdmax is a scalar then all axes are assumed to have the same
%   maximum speed.
%
% See also MTRAJ, LSPB, CTRAJ.

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

function [TG, t, info]  = mstraj(segments, qdmax, tsegment, q0, dt, Tacc, varargin)

    if isempty(q0)
        q0 = segments(1,:);
        segments = segments(2:end,:);
    end

    assert(size(segments,2) == size(q0,2), 'RTB:mstraj:badarg', 'WP and Q0 must have same number of columns');
    assert(xor(~isempty(qdmax), ~isempty(tsegment)), 'RTB:mstraj:badarg', 'Must specify either qdmax or tsegment, but not both');
    if isempty(qdmax)
        assert(length(tsegment) == size(segments,1), 'RTB:mstraj:badarg', 'Length of TSEG does not match number of segments');
    end
    if isempty(tsegment)
        if length(qdmax) == 1
            % if qdmax is a scalar assume all axes have the same speed
            qdmax = repmat(qdmax, 1, numcols(segments));
        end
        assert(length(qdmax) == size(segments,2), 'RTB:mstraj:badarg', 'Length of QDMAX does not match number of axes');
    end

    ns = numrows(segments);
    nj = numcols(segments);

    [opt,args] = tb_optparse([], varargin);

    if length(args) > 0
        qd0 = args{1};
    else
        qd0 = zeros(1, nj);
    end
    if length(args) > 1
        qdf = args{2};
    else
        qdf = zeros(1, nj);
    end

    % set the initial conditions
    q_prev = q0;
    qd_prev = qd0;

    clock = 0;      % keep track of time
    arrive = [];    % record planned time of arrival at via points

    tg = [];
    taxis = [];

    for seg=1:ns
        if opt.verbose
            fprintf('------------------- segment %d\n', seg);
        end

        % set the blend time, just half an interval for the first segment

        if length(Tacc) > 1
            tacc = Tacc(seg);
        else
            tacc = Tacc;
        end

        tacc = ceil(tacc/dt)*dt;
        tacc2 = ceil(tacc/2/dt) * dt;
        if seg == 1
            taccx = tacc2;
        else
            taccx = tacc;
        end

        % estimate travel time
        %    could better estimate distance travelled during the blend
        q_next = segments(seg,:);    % current target
        dq = q_next - q_prev;    % total distance to move this segment

        %% probably should iterate over the next section to get qb right...
        % while 1
        %   qd_next = (qnextnext - qnext)
        %   tb = abs(qd_next - qd) ./ qddmax;
        %   qb = f(tb, max acceleration)
        %   dq = q_next - q_prev - qb
        %   tl = abs(dq) ./ qdmax;

        if ~isempty(qdmax)
            % qdmax is specified, compute slowest axis

            qb = taccx * qdmax / 2;        % distance moved during blend
            tb = taccx;

            % convert to time
            tl = abs(dq) ./ qdmax;
            %tl = abs(dq - qb) ./ qdmax;
            tl = ceil(tl/dt) * dt;

            % find the total time and slowest axis
            tt = tb + tl;
            [tseg,slowest] = max(tt);

            info(seg).slowest = slowest;
            info(seg).segtime = tseg;
            info(seg).axtime = tt;
            info(seg).clock = clock;

            % best if there is some linear motion component
            if tseg <= 2*tacc
                tseg = 2 * tacc;
            end
        elseif ~isempty(tsegment)
            % segment time specified, use that
            tseg = tsegment(seg);
            slowest = NaN;
        end

        % log the planned arrival time
        arrive(seg) = clock + tseg;
        if seg > 1
            arrive(seg) = arrive(seg) + tacc2;
        end

        if opt.verbose
            fprintf('seg %d, slowest axis %d, time required %.4g\n', ...
                seg, slowest, tseg);
        end

        %% create the trajectories for this segment

        % linear velocity from qprev to qnext
        qd = dq / tseg;

        % add the blend polynomial
        qb = jtraj(q0, q_prev+tacc2*qd, 0:dt:taccx, qd_prev, qd);
        tg = [tg; qb(2:end,:)];

        clock = clock + taccx;     % update the clock

        % add the linear part, from tacc/2+dt to tseg-tacc/2
        for t=tacc2+dt:dt:tseg-tacc2
            s = t/tseg;
            q0 = (1-s) * q_prev + s * q_next;       % linear step
            tg = [tg; q0];
            clock = clock + dt;
        end

        q_prev = q_next;    % next target becomes previous target
        qd_prev = qd;
    end
    % add the final blend
    qb = jtraj(q0, q_next, 0:dt:tacc2, qd_prev, qdf);
    tg = [tg; qb(2:end,:)];
    info(seg+1).segtime = tacc2;
    info(seg+1).clock = clock;

    % plot a graph if no output argument
    if nargout == 0
        t = (0:numrows(tg)-1)'*dt;
        clf
        plot(t, tg, '-o');
        hold on
        plot(arrive, segments, 'bo', 'MarkerFaceColor', 'k');
        hold off
        grid
        xlabel('time');
        xaxis(t(1), t(end))
        return
    end
    if nargout > 0
        TG = tg;
    end
    if nargout > 1
        t = (0:numrows(tg)-1)'*dt;
    end
    if nargout > 2
        infout = info;
    end