% CONDSTAT - accumulate surrogate data for comparing two data conditions
%
% Usage:
%     >> [diffres, accres, res1, res2] = condstat(formula, naccu, alpha, ...
%                                              bootside, condboot, arg1, arg2 ...);
%
% Inputs:
%    formula  - [string or cell array of strings] formula(s) to compute a given measure.
%               Takes arguments 'arg1', 'arg2' ... and X as inputs. e.g.,
%        'sum(arg1(:,:,X),3) ./ sqrt(sum(arg2(:,:,X))) ./ sqrt(sum(arg3(:,:,X)))'
%    naccu    - [integer] number of accumulations of surrogate data. e.g., 200
%    alpha    - [float] significance level (0<alpha<0.5)
%    bootside - ['both'|'upper'] side of the surrogate distribution to
%               consider for significance. This parameter affect the size
%               of the last dimension of accumulation array 'accres' (size
%               is 2 for 'both' and 1 for 'upper').
%    condboot - ['abs'|'angle'|'complex'|''] When comparing two conditions, compare
%               either absolute vales ('abs'), angles ('angles') or complex values
%               ('complex'). Either '' or 'complex' leave the formula unchanged;
%               'abs' takes its norm before subtraction, and 'angle' normalizes
%               each value (to norm 1) before taking the difference.
%    arg1     - [cell_array] of two 1D,2D,or 3D matrices of values to compare.
%               The last dimension of the array is shuffled to accumulate data, the
%               other dimensions must be the same size across matrices.
%               e.g. size(arg1{1})=[100 200 500], size(arg1{2})=[100 200 395]
%    arg2     - same as arg1, note that it is compared only to itself, and has
%               nothing to do with arg1 besides using the same formula, alpha, etc.
% ...argn     - may call n number of argument pairs
%
% Outputs:
%    diffres  - difference array for the actual (non-shuffled) data, if more than one
%               arg pair is called, format is a cell array of matrices.
%    accres  -  [cell array of 3D numerical arrays] for shuffled data, one per formula.
%    res1     - result for first condition
%    res2     - result for second condition
%
% Authors: Arnaud Delorme & Scott Makeig
%          CNL/Salk Institute 1998-2001; SCCN/INC/UCSD, La Jolla, 2002-
%
% See also: TIMEF, CROSSF

% Copyright (C) 2002  Arnaud Delorme, Lars Kai Hansen & Scott Makeig, SCCN/INC/UCSD
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function [diffres, accres, res1, res2] = condstat(formula, naccu, alpha, bootside, condboot, varargin);

if nargin < 6
	help condstat;
	return;
end

if ~ischar(formula) && ~iscell(formula)
	error('The first argument must be a string formula or cell array of string');
end
if ischar(formula)
	formula = { formula };
end
if ischar(bootside)
	bootside = { bootside };
end
for index = 1:length(bootside)
	if ~strcmpi(bootside{index}, 'both') && ~strcmpi(bootside{index}, 'upper')
		error('Bootside must be either ''both'' or ''upper''');
	end
end
if ischar(condboot)
	condboot = { condboot };
end
for index = 1:length(condboot)
	if isempty(condboot{index}), condboot{index} = 'complex'; end
end

for index = 1:length(varargin)
	if ~iscell(varargin) || length(varargin{index}) ~=2
		error('Except for the first arguments, all other arguments given to the function must be cell arrays of two numerical array');
	end
end

% accumulate coherence images (all arrays [nb_points x timesout x trials])
% ---------------------------
for index=1:length(varargin)
	tmpvar1 = varargin{index}{1};
	tmpvar2 = varargin{index}{2};
	if index == 1   % Shouldn't this be recalculated for arg2, etc.? TF 2007.06.04
		cond1trials = size(tmpvar1,ndims(tmpvar1));
		cond2trials = size(tmpvar2,ndims(tmpvar2));
		for tmpi = 1:length(formula)
			accres{tmpi} = zeros(size(tmpvar1,1), size(tmpvar1,2), naccu);
		end
	end

	if ndims(tmpvar1) == 2
		eval( [ 'arg' int2str(index) '=zeros(size(tmpvar1,1), cond1trials+cond2trials);' ] );
		eval( [ 'arg' int2str(index) '(:,1:cond1trials)=tmpvar1;' ] );
		eval( [ 'arg' int2str(index) '(:,cond1trials+1:end)=tmpvar2;' ] );
	else
		eval( [ 'arg' int2str(index) '=zeros(size(tmpvar1,1), size(tmpvar1,2), cond1trials+cond2trials);' ] );
		eval( [ 'arg' int2str(index) '(:,:,1:cond1trials)=tmpvar1;' ] );
		eval( [ 'arg' int2str(index) '(:,:,cond1trials+1:end)=tmpvar2;' ] );
	end
end

fprintf('Accumulating permutation statistics:');
alltrials = [1:cond1trials+cond2trials];

% processing formulas
% -------------------
formula1 = [];
formula2 = [];
for index = 1:length(formula)
	% updating formula
	% ----------------
	switch lower(condboot{ min(length(condboot), index) })
	 case 'abs', formula{index} = [ 'abs(' formula{index} ')' ];
	 %case 'angle', formula{index} = [ 'exp(j*angle(' formula{index} '))' ];
	 case 'angle', formula{index} = [ 'angle(' formula{index} ')/(2*pi)' ];
	 case 'complex', ;
	 otherwise, condboot, error('condstat argument must be either ''abs'', ''angle'', ''complex'' or empty');
	end

	% computing difference (non-shuffled)
	% -----------------------------------
	X = 1:cond1trials;
	eval( [ 'res1{index} = ' formula{index} ';'] );
	X = cond1trials+1:cond1trials+cond2trials;
	eval( [ 'res2{index} = ' formula{index}  ';'] );
	diffres{index} = res1{index}-res2{index};

	% build formula to execute
	% ------------------------
	arrayname = [ 'accres{' int2str(index) '}' ];
	if ndims(tmpvar1) == 2 % 2 dimensions
		formula1 = [ formula1 arrayname '(:,index) = ' formula{index} ';'];
		formula2 = [ formula2 arrayname '(:,index) = ' arrayname '(:,index) - ' formula{index}  ';'];
	else % 3 dimensions
		formula1 = [ formula1 arrayname '(:,:,index) = ' formula{index}  ';'];
		formula2 = [ formula2 arrayname '(:,:,index) = ' arrayname '(:,:,index) - ' formula{index}  ';'];
	end
end

% accumulating (shuffling)
% -----------------------
for index=1:naccu
	if rem(index,10) == 0,  fprintf(' %d',index); end
	if rem(index,120) == 0, fprintf('\n'); end

	alltrials = shuffle(alltrials);
	X = alltrials(1:cond1trials);
	eval( formula1 );
	X = alltrials(cond1trials+1:end);
	eval( formula2 );
end

% significance level
% ------------------
for index= 1:length(formula)
	accarray = accres{index};

    % size = nb_points*naccu
    % size = nb_points*naccu*times
	if ~isreal(accarray)    % might want to introduce a warning here: a complex
                            % result may not be desirable, and a single complex value
                            % in accarray could turn this from a 2-tail to 1-tail
                            % bootstrap test, leading to false positives. Hard to
                            % think of a meaningful warning though...
                            % TF 2007.06.04
		accarray = sqrt(accarray .* conj(accarray)); % faster than abs()
    end

    % compute bootstrap significance level
    i = round(naccu*alpha);
    switch ndims(accarray)
	 case 3
	     accarray = sort(accarray,3); % always sort on naccu (when 3D, naccu is the second dim)
         if strcmpi(bootside{min(length(bootside), index)}, 'upper')
			 accarray = accarray(:,:,floor(naccu-i/2+1));
	     else
			 accarray = accarray(:,:,[end:-1:1]);
			 accarraytmp(:,:,2) = accarray(:,:,ceil(i/2));
			 accarraytmp(:,:,1) = accarray(:,:,floor(naccu-i/2+1));
			 accarray = accarraytmp;
		 end

	 case 2
	     accarray = sort(accarray,2); % always sort on naccu (when 3D, naccu is the second dim)
         if strcmpi(bootside{min(length(bootside), index)}, 'upper')
			 accarray = accarray(:,floor(naccu-i/2+1));
	     else
			 accarraytmp(:,2) = accarray(:,ceil(i/2));
			 accarraytmp(:,1) = accarray(:,floor(naccu-i/2+1));
			 accarray = accarraytmp;
		 end
	 case 1
	     accarray = sort(accarray,1); % always sort on naccu (when 3D, naccu is the second dim)
         if strcmpi(bootside{min(length(bootside), index)}, 'upper')
			 accarray = accarray(floor(naccu-i/2+1));
	     else
			 accarraytmp(2) = accarray(ceil(i/2));
			 accarraytmp(1) = accarray(floor(naccu-i/2+1));
			 accarray = accarraytmp;
		 end
    end
    accres{index} = accarray;
end

if length(res1) == 1
	res1 = res1{1};
	res2 = res2{1};
	diffres = diffres{1};
	accres = accres{1};
end
fprintf('\n');
return;

% writing a function
% ------------------
% $$$ fid = fopen('tmpfunc.m', 'w');
% $$$ fprintf(fid, 'function [accres] = tmpfunc(alltrials, cond1trials, naccu,');
% $$$ for index=1:length(varargin)
% $$$ 	fprintf(fid, 'arg%d', index);
% $$$ 	if index ~=length(varargin), fprintf(fid,','); end
% $$$ end
% $$$ fprintf(fid, ')\n');
% $$$ commandstr = [ 'for index=1:naccu, ' ]
% $$$ % 			   'if rem(index,10) == 0,  disp(index); end;' ];
% $$$ % 			   'if rem(index,10) == 0,  fprintf('' %d'',index); end;' ...
% $$$ %	           'if rem(index,120) == 0, fprintf(''\n''); end;' ];
% $$$ commandstr = [ 	commandstr 'shuffle(alltrials);' ];
% $$$ commandstr = [ 	commandstr 'X = alltrials(1:cond1trials);' ];
% $$$ if ndims(tmpvar1) == 2 % 2 dimensions
% $$$ 	commandstr = [ 	commandstr 'accres(:,index) = ' formula ';'];
% $$$ 	commandstr = [ 	commandstr 'X = alltrials(cond1trials+1:end);'];
% $$$ 	commandstr = [ 	commandstr 'accres(:,index) = accres(:,index)-' formula ';end;'];
% $$$ else
% $$$ 	commandstr = [ 	commandstr 'res1 = ' formula ';' 10];
% $$$ 	commandstr = [ 	commandstr 'X = alltrials(cond1trials+1:end);' 10];
% $$$ 	commandstr = [ 	commandstr 'res2 = ' formula ';' 10];
% $$$ 	commandstr = [ 	commandstr 'accres(:,:,index) = res1 - res2; end;'];
% $$$ end;
% $$$ fprintf(fid, commandstr);
% $$$ fclose(fid);
% $$$ profile on;
% $$$ accres = tmpfunc(alltrials, cond1trials, naccu, arg1);
% $$$ profile report;
% $$$ profile off;
% $$$ return;

% evaluating a command
% --------------------
% $$$ commandstr = [ 'for index=1:naccu, ' ...
% $$$ 			   'if rem(index,10) == 0,  fprintf('' %d'',index); end;' ...
% $$$ 	           'if rem(index,120) == 0, fprintf(''\n''); end;' ];
% $$$ commandstr = [ 	commandstr 'shuffle(alltrials);' ];
% $$$ commandstr = [ 	commandstr 'X = alltrials(1:cond1trials);' ];
% $$$ if ndims(tmpvar1) == 2 % 2 dimensions
% $$$ 	commandstr = [ 	commandstr 'accres(:,index) = ' formula ';'];
% $$$ 	commandstr = [ 	commandstr 'X = alltrials(cond1trials+1:end);'];
% $$$ 	commandstr = [ 	commandstr 'accres(:,index) = accres(:,index)-' formula ';end;'];
% $$$ else
% $$$ 	commandstr = [ 	commandstr 'accres(:,:,index) = ' formula ';'];
% $$$ 	commandstr = [ 	commandstr 'X = alltrials(cond1trials+1:end);'];
% $$$ 	commandstr = [ 	commandstr 'accres(:,:,index) = accres(:,:,index)-' formula ';end;'];
% $$$ end;
% $$$ eval(commandstr);
% $$$ return;
% $$$


function g = ndsimKernGradient(kern, t1, varargin)

% NDSIMKERNGRADIENT Gradient of SIM kernel's parameters.
% FORMAT
% DESC computes the gradient of functions with respect to the
% single input motif
% 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 t : 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 t1 : the input locations associated with the rows of the
% kernel matrix.
% ARG t2 : 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.
%
% That is, this function computes sum(sum(deriv(K,a).*partial)) for
% each scalar parameter a, where deriv(K,a) is the matrix-valued
% derivative of the kernel K (computed between times t and t2) with
% respect to the parameter a.
%
% SEEALSO simKernParamInit, kernGradient, simKernDiagGradient, kernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2006
%
% COPYRIGHT : Jaakko Peltonen, 2011

% KERN

if length(varargin)<2
  t2 = t1;
  covGrad = varargin{1};
else
  t2 = varargin{1};
  covGrad = varargin{2};
end

sigma = sqrt(2/kern.inverseWidth);
if isfield(kern, 'isNegativeS') && (kern.isNegativeS == true)
    variancemultiplier = (kern.sensitivity*kern.sensitivity);
else
    variancemultiplier = kern.variance;
end

dim1 = size(t1, 1);
dim2 = size(t2, 1);
t1Mat = t1(:, ones(1, dim2));
t2Mat = t2(:, ones(1, dim1))';
diffT = (t1Mat - t2Mat);

k1a = sqrt(pi)/2*( t1Mat.*erf(t1Mat/sigma) + t2Mat.*erf(t2Mat/sigma) - diffT.*erf(diffT/sigma) );
k1b = sigma/2*( exp(-(t1Mat/sigma).^2) + exp(-(t2Mat/sigma).^2) - exp(-(diffT/sigma).^2) - 1 );
K = (k1a+k1b)*sigma*variancemultiplier;

% k = ndsimKernCompute(kern, t1, t2);
% deriv_variancemultiplier = sum(sum((k/variancemultiplier).*covGrad));
% deriv_sigma = k/sigma + variancemultiplier*sigma/2*(exp(-(t1Mat/sigma).^2)+exp(-(t2Mat/sigma).^2)-exp(-(diffT/sigma).^2)-1);
% deriv_sigma = sum(sum(deriv_sigma.*covGrad));
% % sigma=sqrt(2/inversewidth)
% % --> df/dinversewidth = (df/dsigma)*(dsigma/dinversewidth)
% % = (df/dsigma)*((-1/2)*sqrt(2)*(inversewidth^(-3/2)))
% % = (df/dsigma)*(-1/2*sigma/inversewidth)
% deriv_inversewidth=deriv_sigma*(-0.5*sigma/kern.inverseWidth);

deriv_variancemultiplier = sum(sum(sigma*(k1a+k1b).*covGrad));
deriv_sigma = (k1a+2*k1b)*variancemultiplier;
% deriv_sigma = (k1a+k1b)*variancemultiplier ...
%     + variancemultiplier*sigma/2*(exp(-(t1Mat/sigma).^2)+exp(-(t2Mat/sigma).^2)-exp(-(diffT/sigma).^2)-1) ...
%     + variancemultiplier/sigma*(-(t1Mat.^2).*exp(-(t1Mat/sigma).^2)-(t2Mat.^2).*exp(-(t2Mat/sigma).^2)+(diffT.^2).*exp(-(diffT/sigma).^2)) ...
%    + variancemultiplier/sigma*(-(t1Mat.^2).*exp(-(t1Mat/sigma).^2)-(t2Mat.^2).*exp(-(t2Mat/sigma).^2)+(diffT.^2).*exp(-(diffT/sigma).^2)) ;

deriv_sigma = sum(sum(deriv_sigma.*covGrad));
deriv_inversewidth=deriv_sigma*(-0.5*sigma/kern.inverseWidth);

if isfield(kern, 'isNegativeS') && (kern.isNegativeS == true)
  % variancemultiplier=sensitivity^2
  % --> df/dsensitivity = (df/dvarmult)*(dvarmult/dsensitivity)
  % = (df/dvarmult)*(2*sensitivity)
  deriv_sensitivity=deriv_variancemultiplier*2*kern.sensitivity;
  g = [deriv_inversewidth deriv_sensitivity];
else
  g = [deriv_inversewidth deriv_variancemultiplier];
end

% gaussianinitial currently unsupported


classdef lds < dml.method
%LDS linear dynamical system.
%
%   DESCRIPTION
%   data X is represented as trials x features x timepoints
%
%   State can be partially observed/unobserved during training.
%   partial observability of multiple observations is also supported
%   observations are normally distributed conditional on the state.
%   multiple observation sequences are supported
%
%   bias term is *NOT* automatically added to the model
%
%   K = number of states
%   M = number of observations
%   T = number of timesteps
%
%   X and Y are swapped wrt the Kalman filter conventions
%
%   EXAMPLE
%   rand('seed',2); randn('seed',2);
%
%   nsamples = 1000; ncov = 2; ncycles = 10; ntrials = 2;
%   Y = sin(ncycles * 2 * pi * (1:nsamples) ./ nsamples);
%   Y = repmat(reshape(Y,[1 1 numel(Y)]),[ntrials 1 1]);
%   X = repmat(Y,[1 ncov 1]) + 0.5*randn(ntrials,ncov,nsamples);
%
%   k = dml.lds('inference','smooth','verbose',true,'indims',[ncov nsamples]);
%   k = k.train(X,Y);
%   Z = k.test(X);
%   figure
%   plot(squeeze(Y(1,:,:))','k');
%   hold on;
%   plot(squeeze(Z(1,:,:))','r');
%   legend('real','predicted');
%
%   k = dml.lds('verbose',true);
%   k = k.train(X,nan(size(Y))); % hidden state estimation
%   U = repmat(Y,[1 ncov 1]) + 0.1*randn(ntrials,ncov,nsamples);
%   Z = k.test(U);
%   figure
%   plot(squeeze(Y(1,:,:))','k--','LineWidth',2);
%   hold on;
%   plot(zscore(squeeze(Z(1,:,:))'),'r','LineWidth',2);
%   plot(squeeze(X(1,:,:))','bo');
%   legend('real','predicted','observed');
%
%   k = dml.lds('inference','smooth','verbose',true);
%   k = k.train(X,[Y nan(size(Y))]); % mixture of hidden + observed states
%   U = repmat(Y,[1 ncov 1]) + 0.1*randn(ntrials,ncov,nsamples);
%   Z = k.test(X);
%   figure
%   plot(zscore(squeeze(Y(1,:,:))'),'k');
%   hold on;
%   plot(zscore(squeeze(Z(1,:,:))'),'r');
%   plot(zscore(squeeze(X(1,:,:))'),'bo');
%   legend('real','predicted','observed');
%
%   REFERENCES
%   Pattern Recognition and Machine Learning, Bishop
%   A unifying review of linear dynamical systems, Gharamani
%
%   DEVELOPER
%   Marcel van Gerven (m.vangerven@donders.ru.nl)
%   Ali Bahramisharif (ali@cs.ru.nl)

  properties

    maxiter = 100; % maximum number of EM iterations
    thresh = 1e-4; % EM convergence threshold
    diagQ = 0; % regularize state noise to diagonal (0 <= diagQ <=1)
    diagR = 0; % regularize measurement noise to diagonal (0 <= diagR <=1)

    inference = 'smooth'; % filter / smooth

    epsilon = 0 % added to the diagonal of the covariance matrices for numerical stability (e.g. 1e-7);

    A % K x K transition matrix for the unobservable state
    C % M x K emission matrix for the observations
    mu0 % K x 1 the initial mean of the hidden state
    V0 % K x K the initial hidden noise covariance
    R % M x M measurement noise covariance
    Q % K x K state noise covariance

    loglik % log likelihood

    nhidden = 1 % number of hidden states; automatically determined if Y is given

  end

  methods

    function obj = lds(varargin)

      obj = obj@dml.method(varargin{:});
    end

    function obj = train(obj,X,Y)

      % resize data if indims is specified
      if ~isempty(obj.indims)
        X = reshape(X,[size(X,1) obj.indims]);
      end

      % cast to cell array (multiple observation sequences)
      % representation as variables * timepoints
      if ~iscell(X)
        if ndims(X) ~= 3, error('input should be of size trials x features x time'); end
        Xt = cell(1,size(X,1));
        for c=1:length(Xt)
          Xt{c} = reshape(X(c,:,:),[size(X,2) size(X,3)]);
        end
        X = Xt; clear Xt;
      else
        for c=1:length(X)
          X{c} = X{c};
        end
      end

      if nargin < 3

        % assume nhidden signals underlying the observations
        Y = cell(1,length(X));
        for c=1:length(X)
          Y{c} = nan(obj.nhidden,size(X{c},2));
        end

      else

        if ~iscell(Y)
          if ndims(Y) ~= 3, error('input should be of size trials x features x time'); end
          Yt = cell(1,size(Y,1));
          for c=1:length(Yt)
            Yt{c} = reshape(Y(c,:,:),[size(Y,2) size(Y,3)]);
          end
          Y = Yt; clear Yt;
        else
          for c=1:length(Y)
            Y{c} = Y{c}';
          end
        end

      end

      % remove empty elements
      eidx = cellfun(@(x)(isempty(x)),X);
      eidx = eidx & cellfun(@(x)(isempty(x)),Y);
      X = X(~eidx);
      Y = Y(~eidx);

      K = size(Y{1},1);
      M = size(X{1},1);
      N = length(X);

      if isempty(obj.A), obj.A = 1e-3*randn(K,K); end
      if isempty(obj.C), obj.C = 1e-3*randn(M,K); end
      if isempty(obj.mu0), obj.mu0 = 1e-3*randn(K,1); end
      if isempty(obj.V0), obj.V0 = 1e-3*eye(K); end
      if isempty(obj.R), obj.R = 1e-3*eye(M); end
      if isempty(obj.Q), obj.Q = 1e-3*eye(K); end

%       % DEBUG
%       [obj.A, obj.C, obj.Q, obj.R, obj.mu0, obj.V0, obj.loglik] = learn_kalman(X,obj.A,obj.C,obj.Q,obj.R,obj.mu0,obj.V0);
%       return;

      oldLL = inf;
      LL = 0;
      loglik = [];
      iter = 0;
      while abs(LL - oldLL) > obj.thresh && iter < obj.maxiter

        oldLL = LL;

        % E step
        LL=0;
        for c=1:N % iterate over sequences

          [G1t,G2t,G3t,G4t,G5t,G6t,mu0t,V0t,LLt] = obj.Estep(X{c},Y{c});

          if c==1

            G1=G1t;
            G2=G2t;
            G3=G3t;
            G4=G4t;
            G5=G5t;
            G6=G6t;
            mu0=mu0t;
            V0=V0t + mu0t*mu0t';
            LL=LLt;

          else

            G1 = G1 + G1t;
            G2 = G2 + G2t;
            G3 = G3 + G3t;
            G4 = G4 + G4t;
            G5 = G5 + G5t;
            G6 = G6 + G6t;

            mu0 = cat(2,mu0,mu0t);
            V0 = V0 + V0t + mu0t*mu0t';

            LL = LL + LLt;

          end

        end

        loglik = [loglik LL];

        if obj.verbose && ~isnan(LL)
          fprintf('EM step: %d; log likelihood: %g\n',iter,LL);
        end

        if iter && (LL < oldLL), fprintf('non-decreasing log likelihood!\n'); end

        % M step
        T = sum(cellfun(@(x)(size(x,2)),X));

        obj.C = G6 / G1;

        obj.R = (G5 - obj.C * G6') ./ T;
        obj.R = (obj.R + obj.R') ./ 2;

        if obj.diagR
          obj.R = (1-obj.diagR) * obj.R + obj.diagR * diag(diag(obj.R));
        end

        obj.A = G4 / G3;

        obj.Q = (G2 - obj.A * G4') ./ (T-N);
        obj.Q = (obj.Q + obj.Q') ./ 2;

        if obj.diagQ
          obj.Q = (1-obj.diagQ) * obj.Q + obj.diagQ * diag(diag(obj.Q));
        end
        obj.mu0 = mean(mu0,2);
        obj.V0 = V0/N - obj.mu0*obj.mu0'+(mu0-repmat(obj.mu0,[1 N]))*(mu0-repmat(obj.mu0,[1 N]))'/N;

        % symmetricize and make positive semidefinite
        obj.V0 = (obj.V0 + obj.V0') ./ 2;
        obj.V0 = obj.V0 + obj.epsilon*eye(size(obj.V0));

        iter = iter + 1;

        if ~any(isnan(Y{1}(:)))
          break;
        end

      end

      if obj.verbose
        fprintf('EM step: %d; log likelihood: %g\n',iter,LL);
      end

      obj.loglik = loglik(2:end);

    end

    function [mu,V,loglik] = test(obj,X)
     % LDS inference

     if ~iscell(X)
       if ndims(X) ~= 3, error('input should be of size trials x features x time'); end
       Xt = cell(1,size(X,1));
       for c=1:length(Xt)
         Xt{c} = squeeze(X(c,:,:));
       end
       X = Xt; clear Xt;
     end

     nsets = length(X);

     mu = cell(1,nsets);
     for c=1:nsets

       [m,V,loglik] = obj.filter(X{c});

       if strcmp(obj.inference,'smooth')
         [m,V] = obj.smooth(m,V);
       end

       mu{c} = m';

     end

     % reshape if all sequences are of the same size
     if numel(unique(cellfun(@(x)(size(x,1)),mu)))==1
      Z = zeros(length(mu),size(mu{1},2),size(mu{1},1));
      for z=1:length(mu)
        Z(z,:,:) = mu{z}';
      end
      mu = Z;
     end

    end

    function [mu,V,J,VP] = smooth(obj,mu1,V1,Y)
      % Kalman smoother; uses filtered means and variances

      T = numel(V1);

      nandim=[];
      dim=[];
      if exist('Y','var')
          for i=1:size(Y,1)
              if isnan(Y(i,1))
                  nandim=[nandim i];
              else
                  dim=[dim,i];
              end
          end

          A = obj.A(nandim,nandim);
          C = obj.C(:,nandim);
          Q = obj.Q(nandim,nandim);
      else

          A   = obj.A;
          Q   = obj.Q;
          C   = obj.C;
      end
      % P(t,t-1) at horizon
      P = A * V1{T-1} * A' + Q;
      PC = P * C';
      K = PC / (C * PC + obj.R);
      VP=cell(1,T);
      KC=K*C;
      VP{T}=(eye(size(KC))-KC)*A*V1{T-1};

      % RTS equations

      mu = mu1;
      V = V1;
      J = cell(1,T-1); % needed to compute transition matrix A in EM
      for n=T:-1:2

          P = A * V1{n-1} * A' + Q;
        if det(P)<0
            disp('bad predictive covariance')
            lambda=max(svd(P));
            P=P+(lambda+obj.epsilon)*eye(size(P));
        end
        if ~all(V{n}(:)==0)

          J{n-1} = (V1{n-1} * A') / P;

        if numel(nandim)>0
          mu(:,n-1) = mu1(:,n-1) + J{n-1} * (mu(:,n) - A * mu1(:,n-1)-obj.A(nandim,dim)*Y(dim,n-1));
        else
          mu(:,n-1) = mu1(:,n-1) + J{n-1} * (mu(:,n) - A * mu1(:,n-1));
        end

          V{n-1} = V1{n-1} + J{n-1} * (V{n} - P)*J{n-1}';

          if n<T
            VP{n}=V1{n}*J{n-1}'+J{n}*(VP{n+1}-A*V1{n})*J{n-1}';
          end

        else

          % fix when variance is zero (deterministic state)
          J{n} = zeros(size(V{n}));
          mu(:,n) = mu1(:,n);
          V{n} = zeros(size(V{n}));
          VP{n} = zeros(size(V{n}));

        end

        % symmetricize and make positive semidefinite
        V{n} = (V{n} + V{n}') ./ 2;
        V{n} = V{n} + obj.epsilon*eye(size(V{n}));

      end

    end

    function [mu,V,LL] = filter(obj,X,varargin)
      % Kalman filter algorithm; returns mean mu and covariance matrix V of
      % the states, the log likelihood and the observations Xobs with missing
      % values imputed (the latter remains to be done)

      nandim=[];
      dim=[];
      if ~isempty(varargin)
          Y=varargin{1};
      end
      if exist('Y','var')
          for i=1:size(Y,1)
              if isnan(Y(i,1))
                  nandim=[nandim i];
              else
                  dim=[dim,i];
              end
          end

          A = obj.A(nandim,nandim);
          C = obj.C(:,nandim);
          mu0 = obj.mu0(nandim);
          V0 = obj.V0(nandim,nandim);
          R = obj.R;
          Q = obj.Q(nandim,nandim);
          X1=X;
          X=X-obj.C(:,dim)*Y(dim,:);
      else
          A = obj.A;
          C = obj.C;
          mu0 = obj.mu0;
          V0 = obj.V0;
          R = obj.R;
          Q = obj.Q;
          Y=zeros(size(A,1),size(X,2));
          X1=X;
      end

      K = size(A,1);
      T = size(X,2);

      mu = nan(K,T);
      V = cell(1,T);
      I = eye(K);

      Xpred=mu;
      Vpred=V;

      % take measurements into account
      if isempty(X) || all(isnan(X(:,1)))

        mu(:,1) = mu0;
        V{1} = V0;

      else
        Xpred(:,1)=mu0;
        Vpred{1}=V0;
        obs = ~isnan(X(:,1));
        Cb = C(obs,:); % emission matrix for observed measurements

        % Kalman gain matrix
        K = (V0 * Cb') / (Cb * V0 * Cb' + R(obs,obs));

        mu(:,1) = mu0 + K * (X(obs,1) - Cb * mu0);
        V{1}  = (I - K * Cb) * V0;

      end

      % symmetricize and make positive semidefinite
      V{1} = (V{1} + V{1}') ./ 2;
      V{1} = V{1} + obj.epsilon*eye(size(V{1}));

      for t=2:T

        if numel(nandim)>0
          AM = A * mu(:,t-1)+obj.A(nandim,dim)*Y(dim,t-1);
        else
            AM = A * mu(:,t-1);
        end
        P = A * V{t-1} * A' + Q;

        if isempty(X) || all(isnan(X(:,t)))

          mu(:,t) = AM;
          V{t} = P;

        else
            Xpred(:,t)=AM;
            Vpred{t}=P;
          obs = ~isnan(X(:,t));
          Cb = C(obs,:); % emission matrix for observed measurements

          PC = P * Cb';

          K = PC / (Cb * PC + R(obs,obs));

          mu(:,t) = AM + K * (X(obs,t) - Cb * AM);
          V{t}  = (I - K * Cb) * P;

        end

        % symmetricize and make positive semidefinite
        V{t} = (V{t} + V{t}') ./ 2;
        V{t} = V{t} + obj.epsilon*eye(size(V{t}));

      end

      % compute log likelihood using error decomposition ignoring constant terms
      if nargout >= 3
          V1 = cell(1,T);
          K=size(obj.A,1);
          Y1=zeros(K,T);
          Y1(dim,:)=Y(dim,:);
          if numel(nandim)<1
              Y1=mu;
              V1=V;
          else
              Y1(nandim,:)=Xpred;
              i=1;
              V1{i}=zeros(K);
              V1{i}(dim,dim)=obj.Q(dim,dim);
              V1{i}(dim,nandim)=obj.Q(dim,nandim);
              V1{i}(nandim,dim)=obj.Q(nandim,dim);
              V1{i}(nandim,nandim)=Vpred{i};
              for i=2:T
                  V1{i}=zeros(K);
                  V1{i}(dim,dim)=obj.Q(dim,dim);
                  V1{i}(dim,nandim)=obj.Q(dim,nandim);
                  V1{i}(nandim,dim)=obj.Q(nandim,dim);
                  V1{i}(nandim,nandim)=Vpred{i};
              end
          end
          LL = obj.compute_loglik(X1,Y1,V1);
      end

    end

    function ll = likelihood(obj,X)
      % returns log likelihood of a sequence of observations

      if ~iscell(X)
        if ndims(X) ~= 3, error('input should be of size trials x features x time'); end
        Xt = cell(1,size(X,1));
        for c=1:length(Xt)
          Xt{c} = squeeze(X(c,:,:));
        end
        X = Xt; clear Xt;
      end

      ll = nan(length(X),1);

      for c=1:length(X)

        [mu,V,ll(c)] = obj.filter(X{c});

      end

    end

  end

  methods(Access=protected)

    function [G1,G2,G3,G4,G5,G6,mu0,V0,LL] = Estep(obj,X,Y)

      K = size(Y,1);
      M = size(X,1);
      T = size(X,2);

      nandim=[];
      dim=[];
      if exist('Y','var')
          for i=1:K
              if isnan(Y(i,1))
                  nandim=[nandim i];
              else
                  dim=[dim,i];
              end
          end
      end

      if any(isnan(Y(:))) % hidden state

          [mu1,V1,LL] = obj.filter(X,Y);
          [Y1,V1,J1,VP1] = obj.smooth(mu1,V1,Y);

          Y(nandim,:)=Y1;
          VP=cell(1,T);
          V = cell(1,T);
          %J = cell(1,T-1);
          for i=1:T
              if i>1
                  VP{i}=zeros(K);
                  VP{i}(nandim,nandim)=VP1{i};
              end
              V{i}=zeros(K);
              V{i}(dim,dim)=V{i}(dim,dim)+obj.epsilon*eye(length(dim));
              V{i}(nandim,nandim)=V1{i};
              %if i<T
              %    J{i}=zeros(K);
              %    J{i}(nandim,nandim)=J1{i};
              %end
          end

      else

        V = repmat({0},size(Y));
        %J = repmat({0},[size(Y,1) size(Y,2)-1]);
        VP = repmat({0},size(Y));
        LL = nan;

      end

      G1 = zeros(K,K);
      G4 = zeros(K,K);
      for t=1:T

        G1  = G1 + V{t} + Y(:,t)*Y(:,t)';

        if t>1

          G4 = G4 + Y(:,t)*Y(:,t-1)' + VP{t};
          %G4 = G4 + J{t-1}*V{t} + Y(:,t)*Y(:,t-1)'; % bishop

        end

      end

      G2 = G1 - Y(:,1) * Y(:,1)' - V{1};
      G3 = G1 - Y(:,T) * Y(:,T)' - V{T};

      mu0 = Y(:,1);
      V0 = V{1};

      % deal with partially observed observations
      if any(isnan(X(:)))

        C = obj.C;
        R = obj.R;

        G5 = zeros(M,M);
        for t=1:T

          XX = X(:,t) * X(:,t)';
          nidx = isnan(XX(:));
          if any(nidx)
            E = X(:,t) * Y(:,t)' * C' + R;
            XX(nidx) = E(nidx);
          end
          nidx = isnan(XX(:));
          if any(nidx)
            E = C * Y(:,t) * X(:,t)' + R;
            XX(nidx) = E(nidx);
          end
          nidx = isnan(XX(:));
          if any(nidx)
            E = C * (V{t} + Y(:,t)*Y(:,t)') * C' + R;
            XX(nidx) = E(nidx);
          end

          G5 =  G5 + XX;

        end

        G6 = zeros(M,K);
        for t=1:T

          XY = X(:,t) * Y(:,t)';
          EXY = C * (V{t} + Y(:,t)*Y(:,t)');
          nidx = isnan(XY(:));
          XY(nidx) = EXY(nidx);
          G6 = G6 + XY;

        end

      else
        G5 = X * X';
        G6 = X * Y';
      end

    end

    function LL = compute_loglik(obj,X,Y,V)

      T = size(X,2);

      C = obj.C;
      R = obj.R;

      LL = 0;
      for t=1:T

        obs = ~isnan(X(:,t));

        e = X(obs,t) - C(obs,:)*Y(:,t); % innovation (measurement error)
        S = C(obs,:)*V{t}*C(obs,:)' + R(obs,obs); % covariance of the innovation
        LL = LL - log(det(S)) ./ 2 - ((e' / S) * e) ./ 2;
        if ~isreal(LL)
            LL=real(LL);
            disp('bad likelihood')
        end
        LL = LL - (numel(obs)/2) * log(2*pi); %length of the feature vector

      end

    end

  end

end


function [mu, s2, MM, VV] = simulLMGPmc(hyp, inffcn, meanfcn, covfcn, likfcn, input, target, targetvariance,...
    derivinput, derivtarget, derivvariance, xt, lag, Nsamples)
% simulLMGPmc - Simulation of the dynamic GP model with incorporated local models (LMGP models),
% where the output variance is propagated using the numerical Monte Carlo approximation
%
%% Syntax
%  [mu, s2, MM, VV] = simulLMGPmc(hyp, inf, mean, cov, lik, input, target, targetvariance,...
%     derivinput, derivtarget, derivvariance, xt, lag, Nsamples)
%
%% Description
% Idea: at every time step the output of GP model is approximated with
% the Gaussian distribution, from which we sample one value. We repeat the
% procedure Nsamples-times.
% Nsamples samples, which are used as the future inputs of the GP model.
% Samples are re-used if necessary (ie. y(k-1) for y(k-2) if lag=2 etc.)
% Currently it can be used only with the Gaussian covariance function and
% with the white noise model (sum of covSEard and covNoise) due to the gpSD00.
% Uses routine gpSD00.
% see K. Azman. Identifikacija dinamicnih sistemov z Gaussovimi procesi. PhD
% thesis, University of Ljubljana, Ljubljana, 2007 (in Slovene).
%
% Input:
% * hyp            ... the structure of hyperparameters
% * inf      	   ... the inference method 	      --> not used, for interface compatibility only
% * cov      	   ... the prior covariance function  --> not used, for interface compatibility only
% * mean    	   ... the prior mean function        --> not used, for interface compatibility only
% * lik      	   ... the likelihood function        --> not used, for interface compatibility only
% * input          ... the input part of the training data,  NxD matrix
% * target         ... the output part of the training data (ie. target), Nx1 vector
% * targetvariance ... the target variance, use NaN where not known
% * derivinput     ... the input part of the derivative training data, NEQxD matrix
% * derivtarget    ... target derivatives, NEQxD matrix
% * derivvariance  ... variances of the local model prameters, NEQxD matrix
% * xt             ... the input matrix for simulation, kxD vector, see
%                      construct_ARXsimul_input.m for more info
% * lag            ... the order of the model (number of used lagged outputs)
% * Nsamples       ... the number of samples used in algorithm (ie. runs of simulation)
%
% Output:
% * mu             ... the mean predicted output
% * s2             ... associated variances (including noise variance)
% * MM             ... the matrix of all predicted means, kxNsamples
% * VV             ... associated predicted variances
%
% See Also
% gpSD00.m, simulLMGPnaive.m, simulGPmc.m
%
% Examples
% demo_example_lmgp_simulation.m
%
%%
% * Written by K.Azman, 31.05.2005
% * Based on the work of C.E. Rasmussen and A. Girard.
%
%
% Changelog:
%
% 16.2.2015, Martin Stepancic:
%		 	-changed the function interface as gpml > 3.0
%			-removed the addition of autocovariance - this is now
%			 already included in gpSD00.m.
%

fun_name = 'simulLMGPmc';

[n, D]      = size(input);
fullinput  = [input; repmat(derivinput,D,1)];
fulltarget = [target; derivtarget(:)];
[N, D]      = size(fullinput);
[nD, D] = size(derivinput);

MM = zeros(Nsamples,size(xt,1));
VV = zeros(Nsamples,size(xt,1));

for jjj=1:Nsamples

    if(mod(jjj,10)==0)
        disp([fun_name, ', run:  ',int2str(jjj),'/',int2str(Nsamples)]);
    end

    % 1st point - input is "point"
    test = xt(1,:);
    [mu(1), s2(1)] = gpSD00(hyp, inffcn, meanfcn, covfcn, likfcn, input, target, ...
			targetvariance, derivinput, derivtarget, derivvariance, test);

    for k=2:length(xt)

        test = [xt(k, lag+1:end)];

        % For the NEXT prediction...
        % assumed normal distribution, for more accurate procedure see
        % simulGPmc

        % random sample from assumed distribution
        ysampled = mu(k-1) + randn(1)*sqrt(s2(k-1));
        % generate input for the GP model
        if (k>lag)
            test = [mu(k-lag:k-2) ysampled xt(k, lag+1:end)];
        elseif (k<=lag)
            test = [xt(k, 1:lag-k+1) mu(1:k-2) ysampled xt(k, lag+1:end)];
        end

        [mu(k), s2(k)] = gpSD00(hyp, inffcn, meanfcn, covfcn, likfcn, input, target, ...
			targetvariance, derivinput, derivtarget, derivvariance, test);
    end

    MM(jjj,:) = mu;
    VV(jjj,:) = s2;

end
% individual realisations saved in matrices MM and VV
% approximate all output distributions with Gaussian distribution
mu = mean(MM);
s2 = mean(VV) + mean((MM-repmat(mu,Nsamples,1)).^2);

mu = mu';
s2 = s2';

MM = MM';
VV = VV';

return


function cvx_optval = matrix_frac( x,Y )

%MATRIX_FRAC   Internal cvx version.

narginchk(2,2);
if ndims( Y ) > 2 || size( Y, 1 ) ~= size( Y, 2 ), %#ok

    error( 'Second argument must be a square matrix.' );

elseif ndims( x ) > 2 || size( x, 2 ) > 1, %#ok

    error( 'First argument must be a column vector.' );

elseif size( x, 1 ) ~= size( Y, 1 ),

    error( 'Size of first argument (vector) must match size of second argument (matrix).' );

elseif cvx_isconstant( x ) && cvx_isconstant( Y ),

    cvx_optval = cvx( matrix_frac( cvx_constant( x ), cvx_constant(Y) ) );

elseif cvx_isaffine( x ) && cvx_isaffine( Y ),

    n = size( x, 1 );
    z = [];
    cvx_begin
        epigraph variable z
        [Y x; x' z] == semidefinite( n+1 ); %#ok
    cvx_end

else

    error( 'Disciplined convex programming error:\n    MATRIX_FRAC is convex and nonmonotonic, so its input must be affine.' );

end

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


function [p1,p2,P1_l,P2_l] = ahmLin2ahmPnts(l)

% AHMLIN2AHMPNTS  AHM line to two AHM points conversion.
%   [P1,P2] = AHMLIN2AHMPNTS(L) extracts the two endpoints of the AHM line
%   L in the form of two HMG points with the same anchor of that of the
%   line.
%
%   [p1,p2,P1_l,P2_l] = AHMLIN2AHMPNTS(...) returns the Jacobians wrt L.

%   Copyright 2009 Teresa Vidal.

p1 = l(1:7,:);
p2 = l([1:3 8:11],:);

if nargout > 2

    P1_l = [eye(7) zeros(7,4)];
    P2_l = [eye(3) zeros(3,8) ; zeros(4,7) eye(4)];

end

return

%% jac

syms x y z a1 b1 c1 n1 a2 b2 c2 n2 real
l = [x y z a1 b1 c1 n1 a2 b2 c2 n2]';

[p1,p2,P1_l,P2_l] = ahmLin2ahmPnts(l);

simplify(P1_l - jacobian(p1,l))
simplify(P2_l - jacobian(p2,l))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [StopFlag, Status] = StopCriterion(grad, nhxk, Niter, Nmap, Ngmap, MaxNumIter, MaxNumMapEval, MaxNumGmapEval, T, TimeLimit, epsilon, nhx0, ngradx0, Stopping_Crit)
% `StopCriterion` is a function checking that one of the stopping criteria
% holds to terminate LLM and GLM. It perepare the status determining why
% the algorithm is stopped.
%
% USAGE:
%
%    [StopFlag, Status] = StopCriterion(grad, nhxk, Niter, Nmap, Ngmap, MaxNumIter, MaxNumMapEval, MaxNumGmapEval, T, TimeLimit, epsilon, nhx0, ngradx0, Stopping_Crit)
%
% INPUTS:
%    grad:              gradient of the merit funcrion
%    nhxk:              the norm 2 of `h(xk)`
%    MaxNumIter:        maximum number of iterations
%    MaxNumMapEval:     maximum number of function evaluations
%    MaxNumGmapEval:    maximum number of subgradient evaluations
%    TimeLimit:         maximum running time
%    epsilon:           accuracy parameter
%    Stopping_Crit:     stopping criterion
%
%                         1. stop if :math:`||grad|| \leq \epsilon`
%                         2. stop if :math:`||nhxk|| \leq \epsilon`
%                         3. stop if `MaxNumIter` is reached
%                         4. stop if `MaxNumMapEval` is reached
%                         5. stop if `MaxNumGmapEval` is reached
%                         6. stop if `TimeLimit` is reached
%                         7. stop if :math:`||grad|| \leq \textrm{max}(\epsilon, \epsilon^2 * ngradx0)`
%                         8. stop if :math:`||nhxk|| \leq \textrm{max}(\epsilon, \epsilon^2 * nhx0)`
%                         9. stop if (default) :math:`||hxk|| \leq \epsilon` or `MaxNumIter` is reached
%
% OUTPUTS:
%    StopFlag:          1: if one of the stopping criteria holds, 0: if none of the stopping criteria holds
%    Status:            the reason of the scheme termination

switch Stopping_Crit
  case 1
    if norm(grad) <= epsilon
      StopFlag = 1;
      Status   = 'Local (global) solution of merit function is found.';
    else
      StopFlag = 0;
      Status   = [];
    end
  case 2
    if nhxk <= epsilon
      StopFlag = 1;
      Status   = 'A solution of nonlinear system is found.';
    else
      StopFlag = 0;
      Status   = [];
    end
  case 3
    if Niter >= MaxNumIter
      StopFlag = 1;
      Status   = 'Maximum number of iterations is reached.';
    else
      StopFlag = 0;
      Status   = [];
    end
  case 4
    if Nmap >= MaxNumMapEval
      StopFlag = 1;
      Status   = 'Maximum number of mapping evaluations is reached.';
    else
      StopFlag = 0;
      Status   = [];
    end
  case 5
    if Ngmap >= MaxNumGmapEval
      StopFlag = 1;
      Status   = 'Maximum number of gradient evaluations is reached.';
    else
      StopFlag = 0;
      Status   = [];
    end
  case 6
    if T >= TimeLimit
      StopFlag = 1;
      Status   = 'Time limit is reached.';
    else
      StopFlag = 0;
      Status   = [];
    end

  case 7
    if norm(grad) <= max(epsilon,epsilon^2*ngradx0)
      StopFlag = 1;
      Status   = 'A possible stationary point is found.';
    else
      StopFlag = 0;
      Status   = [];
    end

  case 8
    if nhxk <= max(epsilon,epsilon^2*nhx0)
      StopFlag = 1;
      Status   = 'A possible solution is found.';
    else
      StopFlag = 0;
      Status   = [];
    end

  case 9
    if (nhxk <= epsilon || Niter >= MaxNumIter)
      StopFlag = 1;
      if Niter < MaxNumIter
          Status = 'a solution is found.';
      else
          Status = 'Maximum number of iterations is reached.';
      end
    else
      StopFlag = 0;
      Status   = [];
    end

end

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%% End of StopCriterion.m %%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


classdef matRad_MinMaxDose < DoseConstraints.matRad_DoseConstraint
    % matRad_MinMaxDose Implements a MinMaxDose constraint
    %   See matRad_DoseConstraint for interface description
    %
    % use log sum exp approximation, see appendix A in
    % http://scitation.aip.org/content/aapm/journal/medphys/41/8/10.1118/1.4883837
    %
    % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %
    % Copyright 2020 the matRad development team.
    %
    % This file is part of the matRad project. It is subject to the license
    % terms in the LICENSE file found in the top-level directory of this
    % distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
    % of the matRad project, including this file, may be copied, modified,
    % propagated, or distributed except according to the terms contained in the
    % LICENSE file.
    %
    % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    properties (Constant)
        name = 'Min/Max dose constraint';
        parameterNames = {'d^{min}', 'd^{max}','method'};
        parameterTypes = {'dose','dose',{'approx','voxelwise'}};
    end

    properties
        parameters = {0,30,1};
        epsilon = 1e-3; %slack parameter for the logistic approximation
    end

    methods
        function obj = matRad_MinMaxDose(minDose,maxDose,method)

            %If we have a struct in first argument
            if nargin == 1 && isstruct(minDose)
                inputStruct = minDose;
                initFromStruct = true;
            else
                initFromStruct = false;
                inputStruct = [];
            end

            %Call Superclass Constructor (for struct initialization)
            obj@DoseConstraints.matRad_DoseConstraint(inputStruct);

            %now handle initialization from other parameters
            if ~initFromStruct

                if nargin < 3 || ~ischar(method)
                    method = 'approx';
                end

                methodIx = find(strcmp(method,obj.parameterTypes{3}));

                if isempty(methodIx) || numel(methodIx) > 1
                    methodIx = 1;
                    msg = ['Dose Constraint method can only be ', strjoin(obj.parameterTypes{3},' or '), '! Using method ''', obj.parameterTypes{3}{methodIx}, '''.'];
                    warning(msg);
                end

                obj.parameters{3} = methodIx;

                if nargin >= 2 && isscalar(maxDose)
                    obj.parameters{2} = maxDose;
                end

                if nargin >= 1 && isscalar(minDose)
                    obj.parameters{1} = minDose;
                end
            end
        end

        %Overloads the struct function to add constraint specific
        %parameters
        function s = struct(obj)
            s = struct@DoseConstraints.matRad_DoseConstraint(obj);
            s.epsilon = obj.epsilon;
        end

        function cu = upperBounds(obj,n)
            switch obj.parameters{3}
                case 1 %logsumexp approx
                    %Validate parameters
                    if obj.parameters{1} <= 0 && isinf(obj.parameters{2}) %Constraint doesn't make sense (min = 0 & max = Inf)
                        cu = [];
                    elseif obj.parameters{2} == Inf %Only min dose
                        cu = Inf;
                    elseif obj.parameters{1} <= 0 %Only max dose
                        cu = obj.parameters{2};
                    else %both are set sensible
                        cu = [Inf; obj.parameters{2}];
                    end
                case 2 %voxelwise
                    cu = obj.parameters{2}*ones(n,1);
                otherwise
                    error(['Min/max dose constraint evaluation method not known!']);
            end
            %cu = [Inf; obj.parameters{2}];
        end
        function cl = lowerBounds(obj,n)
            switch obj.parameters{3}
                case 1 %logsumexp approx
                    if obj.parameters{1} <= 0 && isinf(obj.parameters{2})
                        cl = [];
                    elseif obj.parameters{2} == Inf
                        cl = obj.parameters{1};
                    elseif obj.parameters{1} <= 0
                        cl = 0;
                    else
                        cl = [obj.parameters{1}; 0];
                    end
                case 2
                    cl = obj.parameters{1}*ones(n,1);
                otherwise
                    matRad_cfg = MatRad_Config.instance();
                    matRad_cfg.dispError('Min/max dose constraint evaluation method not known!');
            end
        end

        function jStruct = getDoseConstraintJacobianStructure(obj,n)
            switch obj.parameters{3}
                case 1 %logsumexp approx
                    %Validate parameters
                    if obj.parameters{1} <= 0 && isinf(obj.parameters{2}) %Constraint doesn't make sense (min = 0 & max = Inf)
                        jStruct = ones(n,0);
                    elseif obj.parameters{1} > 0 && isfinite(obj.parameters{2}) %both are set sensible
                        jStruct = ones(n,2);
                    else %Only min or max dose
                        jStruct = ones(n,1);
                    end
                    %jStruct = ones(n,2);
                case 2
                    jStruct = speye(n);
                otherwise
                    matRad_cfg = MatRad_Config.instance();
                    matRad_cfg.dispError('Min/max dose constraint evaluation method not known!');
            end

        end

        %% Calculates the Constraint Function value
        function cDose = computeDoseConstraintFunction(obj,dose)
            %cDose(2) = dose_max + obj.epsilon * log( sum(exp((dose - dose_max)/obj.epsilon)));
            %cDose(1) = dose_min - obj.epsilon * log( sum(exp((dose_min - dose)/obj.epsilon)));
            switch obj.parameters{3}
                case 1 %logsumexp approx
                    cDose = obj.computeDoseConstraintFunctionLogSumExp(dose);
                case 2
                    cDose = obj.computeDoseConstraintFunctionVoxelwise(dose);
                otherwise
                    matRad_cfg = MatRad_Config.instance();
                    matRad_cfg.dispError('Min/max dose constraint evaluation method not known!');
            end
        end

        %% Calculates the Constraint jacobian
        function cDoseJacob  = computeDoseConstraintJacobian(obj,dose)
            switch obj.parameters{3}
                case 1 %logsumexp approx
                    cDoseJacob = obj.computeDoseConstraintJacobianLogSumExp(dose);
                case 2
                    cDoseJacob = obj.computeDoseConstraintJacobianVoxelwise(dose);
                otherwise
                    matRad_cfg = MatRad_Config.instance();
                    matRad_cfg.dispError('Min/max dose constraint evaluation method not known!');
            end
        end
    end

    methods (Access = private)
        % LogSumExp Approximation
        function cDose = computeDoseConstraintFunctionLogSumExp(obj,dose)
            dose_min = min(dose);
            dose_max = max(dose);

            %Validate parameters
            if obj.parameters{1} <= 0 && isinf(obj.parameters{2}) %Constraint doesn't make sense (min = 0 & max = Inf)
                cDose = [];
            elseif obj.parameters{2} == Inf %Only min dose
                cDose = dose_min - obj.epsilon * log( sum(exp((dose_min - dose)/obj.epsilon)));
            elseif obj.parameters{1} <= 0 %Only max dose
                cDose = dose_max + obj.epsilon * log( sum(exp((dose - dose_max)/obj.epsilon)));
            else %both are set sensible
                cDose(2,1) = dose_max + obj.epsilon * log( sum(exp((dose - dose_max)/obj.epsilon)));
                cDose(1,1) = dose_min - obj.epsilon * log( sum(exp((dose_min - dose)/obj.epsilon)));
            end

        end
        function cDoseJacob  = computeDoseConstraintJacobianLogSumExp(obj,dose)
            %Validate parameters
            if obj.parameters{1} <= 0 && isinf(obj.parameters{2}) %Constraint doesn't make sense (min = 0 & max = Inf)
                cDoseJacob = [];
            elseif obj.parameters{2} == Inf %Only min dose
                cDoseJacob(:,1) = exp( (min(dose)-dose)/obj.epsilon );
                cDoseJacob(:,1) = cDoseJacob(:,1)/sum(cDoseJacob(:,1));
            elseif obj.parameters{1} <= 0 %Only max dose
                cDoseJacob(:,1) = exp( (dose-max(dose))/obj.epsilon );
                cDoseJacob(:,1) = cDoseJacob(:,1)/sum(cDoseJacob(:,1));
            else %both are set sensible
                cDoseJacob(:,1) = exp( (min(dose)-dose)/obj.epsilon );
                cDoseJacob(:,1) = cDoseJacob(:,1)/sum(cDoseJacob(:,1));

                cDoseJacob(:,2) = exp( (dose-max(dose))/obj.epsilon );
                cDoseJacob(:,2) = cDoseJacob(:,2)/sum(cDoseJacob(:,2));
            end

        end

        %Exact voxel-wise
        function cDose = computeDoseConstraintFunctionVoxelwise(obj,dose)
            cDose = dose;
        end
        function cDoseJacob  = computeDoseConstraintJacobianVoxelwise(obj,dose)
            cDoseJacob = speye(numel(dose),numel(dose));
        end
    end

end


function [positions, time] = tracker_ransac(video_path, img_files, pos, target_sz, ...
	padding, kernel, lambda, output_sigma_factor, interp_factor, cell_size, ...
	features, show_visualization)
%TRACKER Kernelized/Dual Correlation Filter (KCF/DCF) tracking.
%   This function implements the pipeline for tracking with the KCF (by
%   choosing a non-linear kernel) and DCF (by choosing a linear kernel).
%
%   It is meant to be called by the interface function RUN_TRACKER, which
%   sets up the parameters and loads the video information.
%
%   Parameters:
%     VIDEO_PATH is the location of the image files (must end with a slash
%      '/' or '\').
%     IMG_FILES is a cell array of image file names.
%     POS and TARGET_SZ are the initial position and size of the target
%      (both in format [rows, columns]).
%     PADDING is the additional tracked region, for context, relative to
%      the target size.
%     KERNEL is a struct describing the kernel. The field TYPE must be one
%      of 'gaussian', 'polynomial' or 'linear'. The optional fields SIGMA,
%      POLY_A and POLY_B are the parameters for the Gaussian and Polynomial
%      kernels.
%     OUTPUT_SIGMA_FACTOR is the spatial bandwidth of the regression
%      target, relative to the target size.
%     INTERP_FACTOR is the adaptation rate of the tracker.
%     CELL_SIZE is the number of pixels per cell (must be 1 if using raw
%      pixels).
%     FEATURES is a struct describing the used features (see GET_FEATURES).
%     SHOW_VISUALIZATION will show an interactive video if set to true.
%
%   Outputs:
%    POSITIONS is an Nx2 matrix of target positions over time (in the
%     format [rows, columns]).
%    TIME is the tracker execution time, without video loading/rendering.
%
%   Joao F. Henriques, 2014

	%if the target is large, lower the resolution, we don't need that much
	%detail
	resize_image = (sqrt(prod(target_sz)) >= 100);  %diagonal size >= threshold
	if resize_image,
		pos = floor(pos / 2);
		target_sz = floor(target_sz / 2);
	end

	%window size, taking padding into account
	window_sz = floor(target_sz * (1 + padding));

% 	%we could choose a size that is a power of two, for better FFT
% 	%performance. in practice it is slower, due to the larger window size.
% 	window_sz = 2 .^ nextpow2(window_sz);

	%create regression labels, gaussian shaped, with a bandwidth
	%proportional to target size
	output_sigma = sqrt(prod(target_sz)) * output_sigma_factor / cell_size;
	yf = fft2(gaussian_shaped_labels(output_sigma, floor(window_sz / cell_size)));

	%store pre-computed cosine window
	cos_window = hann(size(yf,1)) * hann(size(yf,2))';

	if show_visualization,  %create video interface
		update_visualization = show_video(img_files, video_path, resize_image);
	end

	%note: variables ending with 'f' are in the Fourier domain.

	time = 0;  %to calculate FPS
	positions = zeros(numel(img_files), 2);  %to calculate precision

	for frame = 1:numel(img_files),
		%load image
		im = imread([video_path img_files{frame}]);
		if size(im,3) > 1,
			im = rgb2gray(im);
		end
		if resize_image,
			im = imresize(im, 0.5);
		end

		tic()

		if frame == 1
			camera_feat.detector = cv.FeatureDetector('SURF');
			camera_feat.extractor = cv.DescriptorExtractor('SURF');
			camera_feat.matcher = cv.DescriptorMatcher('FlannBased');
			camera_feat.last_keypoints = camera_feat.detector.detect(im);
			camera_feat.last_descriptors = camera_feat.extractor.compute(im, camera_feat.last_keypoints);
		end

		if frame > 1,
			[camera_feat, camera_H] = find_homography(camera_feat, im, pos, target_sz);
			pos = project_t(pos([2, 1]), camera_H);
            pos = pos([2, 1]);
            pos = min([size(im,1),size(im,2)], pos);
            pos = max([0,0], pos);
			%obtain a subwindow for detection at the position from last
			%frame, and convert to Fourier domain (its size is unchanged)
			patch = get_subwindow(im, pos, window_sz);
			zf = fft2(get_features(patch, features, cell_size, cos_window));

			%calculate response of the classifier at all shifts
			switch kernel.type
			case 'gaussian',
				kzf = gaussian_correlation(zf, model_xf, kernel.sigma);
			case 'polynomial',
				kzf = polynomial_correlation(zf, model_xf, kernel.poly_a, kernel.poly_b);
			case 'linear',
				kzf = linear_correlation(zf, model_xf);
			end
			response = real(ifft2(model_alphaf .* kzf));  %equation for fast detection

			%target location is at the maximum response. we must take into
			%account the fact that, if the target doesn't move, the peak
			%will appear at the top-left corner, not at the center (this is
			%discussed in the paper). the responses wrap around cyclically.
			[vert_delta, horiz_delta] = find(response == max(response(:)), 1);
			if vert_delta > size(zf,1) / 2,  %wrap around to negative half-space of vertical axis
				vert_delta = vert_delta - size(zf,1);
			end
			if horiz_delta > size(zf,2) / 2,  %same for horizontal axis
				horiz_delta = horiz_delta - size(zf,2);
			end
			pos = pos + cell_size * [vert_delta - 1, horiz_delta - 1];
		end

		%obtain a subwindow for training at newly estimated target position
		patch = get_subwindow(im, pos, window_sz);
		xf = fft2(get_features(patch, features, cell_size, cos_window));

		%Kernel Ridge Regression, calculate alphas (in Fourier domain)
		switch kernel.type
		case 'gaussian',
			kf = gaussian_correlation(xf, xf, kernel.sigma);
		case 'polynomial',
			kf = polynomial_correlation(xf, xf, kernel.poly_a, kernel.poly_b);
		case 'linear',
			kf = linear_correlation(xf, xf);
		end
		alphaf = yf ./ (kf + lambda);   %equation for fast training

		if frame == 1,  %first frame, train with a single image
			model_alphaf = alphaf;
			model_xf = xf;
		else
			%subsequent frames, interpolate model
			model_alphaf = (1 - interp_factor) * model_alphaf + interp_factor * alphaf;
			model_xf = (1 - interp_factor) * model_xf + interp_factor * xf;
		end

		%save position and timing
		positions(frame,:) = pos;
		time = time + toc();

		%visualization
		if show_visualization,
			box = [pos([2,1]) - target_sz([2,1])/2, target_sz([2,1])];
			stop = update_visualization(frame, box);
			if stop, break, end  %user pressed Esc, stop early

			drawnow
% 			pause(0.05)  %uncomment to run slower
		end

	end

	if resize_image,
		positions = positions * 2;
	end
end


function [finalVectors] = detMinSpan(model, params, vectors)
% Calculates the MinSpan vectors for a COBRA model. The
% algorithm determines a set of linearly independent basis vectors that
% span the nullspace that meet the criteria of the flux bounds of the
% network and minimizes the number of reactions used. The algorithm
% operates in an interative manner and checkes for convergence after each
% iteration. Parameters may be provided to skip convergence and terminate
% the problem when models are large. See Bordbar et al. Mol Syst Biol 2014
% for more details. This algorithm has only been tested and requires Gurobi
% as MILP solver.
%
% INPUTS:
%    model:             COBRA model structure (requires S, lb, ub)
%                       Note: MinSpan calculations are done w/o biomass
%                       reaction and all reactions must be able to carry a
%                       flux = 0 for the trivial solution to be feasible.
%                       model is auto corrected by bounds but biomass
%                       must be removed manually
%
% OPTIONAL INPUTS:
%    params:            Optional parameters to calculate MinSpan.
%                       Determining the MinSpan is a NP hard calculation.
%                       For large models, the algorithm may not converge
%                       and parameters must be provided to stop the
%                       algorithm to provide an approximate solution.
%
%                       * .coverage - Number of iterations to run the algorithm, if not
%                       converge (Default = 10)
%                       * .timeLimit - Time to spend on each MinSpan calculation (sec),
%                       (Default = 30)
%                       * .saveIntV - Save intermediate vectors in order to restart from
%                       latest iteration (Default = 0)
%                       * .cores - Number of cores to use (Default = 1)
%
%    vectors:           Set of intermediate MinSpan vectors that may
%                       have not yet reached convergence, allowing to
%                       pickup calculation from last spot.
%
% OUTPUTS:
%    finalVectors:      MinSpan vectors for COBRA model
%
% .. Author: Aarash Bordbar 05/15/2017
%            Ronan Fleming, nullspace computation with LUSOL

global CBT_MILP_SOLVER
global CBT_LP_SOLVER
global CBT_QP_SOLVER

if ~strcmp(CBT_MILP_SOLVER, 'gurobi') || ~strcmp(CBT_LP_SOLVER, 'gurobi') || ~strcmp(CBT_QP_SOLVER, 'gurobi')
    error('detMinSpan only runs with Gurobi.\nTry to run `changeCobraSolver(''gurobi'', ''MILP'')` to use Gurobi.');
end

% Ensure model has correct bounds
origModel = model;
model.lb(model.lb < 0) = -1000;
model.ub(model.ub > 0) = 1000;
model.lb(model.lb > 0) = 0;
model.ub(model.ub < 0) = 0;

% Reduce model to just reactions that can carry a flux
[minF, maxF] = fluxVariability(model, 0);
minF(abs(minF) < 1e-8) = 0;
maxF(abs(maxF) < 1e-8) = 0;
remRxns = model.rxns(minF == 0 & maxF == 0);
model = removeRxns(model, remRxns);

% Setup MILP and solving parameters
if ~exist('params', 'var')
    params.coverage = 10;
    params.timeLimit = 30;
    params.saveIntV = 0;
    params.cores = 1;
    params.nullAlg = 'matlab';
else
    if ~isfield(params, 'coverage')
        params.coverage = 10;
    end
    if ~isfield(params, 'timeLimit')
        params.timeLimit = 30;
    end
    if ~isfield(params, 'saveIntV')
        params.saveIntV = 1;
    end
    if ~isfield(params, 'cores')
        params.cores = 1;
    end
    if ~isfield(params, 'nullAlg')
        params.nullAlg = 'matlab';
    end
end

MILPparams.TimeLimit = params.timeLimit;
MILPparams.outputFlag = 1;
MILPparams.Presolve = 2;
MILPparams.Threads = params.cores;
MILPparams.DisplayInterval = 10;

if ~exist('vectors', 'var')
    if strcmp('params.nullAlg','lusol')
        error('nearly but not quite implemented yet')
        [vectors, ~] = getNullSpace(model.S, 0);
    else
        vectors = null(full(model.S));
    end
end

% Prepratory steps for MinSpan determination
rng('shuffle');

if strcmp('params.nullAlg','lusol')
    [N, ~] = getNullSpace(S, 0);
else
    N = null(full(model.S));
end

[m, n] = size(model.S);

locRev = find(model.lb < 0);
lengthRev = length(locRev);
revConstraintMat = zeros(lengthRev, n);
for i = 1:lengthRev
    revConstraintMat(i, locRev(i)) = 1;
end

% Run MinSpan
tmpNvProd = [];
totalNNZ = [];
for k = 1:params.coverage

    numToCheck = randperm(size(N, 2));
    prevNNZ = nnz(vectors);

    for i = 1:length(numToCheck)
        tic;
        oldPath = vectors(:, numToCheck(i));
        pathLength = nnz(vectors(:, numToCheck(i)));
        vectors(:, numToCheck(i)) = zeros(n, 1);

        sizeN = 1;
        theta = N \ vectors;

        if strcmp('params.nullAlg','lusol')
            [Z, ~] = getNullSpace(theta', 0);
            tmpN = sparse(N * Z);
        else
            tmpN = sparse(N * null(theta'));
        end

        tmptmpNprod = tmpN' * oldPath;
        tmpN = tmpN * (1 / tmptmpNprod);

        % Model Formulation: A, b, csense, lb, ub, vartype, c
        MILPproblem.A = [model.S, sparse(m, n + 2 * sizeN);  % S. v = 0
                         revConstraintMat, 1e4 * revConstraintMat, sparse(lengthRev, 2 * sizeN);  % v >= -10000*b
                         speye(n), -1e4 * speye(n), sparse(n, 2 * sizeN);  % v <= 10000*b
                         tmpN', sparse(sizeN, n), (-1001) * speye(sizeN), sparse(sizeN, sizeN);  % N.v >= 1001*fi+ - 1000
                         -tmpN', sparse(sizeN, n), sparse(sizeN, sizeN), (-1001) * speye(sizeN);  % -N.v >= 1001*fi- - 1000
                         sparse(1, 2 * n), ones(1, 2 * sizeN);  % sum(fi+, fi-) >= 1
                         ];

        MILPproblem.b = [zeros(m, 1);  % S. v = 0
                         zeros(lengthRev, 1);  % v >= -10000*b
                         zeros(n, 1);  % v <= 10000*b
                         -1000 * ones(sizeN, 1);  % N.v >= 1000*fi+ - 1000
                         -1000 * ones(sizeN, 1);  % -N.v >= 1000*fi+ - 1000
                         1  % sum(vi+, vi-) >= 1
                         ];

        MILPproblem.csense = '';
        for l = 1:m
            MILPproblem.csense(end + 1, 1) = 'E';  % S.v = 0
        end
        for l = 1:lengthRev
            MILPproblem.csense(end + 1, 1) = 'G';  % v >= -10000*b
        end
        for l = 1:n
            MILPproblem.csense(end + 1, 1) = 'L';  % v <= 10000*b
        end
        for l = 1:sizeN
            MILPproblem.csense(end + 1, 1) = 'G';  % N.v >= 1000*fi+ - 1000
        end
        for l = 1:sizeN
            MILPproblem.csense(end + 1, 1) = 'G';  % -N.v >= 1000*fi+ - 1000
        end
        MILPproblem.csense(end + 1, 1) = 'G';  % sum(vi+, vi-) > 1

        MILPproblem.lb = [model.lb;  % v
                          zeros(n, 1);  % a
                          zeros(2 * sizeN, 1)];  % fi+, fi-

        MILPproblem.ub = [model.ub;  % v
                          ones(n, 1);  % a
                          ones(2 * sizeN, 1)];  % k+, k-

        MILPproblem.vartype = '';
        for l = 1:n
            MILPproblem.vartype(end + 1, 1) = 'C';  % v
        end
        for l = 1:n
            MILPproblem.vartype(end + 1, 1) = 'B';  % b
        end
        for l = 1:2 * sizeN
            MILPproblem.vartype(end + 1, 1) = 'B';  % fi+, fi-
        end

        MILPproblem.c = [zeros(n, 1);  % v
                         ones(n, 1);  % b
                         zeros(2 * sizeN, 1);  % fi+, fi-
                         ];

        MILPproblem.osense = 1;  % minimize

        % Setup initial solution
        binOldPath = zeros(length(oldPath), 1);
        binOldPath(find(oldPath)) = 1;
        MILPproblem.x0 = [oldPath; binOldPath; 1e101; 1e101];

        MILPsolution = solveCobraMILP(MILPproblem, MILPparams);

        % Check solution
        % If unable to find solution, break iteration
        if length(MILPsolution.cont) < n
            break
        end

        % If solution found, normalize and replace vector in intermediate
        % matrix
        vector = MILPsolution.full(1:n);
        vector(abs(vector) < 1e-6) = 0;

        if strcmp('params.nullAlg','lusol')
            [Z, ~] = getNullSpace((N \ [vectors, vector])', 0);
            tmpNullCheck = N * Z;
        else
            tmpNullCheck = N * null((N \ [vectors, vector])');
        end

        if nnz(vector) > 0 && isempty(tmpNullCheck)
            vector = vector / norm(vector);
            vectors(:, numToCheck(i)) = vector;
        else
            vectors(:, numToCheck(i)) = oldPath;
            vector = oldPath;
        end
        tmpNvProd = [tmpNvProd; tmpN' * vector];
        totalNNZ = [totalNNZ; nnz(vectors)];

        time(i, 1) = toc;

        % Save intermediate matrices (within iteration)
        if params.saveIntV == 1
            filename = strcat('save_', num2str(k), '_', num2str(i));
            save(filename, 'MILPproblem', 'MILPsolution', 'vectors', ...
                 'time', 'tmpNvProd', 'totalNNZ', 'numToCheck');
        end

        clear mex
    end

    newNum = nnz(vectors);

    % Save intermediate matrices (after a completed iteration)
    if params.saveIntV == 1
        filename = strcat('save_finalround_', num2str(k));
        save(filename, 'vectors', 'time');
    end

    % If MinSpan solution has converged (same NNZ as previous iteration)
    if newNum == prevNNZ
        break
    end
end

clear x
for i = 1:size(vectors, 2)
    x(i, 1) = nnz(vectors(:, i));
end

completedPaths = find(x < n & x > 0);
vectors = vectors(:, completedPaths);

% Normalize vectors such that smallest flux value in vector is 1
for i = 1:size(vectors, 2)
    loc = find(vectors(:, i));
    tmp = min(abs(vectors(loc, i)));
    vectors(:, i) = vectors(:, i) / tmp;
end

% Cast vectors from reduce model size to full model size
finalVectors = zeros(length(origModel.rxns), size(vectors, 2));
loc = find(ismember(origModel.rxns, model.rxns));
for i = 1:size(vectors, 2)
    finalVectors(loc, i) = vectors(:, i);
end


%% Clear, close
clear; close; clc;

%% Data load
size.subject = 9;
size.trial = 288;
size.class = 4;
size.chan = 22;
size.sr = 250;

for i = 1:size.subject
    [train(i).s, train(i).h] = sload(char("C:\Users\KHJ-work\Documents\MATLAB\BCICIV_2a_gdf\A0" + int2str(i) + "T.gdf"));
    [eval(i).s, eval(i).h] = sload(char("C:\Users\KHJ-work\Documents\MATLAB\BCICIV_2a_gdf\A0" + int2str(i) + "E.gdf"));
    train(i).event_typ = find(train(i).h.EVENT.TYP >= 769 & train(i).h.EVENT.TYP <= 772 );
    eval(i).event_typ = find(eval(i).h.EVENT.TYP == 783);
    train(i).h.Classlabel = load(char("C:\Users\KHJ-work\Documents\MATLAB\BCICIV_2a_gdf\A0" + int2str(i) + "T.mat"));
    eval(i).h.Classlabel = load(char("C:\Users\KHJ-work\Documents\MATLAB\BCICIV_2a_gdf\A0" + int2str(i) + "E.mat"));
end

%% Make Segment
for i = 1:size.subject
    for j = 1:size.trial
        train(i).samp_seg(:, 1:size.chan, j) = train(i).s(train(i).h.TRIG(j) + 626:train(i).h.TRIG(j) + 1125, 1:size.chan);
        eval(i).samp_seg(:, 1:size.chan, j) = eval(i).s(eval(i).h.TRIG(j) + 626:eval(i).h.TRIG(j) + 1125, 1:size.chan);
        if j < size.trial
            train(i).ref_seg(:, 1:size.chan, j) = train(i).s(train(i).h.TRIG(j) + 1626:train(i).h.TRIG(j) + 2000, 1:size.chan);
            eval(i).ref_seg(:, 1:size.chan, j) = eval(i).s(eval(i).h.TRIG(j) + 1626:eval(i).h.TRIG(j) + 2000, 1:size.chan);
        end
    end
end

%% Remove NaN value trial
for i = 1:size.subject
    for j = 1:size.trial
        if find(isnan(train(i).samp_seg(:, :, j)))
            train(i).samp_seg(:, :, j) = zeros;
            train(i).samp_remain(j) = 0;
        else
            train(i).samp_remain(j) = 1;
        end

        if find(isnan(eval(i).samp_seg(:, :, j)))
            eval(i).samp_seg(:, :, j) = zeros;
            eval(i).samp_remain(j) = 0;
        else
            eval(i).samp_remain(j) = 1;
        end

        if j < size.trial
            if find(isnan(train(i).ref_seg(:, :, j)))
                train(i).ref_seg(:, :, j) = zeros;
                train(i).ref_remain(j) = 0;
            else
                train(i).ref_remain(j) = 1;
            end

            if find(isnan(eval(i).ref_seg(:, :, j)))
                eval(i).ref_seg(:, :, j) = zeros;
                eval(i).ref_remain(j) = 0;
            else
                eval(i).ref_remain(j) = 1;
            end
        end
    end
end

%% Bandpass filtering
for i = 1:size.subject
    train(i).samp_segF = reshape(bandpass(reshape(train(i).samp_seg, [500 * size.trial size.chan]), [8 30], size.sr), [500, size.chan, size.trial]);
    eval(i).samp_segF = reshape(bandpass(reshape(eval(i).samp_seg, [500 * size.trial size.chan]), [8 30], size.sr), [500, size.chan, size.trial]);
    train(i).ref_segF = reshape(bandpass(reshape(train(i).ref_seg, [375 * (size.trial - 1) size.chan]), [8 30], size.sr), [375, size.chan, (size.trial - 1)]);
    eval(i).ref_segF = reshape(bandpass(reshape(eval(i).ref_seg, [375 * (size.trial - 1) size.chan]), [8 30], size.sr), [375, size.chan, (size.trial - 1)]);
end

%% Covariance matrix
for i = 1:size.subject
    for j = 1:size.trial
        train(i).samp_cov(:, :, j) = (1 / (length(find(train(i).samp_remain)) - 1)) * (train(i).samp_segF(:, :, j)' * train(i).samp_segF(:, :, j));
        eval(i).samp_cov(:, :, j) = (1 / (length(find(eval(i).samp_remain)) - 1)) * (eval(i).samp_segF(:, :, j)' * eval(i).samp_segF(:, :, j));
        if j < size.trial
            train(i).ref_cov(:, :, j) = (1 / (length(find(train(i).ref_remain)) - 1)) * (train(i).ref_segF(:, :, j)' * train(i).ref_segF(:, :, j));
            eval(i).ref_cov(:, :, j) = (1 / (length(find(eval(i).ref_remain)) - 1)) * (eval(i).ref_segF(:, :, j)' * eval(i).ref_segF(:, :, j));
        end
    end
end

%% Riemannian mean of whole sample and refference
for i = 1:size.subject
    train_samp_tmp = train(i).samp_cov;
    train_samp_tmp(:, :, train(i).samp_remain == 0) = [];
    eval_samp_tmp = eval(i).samp_cov;
    eval_samp_tmp(:, :, eval(i).samp_remain == 0) = [];
    train(i).samp_Rmean = mean(train_samp_tmp, 3);
    eval(i).samp_Rmean = mean(eval_samp_tmp, 3);

    train_ref_tmp = train(i).ref_cov;
    train_ref_tmp(:, :, train(i).ref_remain == 0) = [];
    eval_ref_tmp = eval(i).ref_cov;
    eval_ref_tmp(:, :, eval(i).ref_remain == 0) = [];
    train(i).ref_Rmean = mean(train_ref_tmp, 3);
    eval(i).ref_Rmean = mean(eval_ref_tmp, 3);

    for j = 1:5
        Tsamp = tangentspace(train_samp_tmp, train(i).samp_Rmean);
        Tmean = mean(Tsamp, 3);
        train(i).samp_Rmean = untangentspace(Tmean, train(i).samp_Rmean);

        Tsamp = tangentspace(eval_samp_tmp, eval(i).samp_Rmean);
        Tmean = mean(Tsamp, 3);
        eval(i).samp_Rmean = untangentspace(Tmean, eval(i).samp_Rmean);

        Tsamp = tangentspace(train_samp_tmp, train(i).ref_Rmean);
        Tmean = mean(Tsamp, 3);
        train(i).ref_Rmean = untangentspace(Tmean, train(i).ref_Rmean);

        Tsamp = tangentspace(eval_samp_tmp, eval(i).ref_Rmean);
        Tmean = mean(Tsamp, 3);
        eval(i).ref_Rmean = untangentspace(Tmean, eval(i).ref_Rmean);
    end
end

%% Riemannian mean of each class
for i = 1:size.subject
    for j = 1:size.class
        train_samp_tmp = train(i).samp_cov;
        train_samp_tmp = train_samp_tmp(:, :, train(i).h.Classlabel.classlabel == j & train(i).samp_remain');
        eval_samp_tmp = eval(i).samp_cov;
        eval_samp_tmp = eval_samp_tmp(:, :, eval(i).h.Classlabel.classlabel == j & eval(i).samp_remain');

        train(i).class_Rmean(:, :, j) = mean(train_samp_tmp, 3);
        eval(i).class_Rmean(:, :, j) = mean(eval_samp_tmp, 3);

        for k = 1:5
            Tsamp = tangentspace(train_samp_tmp, train(i).class_Rmean(:, :, j));
            Tmean = mean(Tsamp, 3);
            train(i).class_Rmean(:, :, j) = untangentspace(Tmean, train(i).class_Rmean(:, :, j));

            Tsamp = tangentspace(eval_samp_tmp, eval(i).class_Rmean(:, :, j));
            Tmean = mean(Tsamp, 3);
            eval(i).class_Rmean(:, :, j) = untangentspace(Tmean, eval(i).class_Rmean(:, :, j));
        end
    end
end

%% Rmdm classification
for i = 1:size.subject
    tmp_res = Rmdm(eval(i).samp_cov(:, :, find(eval(i).samp_remain)), train(i).class_Rmean);
    acc1(i) = length(find(tmp_res == eval(i).h.Classlabel.classlabel(find(eval(i).samp_remain))')) / length(eval(i).samp_remain);
    tmp_res = Rmdm(train(i).samp_cov(:, :, find(train(i).samp_remain)), eval(i).class_Rmean);
    acc2(i) = length(find(tmp_res == train(i).h.Classlabel.classlabel(find(train(i).samp_remain))')) / length(train(i).samp_remain);
    def_acc(i) = (acc1(i) + acc2(i)) / 2;
end

%% Covariance matrix using affine transform
for i = 1:size.subject
    for j = 1:size.trial
        train(i).aff_cov(:, :, j) = (train(i).ref_Rmean ^ (-1 / 2)) * train(i).samp_cov(:, :, j) * ((train(i).ref_Rmean) ^ (-1 / 2));
        eval(i).aff_cov(:, :, j) = (eval(i).ref_Rmean ^ (-1 / 2)) * eval(i).samp_cov(:, :, j) * ((eval(i).ref_Rmean) ^ (-1 / 2));
    end
end

%% Riemannian mean of each class of affine transformed samples
for i = 1:size.subject
    for j = 1:size.class
        train_aff_tmp = train(i).aff_cov;
        train_aff_tmp = train_aff_tmp(:, :, train(i).h.Classlabel.classlabel == j & train(i).samp_remain');
        eval_aff_tmp = eval(i).aff_cov;
        eval_aff_tmp = eval_aff_tmp(:, :, eval(i).h.Classlabel.classlabel == j & eval(i).samp_remain');

        train(i).aff_class_Rmean(:, :, j) = mean(train_aff_tmp, 3);
        eval(i).aff_class_Rmean(:, :, j) = mean(eval_aff_tmp, 3);

        for k = 1:5
            Tsamp = tangentspace(train_aff_tmp, train(i).aff_class_Rmean(:, :, j));
            Tmean = mean(Tsamp, 3);
            train(i).aff_class_Rmean(:, :, j) = untangentspace(Tmean, train(i).aff_class_Rmean(:, :, j));

            Tsamp = tangentspace(eval_aff_tmp, eval(i).aff_class_Rmean(:, :, j));
            Tmean = mean(Tsamp, 3);
            eval(i).aff_class_Rmean(:, :, j) = untangentspace(Tmean, eval(i).aff_class_Rmean(:, :, j));
        end
    end
end

%% Rmdm classification of affine transformed samples
for i = 1:size.subject
    tmp_res = Rmdm(eval(i).aff_cov(:, :, find(eval(i).samp_remain)), train(i).aff_class_Rmean);
    acc1(i) = length(find(tmp_res == eval(i).h.Classlabel.classlabel(find(eval(i).samp_remain))')) / length(eval(i).samp_remain);
    tmp_res = Rmdm(train(i).aff_cov(:, :, find(train(i).samp_remain)), eval(i).aff_class_Rmean);
    acc2(i) = length(find(tmp_res == train(i).h.Classlabel.classlabel(find(train(i).samp_remain))')) / length(train(i).samp_remain);
    aff_acc(i) = (acc1(i) + acc2(i)) / 2;
end

%% MINE
for i = 1:size.subject
    for j = 1:size.subject
        for k = 1:size.class
            for l = find(train(j).h.Classlabel.classlabel' == k & train(j).samp_remain)
                tmp_samp(:, :, l) = (train(i).aff_class_Rmean(:, :, k) ^ (-1 / 2)) * train(j).aff_cov(:, :, l) * (train(i).aff_class_Rmean(:, :, k) ^ (-1 / 2));
            end
        end

        if i == 1 && j == 1
            train(i).transfer_sample = tmp_samp;
            continue
        end

        train(i).transfer_sample = cat(3, train(i).transfer_sample, tmp_samp);
    end
end

%%
for i = 1:size.subject
    if i == 1
        train_remain = train(i).samp_remain;
        train_class = train(i).h.Classlabel.classlabel;

    else
        train_remain = cat(2, train_remain, train(i).samp_remain);
        train_class = cat(1, train_class, train(i).h.Classlabel.classlabel);
    end
end

%%
for i = 1:size.subject
    for j = 1:size.class
        train_mine_tmp = train(i).transfer_sample;
        train_mine_tmp = train_mine_tmp(:, :, train_class == j & train_remain');

        train(i).mine_class_Rmean(:, :, j) = mean(train_mine_tmp, 3);

        for k = 1:5
            Tsamp = tangentspace(train_mine_tmp, train(i).mine_class_Rmean(:, :, j));
            Tmean = mean(Tsamp, 3);
            train(i).mine_class_Rmean(:, :, j) = untangentspace(Tmean, train(i).mine_class_Rmean(:, :, j));
        end
    end
end

%%
for i = 1:size.subject
    for j = 1:size.class
        for k = find(eval(j).h.Classlabel.classlabel' == j & eval(j).samp_remain)
            tmp_samp(:, :, k) = (train(i).aff_class_Rmean(:, :, j) ^ (-1 / 2)) * eval(i).aff_cov(:, :, k) * (train(i).aff_class_Rmean(:, :, j) ^ (-1 / 2));
        end
    end

    tmp_res = Rmdm(eval(i).aff_cov(:, :, find(eval(i).samp_remain)), train(i).mine_class_Rmean);
    acc1(i) = length(find(tmp_res == eval(i).h.Classlabel.classlabel(find(eval(i).samp_remain))')) / length(eval(i).samp_remain);
end


function res = vg_method(args)

% Variational Garrote: performs linear regression with L0-norm penalty (Spike and Slab model)
%   See file test_vg.m for an example of use
%
% required parameters (n input dimension, p samples)
%   x     : n x p (training set, input)
%   y     : 1 x s (training set, output)
%   xv    : n x p (validation set, input)
%   yv    : 1 x s (validation set, output)
%
% optional parameters (default)
%   method      : method for optimization 'dual' or 'regression' for fixed gamma ('dual')
%   maxiter     : maximum number of iterations for optimization for fixed gamma (1e4)
%   max_sum_m   : increases gamma values until sum(m)=max_sum_m  (n/2)
%   beta_max    : increases gamma values until beta=beta_max (1e3)
%   n_gamma     : number of gamma values to scan (50)
%   dmmin       : convergence threshold for mean field error (1e-12)

%----------------
% REQUIRED PARAMS
ok = true;
if ~isfield(args, 'x') disp('train input x not provided'); ok = false; else x=args.x; end
if ~isfield(args, 'y') disp('train output y not provided'); ok = false; else y=args.y; end
if ~isfield(args, 'xv') disp('val set input xv not provided'); ok = false; else xv=args.xv; end
if ~isfield(args, 'yv') disp('val set output yv not provided'); ok = false; else yv=args.yv; end
if isfield(args, 'xt') && isfield(args, 'yt')
    xt=args.xt;
    yt=args.yt;
    pt = size(xt,2);
else
    pt = 0;
end

if ~ok
    return;
end

n = size(x,1);
p = size(x,2);
pv = size(xv,2);

%----------------
% OPTIONAL PARAMS

% method for optimization {dual or regression} for fixed gamma
if ~isfield(args, 'method') method='dual'; else method=args.method; end

% maximum number of iterations for optimization for fixed gamma
if ~isfield(args, 'maxiter') maxiter=1e4; else maxiter=args.maxiter; end

% increases gamma values until sum(m)=max_sum_m
if ~isfield(args, 'max_sum_m') max_sum_m=n/2; else max_sum_m=args.max_sum_m; end

% increases gamma values until beta=beta_max
if ~isfield(args, 'beta_max') beta_max=1e3; else beta_max=args.beta_max; end

% number of gamma values to scan
if ~isfield(args, 'n_gamma') n_gamma=50; else n_gamma=args.n_gamma; end

% convergence threshold for mean field error
if ~isfield(args, 'dmmin') dmmin=1e-12; else dmmin=args.dmmin; end

%----------------
% compute garrote solution for range of gammas
% first from gamma_min to gamma_max and then in
% a second pass from gamma_max to gamma_min.

% C is input data covariance matrix.
if strcmp(method, 'regression')
    if n<=1500,
        C=x*x'/p;
    end;
end

% b is input output covariance
b=x*y'/p;

% sigma is output variance
sigmay=y*y'/p;

% set gamma range (min, max and step size)
delta=1e-8;
[b2sort,isort]=sort(b.^2,'descend');
bsort=b(isort);
gamma_min=log(delta*sigmay/p/max(abs(b)));
eps_gamma=0.001;
gamma_max=eps_gamma*gamma_min;
gamma_all =linspace(gamma_min,gamma_max,n_gamma);

% initial step size of mean field update
eta0=1; %e-2;
% initial step size for change in w in dual.m
eta_w0=0.02;

% input data variance
chi_ii=1/p*sum(x.^2,2);
if sum(abs(chi_ii-1)>1e-10),
    fprintf('input design matrix is not normalized\n');
    pause
end;

lg=n_gamma;
kl_all=inf(lg,2);
v_all=inf(lg,2,n);
m_all=inf(lg,2,n);
beta_all=inf(lg,2);
v_mf_all=inf(lg,n);
m_mf_all=inf(lg,n);
iter_all=inf(lg,2);
beta_mf_all=inf(1,lg);
error_mf_all=inf(1,lg);
errorv_mf_all=inf(1,lg);
errort_mf_all=inf(1,lg);
m=zeros(1,n);

% the estimated inverse noise variance beta is initialized as the
% output variance
beta=1/sigmay;
i=0;

% for gamma is gamma_min to gamma_max, or when some criteria are
% satisfied
while (beta<beta_max)&&(i<n_gamma)&&(sum(m)<max_sum_m),
	i=i+1;
	gamma=gamma_all(i);
    eval(method);
	v_all(i,1,:)=v;
	m_all(i,1,:)=m;
	beta_all(i,1)=beta;
	iter_all(i,1)=iter;
	kl_all(i,1)=kl1;
	fprintf('gamma = %f beta = %f sum(m) = %f iter = %d kl = %f\n',gamma,beta,sum(m),iter,kl1);
end;

if beta>=beta_max
 fprintf('-----------------------------------------------------------\n');
 fprintf('beta > beta_max (%.3f > %.3f)\n', beta, beta_max);
 if i>1
    m = squeeze(m_all(i-1,1,:))';
 end
end
if sum(m)>=max_sum_m
 fprintf('-----------------------------------------------------------\n');
 fprintf('sum(m) > max_sum_m (%.3f > %.3f)\n', sum(m), max_sum_m);
end

% for gamma is current gamma decreasing to gamma_min
imax=i-1;
for i=imax:-1:1,
	gamma=gamma_all(i);
    eval(method);
	v_all(i,2,:)=v;
	m_all(i,2,:)=m;
	beta_all(i,2)=beta;
	iter_all(i,2)=iter;
	kl_all(i,2)=kl1;
	fprintf('gamma = %f beta = %f sum(m) = %f iter = %d kl = %f\n',gamma,beta,sum(m),iter,kl1);
end;

% select for each gamma from these two solutions the one with lowest KL
[klmin, imin]=min(kl_all,[],2);
for i=1:imax,
	v_mf_all(i,:)=squeeze(v_all(i,imin(i),:))';
	m_mf_all(i,:)=squeeze(m_all(i,imin(i),:))';
	beta_mf_all(i)=beta_all(i,imin(i));
	error_mf_all(i)=1/p*sum((y-v_mf_all(i,:)*x).^2,2);
	errorv_mf_all(i)=1/pv*sum((yv-v_mf_all(i,:)*xv).^2,2);
	if pt>0,
		errort_mf_all(i)=1/pt*sum((yt-v_mf_all(i,:)*xt).^2,2);
	end;
end;

% select the gamma that optimizes the validation error (errorv_mf_all)
[minerrorv i]=min(errorv_mf_all(1:imax));

res.gamma_mf=gamma_all(i);
res.v_mf=v_mf_all(i,:);
res.m_mf=m_mf_all(i,:);
res.n_mf1=sum(res.m_mf>0.5);

res.error_mf=error_mf_all(i);
res.errorv_mf=errorv_mf_all(i);
res.errort_mf=errort_mf_all(i);
res.beta_mf=beta_mf_all(i);

if (res.beta_mf==beta_max)
	fprintf('beta_max too small: beta_mf %6.4f, beta_max %6.4f\n', beta_mf(iruns),beta_max);
	pause
end;
if (res.gamma_mf==gamma_min)
	fprintf('gamma at minimum range boundary\n');
end;
if (res.gamma_mf==gamma_max)
	fprintf('gamma at maxium range boundary\n');
end;