% SP_PRECOMPUTE_PARAM: precompute all the fields, as in the space structure of the technical report, before mapping to the physical domain.
%
%     space = sp_precompute_param (space, msh);
%     space = sp_precompute_param (space, msh, 'option');
%
% INPUT:
%
%    space: object representing the discrete function space (see sp_scalar).
%    msh: mesh object containing the quadrature information (see msh_cartesian)
%   'option', value: additional optional parameters, currently available options are:
%
%              Name     |   Default value |  Meaning
%           ------------+-----------------+----------------------------------
%            value      |      true       |  compute shape_functions
%            gradient   |      false      |  compute shape_function_gradients
%            hessian    |      false      |  compute shape_function_hessians
%
% OUTPUT:
%
%    space: object containing the information of the input object, plus the
%            fields of the old structure, that are listed below. If no option
%            is given all the fields are computed. If an option is given,
%            only the selected fields will be computed.
%
%    FIELD_NAME      (SIZE)                             DESCRIPTION
%    ncomp           (scalar)                           number of components of the functions of the space (actually, 1)
%    ndof            (scalar)                           total number of degrees of freedom
%    ndof_dir        (1 x ndim vector)                  degrees of freedom along each direction
%    nsh_max         (scalar)                           maximum number of shape functions per element
%    nsh             (1 x msh.nel vector)               actual number of shape functions per each element
%    connectivity    (nsh_max x msh.nel vector)         indices of basis functions that do not vanish in each element
%    shape_functions (msh.nqn x nsh_max x msh.nel)      basis functions evaluated at each quadrature node in each element
%    shape_function_gradients
%          (ndim x msh.nqn x nsh_max x msh.nel)         basis function gradients evaluated at each quadrature node in each element
%    shape_function_hessians
%          (ndim x ndim x msh.nqn x nsh_max x msh.nel)  basis function hessians evaluated at each quadrature node in each element
%
% Copyright (C) 2009, 2010 Carlo de Falco
% Copyright (C) 2011, 2015, 2019 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function sp = sp_precompute_param (sp, msh, varargin)

  if (isempty (varargin))
    value = true;
    gradient = true;
    hessian = true;
  else
    if (~rem (length (varargin), 2) == 0)
      error ('sp_precompute: options must be passed in the [option, value] format');
    end
    value = false;
    gradient = false;
    hessian = false;
    for ii=1:2:length(varargin)-1
      if (strcmpi (varargin{ii}, 'value'))
        value = varargin{ii+1};
      elseif (strcmpi (varargin{ii}, 'gradient'))
        gradient = varargin{ii+1};
      elseif (strcmpi (varargin{ii}, 'hessian'))
        hessian = varargin{ii+1};
      else
        warning ('Ignoring unknown option %s', varargin {ii});
      end
    end
  end

  sp = struct (sp);

  sp_univ = sp.sp_univ;

  for idim = 1:msh.ndim
    elem_list{idim} = 1:msh.nel_dir(idim);
  end

  for idim = 1:msh.ndim
    nsh_dim{idim} = sp_univ(idim).nsh(elem_list{idim});
  end

  [nsh_grid{1:msh.ndim}] = ndgrid (nsh_dim{:});
  nsh = 1;
  for idim = 1:msh.ndim
    nsh = nsh .* nsh_grid{idim};
  end
  sp.nsh = nsh(:)';
  clear nsh

  for idim = 1:msh.ndim
    csize = ones (1, 2*msh.ndim);
    csize([idim, msh.ndim+idim]) = [sp_univ(idim).nsh_max, msh.nel_dir(idim)];
    crep = [sp_univ.nsh_max, msh.nel_dir];
    crep([idim, msh.ndim+idim]) = 1;

    conn{idim} = reshape (sp_univ(idim).connectivity(:,elem_list{idim}), csize);
    conn{idim} = repmat (conn{idim}, crep);
    conn{idim} = reshape (conn{idim}, [], msh.nel);
  end

  connectivity = zeros (sp.nsh_max, msh.nel);
  indices = ones (size (conn{1}));
  for idim = 1:msh.ndim
    indices = indices & conn{idim} ~= 0;
  end
  for idim = 1:msh.ndim
    conn{idim} = conn{idim}(indices);
  end
  connectivity(indices) = sub2ind ([sp.ndof_dir, 1], conn{:}); % The extra 1 makes things work in any dimension
  sp.connectivity = reshape (connectivity, sp.nsh_max, msh.nel);
  clear conn csize crep indices connectivity

  if (value || gradient || hessian)
    shp = cell(1,msh.ndim); shg = cell(1,msh.ndim); shh = cell(1,msh.ndim);
    for idim = 1:msh.ndim
      ssize = ones (1, 3*msh.ndim);
      ssize([idim, msh.ndim+idim, 2*msh.ndim+idim]) = [msh.nqn_dir(idim), sp_univ(idim).nsh_max, msh.nel_dir(idim)];
      srep = [msh.nqn_dir, sp_univ.nsh_max, msh.nel_dir];
      srep([idim, msh.ndim+idim, 2*msh.ndim+idim]) = 1;
      shp{idim} = reshape (sp_univ(idim).shape_functions(:,:,elem_list{idim}), ssize);
      shp{idim} = repmat (shp{idim}, srep);
      shp{idim} = reshape (shp{idim}, msh.nqn, sp.nsh_max, msh.nel);
      shg{idim} = reshape (sp_univ(idim).shape_function_gradients(:,:,elem_list{idim}), ssize);
      shg{idim} = repmat (shg{idim}, srep);
      shg{idim} = reshape (shg{idim}, msh.nqn, sp.nsh_max, msh.nel);
      shh{idim} = reshape (sp_univ(idim).shape_function_hessians(:,:,elem_list{idim}), ssize);
      shh{idim} = repmat (shh{idim}, srep);
      shh{idim} = reshape (shh{idim}, msh.nqn, sp.nsh_max, msh.nel);
    end

    if (value)
      sp.shape_functions = 1;
      for idim = 1:msh.ndim
        sp.shape_functions = sp.shape_functions .* shp{idim};
      end
    end

    if (gradient)
      for idim = 1:msh.ndim
        shape_fun_grad = shg{idim};
        for jdim = setdiff (1:msh.ndim, idim)
          shape_fun_grad = shape_fun_grad .* shp{jdim};
        end
        sp.shape_function_gradients(idim,:,:,:) = shape_fun_grad;
      end
      sp.shape_function_gradients = reshape (sp.shape_function_gradients, ...
                                    msh.ndim, msh.nqn, sp.nsh_max, msh.nel);
    end

% To be changed with isprop, as soon as classdef is working
    if (hessian && isfield (struct (msh), 'geo_map_der2'))
      for idim = 1:msh.ndim
        shape_fun_hess = shh{idim};
        for jdim = setdiff (1:msh.ndim, idim)
          shape_fun_hess = shape_fun_hess .* shp{jdim};
        end
        sp.shape_function_hessians(idim,idim,:,:,:) = shape_fun_hess;

        for jdim = setdiff (1:msh.ndim, idim)
          shape_fun_hess = shg{idim} .* shg{jdim};
          for kdim = setdiff (1:msh.ndim, [idim, jdim])
            shape_fun_hess = shape_fun_hess .* shp{kdim};
          end
          sp.shape_function_hessians(idim,jdim,:,:,:) = shape_fun_hess;
        end
      end
    end

    if (strcmpi (sp.space_type, 'NURBS'))
      sp = bsp_2_nrb__ (sp, msh, sp.weights);
    end
  end

end


function g = polyardKernGradient(kern, x, varargin)

% POLYARDKERNGRADIENT Gradient of POLYARD kernel's parameters.
% FORMAT
% DESC computes the gradient of functions with respect to the
% automatic relevance determination polynomial
% kernel's parameters. As well as the kernel structure and the
% input positions, the user provides a matrix PARTIAL which gives
% the partial derivatives of the function with respect to the
% relevant elements of the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x : the input locations for which the gradients are being
% computed.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The argument takes
% the form of a square matrix of dimension  numData, where numData is
% the number of rows in X.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters. The ordering of the vector should match
% that provided by the function kernExtractParam.
%
% FORMAT
% DESC computes the derivatives as above, but input locations are
% now provided in two matrices associated with rows and columns of
% the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x1 : the input locations associated with the rows of the
% kernel matrix.
% ARG x2 : the input locations associated with the columns of the
% kernel matrix.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The matrix should
% have the same number of rows as X1 and the same number of columns
% as X2 has rows.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters.
%
% SEEALSO polyardKernParamInit, kernGradient, polyardKernDiagGradient, kernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2005, 2006

% KERN

scales = sparse(diag(sqrt(kern.inputScales)));
xScale = x*scales;
if nargin < 4
  innerProd = xScale*xScale';
else
  xScale2 = varargin{1}*scales;
  innerProd = xScale*xScale2';
end
arg = kern.weightVariance*innerProd+kern.biasVariance;
base = kern.variance*kern.degree*arg.^(kern.degree-1);
baseCovGrad = base.*varargin{end};

g(1) = sum(sum(innerProd.*baseCovGrad));
g(2) = sum(sum(baseCovGrad));
g(3) = sum(sum(varargin{end}.*arg.^kern.degree));

if nargin < 4
  for j = 1:kern.inputDimension
    g(3+j) = sum(sum((x(:, j)*x(:, j)').*baseCovGrad))*kern.weightVariance;
  end
else
  for j = 1:kern.inputDimension
    g(3+j) = sum(sum((x(:, j)*varargin{1}(:, j)').*baseCovGrad))*kern.weightVariance;
  end
end


function solution = MP_GWO(UAV, SearchAgents, Max_iter)
%MP_GWO 多种群灰狼优化算法
%Multi Population Gray Wolf Optimization

% 超参数
g = 50;       % 动态更新加权系数

% 算法初始化
[WolfPops, ~] = PopsInit(UAV, SearchAgents, false);   % 随机生成 初始狼群
ClassPops = PopsCluster(WolfPops, UAV);                    % 进行初始聚类（用来获得 k 值）
dim = WolfPops.PosDim;                                                 % 状态变量维度
cSearchAgents = ClassPops.SearchAgents;                    % 搜索智能体个数（子种群数量）
SearchAgents = cSearchAgents * ClassPops.k;              % 对所有智能体个数进行修正（k的整数倍）
WolfPops.Pos = WolfPops.Pos(1:SearchAgents, :);       % 对种群进行修正

% 报错
if cSearchAgents < 4
    error('搜索智能体个数过少')
end

% 初始化解
Alpha_pos = zeros(ClassPops.k, dim);          % α解
Alpha_score = 1 ./ zeros(ClassPops.k, 1);     % α解适应度

Beta_pos = zeros(ClassPops.k, dim);            % β解
Beta_score = 1 ./ zeros(ClassPops.k, 1);       % β解适应度

Delta_pos = zeros(ClassPops.k, dim);           % δ解
Delta_score = 1 ./ zeros(ClassPops.k, 1);      % δ解适应度

Fitness_list = zeros(ClassPops.k, Max_iter);  % 适应度曲线

Pops.PosDim = WolfPops.PosDim;  % 子种群
Pops.lb = WolfPops.lb;
Pops.ub = WolfPops.ub;

% 迭代求解
tic
fprintf('>>MP-GWO 优化中    00.00%%')
for iter = 1 : Max_iter

    % ①  更新参数a
    a = 2 - iter * 2 / Max_iter;               % 线性递减
    %a = 2 * cos((iter/Max_iter)*pi/2);   % 非线性递减

    % ②  聚类
    if iter > 1 %初始化时聚完一次了，节省计算量
    ClassPops = PopsCluster(WolfPops, UAV);
    end
    for k = 1 : ClassPops.k
        Positions = ClassPops.Pos{k};

        % ③  寻找 α、β、δ 狼
        for i = 1 : cSearchAgents
            % 读取目标函数
            fitness = ClassPops.Fitness(k, i);

            % 更新 Alpha、Beta 和 Delta 解
            if fitness <= Alpha_score(k)  % 适应能力最强（因为性能指标越小越好，因此为小于号）
                Alpha_score(k) = fitness;
                Alpha_pos(k, :) = Positions(i, :);
            end
            if fitness > Alpha_score(k) && fitness <= Beta_score(k)
                Beta_score(k) = fitness;
                Beta_pos(k, :) = Positions(i, :);
            end
            if fitness > Alpha_score(k) && fitness > Beta_score(k) && fitness <= Delta_score(k)
                Delta_score(k) = fitness;
                Delta_pos(k, :) = Positions(i, :);
            end
        end

        % ④  更新位置（朝着前三只狼位置前进）
        for i = 1 : cSearchAgents
            for j = 1 : dim

                r1 = rand();
                r2 = rand();
                A1 = 2*a*r1 - a;
                C1 = 2*r2;
                D_alpha = abs(C1*Alpha_pos(k, j) - Positions(i, j));
                X1 = Alpha_pos(k, j) - A1*D_alpha;

                r1 = rand();
                r2 = rand();
                A2 = 2*a*r1 - a;
                C2 = 2*r2;
                D_beta = abs(C2*Beta_pos(k, j) - Positions(i, j));
                X2 = Beta_pos(k, j) - A2*D_beta;

                r1 = rand();
                r2 = rand();
                A3 = 2*a*r1 - a;
                C3 = 2*r2;
                D_delta = abs(C3*Delta_pos(k, j) - Positions(i, j));
                X3 = Delta_pos(k, j) - A3*D_delta;

                % 静态更新
                Positions(i, j) = (X1 + X2 + X3) / 3;
                % 动态更新
                %                 q = g * a; %阈值
                %                 if abs(Alpha_score(k)-Delta_score(k)) > q
                %                     Sum_score = Alpha_score(k) + Beta_score(k) + Delta_score(k);
                %                     Positions(i, j) = (Alpha_score(k)*X1 + Beta_score(k)*X2 + Delta_score(k)*X3) / Sum_score;
                %                 else
                %                     Positions(i, j) = (X1 + X2 + X3) / 3;
                %                 end

            end
        end

    % ⑤  调整不符合要求的状态变量
    Pops.Pos = Positions;
    ProbPoints =  ClassPops.ProbPoints{k};
    [Pops, ~] = BoundAdjust(Pops, ProbPoints, UAV);

    % ⑥  存储适应度
    Fitness_list(k, iter) = Alpha_score(k);

    % ⑦  合并种群
    WolfPops.Pos(cSearchAgents*(k-1)+1:cSearchAgents*k, :) = Pops.Pos;

    end

    if iter/Max_iter*100 < 10
        fprintf('\b\b\b\b\b%.2f%%', iter/Max_iter*100)
    else
        fprintf('\b\b\b\b\b\b%.2f%%', iter/Max_iter*100)
    end
end
fprintf('\n\n>>计算完成！\n\n')
toc

% 寻找 α β δ 位置
n = 3;
A = ClassPops.Fitness;
t = findmin(A, n);
index = cSearchAgents * (t(:, 1) - 1) + t(:, 2);

real_Alpha_no = index(1);
real_Beta_no = index(2);
real_Delta_no = index(3);
Alpha_Data = ClassPops.Data{t(1, 1)}{t(1, 2)}  ;

% 输出值
solution.method = 'MP-GWO';                                 % 算法
% solution.ClassPops = ClassPops;                               % 分类信息
solution.WolfPops = WolfPops;                                % 所有解种群信息
solution.Tracks = Pops2Tracks(WolfPops, UAV);    % 所有解航迹信息
solution.Fitness_list = mean(Fitness_list, 1);            % 所有α解的平均适应度曲线
solution.Alpha_Data = Alpha_Data;                          % 真 · α 的威胁信息
solution.Alpha_no = real_Alpha_no;                          % 真 · α 的位置
solution.Beta_no = real_Beta_no;                               % 真 · β 的位置
solution.Delta_no = real_Delta_no;                            % 真 · δ 的位置

end

%% 寻找A矩阵中最小n个数的位置
function t = findmin(A, n)
    t = sort(A(:));
    [x, y] = find(A <= t(n), n);
    t = [x, y];         % 前n个最小项在矩阵A中的位置[行,列]
    B = zeros(n, 1);
    for i = 1 : n
        B(i) = A(t(i, 1), t(i, 2));
    end
    [~, index] = sort(B);
    t = t(index, :); % 前n个从小到大排序的位置
end

%% 对种群进行聚类
function [ClassPops] = PopsCluster(WolfPops, UAV)

SearchAgents = size(WolfPops.Pos, 1);  % 智能体个数
Dim = WolfPops.PosDim;                        % 智能体维度
Tracks = Pops2Tracks(WolfPops, UAV); % 智能体转换成航迹信息

% 计算适应度
o_Fitness = zeros(SearchAgents, 1); % 60*1
o_subF = []; % 5*60
o_ProbPoints = cell(SearchAgents, 1);  % 60*1
o_Data = cell(SearchAgents, 1); % 60*1

% parfor 并行计算适应度
for i = 1:SearchAgents
    [fitness, subF, Data] = ObjFun(Tracks{i}, UAV);
    o_ProbPoints{i} = Data.ProbPoint;  %cell-cell
    o_Data(i) = {Data}; %cell-struct
    o_Fitness(i) = fitness; %vector-var
    o_subF = [o_subF, subF];
end

% 分类
k = size(subF, 1);                                              % 分 k 类（由objfun决定）
cSearchAgents = floor(SearchAgents / k);    % 并行智能体个数
cFitness = zeros(k, cSearchAgents);               % 保存每类的适应度
cPositions = cell(k, 1);                                     % 保存每类的位置信息
cTracks = cell(k, 1);                                          % 保存每类的航迹信息
cProbPoints = cell(k, 1);                                  % 保存每类的有问题航迹点
cData = cell(k, 1);                                             % 存储每类的检测报告

% 排序
[~, Index] = sort(o_subF, 2, "ascend") ;     % 沿维度2升序排序，返回新矩阵和序号
                                                                       % fitness越小越好
% 聚类
for i = 1:k
    Positions = zeros(cSearchAgents, Dim);
    batchTrack = cell(cSearchAgents, 1);
    batchProbPoints = cell(cSearchAgents, 1);
    batchData = cell(cSearchAgents, 1);
    for j = 1:cSearchAgents
        idx = Index(i, j);
        cFitness(i, j) = o_Fitness(idx); %mat-vector
        Positions(j, :) = WolfPops.Pos(idx, :); %mat-mat
        batchTrack{j} = Tracks{idx}; %cell-cell
        batchProbPoints{j} = o_ProbPoints{idx}; %cell-cell
        batchData{j} = o_Data{idx}; %cell-cell
    end
    cPositions(i) = {Positions}; %cell-mat
    cTracks{i} = batchTrack; %cell-cell
    cProbPoints{i} = batchProbPoints; %cell-cell
    cData{i} = batchData; %cell-cell
end

% 输出
ClassPops.Pos = cPositions;
ClassPops.Tracks = cTracks;
ClassPops.ProbPoints = cProbPoints;
ClassPops.Data = cData;
ClassPops.Fitness = cFitness;
ClassPops.SearchAgents = cSearchAgents;
ClassPops.k = k;
end


function w_omega = rotationalPID_velocity(w_R_b,w_R_b_des,w_omega_des,Kp)

    % ROTATIONALPID_VELOCITY implements a controller trajectory tracking inside
    %                        the group SO(3). The angular velocity is assumed
    %                        to be a control input.
    %
    % FORMAT: w_omega = rotationalPID_velocity(w_R_b,w_R_b_des,w_omega_des,Kp)
    %
    % INPUT: - w_R_b       = [3 * 3] rotation matrix
    %        - w_R_b_des   = [3 * 3] desired rotation matrix
    %        - w_omega     = [3 * 1] angular velocity (expressed in the world frame)
    %        - w_omega_des = [3 * 1] desired angular velocity (expressed in the world frame)
    %        - Kp          = [3 * 3] orientation gains
    %
    % OUTPUT: - w_omega    = [3 * 1] input angular velocity
    %
    % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava
    %
    %          all authors are with the Italian Istitute of Technology (IIT)
    %          email: name.surname@iit.it
    %
    % Genoa, Dec 2017
    %

    %% --- Initialization ---

    skv      = wbc.skewVee(w_R_b*transpose(w_R_b_des));
    w_omega  = w_omega_des -Kp*skv;
end


function XYZ = rspd2xyz(varargin)
%RSPD2XYZ Convert relative spectral power density to XYZ.
%   RSPD2XYZ(lambda,phi) converts a relative spectral power density
%   function to 1931 CIE XYZ tristimulus values. The inputs, lambda and
%   phi, are both vectors with the same length. The input lambda is the
%   wavelength (in nm). The input phi is the relative spectral power
%   density.
%
%   RSPD2XYZ(S) converts a relative spectral power density function
%   represented as either a Px2 numeric matrix or a table. If S is a
%   numeric matrix, then S(:,1) is the wavelength, lambda, and S(:,2) is
%   phi. If S is a table with a variable called "lambda", then that
%   variable is used as lambda and the table's other variable is used as
%   phi. If the table does not have a variable named "lambda", then the
%   table's first variable is used as lambda, and the second variable is
%   used as phi.
%
%   RSPD2XYZ(I,R) converts an illumination spectral power density curve
%   (I) and a relative reflectance curve (R) to tristimulus values. I
%   and R can each be either a Px2 matrix or a table.

%   Written by Steve Eddins to accompany Digital Image Processing Using
%   MATLAB, 3rd edition, Gatesmark Press, 2020,
%   http://imageprocessingplace.com.
%
%   Copyright 2019 The MathWorks, Inc.
%   License: https://github.com/mathworks/matlab-color-tools/blob/master/license.txt

narginchk(1,2);

if nargin == 2
   if isvector(varargin{1}) && isvector(varargin{2})
      % rspd2xyz(lambda,phi)
      % Treat the input as I (the illumination curve). Construct the
      % reflectance curve as a constant from lambda = 300 to lambda =
      % 830.
      lambda = varargin{1};
      phi = varargin{2};
      I = [lambda(:) phi(:)];
      R = [300 1; 830 1];
   else
      % rspd2xyz(I,R)
      I = varargin{1};
      R = varargin{2};
   end
else
   % rspd2xyz(S)
   % Treat the input as I (the illumination curve). Construct the
   % reflectance curve as a constant from lambda = 300 to lambda = 830.
   I = varargin{1};
   R = [300 1; 830 1];
end

% Convert the inputs, in either Px2 matrix or table form, to function
% handles that return phi(lambda) for any lambda using spline
% interpolation and constant extrapolation.
fI = phiFunction(I);
fR = phiFunction(R);

matching_fcns = colorMatchingFunctions;

L = matching_fcns.lambda;
x = matching_fcns.x;
y = matching_fcns.y;
z = matching_fcns.z;
x = x/(100*sum(x));
y = y/(100*sum(y));
z = z/(100*sum(z));

% Perform a simple numerical integration.
XYZ = sum(fI(L) .* fR(L) .* [x y z],1);

%----------------------------------------------------------------------%
function f = phiFunction(S)
%   Create a function handle that returns phi(lambda). The input can be
%   a Px2 numeric matrix or a table as described in the documentation
%   above for rspd2xyz.

if istable(S)
   % Look for table variable called lambda.
   idx = find(S.Properties.VariableNames == "lambda",1);
   if isempty(idx)
      % The table does not have a lambda variable. Assume the first
      % variable is lambda and the second variable is phi.
      lambda = S{:,1};
      phi = S{:,2};
   else
      lambda = S{:,idx};

      % Use the first non-lambda table variable as phi.
      kk = 1:width(S);
      kk(idx) = [];
      phi = S{:,kk(1)};
   end
else
   % S is a matrix. Use the first column as lambda and the second
   % column as phi.
   S = double(S);
   lambda = S(:,1);
   phi = S(:,2);
end

% Add zeros to the beginning and end of phi. When using the spline
% function for cubic spline interpolation, this has the effect of
% enabling constant extrapolation using the first and last values of
% phi. See section 7.2.2.1, Extrapolation, CIE 15:2004, Technical Report
% on Colorimetry.
phi = [0 ; phi ; 0];

% Create a function handle that interpolates phi using cubic spline
% interpolation.  Cubic spline is one of the recommended interpolation
% methods. See section 7.2.1.1, Interpolation, CIE 15:2004, Technical
% Report on Colorimetry.
f = @(lambda_q) spline(lambda,phi,lambda_q);


classdef TiGE2 < ALGORITHM
% <many> <real/integer/label/binary/permutation> <constrained/none>
% Tri-Goal Evolution Framework for CMaOPs

%------------------------------- Reference --------------------------------
% Y. Zhou, Z. Min, J. Wang, Z. Zhang, and J.Zhang, Tri-goal evolution
% framework for constrained many-objective optimization, IEEE Transactions
% on Systems Man and Cybernetics Systems, 2020, 50(8): 3086-3099.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [Epsilon0,row] = Algorithm.ParameterSet(0.05,1.01);

            %% Generate random population
            Population = Problem.Initialization();
            [fpr,fcd]  = Estimation(Population.objs,1/Problem.N^(1/Problem.M));
            fcv        = Calculate_fcv(Population);
            Epsilon    = Epsilon0;
            PopObj_1   = [fpr,fcd];
            [fm,~]     = NDSort(PopObj_1,Problem.N);
            PopObj     = [fm' + Epsilon * fcv,fcv];
            [frank,~]  = NDSort(PopObj,Problem.N);
            fitness    = frank' + fcv./(fcv+1);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool = TournamentSelection(2,Problem.N,fitness);
                Offspring  = OperatorGA(Problem,Population(MatingPool));
                [fpr,fcd]  = Estimation(Offspring.objs,1/Problem.N^(1/Problem.M));
                fcv = Calculate_fcv(Offspring);
                OffObj_1   = [fpr,fcd];
                [fm,~] = NDSort(OffObj_1,Problem.N);
                OffObj = [fm' + Epsilon * fcv,fcv];
                [Population,fitness] = EnvironmentalSelection([Population,Offspring],PopObj,OffObj,Problem.N);
                [fpr,fcd] = Estimation(Population.objs,1/Problem.N^(1/Problem.M));
                fcv = Calculate_fcv(Population);
                PopObj_1 = [fpr,fcd];
                [fm,~]   = NDSort(PopObj_1,Problem.N);
                PopObj   = [fm' + Epsilon * fcv,fcv];
                Epsilon  = row * Epsilon;
            end
        end
    end
end


function [mixparams, y, z, a] = mdnfwd(net, x)
%MDNFWD	Forward propagation through Mixture Density Network.
%
%	Description
%	 MIXPARAMS = MDNFWD(NET, X) takes a mixture density network data
%	structure NET and a matrix X of input vectors, and forward propagates
%	the inputs through the network to generate a structure MIXPARAMS
%	which contains the parameters of several mixture models.   Each row
%	of X represents one input vector and the corresponding row of the
%	matrices in MIXPARAMS  represents the parameters of a mixture model
%	for the conditional probability of target vectors given the input
%	vector.  This is not represented as an array of GMM structures to
%	improve the efficiency of MDN training.
%
%	The fields in MIXPARAMS are
%	  type = 'mdnmixes'
%	  ncentres = number of mixture components
%	  dimtarget = dimension of target space
%	  mixcoeffs = mixing coefficients
%	  centres = means of Gaussians: stored as one row per pattern
%	  covars = covariances of Gaussians
%	  nparams = number of parameters
%
%	[MIXPARAMS, Y, Z] = MDNFWD(NET, X) also generates a matrix Y of the
%	outputs of the MLP and a matrix Z of the hidden unit activations
%	where each row corresponds to one pattern.
%
%	[MIXPARAMS, Y, Z, A] = MLPFWD(NET, X) also returns a matrix A  giving
%	the summed inputs to each output unit, where each row  corresponds to
%	one pattern.
%
%	See also
%	MDN, MDN2GMM, MDNERR, MDNGRAD, MLPFWD
%

%	Copyright (c) Ian T Nabney (1996-2001)
%	David J Evans (1998)

% Check arguments for consistency
errstring = consist(net, 'mdn', x);
if ~isempty(errstring)
  error(errstring);
end

% Extract mlp and mixture model descriptors
mlpnet = net.mlp;
mixes = net.mdnmixes;

ncentres = mixes.ncentres;	% Number of components in mixture model
dim_target = mixes.dim_target;	% Dimension of targets
nparams = mixes.nparams;	% Number of parameters in mixture model

% Propagate forwards through MLP
[y, z, a] = mlpfwd(mlpnet, x);

% Compute the postion for each parameter in the whole
% matrix.  Used to define the mixparams structure
mixcoeff  = [1:1:ncentres];
centres   = [ncentres+1:1:(ncentres*(1+dim_target))];
variances = [(ncentres*(1+dim_target)+1):1:nparams];

% Convert output values into mixture model parameters

% Use softmax to calculate priors
% Prevent overflow and underflow: use same bounds as glmfwd
% Ensure that sum(exp(y), 2) does not overflow
maxcut = log(realmax) - log(ncentres);
% Ensure that exp(y) > 0
mincut = log(realmin);
temp = min(y(:,1:ncentres), maxcut);
temp = max(temp, mincut);
temp = exp(temp);
mixpriors = temp./(sum(temp, 2)*ones(1,ncentres));

% Centres are just copies of network outputs
mixcentres =  y(:,(ncentres+1):ncentres*(1+dim_target));

% Variances are exp of network outputs
mixwidths = exp(y(:,(ncentres*(1+dim_target)+1):nparams));

% Now build up all the mixture model weight vectors
ndata = size(x, 1);

% Return parameters
mixparams.type = mixes.type;
mixparams.ncentres = mixes.ncentres;
mixparams.dim_target = mixes.dim_target;
mixparams.nparams = mixes.nparams;

mixparams.mixcoeffs = mixpriors;
mixparams.centres   = mixcentres;
mixparams.covars      = mixwidths;


function gradu = singletrack_gradu_f(in1,in2,in3)
%SINGLETRACK_GRADU_F
%    GRADU = SINGLETRACK_GRADU_F(IN1,IN2,IN3)

%    This function was generated by the Symbolic Math Toolbox version 8.3.
%    09-Jan-2020 11:52:34

I_z = in3(2,:);
M = in3(1,:);
T = in2(2,:);
V_vx = in1(4,:);
V_vy = in1(5,:);
c_f = in3(8,:);
delta = in2(1,:);
deltamax = in3(5,:);
l_f = in3(3,:);
maxbrakeWForce = in3(6,:);
maxmotorWForce = in3(7,:);
psi_dot = in1(6,:);
t2 = 1.0./I_z;
t3 = 1.0./M;
t4 = T.*5.0e+1;
t5 = delta.*5.0e+1;
t6 = deltamax.*5.0e+1;
t8 = V_vy.*1i;
t9 = V_vx.*1.0e+2;
t13 = l_f.*psi_dot.*1i;
t7 = -t4;
t10 = -t5;
t11 = -t6;
t12 = tanh(t9);
t24 = V_vx+t8+t13;
t14 = t7+5.0e+1;
t15 = t7-5.0e+1;
t18 = t6+t10;
t21 = t10+t11;
t26 = angle(t24);
t16 = exp(t14);
t17 = exp(t15);
t19 = exp(t18);
t22 = exp(t21);
t20 = t16+1.0;
t23 = t17+1.0;
t25 = t19+1.0;
t29 = t22+1.0;
t27 = 1.0./t20;
t30 = 1.0./t23;
t32 = 1.0./t25;
t34 = 1.0./t29;
t28 = t27.^2;
t31 = t30.^2;
t33 = t32.^2;
t35 = t34.^2;
t36 = t27.*5.0e+1;
t37 = t27-1.0;
t38 = t30.*5.0e+1;
t40 = deltamax.*t32;
t41 = t32-1.0;
t43 = t34-1.0;
t39 = -t36;
t42 = -t38;
t44 = -t40;
t45 = deltamax.*t43;
t47 = t16.*t28.*5.0e+1;
t49 = t17.*t31.*5.0e+1;
t50 = t16.*t28.*2.5e+3;
t53 = t17.*t31.*2.5e+3;
t55 = t6.*t19.*t33;
t56 = deltamax.*t19.*t33.*-5.0e+1;
t57 = t6.*t22.*t35;
t58 = t30.*t37;
t59 = deltamax.*t22.*t35.*-5.0e+1;
t61 = t37.*t38;
t64 = t34.*t41;
t67 = t4.*t16.*t28.*t30;
t69 = t4.*t17.*t31.*t37;
t70 = t5.*t19.*t33.*t34;
t72 = t5.*t22.*t35.*t41;
t46 = -t45;
t48 = -t47;
t51 = -t49;
t52 = -t50;
t54 = -t53;
t60 = T.*t58;
t63 = t4.*t58;
t65 = delta.*t64;
t68 = T.*t30.*t50;
t71 = T.*t37.*t53;
t90 = t56+t59+t64+t70+t72;
t62 = -t60;
t66 = -t65;
t74 = t39+t42+t63+5.0e+1;
t86 = t26+t44+t46+t65;
t87 = t48+t51+t58+t67+t69;
t89 = t52+t54+t61+t68+t71;
t73 = t30+t37+t62;
t75 = exp(t74);
t77 = t40+t45+t66;
t76 = t75+1.0;
t78 = sin(t77);
t81 = cos(t77);
t79 = 1.0./t76;
t80 = t79.^2;
t82 = maxmotorWForce.*t79;
t83 = t79-1.0;
t84 = maxbrakeWForce.*t12.*t83;
t91 = maxmotorWForce.*t75.*t80.*t89;
t92 = maxbrakeWForce.*t12.*t75.*t80.*t89;
t85 = -t84;
t93 = -t92;
t88 = t82+t85;
t94 = t91+t93;
gradu = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t3.*(c_f.*t78.*t90-c_f.*t81.*t86.*t90+(t73.*t78.*t88.*t90)./2.0),-t3.*((t73.*t94)./2.0+(t87.*t88)./2.0+(t73.*t81.*t94)./2.0+(t81.*t87.*t88)./2.0),0.0,-t3.*(c_f.*t81.*t90+c_f.*t78.*t86.*t90+(t73.*t81.*t88.*t90)./2.0),-t3.*((t73.*t78.*t94)./2.0+(t78.*t87.*t88)./2.0),0.0,-t2.*(c_f.*l_f.*t81.*t90+c_f.*l_f.*t78.*t86.*t90+(l_f.*t73.*t81.*t88.*t90)./2.0),-t2.*((l_f.*t73.*t78.*t94)./2.0+(l_f.*t78.*t87.*t88)./2.0),0.0,0.0,0.0,1.0],[3,7]);


function g = diffy(f, n)
%DIFFY   Differentiate a CHEBFUN2 with respect to its second argument.
%
%   G = DIFFY(F) returns a CHEBFUN2 representing the derivative of F in its
%   second argument. This is the same as DIFF(F,1,1).
%
%   G = DIFFY(F,N) returns a CHEBFUN2 representing the Nth derivative of F in
%   its second argument. This is the same as DIFF(F,N,1).
%
%   This command is for convenience as the syntax for DIFF, inherited from the
%   DIFF command for matrices, can be confusing.
%
% See also DIFFX, DIFF.

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

% Default to first derivative:
if ( nargin == 1 )
    n = 1;
end

% Call diff:
g = diff(f, n, 1);

end


function G_vctr_sym = G_vctr_fcn(in1,in2)
%G_VCTR_FCN
%    G_VCTR_SYM = G_VCTR_FCN(IN1,IN2)

%    This function was generated by the Symbolic Math Toolbox version 8.2.
%    19-Oct-2021 16:04:40

pi171 = in2(17,:);
pi191 = in2(19,:);
pi271 = in2(27,:);
pi291 = in2(29,:);
pi301 = in2(30,:);
pi371 = in2(37,:);
pi391 = in2(39,:);
pi401 = in2(40,:);
pi471 = in2(47,:);
pi481 = in2(48,:);
pi491 = in2(49,:);
pi501 = in2(50,:);
pi571 = in2(57,:);
pi581 = in2(58,:);
pi591 = in2(59,:);
pi601 = in2(60,:);
q2 = in1(2,:);
q3 = in1(3,:);
q4 = in1(4,:);
q5 = in1(5,:);
q6 = in1(6,:);
t2 = sin(1.57079632679);
t3 = cos(q3);
t4 = sin(q2);
t5 = cos(q2);
t6 = sin(q3);
t7 = t2.*t4.*(1.81e2./1.0e3);
t8 = t7-6.13e2./1.0e3;
t9 = t2.*t3.*t5.*(9.81e2./1.0e2);
t15 = t2.*t4.*t6.*(9.81e2./1.0e2);
t10 = t9-t15;
t11 = t2.*t5.*t6.*(1.81e2./1.0e3);
t12 = t2.*t5.*t6.*(9.81e2./1.0e2);
t13 = t2.*t3.*t4.*(9.81e2./1.0e2);
t14 = t12+t13;
t16 = cos(q4);
t17 = sin(q4);
t18 = t2.*t14.*t16;
t19 = t2.*t10.*t17;
t20 = t18+t19;
t21 = t3.*t8;
t22 = t6.*t8;
t23 = t2.*t3.*t5.*(1.81e2./1.0e3);
t24 = t22-t23;
t25 = t11+t21-5.71e2./1.0e3;
t26 = cos(q6);
t27 = cos(q5);
t28 = sin(q6);
t29 = t2.*t10.*t16;
t40 = t2.*t14.*t17;
t30 = t29-t40;
t31 = sin(q5);
t32 = t31.*(8.7e1./5.0e2);
t33 = t2.*t16.*t25;
t50 = t2.*t17.*t24;
t34 = t33-t50;
t35 = t27.*t34;
t36 = t32+t35;
t37 = t2.*t16.*t24;
t38 = t2.*t17.*t25;
t39 = t37+t38+3.0./2.5e1;
t41 = t2.*t3.*t4.*(1.81e2./1.0e3);
t42 = t11+t41;
t44 = t2.*t4.*t6.*(1.81e2./1.0e3);
t43 = t23-t44;
t45 = t2.*t16.*t42;
t46 = t2.*t17.*t43;
t47 = t45+t46;
t48 = t2.*t16.*t43;
t51 = t2.*t17.*t42;
t49 = t48-t51;
t52 = t27.*(8.7e1./5.0e2);
t68 = t31.*t34;
t53 = t52-t68;
t54 = t30.*t31.*t53;
t55 = t31.^2;
t56 = t20.*t28;
t72 = t26.*t27.*t30;
t57 = t56-t72;
t58 = t20.*t26;
t59 = t27.*t28.*t30;
t60 = t58+t59;
t61 = t37+t38;
t62 = t20.*t61;
t63 = t11+t21;
t64 = t27.^2;
t65 = t2.*t17.*t63;
t66 = t37+t65;
t67 = t27.*t30.*t36;
t69 = pi371.*t30;
t70 = pi391.*t20;
t71 = pi491.*t20;
t73 = pi591.*t60;
t74 = pi271.*t14;
t77 = t2.*t16.*t63;
t75 = t50-t77;
t76 = t30.*t34;
t78 = t28.*t30;
t79 = t20.*t26.*t27;
t80 = t78+t79;
t81 = t26.*t30;
t91 = t20.*t27.*t28;
t82 = t81-t91;
t83 = t28.*t36;
t84 = t26.*t39;
t85 = t83+t84;
t86 = t26.*t36;
t92 = t28.*t39;
t87 = t86-t92;
t88 = t57.*t87;
t89 = t20.*t55.*t66;
t90 = pi471.*t27.*t30;
t93 = t20.*t55.*t61;
G_vctr_sym = [0.0;t69+t70+t71+t73+t74+t90-pi291.*t10-pi571.*t57+pi301.*(-t10.*t42+t14.*t43+t10.*t63+t14.*(t22-t2.*t3.*t5.*(1.81e2./1.0e3)))-pi601.*(t54+t88+t80.*(t28.*t47-t26.*t27.*t49)+t82.*(t26.*t47+t27.*t28.*t49)-t60.*t85-t20.*t49.*t55)+pi401.*(t62+t76+t20.*t49-t30.*t47)+pi501.*(-t54+t62+t67-t30.*t47+t20.*t49.*t55+t20.*t49.*t64)+pi171.*t2.*t4.*(9.81e2./1.0e2)-pi191.*t2.*t5.*(9.81e2./1.0e2)-pi481.*t30.*t31-pi581.*t30.*t31;t69+t70+t71+t73+t74+t90-pi601.*(t54+t88+t89-t80.*(t28.*t75-t26.*t27.*t66)-t82.*(t26.*t75+t27.*t28.*t66)-t60.*t85)-pi291.*t10-pi571.*t57+pi401.*(t62+t76-t20.*t66+t30.*t75)-pi501.*(t54-t62-t67+t89-t30.*t75+t20.*t64.*t66)-pi481.*t30.*t31-pi581.*t30.*t31;t69+t70+t71+t73+t90-pi601.*(t54+t88+t93+t80.*(t28.*t34+t26.*t27.*t61)+t82.*(t26.*t34-t27.*t28.*t61)-t60.*t85)-pi571.*t57-pi501.*(t54-t62-t67+t76+t93+t20.*t61.*t64)-pi481.*t30.*t31-pi581.*t30.*t31;-pi601.*(-t20.*t31.*t36+t20.*t27.*t53-t26.*t53.*t80+t28.*t53.*t82+t20.*t26.*t31.*t87+t20.*t28.*t31.*t85)-pi471.*t20.*t31-pi481.*t20.*t27-pi581.*t20.*t27-pi571.*t20.*t26.*t31-pi591.*t20.*t28.*t31;pi571.*t82+pi591.*t80];


% SP_EVAL: Compute the value or the derivatives of a function, given by its degrees of freedom, at a given set of points.
%
%   [eu, F] = sp_eval (u, space, geometry, pts, [options]);
%   [eu, F] = sp_eval (u, space, geometry, npts, [options]);
%
% INPUT:
%
%     u:           vector of dof weights
%     space:       object defining the discrete space (see sp_scalar)
%     geometry:    geometry structure (see geo_load)
%     pts:         cell array with coordinates of points along each parametric direction
%     npts:        number of points along each parametric direction
%     options:     cell array with the fields to plot
%                   accepted options are 'value' (default), 'gradient' and 'laplacian'
%
% OUTPUT:
%
%     eu: cell-array with the fields evaluated at the given points
%     F:  grid points in the physical domain, that is, the mapped points
%
% Copyright (C) 2009, 2010 Carlo de Falco
% Copyright (C) 2011, 2012, 2014, 2015, 2018 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function [eu, F] = sp_eval (u, space, geometry, npts, options)

  if (numel (u) ~= space.ndof)
    error ('The number of degrees of freedom of the vector and the space do not match')
  end

  if (nargin < 5)
    options = {'value'};
  end
  if (~iscell (options))
    options = {options};
  end
  nopts = numel (options);

  ndim = numel (space.knots);

  endpoints = zeros (2, ndim);
  if (isfield (geometry, 'nurbs'))
    nurbs = geometry.nurbs;
    if (ndim == 1)
      nurbs.knots = {nurbs.knots};
    end
    for idim=1:ndim
      endpoints(:,idim) = nurbs.knots{idim}([nurbs.order(idim), end-nurbs.order(idim)+1]);
    end
    clear nurbs
  elseif (isfield (struct(space), 'knots'))
    degree = space.degree;
    for idim=1:ndim
      endpoints(:,idim) = space.knots{idim}([degree(idim)+1, end-degree(idim)]);
    end
  else
    endpoints(2,:) = 1;
  end

  if (iscell (npts))
    pts = npts;
    npts = cellfun (@numel, pts);
  elseif (isvector (npts))
    if (numel (npts) == 1)
      npts = npts * ones (1,ndim);
    end
    for idim = 1:ndim
      pts{idim} = linspace (endpoints(1,idim), endpoints(2,idim), npts(idim));
    end
  end

  for jj = 1:ndim
    pts{jj} = pts{jj}(:)';
    if (numel (pts{jj}) > 1)
      brk{jj} = [endpoints(1,jj), pts{jj}(1:end-1) + diff(pts{jj})/2, endpoints(2,jj)];
    else
      brk{jj} = endpoints(:,jj).';
    end
  end

  msh = msh_cartesian (brk, pts, [], geometry, 'boundary', false);
  sp  = space.constructor (msh);

  value = false; grad = false; laplacian = false; hessian = false;

  for iopt = 1:nopts
    switch (lower (options{iopt}))
      case 'value'
        eu{iopt} = zeros (msh.nqn, msh.nel);
        eunum{iopt} = {1:msh.nqn};
        eusize{iopt} = npts;
        value = true;

      case 'gradient'
        eu{iopt} = zeros (msh.rdim, msh.nqn, msh.nel);
        eunum{iopt} = {1:msh.rdim, 1:msh.nqn};
        eusize{iopt} = [msh.rdim, npts];
        grad = true;

      case 'laplacian'
        eu{iopt} = zeros (msh.nqn, msh.nel);
        eunum{iopt} = {1:msh.nqn};
        eusize{iopt} = npts;
        laplacian = true;

      case 'hessian'
        eu{iopt} = zeros (msh.rdim, msh.rdim, msh.nqn, msh.nel);
        eunum{iopt} = {1:msh.rdim, 1:msh.rdim, 1:msh.nqn};
        eusize{iopt} = [msh.rdim, msh.rdim, npts];
        hessian = true;
    end
  end

  F = zeros (msh.rdim, msh.nqn, msh.nel);

  for iel = 1:msh.nel_dir(1)
    msh_col = msh_evaluate_col (msh, iel);
    sp_col  = sp_evaluate_col (sp, msh_col, 'value', value, 'gradient', grad, ...
          'laplacian', laplacian, 'hessian', hessian);

    eu_aux = sp_eval_msh (u, sp_col, msh_col, options);

    F(:,:,msh_col.elem_list) = msh_col.geo_map;
    for iopt = 1:nopts
      eu{iopt}(eunum{iopt}{:},msh_col.elem_list) = eu_aux{iopt};
    end
  end

  F = reshape (F, [msh.rdim, npts]);
  for iopt = 1:nopts
    eu{iopt} = reshape (eu{iopt}, [eusize{iopt}, 1]); % The extra 1 makes things work also in 1D
  end

  if (nopts == 1)
    eu = eu{1};
  end
end


function [favar]=favar_fevdols(gamma_record,It,Bu,n,IRFperiods,FEVDband,favar)

% function [fevd_estimates]=bear.olsfevd(irf_estimates,IRFperiods,gamma,n,endo,datapath)
% computes and displays fevd values for the OLS VAR model
% inputs:  - cell 'irf_estimates': lower bound, point estimates, and upper bound for the IRFs
%          - integer 'IRFperiods': number of periods for IRFs
%          - matrix 'gamma': structural disturbance variance-covariance matrix (defined p 48 of technical guide)
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - cell 'endo': list of endogenous variables of the model
%          - string 'datapath': user-supplied path to excel data spreadsheet
% outputs: - cell 'fevd_estimates': lower bound, point estimates, and upper bound for the FEVD

% preliminary tasks
npltX=favar.npltX;
npltXshck=favar.IRF.npltXshck;

%relevant loadings of restricted information variables
L=favar.L(favar.plotX_index,favar.IRF.plotXshock_index);
% gamma=gamma(:,favar.FEVD.plotXshock_index);
% % scale gamma, irf estimates are already scaled
%         for ii=1:npltX
%             for ll=1:npltXshck
%                 favar_gamma{ii}(:,ll)=L(ii,ll)*gamma(:,ll);
%             end
%         end

% create the first cell
temp=cell(npltX,npltXshck+1);

% start by filling the first column of every Tij matrix in the cell
% loop over rows of temp
for jj=1:npltX
   % loop over columns of temp
   for ii=1:npltXshck
   % square each element
   temp{jj,ii}(:,1)=favar.IRF.favar_irf_estimates{jj,ii}(:,1).^2;
   end
end
% fill all the other entries of the Tij matrices
% loop over rows of temp
for jj=1:npltX
   % loop over columns of temp
   for ii=1:npltXshck
      % loop over remaining columns
      for kk=2:IRFperiods
      % define the column as the square of the corresponding column in orthogonalised_irf_record
      % additioned to the value of the preceeding columns, which creates the cumulation
      temp{jj,ii}(:,kk)=favar.IRF.favar_irf_estimates{jj,ii}(:,kk).^2+temp{jj,ii}(:,kk-1);
      end
   end
end

% multiply each matrix in the cell by the variance of the structural shocks
% to do so, loop over simulations (rows of the Tij matrices)
for kk=1:It-Bu
% recover the covariance matrix of structural shocks gamma for this iteration
gamma=reshape(gamma_record(:,kk),n,n);

gamma=gamma(:,favar.IRF.plotXshock_index);
% scale gamma, irf estimates are already scaled
        for ii=1:npltX
            for ll=1:npltXshck
                favar_gamma{ii}(:,ll)=L(ii,ll)*gamma(:,ll);
            end
        end

% loop over rows of temp
for ii=1:npltX
% loop over columns of temp
   for jj=1:npltXshck
   % multiply column jj of the matrix by the variance of the structural shock
   temp{ii,jj}(1,:)=temp{ii,jj}(1,:)*favar_gamma{ii}(jj,jj);
   end
end
end

% obtain now the values for Ti, the (n+1)th matrix of each row
% loop over rows of temp
for ii=1:npltX
% start the summation over Tij matrices
temp{ii,npltXshck+1}=temp{ii,1};
   % sum over remaining columns
   for jj=2:npltXshck
   temp{ii,npltXshck+1}=temp{ii,npltXshck+1}+temp{ii,jj};
   end
end

% create the output cell fevd_record
favar_fevd_record=cell(npltX,npltXshck);
% fill the cell
% loop over rows of fevd_estimates
for ii=1:npltX
   % loop over columns of fevd_estimates
   for jj=1:npltXshck
   % define the matrix Vfij as the division (pairwise entry) of Tfij by Tfj
   favar_fevd_record{ii,jj}=temp{ii,jj}./temp{ii,npltXshck+1};
   end
end

%% create the FEVD estimates output
% create first the cell that will contain the estimates
favar_fevd_estimates=cell(npltX,npltXshck);

% for each variable and each variable contribution along with each period, compute the median, lower and upper bound from the Gibbs sampler records
% consider variables in turn
for ii=1:npltX
   % consider contributions in turn
   for jj=1:npltXshck
      % consider periods in turn
      for kk=1:IRFperiods
      % compute first the lower bound
      favar_fevd_estimates{ii,jj}(1,kk)=quantile(favar_fevd_record{ii,jj}(:,kk),(1-FEVDband)/2);
      % then compute the median
      favar_fevd_estimates{ii,jj}(2,kk)=quantile(favar_fevd_record{ii,jj}(:,kk),0.5);
      % finally compute the upper bound
      favar_fevd_estimates{ii,jj}(3,kk)=quantile(favar_fevd_record{ii,jj}(:,kk),1-(1-FEVDband)/2);
      end
   end
end

%save output
favar.FEVD.favar_fevd_estimates=favar_fevd_estimates;

% % % % finally, save on excel
% % % if pref.results==1
% % % % create the cell that will be saved on excel
% % % fevd_estimates=fevd_estimates';
% % % fevdcell={};
% % % % build preliminary elements: space between the tables
% % % horzspace=repmat({''},2,3*n);
% % % vertspace=repmat({''},IRFperiods+3,1);
% % % % loop over variables (vertical dimension)
% % % for ii=1:n
% % % tempcell={};
% % %    % loop over shocks (horizontal dimension)
% % %    for jj=1:n
% % %    % create cell of fevd record for variable ii in response to shock jj
% % %    temp=['part of ' endo{ii,1} ' fluctuation due to ' endo{jj,1} ' shocks'];
% % %    fevd_ij=[temp {''};{''} {''};{''} {'median'};num2cell((1:IRFperiods)') num2cell((fevd_estimates{ii,jj})')];
% % %    tempcell=[tempcell fevd_ij vertspace];
% % %    end
% % % fevdcell=[fevdcell;horzspace;tempcell];
% % % end
% % % % trim
% % % fevdcell=fevdcell(3:end,1:end-1);
% % % % write in excel
% % %     xlswrite(fullfile(pref.results_path, [pref.results_sub '.xlsx']),fevdcell,'FEVD','B2');
% % % end


function [mResult, mResult2]=calc_pwtime(a,fMinTime, fMaxTime, fTstep, timef,bootloops,maepi, nMod)
% function [mResult] = calc_pwtime(a,fMinTime, fMaxTime, fTstep, timef,bootloops,maepi, nMod);
% -------------------------------------------------------------------------
% Calculate and plot p,c,k-value evolution with time
%
% Input variables:
% a         : earthquake catalog
% fMinTime  : Starting time after mainshock to calculate Omori-parameters
%             in days
% fMaxTime  : End time after mainshock to calculate Omori-parameters
%             in days
% fTstep    : Time step length fMinTime:fTstep:fMaxTime [days]
% timef     : dummy for forecast period, usually =1
% bootloops : Number of bootstraps
% maepi     : mainshock
% nMod      : Models to fit aftershock sequence
%
% j.woessner@sed.ethz.ch
% last update: 29.09.2004

% Surpress warnings from fmincon
warning off;

% Initialize
mResult = [];
mResult2 = [];

% Time loop
for time = fMinTime:fTstep:fMaxTime
    %[m_main, main] = max(a.Magnitude);
    date_matlab = datenum(a.Date.Year,a.Date.Month,a.Date.Day,a.Date.Hour,a.Date.Minute,zeros(size(a,1),1));
    date_main = datenum(floor(maepi(3)),maepi(4),maepi(5),maepi(8),maepi(9),0);
    time_aftershock = date_matlab-date_main;
    % Select biggest aftershock earliest in time, but more than 1 day after mainshock
    fDay = 1;
    ft_c=fDay/365; % Time not considered to find biggest aftershock
    vSel = (a.Date > maepi(:,3)+ft_c & a.Date<= maepi(:,3)+time/365);
    mCat = a.subset(vSel);
    vSel = mCat(:,6) == max(mCat(:,6));
    vBigAf = mCat(vSel,:);
    if length(mCat(:,1)) > 1
        vSel = vBigAf(:,3) == min(vBigAf(:,3));
        vBigAf = vBigAf(vSel,:);
    end

    date_biga = datenum(floor(vBigAf(3)),vBigAf(4),vBigAf(5),vBigAf(8),vBigAf(9),0);
    fT1 = date_biga - date_main; % Time of big aftershock

    % Aftershock times
    l = time_aftershock(:) > 0;
    tas = time_aftershock(l);
    eqcatalogue = a.subset(l);

    % time_as: Learning period
    l = tas <= time;
    time_as=tas(l);
    if length(time_as) > 50

        % Times up to the forecast time
        lf = tas <= time+timef ;
        time_asf= [tas(lf) ];
        time_asf=sort(time_asf);

        % Calculate uncertainty and mean values of p,c,and k
        [mMedModF, mStdL, loopout] = brutebootloglike_a2(time_as, time_asf, bootloops,fT1,nMod);
        pmed1 = mMedModF(1,1);
        pstd1 = mMedModF(1,2);
        pmed2 = mMedModF(1,3);
        pstd2 = mMedModF(1,4);
        cmed1 = mMedModF(1,5);
        cstd1 = mMedModF(1,6);
        cmed2 = mMedModF(1,7);
        cstd2 = mMedModF(1,8);
        kmed1 = mMedModF(1,9);
        kstd1 = mMedModF(1,10);
        kmed2 = mMedModF(1,11);
        kstd2 = mMedModF(1,12);

        % Compute model according to model choice
        if nMod == 1
            [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
        elseif nMod == 2
            [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
        elseif nMod == 3
            [pval1, pval2, cval1, cval2, kval1, kval2 , fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
        else
            [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
        end

        % Calculate aftershock sequence ...
        cumnrf = (1:length(time_as))';
        cumnr_modelf = [];
        if nMod == 1
            for i=1:length(time_as)
                if pval1 ~= 1
                    cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_as(i)+cval1)^(1-pval1));
                else
                    cm = kval1*log(time_as(i)/cval1+1);
                end
                cumnr_modelf = [cumnr_modelf; cm];
            end % END of FOR on length(time_as)
        else
            for i=1:length(time_as)
                if time_as(i) <= fT1
                    if pval1 ~= 1
                        cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_as(i)+cval1)^(1-pval1));
                    else
                        cm = kval1*log(time_as(i)/cval1+1);
                    end
                    cumnr_modelf = [cumnr_modelf; cm];
                else
                    if (pval1 ~= 1 & pval2 ~= 1)
                        cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_as(i)+cval1)^(1-pval1))+ kval2/(pval2-1)*(cval2^(1-pval2)-(time_as(i)-fT1+cval2)^(1-pval2));
                    else
                        cm = kval1*log(time_as(i)/cval1+1) + kval2*log((time_as(i)-fT1)/cval2+1);
                    end
                    cumnr_modelf = [cumnr_modelf; cm];
                end; %END of IF on fT1
            end; % End of FOR length(time_as)
        end; % End of if on nMod
        time_as=sort(time_as);
        cumnr_modelf=sort(cumnr_modelf);

        % Calculate KSTEST2 as a measure of the goodness of fit
        [H,P,KSSTAT] = kstest2(cumnr_modelf,cumnrf);

        % Calculate RMS
        i=(1:1:length(time_as))';
        fRMS = (sum((i-cumnr_modelf).^2)/length(i))^0.5;

        % Result matrix
        mResult = [mResult; time pval1 pval2 cval1 cval2 kval1 kval2 fAIC fL H P KSSTAT fRMS];
        mResult2 = [mResult2; pmed1 pstd1 pmed2 pstd2 cmed1 cstd1 cmed2 cstd2 kmed1 kstd1 kmed2 kstd2];
    else
        mResult = [mResult; NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN];
        mResult2 = [mResult2; NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN ];
    end % ENDIF on length(time_as)
end % FOR time

%save Result_pwtime.mat mResult mResult2;

% figure_w_normalized_uicontrolunits('tag','p-value time series', 'visible','on')
% hPplot4 = plot(mResult(:,1),mResult2(:,1));
% hold on
% hPplot=plot(mResult(:,1),mResult(:,2),'-k','Linewidth',2)
% hPplot2 = plot(mResult(:,1),mResult2(:,1)+mResult2(:,2));
% hPplot3 = plot(mResult(:,1),mResult2(:,1)-mResult2(:,2));
% set(hPplot2,'Linestyle','--','Linewidth',2,'Color',[0.5 0.5 0.5]);
% set(hPplot3,'Linestyle','--','Linewidth',2,'Color',[0.5 0.5 0.5]);
% set(hPplot4,'Linestyle','--','Linewidth',2,'Color',[0.5 0.5 0.5]);
% set(hPplot,'Linewidth',2)
% xlabel('Time after mainshock [days]','Fontweight','bold','FontSize',12)
% ylabel('p-value','Fontweight','bold','FontSize',12)
% set(gca,'Fontweight','bold','FontSize',12,'Box','on','Tickdir','out')

% Smoothing the plot
nWindowSize = 5;
figure_w_normalized_uicontrolunits('tag','p-value time series smooth', 'visible','on')
mPmean = filter(ones(1,nWindowSize)/nWindowSize,1,mResult2(:,1));
mPmean(1:nWindowSize,1)=mResult2(1:nWindowSize,1);
mPstd1 = filter(ones(1,nWindowSize)/nWindowSize,1,mResult2(:,2));
mPstd1(1:nWindowSize,1)=mResult2(1:nWindowSize,2);

mP = filter(ones(1,nWindowSize)/nWindowSize,1,mResult(:,2));
mP(1:nWindowSize,1)=mResult(1:nWindowSize,2);
% Plotting
hp1=plot(mResult(:,1),mPmean,'--','Linewidth',2,'Color',[0.3 0.3 0.3]);
hold on;
hp2=plot(mResult(:,1),mPmean-mPstd1,'-.','Linewidth',2,'Color',[0.5 0.5 0.5]);
plot(mResult(:,1),mPmean+mPstd1,'-.','Linewidth',2,'Color',[0.5 0.5 0.5]);
hPplot=plot(mResult(:,1),mResult(:,2),'-k','Linewidth',2)
ylabel('p-value','Fontweight','bold','FontSize',12)
% xlim([(min(mResult(:,1))) (max(mResult(:,1)))])
% ylim([floor(min(mP(:,1))) ceil(max(mP(:,1)))])
xlabel('Time / [days after main shock]','Fontweight','bold','FontSize',12)
l1=legend([hp1 hp2 hPplot],'p-value (mean)','\sigma p','p-value');
set(l1,'Fontweight','bold')
set(gca,'Fontweight','bold','FontSize',10,'Linewidth',2,'Tickdir','out')


% Demo for phase-space reduction (cf. Centre-Manifold theory): In this
% example we show that Generalised filtering can be used to approximate the
% dynamics on the Centre-Manifold of a chaotic system. This example uses a
% nominal polynomial for the reduced dynamics (cf, order parameters) and
% exploits parameter optimisation to learn their coefficients. The approach
% appeals to Centre Manifold Theory and the enslaving principle by assuming
% that stable modes collapse sufficiently quickly to be treated as random
% fluctuations. This means that the unstable (slow modes or patterns)
% dynamics are sufficient to characterise the emergent order (e.g. pattern
% formation) and can be accessed using a generative model that tries to
% explain observed motion in terms of a few hidden states (i.e. order
% parameters) that implicitly enslave random fluctuations.
%
% THIS DEMO IS UNDER CONSTRUCTION

% non-hierarchical non-linear generative model (dynamic & chaotic)
%==========================================================================
spm_figure('GetWin','Figure 1');

% get model parameters and initial states
%--------------------------------------------------------------------------
x      = [4 4 28]';
P.A    = [-10  10  0;
           32 -1   0;
           0   0  -8/3];
P.B{1} = [ 0  0  0;
           0  0 -1;
           0  1  0];
P.B{2} = [ 0  0  0;
           0  0  0;
           0  0  0];
P.B{3} = [ 0  0  0;
           0  0  0;
           0  0  0];
P.C    = {};

V.A    = [ 1  1  1;
           1  1  1;
           1  1  1];
V.B{1} = [ 1  1  1;
           1  1  1;
           1  1  1];
V.B{2} = [ 0  1  1;
           0  1  1;
           0  1  1];
V.B{3} = [ 0  0  1;
           0  0  1;
           0  0  1];
V.C    = {};
pE     = spm_unvec(spm_vec(P)/2,P);
pC     = diag(spm_vec(V));

% level 1
%--------------------------------------------------------------------------
M(1).f  = inline('spm_fx_poly(x,v,P)/32','x','v','P');
M(1).g  = inline('x','x','v','P');
M(1).x  = x;
M(1).pE = P;
M(1).V  = exp(8);
M(1).W  = exp(8);

% level 2
%--------------------------------------------------------------------------
M(2).v  = 0;
M(2).V  = exp(16);

% create data
%==========================================================================

% create innovations & add causes
%--------------------------------------------------------------------------
N       = 128;
U       = sparse(1,N);
DEM     = spm_DEM_generate(M,U);
spm_DEM_qU(DEM.pU)

% DEM estimation
%==========================================================================
DEM.M(1).E.n  = 4;
DEM.M(1).E.s  = 1/8;
DEM.M(1).pE   = pE;
DEM.M(1).pC   = pC*exp(0);
DEM           = spm_DEM(DEM);

M(1).pE = DEM.qP.P{1};
SIM     = spm_DEM_generate(M,U);

% graphics
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 1');
spm_DEM_qU(SIM.pU)

subplot(2,2,3)
x      = DEM.pU.x{1};
plot(x(1,:),x(2,:))
title('true manifold dynamics')
xlabel('time')
axis square

subplot(2,2,4)
x      = SIM.pU.x{1};
plot(x(1,:),x(2,:))
title('estimated dynamics')
xlabel('time')
axis square

return

% Notes for creation of a globally coupled map
%==========================================================================

% Demo of synchronization manifold using coupled Lorenz attractors
%--------------------------------------------------------------------------
W    = 2;                            % amplitude of random fluctuations
N    = 16;                           % number of (Lorenz) oscillators
T    = 512;                          % number of time bins
dt   = 1/32;                         % time interval

% parameters (set s.d. P.t to 1/4 to see oscillator death)
%--------------------------------------------------------------------------
P.t  = randn(N,1)/16;                 % variations in log-rate constants
P.k  = 2;                            % global coupling parameter

% states
%--------------------------------------------------------------------------
x      = randn(3,N)*8;               % microstates
x(1,:) = x(1,:) + 0;
x(2,:) = x(2,:) + 0;
x(3,:) = x(3,:) + 28;
v      = 0;

% integrate
%--------------------------------------------------------------------------
for i = 1:T

    [dfdx f]  = spm_diff('spm_lorenz_k',x,v,P,1);
    f         = f + randn(N*3,1)*W;
    dx        = spm_dx(dfdx,f,dt);
    x         = x + spm_unvec(dx,x);
    y(:,i)    = x(:);               % microstates
    X(:,i)    = mean(x,2);          % macrostates

end

% plot
%--------------------------------------------------------------------------
subplot(2,1,1)
plot(y',':'), hold on
plot(X','k'), hold off
axis tight

t = T/4:T;
subplot(2,1,2)
plot(X(1,t),X(2,t),'k')
axis square
axis([-16 16 -16 16])

return

% plot synchronisation manifold
%--------------------------------------------------------------------------
clf
subplot(2,1,1)
for i = 1:12
    plot(y(i*3 - 2,t),y(i*3 + 1,t),'.','Color',[1 1/2 1/2]),hold on
end
plot(X(1,t),X(1,t),'.k'),hold off
axis square
axis([-16 16 -16 16])


function [X, optinf] = cbpdnms(D, S, lambda, opt)

% cbpdnms -- Convolutional Basis Pursuit DeNoising (Mask Simulation)
%
%         argmin_{x_k} (1/2)||W (\sum_k d_k * x_k - s)||_2^2 +
%                           lambda \sum_k ||x_k||_1
%
%         The solution is computed using an ADMM approach (see
%         boyd-2010-distributed) with efficient solution of the main
%         linear systems (see wohlberg-2016-efficient and
%         wohlberg-2016-boundary).
%
% Usage:
%       [X, optinf] = cbpdnms(D, S, lambda, opt)
%
% Input:
%       D           Dictionary filter set (3D array)
%       S           Input image
%       lambda      Regularization parameter
%       opt         Algorithm parameters structure
%
% Output:
%       X           Dictionary coefficient map set (3D array)
%       optinf      Details of optimisation
%
%
% Options structure fields:
%   Verbose          Flag determining whether iteration status is displayed.
%                    Fields are iteration number, functional value,
%                    data fidelity term, l1 regularisation term, and
%                    primal and dual residuals (see Sec. 3.3 of
%                    boyd-2010-distributed). The value of rho is also
%                    displayed if options request that it is automatically
%                    adjusted.
%   MaxMainIter      Maximum main iterations
%   AbsStopTol       Absolute convergence tolerance (see Sec. 3.3.1 of
%                    boyd-2010-distributed)
%   RelStopTol       Relative convergence tolerance (see Sec. 3.3.1 of
%                    boyd-2010-distributed)
%   L1Weight         Weighting array for coefficients in l1 norm of X
%   Y0               Initial value for Y
%   U0               Initial value for U
%   rho              ADMM penalty parameter
%   AutoRho          Flag determining whether rho is automatically updated
%                    (see Sec. 3.4.1 of boyd-2010-distributed)
%   AutoRhoPeriod    Iteration period on which rho is updated
%   RhoRsdlRatio     Primal/dual residual ratio in rho update test
%   RhoScaling       Multiplier applied to rho when updated
%   AutoRhoScaling   Flag determining whether RhoScaling value is
%                    adaptively determined (see wohlberg-2015-adaptive). If
%                    enabled, RhoScaling specifies a maximum allowed
%                    multiplier instead of a fixed multiplier.
%   RhoRsdlTarget    Residual ratio targeted by auto rho update policy.
%   StdResiduals     Flag determining whether standard residual definitions
%                    (see Sec 3.3 of boyd-2010-distributed) are used instead
%                    of normalised residuals (see wohlberg-2015-adaptive)
%   RelaxParam       Relaxation parameter (see Sec. 3.4.3 of
%                    boyd-2010-distributed)
%   NoBndryCross     Flag indicating whether all solution coefficients
%                    corresponding to filters crossing the image boundary
%                    should be forced to zero.
%   AuxVarObj        Flag determining whether objective function is computed
%                    using the auxiliary (split) variable
%   HighMemSolve     Use more memory for a slightly faster solution
%   W                Synthesis spatial weighting matrix
%
%
% Author: Brendt Wohlberg <brendt@lanl.gov>  Modified: 2016-05-10
%
% This file is part of the SPORCO library. Details of the copyright
% and user license can be found in the 'License' file distributed with
% the library.

if nargin < 4,
  opt = [];
end
checkopt(opt, defaultopts([]));
opt = defaultopts(opt);

% Set up status display for verbose operation
hstr = 'Itn   Fnc       DFid      l1        r         s      ';
sfms = '%4d %9.2e %9.2e %9.2e %9.2e %9.2e';
nsep = 54;
if opt.AutoRho,
  hstr = [hstr '   rho  '];
  sfms = [sfms ' %9.2e'];
  nsep = nsep + 10;
end
if opt.Verbose && opt.MaxMainIter > 0,
  disp(hstr);
  disp(char('-' * ones(1,nsep)));
end

% Start timer
tstart = tic;

% Insert impulse filter into dictionary
imp = zeros(size(D,1), size(D,2), 1);
imp(1,1,1) = 1.0;
Di = cat(3, D, imp);

% Collapsing of trailing singleton dimensions greatly complicates
% handling of both SMV and MMV cases. The simplest approach would be
% if s could always be reshaped to 4d, with dimensions consisting of
% image rows, image cols, a single dimensional placeholder for number
% of filters, and number of measurements, but in the single
% measurement case the third dimension is collapsed so that the array
% is only 3d.
if size(S,3) > 1 && size(S,4) == 1,
  xsz = [size(S,1) size(S,2) size(Di,3) size(S,3)];
  % Insert singleton 3rd dimension (for number of filters) so that
  % 4th dimension is number of images in input s volume
  S = reshape(S, [size(S,1) size(S,2) 1 size(S,3)]);
  if ~isscalar(opt.W) & ndims(opt.W) > 2,
    opt.W = reshape(opt.W, [size(opt.W,1) size(opt.W,2) 1 size(opt.W,3)]);
  end
else
  xsz = [size(S,1) size(S,2) size(Di,3) size(S,4)];
end
IYW = 1.0 - opt.W;

% Compute filters in DFT domain
Df = fft2(Di, size(S,1), size(S,2));
% Convolve-sum and its Hermitian transpose
Dop = @(x) sum(bsxfun(@times, Df, x), 3);
DHop = @(x) bsxfun(@times, conj(Df), x);
% Compute signal in DFT domain
Sf = fft2(S);
% S convolved with all filters in DFT domain
DSf = DHop(Sf);

% Default lambda is 1/10 times the lambda value beyond which the
% solution is a zero vector
if nargin < 3 | isempty(lambda),
  b = ifft2(DHop(Sf), 'symmetric');
  lambda = 0.1*max(vec(abs(b)));
end

% Set up algorithm parameters and initialise variables
rho = opt.rho;
if isempty(rho), rho = 50*lambda+1; end;
if isempty(opt.RhoRsdlTarget),
  if opt.StdResiduals,
    opt.RhoRsdlTarget = 1;
  else
    opt.RhoRsdlTarget = 1 + (18.3).^(log10(lambda) + 1);
  end
end
if opt.HighMemSolve,
  C = bsxfun(@rdivide, Df, sum(Df.*conj(Df), 3) + rho);
else
  C = [];
end
Nx = prod(xsz);
optinf = struct('itstat', [], 'opt', opt);
r = Inf;
s = Inf;
epri = 0;
edua = 0;

% Initialise main working variables
X = [];
if isempty(opt.Y0),
  Y = zeros(xsz);
else
  Y = opt.Y0;
end
Yprv = Y;
if isempty(opt.U0),
  if isempty(opt.Y0),
    U = zeros(xsz);
  else
    U = (lambda/rho)*sign(Y);
  end
else
  U = opt.U0;
end

% Main loop
k = 1;
while k <= opt.MaxMainIter && (r > epri | s > edua),

  % Solve X subproblem
  Xf = solvedbi_sm(Df, rho, DSf + rho*fft2(Y - U), C);
  X = ifft2(Xf, 'symmetric');

  % See pg. 21 of boyd-2010-distributed
  if opt.RelaxParam == 1,
    Xr = X;
  else
    Xr = opt.RelaxParam*X + (1-opt.RelaxParam)*Y;
  end

  % Solve Y subproblem
  Y(:,:,1:(end-1),:) = shrink(Xr(:,:,1:(end-1),:) + U(:,:,1:(end-1),:), ...
                              (lambda/rho)*opt.L1Weight);
  Y(:,:,end,:) = bsxfun(@times, IYW, Xr(:,:,end,:) + U(:,:,end,:));
  if opt.NoBndryCross,
    Y((end-size(D,1)+2):end,:,1:(end-1),:) = 0;
    Y(:,(end-size(D,2)+2):end,1:(end-1),:) = 0;
  end

  % Update dual variable
  U = U + Xr - Y;

  % Compute data fidelity term in Fourier domain (note normalisation)
  if opt.AuxVarObj,
    Yf = fft2(Y); % This represents unnecessary computational cost
    Jdf = sum(vec(abs(sum(bsxfun(@times,Df,Yf),3)-Sf).^2))/(2*xsz(1)*xsz(2));
    Jl1 = sum(abs(vec(bsxfun(@times, opt.L1Weight, Y(:,:,1:(end-1),:)))));
  else
    Jdf = sum(vec(abs(sum(bsxfun(@times,Df,Xf),3)-Sf).^2))/(2*xsz(1)*xsz(2));
    Jl1 = sum(abs(vec(bsxfun(@times, opt.L1Weight, X(:,:,1:(end-1),:)))));
  end
  Jfn = Jdf + lambda*Jl1;

  nX = norm(X(:)); nY = norm(Y(:)); nU = norm(U(:));
  if opt.StdResiduals,
    % See pp. 19-20 of boyd-2010-distributed
    r = norm(vec(X - Y));
    s = norm(vec(rho*(Yprv - Y)));
    epri = sqrt(Nx)*opt.AbsStopTol+max(nX,nY)*opt.RelStopTol;
    edua = sqrt(Nx)*opt.AbsStopTol+rho*nU*opt.RelStopTol;
  else
    % See wohlberg-2015-adaptive
    r = norm(vec(X - Y))/max(nX,nY);
    s = norm(vec(Yprv - Y))/nU;
    epri = sqrt(Nx)*opt.AbsStopTol/max(nX,nY)+opt.RelStopTol;
    edua = sqrt(Nx)*opt.AbsStopTol/(rho*nU)+opt.RelStopTol;
  end

  % Record and display iteration details
  tk = toc(tstart);
  optinf.itstat = [optinf.itstat; [k Jfn Jdf Jl1 r s epri edua rho tk]];
  if opt.Verbose,
    if opt.AutoRho,
      disp(sprintf(sfms, k, Jfn, Jdf, Jl1, r, s, rho));
    else
      disp(sprintf(sfms, k, Jfn, Jdf, Jl1, r, s));
    end
  end

  % See wohlberg-2015-adaptive and pp. 20-21 of boyd-2010-distributed
  if opt.AutoRho,
    if k ~= 1 && mod(k, opt.AutoRhoPeriod) == 0,
      if opt.AutoRhoScaling,
        rhomlt = sqrt(r/(s*opt.RhoRsdlTarget));
        if rhomlt < 1, rhomlt = 1/rhomlt; end
        if rhomlt > opt.RhoScaling, rhomlt = opt.RhoScaling; end
      else
        rhomlt = opt.RhoScaling;
      end
      rsf = 1;
      if r > opt.RhoRsdlTarget*opt.RhoRsdlRatio*s, rsf = rhomlt; end
      if s > (opt.RhoRsdlRatio/opt.RhoRsdlTarget)*r, rsf = 1/rhomlt; end
      rho = rsf*rho;
      U = U/rsf;
      if opt.HighMemSolve && rsf ~= 1,
        C = bsxfun(@rdivide, Df, sum(Df.*conj(Df), 3) + rho);
      end
    end
  end

  Yprv = Y;
  k = k + 1;

end

% Record run time and working variables
optinf.runtime = toc(tstart);
optinf.X = X;
optinf.Xf = Xf;
optinf.Y = Y;
optinf.U = U;
optinf.lambda = lambda;
optinf.rho = rho;

if opt.Verbose && opt.MaxMainIter > 0,
  disp(char('-' * ones(1,nsep)));
end

% Remove coefficient map for impulse filter
X = X(:,:,1:(end-1), :);

return

function u = vec(v)

  u = v(:);

return

function u = shrink(v, lambda)

  if isscalar(lambda),
    u = sign(v).*max(0, abs(v) - lambda);
  else
    u = sign(v).*max(0, bsxfun(@minus, abs(v), lambda));
  end

return

function opt = defaultopts(opt)

  if ~isfield(opt,'Verbose'),
    opt.Verbose = 0;
  end
  if ~isfield(opt,'MaxMainIter'),
    opt.MaxMainIter = 1000;
  end
  if ~isfield(opt,'AbsStopTol'),
    opt.AbsStopTol = 0;
  end
  if ~isfield(opt,'RelStopTol'),
    opt.RelStopTol = 1e-4;
  end
  if ~isfield(opt,'L1Weight'),
   opt.L1Weight = 1;
  end
  if ~isfield(opt,'Y0'),
    opt.Y0 = [];
  end
  if ~isfield(opt,'U0'),
    opt.U0 = [];
  end
  if ~isfield(opt,'rho'),
    opt.rho = [];
  end
  if ~isfield(opt,'AutoRho'),
    opt.AutoRho = 1;
  end
  if ~isfield(opt,'AutoRhoPeriod'),
    opt.AutoRhoPeriod = 1;
  end
  if ~isfield(opt,'RhoRsdlRatio'),
    opt.RhoRsdlRatio = 1.2;
  end
  if ~isfield(opt,'RhoScaling'),
    opt.RhoScaling = 100;
  end
  if ~isfield(opt,'AutoRhoScaling'),
    opt.AutoRhoScaling = 1;
  end
  if ~isfield(opt,'RhoRsdlTarget'),
    opt.RhoRsdlTarget = [];
  end
  if ~isfield(opt,'StdResiduals'),
    opt.StdResiduals = 1;
  end
  if ~isfield(opt,'RelaxParam'),
    opt.RelaxParam = 1.8;
  end
  if ~isfield(opt,'NoBndryCross'),
    opt.NoBndryCross = 0;
  end
  if ~isfield(opt,'AuxVarObj'),
    opt.AuxVarObj = 0;
  end
  if ~isfield(opt,'HighMemSolve'),
    opt.HighMemSolve = 0;
  end
  if ~isfield(opt,'W'),
    opt.W = 1.0;
  end

return


function [model, varargout] = grlvq_core(trainSet, trainLab, varargin)
%grlvq_core.m - trains the Generalized Relevance LVQ algorithm
%NOTE: minimal requirement version 7.4.0.336 (R2007a)
%
%  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  %% Use the wrapper GRLVQ.M to access the functionality in style of %%
%  %% the SOM Toolbox (i.e. with data structs).                       %%
%  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  example for usage:
%  trainSet = [1,2,3;4,5,6;7,8,9];
%  trainLab = [1;1;2];
%  GRLVQ_model=GRLVQ_train(trainSet,trainLab); % minimal parameters required
%  estimatedTrainLabels = GRLVQ_classify(trainSet, GRLVQ_model);
%  trainError = mean( trainLab ~= estimatedTrainLabels );
%
% input:
%  trainSet : matrix with training samples in its rows
%  trainLab : vector with the labels of the training set
% optional parameters:
%  PrototypesPerClass: (default=1) the number of prototypes per class used. This could
%  be a number or a vector with the number for each class
%  initialPrototypes : (default=[]) a set of prototypes to start with. If not given initialization near the class means
%  initialRelevances : the relevances to start with. If not given all
%  relevances are equal at start
%  regularization    : (default=0) values usually between 0 and 1 treat with care.
%  Regularizes the relevances to be more homogeneous
%  testSet           : (default=[]) an optional test set used to compute
%  the test error. The last column is expected to be a label vector
%  comparable        : (default=0) a flag which resets the random generator
%  to produce comparable results if set to 1
%  optimization      : (default=fminlbfgs) indicates which optimization is used: sgd or fminlbfgs
% parameter for the stochastic gradient descent sgd
%  nb_epochs             : (default=100) the number of epochs for sgd
%  learningRatePrototypes: (default=[]) the learning rate for the prototypes.
%  Could be the start and end value used for a sigmoidal spectrum or a vector of length nb_epochs
%  learningRateRelevances: (default=[]) the learning rate for the matrix.
%  Could be the start and end value used for a sigmoidal spectrum or a vector of length nb_epochs
%  relevanceStart        : (default=1) the epoch to start the matrix training
% parameter for the build-in function fminlbfgs
%  threshstop       : (default=0) the training error for early stopping
%  useEarlyStopping : (default=1) use early stopping based on threshstop
%  Display          : (default='off') the optimization output 'iter' or 'off'
%  GradObj          : (default=on) use the gradient information or not
%  HessUpdate       : (default=lbfgs) the update can be 'lbfgs', 'bfgs' or 'steepdesc'
%  TolFun           : (default=1e-6) the tolerance
%  MaxIter          : (default=2500) the maximal number of iterations
%  MaxFunEvals      : (default=1000000) the maximal number of function evaluations
%  TolX             : (default=1e-10) tolerance
%  DiffMinChange    : (default=1e-10) minimal change
%
% output: the GRLVQ model with prototypes w their labels c_w and the relevances lamda
%  optional output:
%  initialization : a struct containing the settings
%  trainError     : error in the training set
%  testError      : error in the training set (only computed if 'testSet' is given)
%  costs          : the output of the cost function
%
% Citation information:
% B. Hammer and T. Villmann, Generalized relevance learning vector quantization, Neural Networks 15, 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.
%
% Kerstin Bunte
% uses the Fast Limited Memory Optimizer fminlbfgs.m written by Dirk-Jan Kroon available at the MATLAB central
% kerstin.bunte@googlemail.com
% Fri Nov 09 14:13:52 CEST 2012
%
% Conditions of GNU General Public License, version 2 and BSD License apply.
% See file 'license-gpl2.txt' and 'BSD_license.txt' enclosed in this package.
% Programs are not for use in critical applications!
%

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

nout = max(nargout,1)-1;
p = inputParser;   % Create an instance of the class.
p.addRequired('trainSet', @isfloat);
p.addRequired('trainLab', @(x) length(x)==size(trainSet,1) & isnumeric(x));

p.addOptional('testSet', [], @(x)(size(x,2)-1)==size(trainSet,2) & isfloat(x));
p.addParamValue('PrototypesPerClass', ones(1,length(unique(trainLab))), @(x)(sum(~(x-floor(x)))/length(x)==1 && (length(x)==length(unique(trainLab)) || length(x)==1)));
p.addParamValue('initialPrototypes',[], @(x)(size(x,2)-1==size(trainSet,2) && isfloat(x)));
p.addParamValue('initialRelevances',[], @(x)(numel(x)==size(trainSet,2) && isfloat(x)));
p.addOptional('comparable', 0, @(x)(~(x-floor(x))));
p.addParamValue('regularization',0, @(x)(isfloat(x) && x>=0));
p.addOptional('optimization', 'fminlbfgs', @(x)any(strcmpi(x,{'sgd','fminlbfgs'})));
% parameter for the stochastic gradient descent
p.addOptional('nb_epochs', 100, @(x)(~(x-floor(x))));
p.addParamValue('learningRatePrototypes', [], @(x)(isfloat(x) || isa(x,'function_handle')));
p.addParamValue('learningRateRelevances', [], @(x)(isfloat(x) || isa(x,'function_handle')));
p.addOptional('relevanceStart', 1, @(x)(~(x-floor(x))));
% parameter for the build-in function
p.addOptional('threshstop',0,@(x) isfloat(x));
p.addOptional('useEarlyStopping',1,@(x)(~(x-floor(x))));
p.addOptional('Display', 'off', @(x)any(strcmpi(x,{'iter','off'})));
p.addOptional('GradObj', 'on', @(x)any(strcmpi(x,{'on','off'})));
p.addOptional('HessUpdate', 'lbfgs', @(x)any(strcmpi(x,{'lbfgs', 'bfgs', 'steepdesc'})));
p.addOptional('TolFun',1e-6,@(x) isfloat(x));
p.addOptional('MaxIter', 2500, @(x)(~(x-floor(x))));
p.addOptional('MaxFunEvals', 1000000, @(x)(~(x-floor(x))));
p.addOptional('TolX',1e-10,@(x) isfloat(x));
p.addOptional('DiffMinChange',1e-10,@(x)isfloat(x));
p.CaseSensitive = true;
p.FunctionName = 'GRLVQ';
% Parse and validate all input arguments.
p.parse(trainSet, trainLab, varargin{:});

%%% check if results should be comparable
if p.Results.comparable,
    rng('default');
end
%%% set useful variables
nb_samples = size(trainSet,1);
nb_features = size(trainSet,2);
if size(trainLab,1)~=nb_samples, trainLab = trainLab';end

classes = unique(trainLab);
nb_classes = length(classes);
testSet = p.Results.testSet;

% global regularization;
regularization = p.Results.regularization;
if regularization, disp(['Regularize the relevances with ',num2str(regularization)]);end

initialization = rmfield(p.Results, 'trainSet');
initialization.trainSet = [num2str(nb_samples),'x',num2str(nb_features),' matrix'];
initialization = rmfield(initialization, 'trainLab');
initialization.trainLab = ['vector of length ',num2str(length(trainLab))];
if ~isempty(testSet)
    initialization = rmfield(initialization, 'testSet');
    initialization.testSet = [num2str(size(testSet,1)),'x',num2str(size(testSet,2)),' matrix'];
end
switch(p.Results.optimization)
case{'sgd'}
    initialization = rmfield(initialization, 'useEarlyStopping');
    initialization = rmfield(initialization, 'Display');
    initialization = rmfield(initialization, 'GradObj');
    initialization = rmfield(initialization, 'HessUpdate');
    initialization = rmfield(initialization, 'TolFun');
    initialization = rmfield(initialization, 'MaxIter');
    initialization = rmfield(initialization, 'MaxFunEvals');
    initialization = rmfield(initialization, 'TolX');
    initialization = rmfield(initialization, 'DiffMinChange');
case{'fminlbfgs'}
    disp('The fminlbfgs optimization uses some global variables:');
    disp('threshstop earlystopped useEarlyStopping');
    initialization = rmfield(initialization, 'nb_epochs');
    initialization = rmfield(initialization, 'learningRatePrototypes');
    initialization = rmfield(initialization, 'learningRateRelevances');
    initialization = rmfield(initialization, 'relevanceStart');
end
% Display all arguments.
%disp 'Settings for GRLVQ:'
%disp(initialization);

%%% check the number of prototypes per class if one integer is given and turn
%%% it into a vector
nb_ppc = p.Results.PrototypesPerClass;
if length(nb_ppc)~=nb_classes,
    nb_ppc = ones(1,nb_classes)*nb_ppc;
end

%%% initialize the prototypes
if isempty(p.Results.initialPrototypes)
    % initialize near the class centers
    w = zeros(sum(nb_ppc),nb_features);
    c_w = zeros(sum(nb_ppc),1);
    actPos = 1;
    for actClass=1:nb_classes
        nb_prot_c = nb_ppc(actClass);
        classMean = mean(trainSet(trainLab==classes(actClass),:));
        % set the prototypes to the class mean and add a random variation
        % between -0.1 and 0.1
        w(actPos:actPos+nb_prot_c-1,:) = classMean(ones(nb_prot_c,1),:)+(rand(nb_prot_c,nb_features)*2-ones(nb_prot_c,nb_features))/10;
        c_w(actPos:actPos+nb_prot_c-1) = classes(actClass);
        actPos = actPos+nb_prot_c;
    end
else
    % initialize with given w
    w = p.Results.initialPrototypes(:,1:end-1);
    c_w = p.Results.initialPrototypes(:,end);
end
%%% initialize the matrix
if isempty(p.Results.initialRelevances)
    lambda = ones(1,nb_features);
else
%     if isvector(p.Results.initialRelevances)
%         omega = diag(p.Results.initialRelevances);
%     else
    lambda = p.Results.initialRelevances;
%     end
end
% normalize the relevances
lambda = lambda / sum(lambda);
model = struct('w',w,'c_w',c_w,'lambda',lambda);
clear w c_w lambda;

dim = length(model.lambda);

switch(p.Results.optimization)
case{'sgd'}
    %%% gradient descent variables
    nb_epochs = p.Results.nb_epochs;
    relevanceStart = p.Results.relevanceStart;
    % compute the vector of nb_epochs learning rates alpha for the prototype learning
    if isa(p.Results.learningRatePrototypes,'function_handle')
        % with a given function specified from the user
        alphas = arrayfun(p.Results.learningRatePrototypes, 1:nb_epochs);
    elseif length(p.Results.learningRatePrototypes)>2
        if length(p.Results.learningRatePrototypes)==nb_epochs
            alphas = p.Results.learningRatePrototypes;
        else
            disp('The learning rate vector for the prototypes does not fit the nb of epochs');
            return;
        end
    else
        % or use an decay with a start and a decay value
        if isempty(p.Results.learningRatePrototypes)
            initialization.learningRatePrototypes = [nb_features/100, nb_features/10000];
        end
        alpha_start = initialization.learningRatePrototypes(1);
        alpha_end = initialization.learningRatePrototypes(2);
        alphas = arrayfun(@(x) alpha_start * (alpha_end/alpha_start)^(x/nb_epochs), 1:nb_epochs);
    %     alphas = arrayfun(@(x) alpha_start / (1+(x-1)*alpha_end), 1:nb_epochs);
    end
    % compute the vector of nb_epochs learning rates epsilon for the Matrix learning
    epsilons = zeros(1,nb_epochs);
    if isa(p.Results.learningRateRelevances,'function_handle')
        % with a given function specified from the user
    % 	epsilons = arrayfun(p.Results.learningRateRelevances, 1:nb_epochs);
        epsilons(relevanceStart:nb_epochs) = arrayfun(p.Results.learningRateRelevances, relevanceStart:nb_epochs);
    elseif length(p.Results.learningRateRelevances)>2
        if length(p.Results.learningRateRelevances)==nb_epochs
            epsilons = p.Results.learningRateRelevances;
        else
            disp('The learning rate vector for the Matrix does not fit the nb of epochs');
            return;
        end
    else
        % or use an decay with a start and a decay value
        if isempty(p.Results.learningRateRelevances)
            initialization.learningRateRelevances = [nb_features/1000, nb_features/100000];
        end
        eps_start = initialization.learningRateRelevances(1);
        eps_end = initialization.learningRateRelevances(2);
    %     epsilons = arrayfun(@(x) eps_start * (eps_end/eps_start)^(x/nb_epochs), 1:nb_epochs);
        epsilons(relevanceStart:nb_epochs) = arrayfun(@(x) eps_start * (eps_end/eps_start)^((x-relevanceStart)/(nb_epochs-relevanceStart)), relevanceStart:nb_epochs);
    end

    %%% initialize requested outputs
    trainError = [];
    costs = [];
    testError = [];
    if nout>=2,
        % train error requested
        trainError = ones(1,nb_epochs+1);
        estimatedLabels = GRLVQ_classify(trainSet, model); % error after initialization
        trainError(1) = sum( trainLab ~= estimatedLabels )/nb_samples;
        if nout>=3,
            % test error requested
            if isempty(testSet)
                testError = [];
                disp('The test error is requested, but no labeled test set given. Omitting the computation.');
            else
                testError = ones(1,nb_epochs+1);
                estimatedLabels = GRLVQ_classify(testSet(:,1:end-1), model); % error after initialization
                testError(1) = sum( testSet(:,end) ~= estimatedLabels )/length(estimatedLabels);
            end
            if nout>=4,
                % costs requested
                disp('The computation of the costs is an expensive operation, do it only if you really need it!');
                costs(1) = GRLVQ_costfun(trainSet, trainLab, model, regularization);
            end
        end
    end

    %%% optimize with stochastic gradient descent
    for epoch=1:nb_epochs
        if mod(epoch,100)==0, disp(epoch); end
        % generate order to sweep through the trainingset
        order = randperm(nb_samples);

        % perform one sweep through trainingset
        for i=1:nb_samples
            % select one training sample randomly
            xi = trainSet(order(i),:);
            c_xi = trainLab(order(i));
%             dist = ((xi(ones(size(model.w,1),1),:))-model.w)*model.omega'*(model.omega*((xi(ones(size(model.w,1),1),:))-model.w)');
%             dist = diag(dist);
            dist = sum(bsxfun(@times,bsxfun(@minus,xi,model.w).^2,model.lambda), 2);
            % determine the two winning prototypes
            % nearest prototype with the same class
            [sortDist,sortIdx] = sort(dist);
            count = 1;
            J = sortIdx(count);
            while model.c_w(sortIdx(count)) ~= c_xi,
                count = count+1;
                J = sortIdx(count);
            end
            dJ = sortDist(count);
            count = 1;
            K = sortIdx(count);
            while model.c_w(sortIdx(count)) == c_xi,
                count = count+1;
                K = sortIdx(count);
            end
            dK = sortDist(count);

            wJ = model.w(J,:);
            wK = model.w(K,:);
            % prototype update
            norm_factor = (dJ + dK)^2;
            DJ = (xi-wJ);
            DK = (xi-wK);

            dwJ = (2*dK/norm_factor)*2*model.lambda.*DJ;
            dwK = (2*dJ/norm_factor)*2*model.lambda.*DK;

            model.w(J,:) = wJ + alphas(epoch) * dwJ;
            model.w(K,:) = wK - alphas(epoch) * dwK;

            % update relevances
            if epsilons(epoch)>0, % epoch >= RelevanceStart
%                 f1 = (2*dK/norm_factor)*2*(model.omega*DJ')*DJ;
%                 f2 = (2*dJ/norm_factor)*2*(model.omega*DK')*DK;
                f1 = (2*dK/norm_factor)*DJ.^2;
                f2 = (2*dJ/norm_factor)*DK.^2;
                % update lambda
                if regularization,
                    f3 = diag(pinv(sqrt(diag(model.lambda))))';
%                     test = bsxfun(@times,model.lambda,ones(1,length(model.lambda))');
%                     test(1:nb_features+1:nb_features*nb_features) = 1;
%                     f3 = 1/(prod(model.lambda)+eps)*prod(test,2)';
                else
                    f3 = 0;
                end
                model.lambda = model.lambda-epsilons(epoch) * (f1-f2  - regularization * f3);
                % normalization
                model.lambda (model.lambda < 0) = 0;
                model.lambda = model.lambda / sum(model.lambda);
            end
        end
        if nout>=2,
            % train error requested
            estimatedLabels = GRLVQ_classify(trainSet, model); % error after epoch
            trainError(epoch+1) = sum( trainLab ~= estimatedLabels )/nb_samples;
            if nout>=3,
                % test error requested
                if ~isempty(testSet)
                    estimatedLabels = GRLVQ_classify(testSet(:,1:end-1), model); % error after initialization
                    testError(epoch+1) = sum( testSet(:,end) ~= estimatedLabels )/length(estimatedLabels);
                end
                if nout>=4,
                    % costs requested
%                     dist = computeDistance(trainSet, model.w, model);
%                     if regularization,
%                         regTerm = regularization * log(det(model.omega*model.omega'));
%                     else
%                         regTerm = 0;
%                     end
%                     costs(epoch+1) = sum(arrayfun(@(idx) GLVQ_costfun(min(dist(idx,model.c_w == trainLab(idx))),...
%                                                                       min(dist(idx,model.c_w ~= trainLab(idx)))), 1:size(dist,1)))-regTerm;
                    costs(epoch+1) = GRLVQ_costfun(trainSet, trainLab, model, regularization);
                end
            end
        end
    end
case{'fminlbfgs'}
    %%% optimization options
    options = struct( ...
      'Display',p.Results.Display, ...
      'GradObj',p.Results.GradObj, ...
      'GradConstr',false, ...
      'GoalsExactAchieve',0, ...
      'TolFun',p.Results.TolFun, ...
      'MaxIter',p.Results.MaxIter, ...
      'MaxFunEvals', p.Results.MaxFunEvals, ...
      'TolX',p.Results.TolX, ...
      'DiffMinChange',p.Results.DiffMinChange, ...
      'OutputFcn','LVQ_progresser', ...
      'HessUpdate',p.Results.HessUpdate ...
    );
    clear('progresser'); % memory therein might need reset
    global threshstop earlystopped useEarlyStopping % for LVQ_progresser.m datval labval n_vec
    useEarlyStopping = p.Results.useEarlyStopping; % use early stopping
%     useEarlyStopping = []; % if empty, no validation set given/used for early stopping
    earlystopped = false;
    threshstop = p.Results.threshstop; % stop if classification below this threshold for early stopping
%     global training_data training_label LabelEqualsPrototype LRrelevances LRprototypes prototypeLabel;
%     training_data = trainSet;
%     training_label = trainLab;
%     prototypeLabel = model.c_w;
    nb_prototypes = numel(model.c_w);
%     nb_samples = size(training_data,1);
%     LabelEqualsPrototype = trainLab*ones(1,nb_prototypes) == (model.c_w*ones(1,nb_samples))';
    LabelEqualsPrototype = bsxfun(@eq,trainLab,model.c_w');
    earlystopped = false; % don't change, assigned in progresser.m
    clear('progresser'); % memory therein might need reset
    newfval = realmax('single');
    % fminlbfgs optimizer courtesy of Dirk-Jan Kroon:
    % http://www.mathworks.de/matlabcentral/fileexchange/23245
    % early stopping to be implemented in progresser function
    variables = zeros(nb_prototypes+nb_features,size(trainSet,2));
    variables(1:nb_prototypes,:) = model.w;
    variables(nb_prototypes+1:end,:) = diag(sqrt(model.lambda));
    LRprototypes = 1; % learn prototype locations
    LRrelevances = 0; % don't learn metric
%     [variables,fval] = fminlbfgs(@GRLVQ_optfun,variables,options);
    [variables,fval] = fminlbfgs(@(variables) GMLVQ_optfun(variables,trainSet,LabelEqualsPrototype,LRrelevances,LRprototypes,model.c_w,regularization),variables,options);
    if not(isempty(fval))
        newfval = fval;
    end
    LRprototypes = 0; % don't learn prototype locations
    LRrelevances = 1; % learn metric
%     [variables,fval] = fminlbfgs(@GRLVQ_optfun,variables,options);
    [variables,fval] = fminlbfgs(@(variables) GMLVQ_optfun(variables,trainSet,LabelEqualsPrototype,LRrelevances,LRprototypes,model.c_w,regularization),variables,options);
    if not(isempty(fval))
        newfval = fval;
    end
    if not(isempty(fval))
      clear('progresser'); % memory therein might need reset
      LRprototypes = 1;
      for i = 1:100  % depending on data, re-iterations might further improve
%         [variables,fval] = fminlbfgs(@GRLVQ_optfun,variables,options);
        [variables,fval] = fminlbfgs(@(variables) GMLVQ_optfun(variables,trainSet,LabelEqualsPrototype,LRrelevances,LRprototypes,model.c_w,regularization),variables,options);
        if not(isempty(fval))
          if abs(fval - newfval) < 1e-3
            newfval = fval;
            break
          end
          newfval = fval;
        end
        if isempty(fval) || earlystopped
          break
        end
      end
    end
    model.w = variables(1:nb_prototypes,:);
    model.lambda = diag(variables(nb_prototypes+1:end,:)'*variables(nb_prototypes+1:end,:))';
%     model.lambda = model.lambda/sum(model.lambda);
    if nout>=2,
        % train error requested
        estimatedLabels = GRLVQ_classify(trainSet, model); % error after initialization
        trainError = mean( trainLab ~= estimatedLabels );
        if nout>=3,
            % test error requested
            if isempty(testSet)
                testError = [];
                disp('The test error is requested, but no labeled test set given. Omitting the computation.');
            else
                estimatedLabels = GRLVQ_classify(testSet(:,1:end-1), model); % error after initialization
                testError = mean( testSet(:,end) ~= estimatedLabels );
            end
            if nout>=4,
                % costs requested
%                 dist = computeDistance(trainSet, model.w, model);
%                 if regularization,
%                     regTerm = regularization * log(det(model.omega*model.omega'));
%                 else
%                     regTerm = 0;
%                 end
%                 costs = sum(arrayfun(@(idx) GLVQ_costfun(min(dist(idx,model.c_w == trainLab(idx))),...
%                                                          min(dist(idx,model.c_w ~= trainLab(idx)))), 1:size(dist,1)))-regTerm;
                costs = GRLVQ_costfun(trainSet, trainLab, model, regularization);
            end
        end
    end
end

%%% output of the training
varargout = cell(nout);
for k=1:nout
	switch(k)
		case(1)
			varargout(k) = {initialization};
		case(2)
			varargout(k) = {trainError};
		case(3)
			varargout(k) = {testError};
		case(4)
            varargout(k) = {costs};
	end
end

function cost = GRLVQ_costfun(trainSet, trainLab, model, regularization)
%GRLVQ_costfun.m - computes the costs for a given training set and GRLVQ
%model with or without regularization
%  example for usage:
%  trainSet = [1,2,3;4,5,6;7,8,9];
%  trainLab = [1;1;2];
%  GMLVQ_model=GMLVQ_train(trainSet,trainLab); % minimal parameters required
%  costs = GMLVQ_costfun(trainSet, trainLab, GMLVQ_model, 0);
%
% input:
%  trainSet : matrix with training samples in its rows
%  trainLab : a vector of training labels
%  model    : GMLVQ model with prototypes w their labels c_w and the matrix omega
%  regularization: the factor>=0 for the regularization
%
% output    : cost function value
%
% Kerstin Bunte (based on the code from Marc Strickert)
% kerstin.bunte@googlemail.com
% Mon Nov 05 09:05:52 CEST 2012
%
% Conditions of GNU General Public License, version 2 apply.
% See file 'license-gpl2.txt' enclosed in this package.
% Programs are not for use in critical applications!
%
nb_samples = length(trainLab);
% labels should be a row vector
if size(trainLab,1)~=nb_samples, trainLab = trainLab';end

% LabelEqPrototype = trainLab*ones(1,numel(model.c_w)) == (model.c_w*ones(1,nb_samples))';
LabelEqPrototype = bsxfun(@eq,trainLab,model.c_w');
dists = computeDistance(trainSet, model.w, model);
if regularization,
    regTerm = regularization * log(prod(model.lambda));
% if strcmp(p.Results.optimization,'sgd')
else
    regTerm = 0;
end
Dwrong = dists;
Dwrong(LabelEqPrototype) = realmax(class(Dwrong));   % set correct labels impossible
distwrong = min(Dwrong.'); % closest wrong
clear Dwrong;

Dcorrect = dists;
Dcorrect(~LabelEqPrototype) = realmax(class(Dcorrect)); % set wrong labels impossible
distcorrect = min(Dcorrect.'); % closest correct
clear Dcorrect;
clear dists;
distcorrectpluswrong = distcorrect + distwrong;
distcorrectminuswrong = distcorrect - distwrong;
mu = distcorrectminuswrong ./ distcorrectpluswrong;
if regularization,
    regTerm = regularization * log(prod(model.lambda));
else
    regTerm = 0;
end
cost = sum(mu)-regTerm;

function [f G]  = GMLVQ_optfun(variables,training_data,LabelEqualsPrototype,LRrelevances,LRprototypes,prototypeLabel,regularization)
% [f G] = GMLVQ_optfun(variables)
% function to be optimzed by matrix relevance learning vector quantization
% variables = [prototype matrix;omega matrix]
% global variables are
%   training_data        : data vectors as row vectors, i.e. attributes in columns
%   LabelEqualsPrototype : binary matrix indicating coocurrences of
%   training labels and prototype labels
%   prototypeLabel       : label vector for the prototypes
%   regularization       : the regularization parameter
%   LRrelevances         : learning rate for the relevance matrix
%   LRprototypes         : learning rate for the prototypes
%
% Kerstin Bunte (modified based on the code of Marc Strickert http://www.mloss.org/software/view/323/)
% kerstin.bunte@googlemail.com
% Fri Nov 09 14:13:52 CEST 2012
%
% Conditions of GNU General Public License, version 2 apply.
% See file 'license-gpl2.txt' enclosed in this package.
% Programs are not for use in critical applications!
%
if isempty(LRprototypes) % values between 1e-2,1e-3,... 1e-8 seem pragmatic
    LRprototypes = 1; % no relevance learning by default
end
if isempty(LRrelevances) % values between 1e-2,1e-3,... 1e-8 seem pragmatic
    LRrelevances = 0; % no relevance learning by default
end
[n_data, n_dim] = size(training_data);
nb_prototypes =  numel(prototypeLabel);
omegaT = variables(nb_prototypes+1:end,:)';
n_vec = size(variables,1) - nb_prototypes;

dists = squaredEuclidean(training_data*omegaT, variables(1:nb_prototypes,:)*omegaT);

Dwrong = dists;
Dwrong(LabelEqualsPrototype) = realmax(class(Dwrong));   % set correct labels impossible
[distwrong pidxwrong] = min(Dwrong.'); % closest wrong
clear Dwrong;

Dcorrect = dists;
Dcorrect(~LabelEqualsPrototype) = realmax(class(Dcorrect)); % set wrong labels impossible
[distcorrect pidxcorrect] = min(Dcorrect.'); % closest correct
clear Dcorrect;

distcorrectpluswrong = distcorrect + distwrong;
distcorrectminuswrong = distcorrect - distwrong;
mu = distcorrectminuswrong ./ distcorrectpluswrong;
% callitq = 1./(1 + exp(-squashsigmoid * mu)); % apply sigmoidal

if regularization,
    regTerm = regularization * log(det(omegaT'*omegaT));
else
    regTerm = 0;
end
f = sum(mu)-regTerm;
% f = mean(callitq);

if nargout > 1  % gradient needed not just function eval
    G = zeros(size(variables)); % initially no gradient
    %       callitq = squashsigmoid * callitq .* (1-callitq); % derivative of sigmoid
    %       distcorrectpluswrong = 2 * callitq ./ distcorrectpluswrong.^2; % degeneration?
    distcorrectpluswrong = 4 ./ distcorrectpluswrong.^2; % norm_factor for derivative for every data sample
    if LRrelevances > 0
        Gw = zeros(n_vec,n_dim);
    end
    for k=1:nb_prototypes%(n_vec+1):size(lambda,1) % update all prototypes
        idxc = (k == pidxcorrect);  % Js: idxs where actual prototype is nearest correct
        idxw = (k == pidxwrong);    % Ks: idxs where actual prototype is nearest wrong

        dcd =  distcorrect(idxw) .* distcorrectpluswrong(idxw);
        dwd =    distwrong(idxc) .* distcorrectpluswrong(idxc);
        if LRrelevances > 0
            % part of derivative of distance
            difc = bsxfun(@minus,training_data(idxc,:),variables(k,:)); % DJs
            difw = bsxfun(@minus,training_data(idxw,:),variables(k,:)); % DKs
            % update omega
            Gw = Gw - (bsxfun(@times,difw,dcd.') * omegaT).' * difw + ...
                      (bsxfun(@times,difc,dwd.') * omegaT).' * difc;
            if LRprototypes > 0
                G(k,:) = dcd * difw - dwd * difc;
            end
        else
            if LRprototypes > 0
                G(k,:) = dcd * training_data(idxw,:) - dwd * training_data(idxc,:) + (sum(dwd)-sum(dcd)) * variables(k,:);
            end
        end
    end
if regularization,
    f3 = (pinv(omegaT'))';
else
    f3 = 0;
end
    % some rescalings needed
    if LRrelevances > 0
        G(nb_prototypes+1:nb_prototypes+n_vec,:) = 2/n_data * LRrelevances * Gw - regularization*f3;
    end
    if LRprototypes > 0
        G(1:nb_prototypes,:) = 1./n_data * LRprototypes * G(1:nb_prototypes,:) * omegaT * omegaT.';
    end
    G = G .* (1 + .0001 * (rand(size(G))-.5)); % help break symmetries
end
% if 0,
% w = variables(1:nb_prototypes,:);
% dJs = zeros(1,nb_samples);
% dKs = zeros(1,nb_samples);
% Js = zeros(1,nb_samples);
% Ks = zeros(1,nb_samples);
% norm_factors = zeros(1,nb_samples);
% DJs = zeros(nb_samples,size(training_data,2));
% DKs = zeros(nb_samples,size(training_data,2));
% for i=1:nb_samples
%     % select one training sample randomly
%     xi = training_data(i,:);
%     c_xi = trainLab(i);
%
%     dist = ((xi(ones(size(w,1),1),:))-w)*omegaT*(omegaT'*((xi(ones(size(w,1),1),:))-w)');
%     dist = diag(dist);
%     % determine the two winning prototypes
%     % nearest prototype with the same class
%     [sortDist,sortIdx] = sort(dist);
%     count = 1;
%     J = sortIdx(count);
%     while prototypeLabel(sortIdx(count)) ~= c_xi,
%         count = count+1;
%         J = sortIdx(count);
%     end
%     dJ = sortDist(count);
%     dJs(i) = dJ;
%     Js(i) = J;
%     count = 1;
%     K = sortIdx(count);
%     while prototypeLabel(sortIdx(count)) == c_xi,
%         count = count+1;
%         K = sortIdx(count);
%     end
%     dK = sortDist(count);
%     dKs(i) = dK;
%     Ks(i) = K;
%
%     wJ = w(J,:);
%     wK = w(K,:);
%     % prototype update
%     norm_factors(i) = 4/((dJ + dK)^2);
%     DJ = (xi-wJ);
%     DK = (xi-wK);
%     DJs(i,:) = DJ;
%     DKs(i,:) = DK;
% end
% end

function D = squaredEuclidean(A, B)
% computes the sqared Euclidean distance
%
%   D = squaredEuclidean(X) returns the squared Euclidean distance matrix of data in rows of X
%   D = squaredEuclidean(X, Y) returns the distance matrix with all distances between the points in X and Y.
%
if nargin == 1 % means that one matrix
    D = bsxfun(@plus, sumsquared(A,2), bsxfun(@minus, sumsquared(A,2).', 2*A*A.')); % 2*(Y*Y.')
else
    D = bsxfun(@plus, sumsquared(A,2), bsxfun(@minus, sumsquared(B,2).', 2*A*B.'));
end
D = max(D,0);

function sq  = sumsquared(x,dim)
% sq  = sumsquared(x,dim)
% sum of all squared element of matrix x along dimension dim

persistent isoctave

if isempty(isoctave)
  isoctave = exist('OCTAVE_VERSION','builtin');
end

if nargin == 1
  dim = 1;
end

if isoctave
  sq = sumsq(x,dim);
else
  sq = sum(x.^2, dim);
end