function [Eft, Varft, lpyt, Eyt, Varyt] = gpla_loopred(gp, x, y, varargin)
%GPLA_LOOPRED  Leave-one-out predictions with Laplace approximation
%
%  Description
%    [EFT, VARFT, LPYT, EYT, VARYT] = GPLA_LOOPRED(GP, X, Y, OPTIONS)
%    takes a Gaussian process structure GP together with a matrix X
%    of training inputs and vector Y of training targets, and
%    evaluates the leave-one-out predictive distribution at inputs
%    X and returns means EFT and variances VARFT of latent
%    variables, the logarithm of the predictive densities PYT, and
%    the predictive means EYT and variances VARYT of observations
%    at input locations X.
%
%    OPTIONS is optional parameter-value pair
%      z      - optional observed quantity in triplet (x_i,y_i,z_i)
%               Some likelihoods may use this. For example, in case of
%               Poisson likelihood we have z_i=E_i, that is, expected value
%               for ith case.
%
%    Laplace leave-one-out is approximated in linear response style
%    by expressing the solutions for LOO problem in terms of
%    solution for the full problem. The computationally cheap
%    solution can be obtained by making the assumption that the
%    difference between these two solution is small such that their
%    difference may be treated as an Taylor expansion truncated at
%    first order (Winther et al 2012, in progress).
%
%  See also
%    GP_LOOPRED, GP_PRED
%
% Copyright (c) 2011-2012  Aki Vehtari, Ville Tolvanen

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'GPLA_LOOPRED';
  ip.addRequired('gp', @(x) isstruct(x));
  ip.addRequired('x', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addRequired('y', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('z', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('method', 'cavity', @(x) ismember(x, {'lrs' 'cavity' 'inla'}))
  ip.addParamValue('fcorr', 'off', @(x) ismember(x, {'off', 'fact', 'cm2', 'on'}));
  ip.parse(gp, x, y, varargin{:});
  z=ip.Results.z;
  method = ip.Results.method;
  fcorr=ip.Results.fcorr;
  [tn,nin] = size(x);

  switch method

    case 'lrs'
      % Manfred Opper and Ole Winther (2000). Gaussian Processes for
      % Classification: Mean-Field Algorithms. In Neural
      % Computation, 12(11):2655-2684.
      %
      % Ole Winther et al (2012). Work in progress.

      % latent posterior
      [f, sigm2ii] = gpla_pred(gp, x, y, 'z', z, 'tstind', [], 'fcorr', fcorr);
      if isnan(f)
        Eft=NaN; Varft=NaN; lpyt=NaN; Eyt=NaN; Varyt=NaN;
        return
      end

      deriv = gp.lik.fh.llg(gp.lik, y, f, 'latent', z);
      La = 1./-gp.lik.fh.llg2(gp.lik, y, f, 'latent', z);
      % really large values don't contribute, but make variance
      % computation unstable. 2e15 approx 1/(2*eps)
      La = min(La,2e15);

      switch gp.type
        case 'FULL'
          % FULL GP (and compact support GP)
          K = gp_trcov(gp,x);
          Varft=1./diag(inv(K+diag(La)))-La;

        case 'FIC'
          % FIC
          % Use inverse lemma for FIC low rank covariance matrix approximation
          % Code adapated from gp_pred

          u = gp.X_u;
          m = size(u,1);
          % Turn the inducing vector on right direction
          if size(u,2) ~= size(x,2)
            u=u';
          end
          [Kv_ff, Cv_ff] = gp_trvar(gp, x);  % 1 x f  vector
          K_fu = gp_cov(gp, x, u);   % f x u
          K_uu = gp_trcov(gp, u);     % u x u, noiseles covariance K_uu
          Luu = chol(K_uu,'lower');
          B=Luu\(K_fu');
          Qv_ff=sum(B.^2)';
          % Add also La to the vector of diagonal elements
          Lav = Cv_ff-Qv_ff + La;   % 1 x f, Vector of diagonal elements

          % iLaKfu = diag(inv(Lav))*K_fu = inv(La)*K_fu
          iLaKfu = zeros(size(K_fu));  % f x u,
          n=size(x,1);
          for i=1:n
            iLaKfu(i,:) = K_fu(i,:)./Lav(i);  % f x u
          end
          A = K_uu+K_fu'*iLaKfu;
          A = (A+A')./2;
          L = iLaKfu/chol(A);

          %Varft=1./diag(inv(K+diag(La)))-La;
          Varft=1./(1./Lav - sum(L.^2,2))-La;

        case {'PIC' 'PIC_BLOCK'}
          % PIC
          % Use inverse lemma for PIC low rank covariance matrix approximation
          % Code adapated from gp_pred (here Lab is same La in gp_pred)

          u = gp.X_u;
          ind = gp.tr_index;
          if size(u,2) ~= size(x,2)
            % Turn the inducing vector on right direction
            u=u';
          end

          % Calculate some help matrices
          [Kv_ff, Cv_ff] = gp_trvar(gp, x);  % 1 x f  vector
          K_fu = gp_cov(gp, x, u);         % f x u
          K_uu = gp_trcov(gp, u);    % u x u, noiseles covariance K_uu
          Luu = chol(K_uu)';

          % Evaluate the Lambda (La) for specific model
          % Q_ff = K_fu*inv(K_uu)*K_fu'
          % Here we need only the diag(Q_ff), which is evaluated below
          B=Luu\K_fu';
          iLaKfu = zeros(size(K_fu));  % f x u
          for i=1:length(ind)
            Qbl_ff = B(:,ind{i})'*B(:,ind{i});
            [Kbl_ff, Cbl_ff] = gp_trcov(gp, x(ind{i},:));
            % Add also La to the diagonal
            Lab{i} = Cbl_ff - Qbl_ff + diag(La(ind{i}));
            iLaKfu(ind{i},:) = Lab{i}\K_fu(ind{i},:);
          end
          A = K_uu+K_fu'*iLaKfu;
          A = (A+A')./2;            % Ensure symmetry
          L = iLaKfu/chol(A);

          % From this on evaluate the prediction
          % See Snelson and Ghahramani (2007) for details
          n=size(y,1);
          iCv=zeros(n,1);
          for i=1:length(ind)
            iCv(ind{i},:) = diag(inv(Lab{i}));
          end

          %Varft=1./diag(inv(K+diag(La)))-La;
          Varft=1./(iCv - sum(L.^2,2))-La;

        case 'CS+FIC'
          % CS+FIC
          % Use inverse lemma for CS+FIC
          % Code adapated from gp_pred (Here Las is same as La in gp_pred)

          u = gp.X_u;
          if size(u,2) ~= size(x,2)
            % Turn the inducing vector on right direction
            u=u';
          end

          n = size(x,1);
          m = size(u,1);
          ncf = length(gp.cf);

          % Indexes to all non-compact support and compact support covariances.
          cf1 = [];
          cf2 = [];

          % Loop through all covariance functions
          for i1 = 1:ncf
            if ~isfield(gp.cf{i1},'cs')
              % Non-CS covariances
              cf1 = [cf1 i1];
            else
              % CS-covariances
              cf2 = [cf2 i1];
            end
          end

          % First evaluate needed covariance matrices
          % v defines that parameter is a vector
          [Kv_ff, Cv_ff] = gp_trvar(gp, x, cf1); % f x 1  vector
          K_fu = gp_cov(gp, x, u, cf1);          % f x u
          K_uu = gp_trcov(gp, u, cf1);    % u x u, noiseles covariance K_uu
          K_uu = (K_uu+K_uu')./2;         % ensure the symmetry of K_uu

          Luu  = chol(K_uu)';
          % Evaluate the Lambda (La)
          % Q_ff = K_fu*inv(K_uu)*K_fu'
          B=Luu\(K_fu');       % u x f
          Qv_ff=sum(B.^2)';
          % Add also La to the vector of diagonal elements
          Lav = Cv_ff-Qv_ff + La;   % f x 1, Vector of diagonal elements

          K_cs = gp_trcov(gp,x,cf2);
          Las = sparse(1:n,1:n,Lav,n,n) + K_cs;

          iLaKfu = Las\K_fu;
          A = K_uu+K_fu'*iLaKfu;
          A = (A+A')./2;     % Ensure symmetry
          L = iLaKfu/chol(A);

          %Varft=1./diag(inv(K+diag(La)))-La;
          Varft=1./(diag(inv(Las)) - sum(L.^2,2))-La;

        otherwise
          error('Unknown type of Gaussian process')
      end

      % check if cavity variances are negative
      ii=find(Varft<0);
      if ~isempty(ii)
        warning('gpla_loopred: some LOO latent variances are negative');
        Varft(ii) = gp.jitterSigma2;
      end
      Eft=f-Varft.*deriv;

      if nargout==3
        lpyt = gp.lik.fh.predy(gp.lik, Eft, Varft, y, z);
      elseif nargout>3
        [lpyt,Eyt,Varyt] = gp.lik.fh.predy(gp.lik, Eft, Varft, y, z);
      end

    case 'cavity'
      % using EP equations

      % latent posterior
      [f, sigm2ii] = gpla_pred(gp, x, y, 'z', z, 'tstind', [], 'fcorr', fcorr);
      % remove the jitter variance (see sigm2_t below)
      sigm2ii  = sigm2ii-gp.jitterSigma2;

      % "site parameters"
      W        = -gp.lik.fh.llg2(gp.lik, y, f, 'latent', z);
      deriv    = gp.lik.fh.llg(gp.lik, y, f, 'latent', z);
      % add the jitter variance (see sigm2ii above)
      sigm2_t  = 1./W+gp.jitterSigma2;
      mu_t     = f + sigm2_t.*deriv;

      % "cavity parameters"
      sigma2_i = 1./(1./sigm2ii-1./sigm2_t);
      myy_i    = sigma2_i.*(f./sigm2ii-mu_t./sigm2_t);
      % check if cavity varianes are negative
      ii=find(sigma2_i<0);
      if ~isempty(ii)
        warning('gpla_loopred: some cavity variances are negative');
        sigma2_i(ii) = sigm2ii(ii);
        myy_i(ii) = f(ii);
      end

      % leave-one-out predictions
      Eft=myy_i;
      Varft=sigma2_i;

      if nargout==3
        lpyt = gp.lik.fh.predy(gp.lik, Eft, Varft, y, z);
      elseif nargout>3
        [lpyt,Eyt,Varyt] = gp.lik.fh.predy(gp.lik, Eft, Varft, y, z);
      end

    case 'inla'
      % Leonhard Held and Birgit Schr�dle and H�vard Rue (2010)
      % Posterior and Cross-validatory Predictive Checks: A
      % Comparison of MCMC and INLA. In (eds) Thomas Kneib and
      % Gerhard Tutz, Statistical Modelling and Regression
      % Structures, pp. 91-110. Springer.

      % latent posterior
      [f, sigm2ii, lp] = gpla_pred(gp, x, y, 'z', z, 'tstind', [], 'fcorr', fcorr);

      Eft = zeros(tn,1);
      Varft = zeros(tn,1);
      lpyt = zeros(tn,1);
      minf = f-6.*sqrt(sigm2ii);
      maxf = f+6.*sqrt(sigm2ii);
      for i=1:tn
        if isempty(z)
          z1 = [];
        else
          z1 = z(i);
        end
        [m0, m1, m2] = quad_moments(@(x) norm_pdf(x, f(i), sqrt(sigm2ii(i)))./llvec(gp.lik,y(i),x,z1), minf(i), maxf(i));
        Eft(i) = m1;
        Varft(i) = m2-Eft(i)^2;
        lpyt(i) = -log(m0);
      end

      if nargout>3
        [tmp,Eyt,Varyt] = gp.lik.fh.predy(gp.lik, Eft, Varft, y, z);
      end
      if sum((abs(lpyt)./abs(lp) > 5) == 1) > 0.1*tn;
        warning('Very bad predictive densities, gpla_loopred might not be reliable, check results!');
      end

  end

end

function expll = llvec(gplik, y, f, z)
  for i=1:size(f,2)
    expll(i) = exp(gplik.fh.ll(gplik, y, f(i), z));
  end
end


function [x,f,funEvals] = minConF_BC(funObj,x,LB,UB,options)
% function [x,f] = minConF_BC(funObj,x,LB,UB,options)
%
% Function for using Two-Metric Projection to solve problems of the form:
%   min funObj(x)
%   s.t. LB_i <= x_i <= UB_i
%
%   @funObj(x): function to minimize (returns gradient as second argument)
%
%   options:
%       verbose: level of verbosity (0: no output, 1: final, 2: iter (default), 3:
%       debug)
%       optTol: tolerance used to check for progress (default: 1e-7)
%       maxIter: maximum number of calls to funObj (default: 250)
%       numDiff: compute derivatives numerically (0: use user-supplied
%       derivatives (default), 1: use finite differences, 2: use complex
%       differentials)
%       method: 'sd', 'lbfgs', 'newton'

nVars = length(x);

% Set Parameters
if nargin < 5
    options = [];
end
[verbose,numDiff,optTol,maxIter,suffDec,interp,method,corrections,damped] = ...
    myProcessOptions(...
    options,'verbose',3,'numDiff',0,'optTol',1e-6,'maxIter',500,'suffDec',1e-4,...
    'interp',1,'method','lbfgs','corrections',100,'damped',0);

% Output Log
if verbose >= 2
    fprintf('%10s %10s %15s %15s %15s\n','Iteration','FunEvals','Step Length','Function Val','Opt Cond');
end

% Make objective function (if using numerical derivatives)
funEvalMultiplier = 1;
if numDiff
    if numDiff == 2
        useComplex = 1;
    else
        useComplex = 0;
    end
    funObj = @(x)autoGrad(x,useComplex,funObj);
    funEvalMultiplier = nVars+1-useComplex;
end

% Evaluate Initial Point
x = projectBounds(x,LB,UB);
if strcmp(method,'newton')
    [f,g,H] = funObj(x);
    secondOrder = 1;
else
    [f,g] = funObj(x);
    secondOrder = 0;
end
funEvals = 1;
if verbose >= 1
  fprintf('Initial function value: %15.5e\n', f);
end

% Compute Working Set
working = ones(nVars,1);
working((x < LB+optTol*2) & g >= 0) = 0;
working((x > UB-optTol*2) & g <= 0) = 0;
working = find(working);

% Check Optimality
if isempty(working)
    if verbose >= 1
        fprintf('All variables are at their bound and no further progress is possible at initial point\n');
    end
    return;
elseif norm(g(working)) <= optTol
    if verbose >=1
        fprintf('All working variables satisfy optimality condition at initial point\n');
    end
    return;
end

if verbose >= 3
    switch method
        case 'sd'
            fprintf('Steepest Descent\n');
        case 'lbfgs'
            fprintf('L-BFGS\n');
        case 'bfgs'
            fprintf('BFGS\n');
        case 'newton'
            fprintf('Newton\n');
    end
end

i = 1;
while funEvals <= maxIter

    % Compute Step Direction
    d = zeros(nVars,1);
    switch(method)
        case 'sd'
            d(working) = -g(working);
        case 'lbfgs'
            if i == 1
                d(working) = -g(working);
                old_dirs = zeros(nVars,0);
                old_stps = zeros(nVars,0);
                Hdiag = 1;
            else
                if damped
                    [old_dirs,old_stps,Hdiag] = dampedUpdate(g-g_old,x-x_old,corrections,verbose==3,old_dirs,old_stps,Hdiag);
                else
                    [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,x-x_old,corrections,verbose==3,old_dirs,old_stps,Hdiag);
                end
                    curvSat = sum(old_dirs(working,:).*old_stps(working,:)) > 1e-10;
               d(working) = lbfgsC(-g(working),old_dirs(working,curvSat),old_stps(working,curvSat),Hdiag);
            end
            g_old = g;
            x_old = x;
        case 'bfgs'
            if i == 1
                d(working) = -g(working);
                B = eye(nVars);
            else
                y = g-g_old;
                s = x-x_old;

                ys = y'*s;

                if i == 2
                    if ys > 1e-10
                        B = ((y'*y)/(y'*s))*eye(nVars);
                    end
                end
                if ys > 1e-10
                    B = B + (y*y')/(y'*s) - (B*s*s'*B)/(s'*B*s);
                else
                    if verbose == 2
                        fprintf('Skipping Update\n');
                    end
                end
                d(working) = -B(working,working)\g(working);
            end
            g_old = g;
            x_old = x;

        case 'newton'
            [R,posDef] = chol(H(working,working));

            if posDef == 0
                d(working) = -R\(R'\g(working));
            else
                if verbose == 3
                    fprintf('Adjusting Hessian\n');
                end
                H(working,working) = H(working,working) + eye(length(working)) * max(0,1e-12 - min(real(eig(H(working,working)))));
                d(working) = -H(working,working)\g(working);
            end
        otherwise
            fprintf('Unrecognized Method: %s\n',method);
            break;
    end

    % Check that Progress can be made along the direction
    f_old = f;
    gtd = g'*d;
    if gtd > -optTol
        if verbose >= 2
            fprintf('Directional Derivative below optTol\n');
        end
        break;
    end

    % Select Initial Guess to step length
    if i == 1 && ~secondOrder
        t = min(1,1/sum(abs(g(working))));
    else
        t = 1;
    end

    % Evaluate the Objective and Projected Gradient at the Initial Step
    x_new = projectBounds(x+t*d,LB,UB);
    if secondOrder
        [f_new,g_new,H] = funObj(x_new);
    else
        [f_new,g_new] = funObj(x_new);
    end
    funEvals = funEvals+1;

    % Backtracking Line Search
    lineSearchIters = 1;
    while f_new > f + suffDec*g'*(x_new-x) || ~isLegal(f_new)
        temp = t;
        if interp == 0 || ~isLegal(f_new) || ~isLegal(g_new)
            if verbose == 3
                fprintf('Halving Step Size\n');
            end
            t = .5*t;
        else
            if verbose == 3
                fprintf('Cubic Backtracking\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d]);
        end

        % Adjust if change is too small
        if t < temp*1e-3
            if verbose == 3
                fprintf('Interpolated value too small, Adjusting\n');
            end
            t = temp*1e-3;
        elseif t > temp*0.6
            if verbose == 3
                fprintf('Interpolated value too large, Adjusting\n');
            end
            t = temp*0.6;
        end

        % Check whether step has become too small
        if sum(abs(t*d)) < optTol
            if verbose == 3
                fprintf('Line Search failed\n');
            end
            t = 0;
            f_new = f;
            g_new = g;
            break;
        end

        % Evaluate New Point
        x_new = projectBounds(x+t*d,LB,UB);
        [f_new,g_new] = funObj(x_new);
        funEvals = funEvals+1;
        lineSearchIters = lineSearchIters+1;

    end

    % Take Step
    x = x_new;
    f = f_new;
    g = g_new;

    % Compute Working Set
    working = ones(nVars,1);
    working((x < LB+optTol*2) & g >= 0) = 0;
    working((x > UB-optTol*2) & g <= 0) = 0;
    working = find(working);

    % Output Log
    if verbose >= 2
        fprintf('%10d %10d %15.5e %15.5e %15.5e %15.5e\n',i,funEvals*funEvalMultiplier,t,f,sum(abs(g(working))),norm(g(working)));
    end

    % Check Optimality
    if isempty(working)
        if verbose >= 1
            fprintf('All variables are at their bound and no further progress is possible\n');
        end
        break;
    elseif norm(g(working)) <= optTol
        if verbose >=1
            fprintf('All working variables satisfy optimality condition\n');
        end
        break;
    end

    % Check for lack of progress
    if sum(abs(t*d)) < optTol
        if verbose >= 1
            fprintf('Step size below optTol\n');
        end
        break;
    end

    if abs(f-f_old) < optTol
        if verbose >= 1
            fprintf('Function value changing by less than optTol\n');
        end
        break;
    end

    if funEvals*funEvalMultiplier > maxIter
        if verbose >= 1
            fprintf('Function Evaluations exceeds maxIter\n');
        end
        break;
    end

    % If necessary, compute Hessian
    if secondOrder && lineSearchIters > 1
        [f_new,g_new,H] = funObj(x);
    end

    i = i + 1;
end
end

function [x] = projectBounds(x,LB,UB)
x(x < LB) = LB(x < LB);
x(x > UB) = UB(x > UB);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%                                                                  %%%%%
%%%%    IEEE PES Power Grid Library - Optimal Power Flow - v21.07     %%%%%
%%%%          (https://github.com/power-grid-lib/pglib-opf)           %%%%%
%%%%             Benchmark Group - Small Angle Difference             %%%%%
%%%%                         29 - July - 2021                         %%%%%
%%%%                                                                  %%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function mpc = pglib_opf_case5_pjm__sad
mpc.version = '2';
mpc.baseMVA = 100.0;

%% area data
%	area	refbus
mpc.areas = [
	1	 4;
];

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
mpc.bus = [
	1	 2	 0.0	 0.0	 0.0	 0.0	 1	    1.00000	    0.00000	 230.0	 1	    1.10000	    0.90000;
	2	 1	 300.0	 98.61	 0.0	 0.0	 1	    1.00000	    0.00000	 230.0	 1	    1.10000	    0.90000;
	3	 2	 300.0	 98.61	 0.0	 0.0	 1	    1.00000	    0.00000	 230.0	 1	    1.10000	    0.90000;
	4	 3	 400.0	 131.47	 0.0	 0.0	 1	    1.00000	    0.00000	 230.0	 1	    1.10000	    0.90000;
	5	 2	 0.0	 0.0	 0.0	 0.0	 1	    1.00000	    0.00000	 230.0	 1	    1.10000	    0.90000;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin
mpc.gen = [
	1	 20.0	 0.0	 30.0	 -30.0	 1.0	 100.0	 1	 40.0	 0.0;
	1	 85.0	 0.0	 127.5	 -127.5	 1.0	 100.0	 1	 170.0	 0.0;
	3	 260.0	 0.0	 390.0	 -390.0	 1.0	 100.0	 1	 520.0	 0.0;
	4	 100.0	 0.0	 150.0	 -150.0	 1.0	 100.0	 1	 200.0	 0.0;
	5	 300.0	 0.0	 450.0	 -450.0	 1.0	 100.0	 1	 600.0	 0.0;
];

%% generator cost data
%	2	startup	shutdown	n	c(n-1)	...	c0
mpc.gencost = [
	2	 0.0	 0.0	 3	   0.000000	  14.000000	   0.000000;
	2	 0.0	 0.0	 3	   0.000000	  15.000000	   0.000000;
	2	 0.0	 0.0	 3	   0.000000	  30.000000	   0.000000;
	2	 0.0	 0.0	 3	   0.000000	  40.000000	   0.000000;
	2	 0.0	 0.0	 3	   0.000000	  10.000000	   0.000000;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status	angmin	angmax
mpc.branch = [
	1	 2	 0.00281	 0.0281	 0.00712	 400.0	 400.0	 400.0	 0.0	 0.0	 1	 -1.33164584752	 1.33164584752;
	1	 4	 0.00304	 0.0304	 0.00658	 426.0	 426.0	 426.0	 0.0	 0.0	 1	 -1.33164584752	 1.33164584752;
	1	 5	 0.00064	 0.0064	 0.03126	 426.0	 426.0	 426.0	 0.0	 0.0	 1	 -1.33164584752	 1.33164584752;
	2	 3	 0.00108	 0.0108	 0.01852	 426.0	 426.0	 426.0	 0.0	 0.0	 1	 -1.33164584752	 1.33164584752;
	3	 4	 0.00297	 0.0297	 0.00674	 426.0	 426.0	 426.0	 0.0	 0.0	 1	 -1.33164584752	 1.33164584752;
	4	 5	 0.00297	 0.0297	 0.00674	 240.0	 240.0	 240.0	 0.0	 0.0	 1	 -1.33164584752	 1.33164584752;
];

% INFO    : === Translation Options ===
% INFO    : Phase Angle Bound:           1.33164584752 (deg.)
% INFO    :
% INFO    : === Generator Bounds Update Notes ===
% INFO    :
% INFO    : === Base KV Replacement Notes ===
% INFO    :
% INFO    : === Transformer Setting Replacement Notes ===
% INFO    :
% INFO    : === Line Capacity Monotonicity Notes ===
% INFO    :
% INFO    : === Writing Matpower Case File Notes ===


function [M,Minit,output] = cp_apr(X, R, varargin)
%CP_APR Compute nonnegative CP with alternating Poisson regression.
%
%   M = CP_APR(X, R) computes an estimate of the best rank-R
%   CP model of a tensor X using an alternating Poisson regression.
%   The input X can be a tensor, sptensor, ktensor, or ttensor. The
%   result P is a ktensor.
%
%   M = CP_APR(X, R, 'param', value, ...) specifies optional parameters and
%   values. Valid parameters and their default values are:
%      'tol' - Tolerance on the inner KKT violation {1.0e-4}
%      'maxiters' - Maximum number of iterations {1000}
%      'maxinneriters' = Maximum number of inner iterations {10}
%      'init' - Initial guess [{'random'}|ktensor]
%      'epsilon' - parameter to avoid divide by zero {100*eps}
%      'kappatol' - tolerance on complementary slackness {100*eps}
%      'kappa' - offset to fix complementary slackness {10*eps}
%      'printitn' - Print every n outer iterations; 0 for no printing {1}
%      'printinneritn' - Print every n inner iterations {0}
%
%   [M,M0] = CP_APR(...) also returns the initial guess.
%
%   [M,M0,out] = CP_APR(...) also returns additional output.
%      out.kktViolations - maximum kkt violation per iteration
%      out.nInnerIters   - number of inner iterations per iteration
%      out.nViolations   - number of factor matrices needing complementary
%                          slackness adjustment per iteration
%      out.nTotalIters   - total number of inner iterations
%
%   REFERENCE: E. C. Chi and T. G. Kolda. On Tensors, Sparsity, and
%   Nonnegative Factorizations, arXiv:1112.2414 [math.NA], December 2011,
%   URL: http://arxiv.org/abs/1112.2414. Submitted for publication.
%
%   See also CP_ALS, KTENSOR, TENSOR, SPTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Extract dimensions of X and number of dimensions of X.
N = ndims(X);

%% Set algorithm parameters from input or by using defaults.
params = inputParser;
params.addParamValue('epsilon',1e-10,@isscalar);
params.addParamValue('tol',1e-4,@isscalar);
params.addParamValue('maxiters',1000,@(x) isscalar(x) & x > 0);
params.addParamValue('init','random',@(x) (isa(x,'ktensor') || ismember(x,{'random'})));
params.addParamValue('printitn',1,@isscalar);
params.addParamValue('kappa',1e-2,@isscalar);
params.addParamValue('kappatol',1e-10,@isscalar);
params.addParamValue('maxinneriters',10,@isscalar);
params.addParamValue('printinneritn',0,@isscalar);
params.parse(varargin{:});

%% Copy from params object.
epsilon = params.Results.epsilon;
tol = params.Results.tol;
maxOuterIters = params.Results.maxiters;
Minit = params.Results.init;
kappa = params.Results.kappa;
kappaTol = params.Results.kappatol;
maxInnerIters = params.Results.maxinneriters;
printOuterItn = params.Results.printitn;
printInnerItn = params.Results.printinneritn;
kktViolations = -ones(maxOuterIters,1);
nInnerIters = zeros(maxOuterIters,1);

%% Set up and error checking on initial guess for U.
if isa(Minit,'ktensor')
    if ndims(Minit) ~= N
        error('Initial guess does not have the right number of dimensions');
    end

    if ncomponents(Minit) ~= R
        error('Initial guess does not have the right number of components');
    end

    for n = 1:N
        if size(Minit,n) ~= size(X,n)
            error('Dimension %d of the initial guess is the wrong size',n);
        end
    end
elseif strcmp(Minit,'random')
    F = cell(N,1);
    for n = 1:N
        F{n} = rand(size(X,n),R);
    end
    Minit = ktensor(F);
else
    error('The selected initialization method is not supported');
end

%% Set up for iterations - initializing M and Phi.
M = normalize(Minit,[],1);
Phi = cell(N,1);
kktModeViolations = zeros(N,1);

if printOuterItn > 0
  fprintf('\nCP_APR:\n');
end

nViolations = zeros(maxOuterIters,1);

%% Main Loop: Iterate until convergence.
for iter = 1:maxOuterIters

    isConverged = true;
    for n = 1:N

        % Make adjustments to entries of M{n} that are violating
        % complementary slackness conditions.
        if (iter > 1)
            V = (Phi{n} > 1) & (M{n} < kappaTol);
            if any(V(:))
                nViolations(iter) = nViolations(iter) + 1;
                M{n}(V>0) = M{n}(V>0) + kappa;
            end
        end

        % Shift the weight from lambda to mode n
        M = redistribute(M,n);

        % Calculate product of all matrices but the n-th
        % (In sparse case, only calcuates entries corresponding to nonzeros in X.)
        Pi = calculatePi(X, M, R, n, N);

        % Do the multiplicative updates
        for i = 1:maxInnerIters

            % Count the inner iterations
            nInnerIters(iter) = nInnerIters(iter) + 1;

            % Calculate matrix for multiplicative update
            Phi{n} = calculatePhi(X, M, R, n, Pi, epsilon);

            % Check for convergence
            kktModeViolations(n) = max(abs(vec(min(M.U{n},1-Phi{n}))));
            if (kktModeViolations(n) < tol)
                break;
            else
                isConverged = false;
            end

            % Do the multiplicative update
            M{n} = M{n} .* Phi{n};

            % Print status
             if mod(i, printInnerItn)==0
                 fprintf('    Mode = %1d, Inner Iter = %2d, KKT violation = %.6e\n', n, i, kktModeViolations(n));
             end
        end

        % Shift weight from mode n back to lambda
        M = normalize(M,[],1,n);

    end

    kktViolations(iter) = max(kktModeViolations);

    if (mod(iter,printOuterItn)==0)
        fprintf(' Iter %4d: Inner Its = %2d KKT violation = %.6e, nViolations = %2d\n', ...
        iter, nInnerIters(iter), kktViolations(iter), nViolations(iter));
    end

    % Check for convergence
    if (isConverged)
        break;
    end
end

%% Clean up final result
M = normalize(M,'sort',1);

if printOuterItn>0
    normX = norm(X);
    normresidual = sqrt( normX^2 + norm(M)^2 - 2 * innerprod(X,M) );
    fit = 1 - (normresidual / normX); %fraction explained by model
    fprintf('===========================================\n');
    fprintf(' Final log-likelihood = %e \n', tt_loglikelihood(X,M));
    fprintf(' Final least squares fit = %e \n', fit);
    fprintf(' Final KKT violation = %7.7e\n', kktViolations(iter));
    fprintf(' Total inner iterations = %d\n', sum(nInnerIters));
end

output = struct;
output.params = params.Results;
output.kktViolations = kktViolations(1:iter);
output.nInnerIters = nInnerIters(1:iter);
output.nViolations = nViolations(1:iter);
output.nTotalIters = sum(nInnerIters);

end

function Pi = calculatePi(X, M, R, n, N)

if (isa(X,'sptensor'))
    Pi = ones(nnz(X), R);
    for nn = [1:n-1,n+1:N]
        Pi = M{nn}(X.subs(:,nn),:).*Pi;
    end
else
    U = M.U;
    Pi = khatrirao(U{[1:n-1,n+1:N]},'r');
end

end

function Phi = calculatePhi(X, M, R, n, Pi, epsilon)

if (isa(X,'sptensor'))
    Phi = -ones(size(X,n),R);
    xsubs = X.subs(:,n);
    v = sum(M.U{n}(xsubs,:).*Pi,2);
    wvals = X.vals ./ max(v, epsilon);
    for r = 1:R
        Yr = accumarray(xsubs, wvals .* Pi(:,r), [size(X,n) 1]);
        Phi(:,r) = Yr;
    end
else
    Xn = double(tenmat(X,n));
    V = M.U{n}*Pi';
    W = Xn ./ max(V, epsilon);
    Y = W * Pi;
    Phi = Y;
end

end

function y = vec(x)
y = x(:);
end


classdef CurveBall < solvers.Solver
  % CurveBall - Newton solver variant
  %
  % This variant works by 2 gradient descent updates: one for the linear
  % system (inversion), and another for the main objective.
  %
  % Assumptions:
  % - Network's output is a single tensor of predictions (not loss).
  % - Forward pass was executed just before calling this solver's step().

  properties
    loss = 'logistic'  % loss to use: 'ls' (least-squares), 'logistic' (softmax-log)

    lambda = 1  % Hessian regularization (0 = pure Newton)

    momentum = 0.9  % forget factor for linear system solution
    beta = 0.1  % learning rate for linear system GD step

    autoparam = true  % adapt momentum and beta
    autoparam_interval = 1  % only run autoparam every N steps. NOTE: autolambda depends on autoparam running in the same step.
    autoparam_print = false  % to print momentum/beta for debugging

    autolambda = true  % adapt lambda
    autolambda_w1 = 0.999  % factor, how much to change lambda each time
    autolambda_interval = 5  % only run autolambda every N steps. NOTE: autolambda depends on autoparam running in the same step.
    autolambda_thresh = 0.5  % rho thresholds to trigger lambda change; [low, high] vector, or scalar T to use [T, 2 - T]

    labels  % labels for this mini-batch (batch size assumed to be last dim.; cannot run with batch of 1)
    net  % pointer to network object

    count = 0  % iteration count

    state  % store for momentum (z)

    ratio = 0  % autolambda ratio is stored here for debugging
    last_loss  % last computed loss value
  end

  methods
    function o = CurveBall(varargin)
      % parse generic Solver arguments
      % default weight decay is 0, but can override with name-value pair
      varargin = o.parseGenericArgs([{'weightDecay', 0}, varargin]);

      % parse arguments specific to this solver
      vl_parseprop(o, varargin, {'loss', 'lambda', 'beta', 'momentum', ...
        'autoparam', 'autoparam_interval', 'autoparam_print', 'autolambda', ...
        'autolambda_w1', 'autolambda_thresh', 'autolambda_interval'});
      % don't let parent class clear some parameters
      o.conserveMemory = false;
      o.state = {};
    end

    function w = gradientStep(o, w, ~, lr, decay)
      % store properties in local vars
      net = o.net;  %#ok<*PROPLC>
      lambda = o.lambda;

      % initialize state if needed
      z = o.state;
      if isempty(z)
        z = cell(size(w));
        for i = 1:numel(w)
          z{i} = zeros(size(w{i}), 'like', w{i});
        end
      end

      use_decay = any(decay);
      assert(~use_decay, 'Not implemented.');

      %
      % do two GD steps. first update z (linear system state), then use the
      % result (whitened gradient estimate) to update the parameters w.
      %
      % zdelta = J' * Hl * J * z + lambda * z + J' * Jl'
      %
      % znew = momentum * z - beta * zdelta
      %
      % wnew = w + lr * znew
      %

      % compute factor Jz = J * z (for Hessian term)
      Jz = fmad(net, z);

      % evaluate loss, loss gradient Jl, and loss Hessian (Hl).
      % compute hessian_term = J' * Hl * Jz + Jl
      [Jz_, Jl, o.last_loss] = hessian_grad_loss(net, o.loss, o.labels, Jz);

      % backpropagate Jz_ + Jl
      hessian_term = backward(net, Jz_ + Jl);

      % compute z update (gradient descent on linear system)
      delta_z = cell(size(w));
      for i = 1:numel(w)
        delta_z{i} = hessian_term{i} + lambda * z{i};
      end

      %
      % automatic hyperparameters rho (momentum) and beta
      %

      if o.autoparam && rem(o.count, o.autoparam_interval) == 0
        % compute factor Jz = J * z (for Hessian term)
        Jdz = fmad(net, delta_z);

        % evaluate loss, loss gradient Jl, and loss Hessian (Hl).
        % compute hessian_term = J' * Hl * Jz
        Jdz_ = hessian_grad_loss(net, o.loss, o.labels, Jdz);

        % compute momentum and update by solving a 2x2 system A * x = b
        A11 = gather(Jdz(:)' * Jdz_(:));
        A12 = gather(Jz(:)' * Jdz_(:));
        A22 = gather(Jz(:)' * Jz_(:));

        b1 = gather(Jl(:)' * Jdz(:));
        b2 = gather(Jl(:)' * Jz(:));

        for i = 1:numel(w)
          % compute the system we want to invert
          z_vec = z{i}(:);
          dz_vec = delta_z{i}(:);

          % expand scalar if needed
          if isscalar(z_vec), z_vec(1:numel(dz_vec),1) = z_vec; end

          A11 = A11 + gather(dz_vec' * dz_vec) * lambda;  % accumulating scalars is faster (JIT)
          A12 = A12 + gather(dz_vec' * z_vec) * lambda;
          A22 = A22 + gather(z_vec' * z_vec) * lambda;
        end

        % compute beta and momentum coefficient
        A = double([A11, A12; A12, A22]);
        b = double([b1; b2]);
        m_b = A \ b;
        beta = m_b(1);
        momentum = -m_b(2);

        % sanity check
        if ~isfinite(momentum), momentum = 0; end
        if ~isfinite(beta), beta = 0; end

        % store values for next iterations if needed
        o.momentum = momentum;
        o.beta = beta;
      else
        momentum = o.momentum;
        beta = o.beta;
      end

      if o.autoparam_print
        fprintf('mom: %.1g beta: %.1g ', momentum, beta);
      end

      % update parameters with computed beta and momentum
      for i = 1:numel(w)
        % update linear system state
        z{i} = vl_taccum(momentum, z{i}, -beta, delta_z{i}) ;

        % update parameters (returned as this method's output)
        w{i} = vl_taccum(1, w{i}, lr(i), z{i}) ;
      end
      o.state = z;

      %
      % automatic lambda (trust region) hyperparameter
      %

      if o.autolambda && rem(o.count, o.autolambda_interval) == 0
        h_curr = o.last_loss ;
        M = -0.5 * m_b(:)' * b(:);

        % compute new value
        labels = o.labels;
        if isvector(labels)  % categorical labels
          batch_size = numel(labels);
        else  % regression targets, get last dimension
          batch_size = size(labels, ndims(labels));
        end

        % assign new parameter values now (don't wait for Solver.step)
        idx = [net.params.var];
        is_grad = ([net.params.trainMethod] == 1);
        net.setValue(idx(is_grad), w) ;

        % run network forward with new parameters
        net.eval({}, 'forward');

        % get value of last var (assumes only one output)
        pred = net.vars{end - 1};
        pred = reshape(pred, 1, 1, [], batch_size);  % reshape to 4D tensor for vl_nnloss

        % compute loss value
        switch o.loss
        case 'logistic'
          loss_value = vl_nnloss(pred, labels, 'loss', 'softmaxlog');
        otherwise
          error('Loss not yet supported with autolambda');
        end

        h_new = loss_value;

        % ratio between true curvature and quadratic fit curvature
        ratio = (h_new - h_curr) / M;

        % increase or decrease lambda based on ratio
        w1  = o.autolambda_w1 ^ o.autolambda_interval;
        thresh = o.autolambda_thresh;
        if isscalar(thresh)
          thresh = [thresh, 2 - thresh];
        end
        assert(diff(thresh) >= 0);

        if ratio < thresh(1)
          lambda = lambda / w1;
        elseif ratio > thresh(2)
          lambda = lambda * w1;
        end
        o.lambda = lambda;
        o.ratio = ratio;
      end

      o.count = o.count + 1;

    end

    function s = saveobj(o)
      % serialize to struct (called by the built-in function SAVE)
      % transfer state to CPU first
      s = o.saveGeneric();  % call parent class
      s.last_loss = gather(o.last_loss);
    end
  end

  methods (Static)
    function o = loadobj(s)
      % deserialize from struct (called by the built-in function LOAD)
      o = solvers.CurveBall();
      o.last_loss = s.last_loss;
      o.loadGeneric(s);  % call parent class

      % don't let parent class clear some parameters
      o.conserveMemory = false;
    end
  end
end

% computes the loss value, its gradient, and the hessian (multiplied by a
% vector x).
function [Hlx, Jl, loss_value] = hessian_grad_loss(net, loss, labels, x)
  if isvector(labels)  % categorical labels
    batch_size = numel(labels);
  else  % regression targets, get last dimension
    batch_size = size(labels, 4);
  end

  % get value of last var (assumes only one output)
  pred = net.vars{end - 1};
  pred = reshape(pred, 1, 1, [], batch_size);  % reshape to 4D tensor for vl_nnloss

  switch loss
  case 'ls'  % least-squares loss.
    % compute Hl * x. Hl = 2 / batch_size * I.
    Hlx = 2 / batch_size * x;

    % loss Jacobian, Jl
    assert(isequal(size(pred), size(labels)));
    Jl = 2 / batch_size * (pred - labels);

    % compute loss value
    loss_value = mean(sum((pred - labels).^2, 3), 4);

  case 'logistic'  % logistic loss.
    % compute Hl * x. for a single sample, Hl = diag(p) - p * p', where p
    % is a column-vector. for many samples, p has one column per sample,
    % and Hl is a block-diag with each block as above (so one independent
    % matrix-vec product per sample). first compute p' * x for all samples

    p = vl_nnsoftmax(pred);  % softmaxed probabilities

    if ismatrix(p)
      px = sum(p .* x, 1);
    else  % 4D tensor
      px = sum(p .* x, 3);
    end

    % now finish computing Hl * x = diag(p) * x - p * p' * x for every sample.
    Hlx = p .* x - p .* px;
    Hlx = Hlx / batch_size;

    if nargout <= 1, return, end  % early exit for single output

    % compute loss Jacobian, Jl. use backward pass of vl_nnloss
    Jl = vl_nnloss(pred, labels, single(1), 'loss', 'softmaxlog');  % loss gradient

    % compute loss value
    loss_value = vl_nnloss(pred, labels, 'loss', 'softmaxlog');

  otherwise
    error('Unknown loss.');
  end
end


function [ MRI_signal, bvalue ] = dt_from_smoldyn_pt2( positions, voxel_size, step_time, delta, Delta, gradient_directions, gradient_strength, bvalue )
%DT_FROM_SMOLDYN_PT2 Calculates the MRI signal
%
% PART 2 OF THE CALCULATION OF THE DIFFUSION TENSOR FROM THE SMOLDYN MODEL DATA
% This part calculates the diffusion MRI signal from the positions data.
% Note that providing each set of positions data contains the same number
% of molecules, the MRI signal from different sets of positions data can be
% combined by just taking the mean.
%
% Inputs are:
%   positions: matrix of size no of molecules x 3 x no of timesteps generated
%       from the Smoldyn program and cut down in part 1
%   voxel_size: scalar. the total (isotropic) size of the volume simulated (um)
%   step_time: scalar. the time of each step as used to generate the Smoldyn diffusion (us)
%   delta: scalar. the gradient pulse time (us)
%   Delta: scalar. the time between gradient pulses (us)
%   gradient_directions: matrix of size no of directions x 3 which gives
%       the x,y,z components of the gradient directions
%   gradient_strength: IF UNKNOWN, SET TO 0 & IT WILL BE CALCULATED FROM
%       BVALUE. scalar. (T/um)
%   bvalue: IF UNKNOWN, SET TO 0 & IT WILL BE CALCULATED FROM
%       GRADIENT_STRENGTH. scalar. (um^2/us)
%
% Outputs are:
%   MRI_signal: relative signal for each of the gradient directions
%   bvalue: as input, included as an output as needed later

% Author: Jo Bates <jobates81@gmail.com>
% Copyright © 2014 University of Oxford
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

no_of_directions = size(gradient_directions,1);
no_of_molecules = size(positions,1);
% number of jumps
no_of_timesteps = size(positions, 3);

% define the gyromagnetic ratio of water (rad/us/T)
gyro_ratio = 267.5;

% calculate the bvalue or gradient_strength if required
if bvalue == 0;
    bvalue = gradient_strength^2 * gyro_ratio^2 * delta^2 *(Delta - (delta/3));
end
if gradient_strength == 0;
    gradient_strength = sqrt(bvalue/(gyro_ratio^2 * delta^2 *(Delta - (delta/3))));
end

% resolution & range of the co-ordinates
xres = 1;
yres = 1;
zres = 1;
xmax = (voxel_size-1)/2;
ymax = (voxel_size-1)/2;
zmax = (voxel_size-1)/2;

% parameters independent of gradient direction or voxel
pulse_1_end_step = (delta/step_time) + 1;
pulse_2_start_step = (Delta/step_time)+1;
pulse_2_end_step = ((Delta+delta)/step_time) + 1;

% calc parameters which are independent of voxel
start_corner = zeros(no_of_directions, 3);
for g = 1:no_of_directions
    % work out the starting corner of the image, where the MRI pulse with be 0
    sign_x = sign(gradient_directions(g,1));
    sign_y = sign(gradient_directions(g,2));
    sign_z = sign(gradient_directions(g,3));
    if sign_x < 0
        x_1 = xmax*2+1;
    else
        x_1 = 1;
    end
    if sign_y < 0
        y_1 = ymax*2+1;
    else
        y_1 = 1;
    end
    if sign_z < 0
        z_1 = zmax*2+1;
    else
        z_1 = 1;
    end
    start_corner (g,:) = [x_1, y_1, z_1];
end

MRI_signal = zeros(no_of_directions,1);
% for every gradient direction
for g = 1:no_of_directions
    x_1 = start_corner (g,1);
    y_1 = start_corner (g,2);
    z_1 = start_corner (g,3);
        % for each molecule, for each position, the position in gradient
        % direction is the dot product of the position times the gradient
        % direction. since A.B = a1*b1 + a2*b2 + a3*b3, calculate like this as
        % need to include the potential for different resolutions in each
        % direction and can't seem to do dot product of correct parts for these
        % 2 matrices.

        x_pos = (positions(:,1,:)-x_1).*xres.*gradient_directions(g,1);
        y_pos = (positions(:,2,:)-y_1).*yres.*gradient_directions(g,2);
        z_pos = (positions(:,3,:)-z_1).*zres.*gradient_directions(g,3);
        position_grad_dirn = x_pos + y_pos + z_pos;

        % calculate phase shift for each molecule
        mol_pulse = zeros(size(position_grad_dirn));
        mol_pulse(:,:,1:pulse_1_end_step) = position_grad_dirn(:,:,1:pulse_1_end_step).*gradient_strength;
        mol_pulse(:,:,pulse_2_start_step:pulse_2_end_step) = -position_grad_dirn(:,:,pulse_2_start_step:pulse_2_end_step).*gradient_strength;
        molpulsesum = sum(mol_pulse,3);

        % calc MRI signal
        phasech = gyro_ratio*molpulsesum*step_time;
        cosphase = cos(phasech);
        MRI_signal(g) = mean(cosphase);
end

end


 function mse = ComputeMultiscaleEntropy(data, m, r, maxTau, maxVecSize,cg_moment,cg_method,constant_r)

% mse = ComputeMultiscaleEntropy(data, m, r, max_tau,cg_moment)
%
% Overview
%	Calculates multiscale entropy on a vector of input data.
%
% Input
%	data        - data to analyze; vector of doubles
%   m           - pattern length; int
%   r           - radius of similarity (% of std); double
%   maxTau      - maximum number of coarse-grainings; int
%   maxVecSize  - optional, parameter to switch from SampEn to FastSempEn
%   cg_moment   - optional, moment used to coarse-grain the time series,
%                 by default use the mean 'mean'
%                 Options: 'mean', 'varaince'
%   cg_method   - [optional] - method use to generate the coarse-grain
%                 time series. By default use the original method proposed
%                 by Costa et al. ('fir' with mean).
%                 Options: 'fir' [Costa et al.], 'butter' [Valencia et al.]
%   constant_r  - [DEFAULT] 1 : use r as function of std of original time
%                 series in this implementation correspond to zscore only
%                 the original time series
%               - 0 : r as a function of scale factor (tau), i.e.,
%                 r(i)= r*std(ScaleData(i)), in this implementation it
%                 corresponds to zscore  each coarse-grain time siries
%
%
% Output
%   mse          - vector of [max_tau, 1] doubles
%
% Example
%   data = rand(1e4, 1); % generate random data
%   m = 2; % template length
%   r = 0.2; % radius of similarity
%   maxTau = 4; % calculate sample entropy over four coarse grainings
%   mse = multiscaleEntropy(data, m, r, maxTau, 'Fast','mean');
%
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%
% Reference(s)
%
% Copyright (C) 2017 Erik Reinertsen <er@gatech.edu>
% All rights reserved.
%
%
% 08-23-2017 Modyfied by Giulia Da Poian to be inclused in the HRV Toolbox
% for PhysioNet Cardiovascular Signal ToolboxToolbox
% Removed the possibility to use different types of entropy, only
% fastSampen method in this version
%
% 10-10-2017 Modyfied by Giulia Da Poian, use FastSampEn in series < 34000
% otherwise use traditional SampEn that is faster for long series
%
% 10-23-2017 Modyfied by Giulia Da Poian, using scales like in Costa's
% paper instead of Coarse-grain data using halving method.
%
% This software may be modified and distributed under the terms
% of the BSD license. See the LICENSE file in this repo for details.

if nargin<5 || isempty(maxVecSize)
    maxVecSize = 34000;
end
if nargin<6 || isempty(cg_moment)
    cg_moment = 'mean';
end
if nargin<7 || isempty(cg_method)
    cg_method = 'fir';
end
if nargin<8 || isempty(constant_r)
    constant_r = 0;
end

data = zscore(data);  % (introduced by GDP) normalization of the signal that
                      % replace the common practice of expressing the
                      % tolerance as r times the standard deviation

mse = NaN(maxTau, 1); % Initialize output vector

SampEnType = 'Maxim'; % Initialize default SampEn method

% Check data length, if < 34000 use Fast Implementation (introduced GDP)
if length(data) < maxVecSize
     SampEnType = 'Fast';
end

% Loop through each window

% Loop through each timescale
% Note: i_tau == 1 is the original time series
for i_tau = 1:maxTau

    scaledData = coarsegrain(data,i_tau,cg_moment,cg_method); % Changed by GDP

    if ~constant_r
        scaledData = zscore(scaledData);
    end

    switch SampEnType
        case 'Fast'
           mse(i_tau) = fastSampen(scaledData, m, r);
        otherwise
           mse(i_tau) = sampenMaxim(scaledData, m, r);
    end

end % end for loop

% REFERENCES

% Costa et al. "Multiscale entropy analysis of complex physiologic time
% series." Physical review letters 89.6 (2002): 068102

% Valencia et al. "Refined multiscale entropy: Application to 24-h holter
% recordings of heart period variability in healthy and aortic stenosis
% subjects." IEEE Transactions on Biomedical Engineering 56, no. 9 (2009).


function [fProbability, fAICc] = calc_loglikelihood_nochange2(mCat1, mCat2)
% function [fProbability, fAICc] = calc_loglikelihood_nochange2(mCat1, mCat2);
% ----------------------------------------------------------------------------------------------
% Calculate log-likelihood estimation of NO CHANGE between the two periods
%
% Incoming variable
% mCat1 : EQ catalog period 1 (Catalog to be modified)
% mCat2 : EQ catalog period 2 (Observed catalog)
%
% Outgoing variable
% fProbability : log-likelihood probabilty
% fAICc         : Corrected Akaike Information Criterion
%
% Author: J. Woessner, woessner@seismo.ifg,.ethz.ch
% last update: 26.11.02

fBinning = 0.1;

% Determine exact time period
fPeriod1 = max(mCat1(:,3)) - min(mCat1(:,3));
fPeriod2 = max(mCat2(:,3)) - min(mCat2(:,3));

% Initialize values
fMinMag = min([min(mCat1(:,6)) min(mCat2(:,6))]);
fMaxMag = max([max(mCat1(:,6)) max(mCat2(:,6))]);

try
    %% Calculate model for best fiting Mc
    [mResult, fMls, fMc, fMu, fSigma, mDatPred, vPredBest, fBvalue] = calc_McCdfnormal(mCat1, fBinning);
    vPredFMD = mDatPred(:,1)'./fPeriod1;
    vMags = mDatPred(:,2)';
    % FMD to be modeled
    [vObsFMD,vBin2] = hist(mCat2(:,6),min(vMags):0.1:max(vMags));
    vObsFMD = ceil(vObsFMD./fPeriod2);
    % Calculate the likelihoods for both models
    vProb_ = calc_log10poisspdf2(vObsFMD', vPredFMD');
    % Sum the probabilities
    fProbability = (-1) * sum(vProb_);

    nDegFree = 0; % degree of freedom
    n_samples = length(mCat2(:,6));
    %% Corrected Akaike Information Criterion (AICc)
    fAICc = -2*(-fProbability)+2*nDegFree+2*nDegFree*(nDegFree+1)/(n_samples-nDegFree-1);
catch
    fAICc = nan;
    fProbability = nan;
end


function [ n_data, n, a, x, fx ] = gegenbauer_poly_values ( n_data )

%*****************************************************************************80
%
%% GEGENBAUER_POLY_VALUES returns some values of the Gegenbauer polynomials.
%
%  Discussion:
%
%    The Gegenbauer polynomials are also known as the "spherical
%    polynomials" or "ultraspherical polynomials".
%
%    In Mathematica, the function can be evaluated by:
%
%      GegenbauerC[n,m,x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the order parameter of the function.
%
%    Output, real A, the real parameter of the function.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 38;

  a_vec = [ ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.5E+00, ...
      0.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      3.0E+00, ...
      4.0E+00, ...
      5.0E+00, ...
      6.0E+00, ...
      7.0E+00, ...
      8.0E+00, ...
      9.0E+00, ...
     10.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00, ...
      3.0E+00 ];

  fx_vec = [ ...
       1.0000000000E+00, ...
       0.2000000000E+00, ...
      -0.4400000000E+00, ...
      -0.2800000000E+00, ...
       0.2320000000E+00, ...
       0.3075200000E+00, ...
      -0.0805760000E+00, ...
      -0.2935168000E+00, ...
      -0.0395648000E+00, ...
       0.2459712000E+00, ...
       0.1290720256E+00, ...
       0.0000000000E+00, ...
      -0.3600000000E+00, ...
      -0.0800000000E+00, ...
       0.8400000000E+00, ...
       2.4000000000E+00, ...
       4.6000000000E+00, ...
       7.4400000000E+00, ...
      10.9200000000E+00, ...
      15.0400000000E+00, ...
      19.8000000000E+00, ...
      25.2000000000E+00, ...
      -9.0000000000E+00, ...
      -0.1612800000E+00, ...
      -6.6729600000E+00, ...
      -8.3750400000E+00, ...
      -5.5267200000E+00, ...
       0.0000000000E+00, ...
       5.5267200000E+00, ...
       8.3750400000E+00, ...
       6.6729600000E+00, ...
       0.1612800000E+00, ...
      -9.0000000000E+00, ...
     -15.4252800000E+00, ...
      -9.6969600000E+00, ...
      22.4409600000E+00, ...
     100.8892800000E+00, ...
     252.0000000000E+00 ];

  n_vec = [ ...
     0,  1,  2, ...
     3,  4,  5, ...
     6,  7,  8, ...
     9, 10,  2, ...
     2,  2,  2, ...
     2,  2,  2, ...
     2,  2,  2, ...
     2,  5,  5, ...
     5,  5,  5, ...
     5,  5,  5, ...
     5,  5,  5, ...
     5,  5,  5, ...
     5,  5 ];

  x_vec = [ ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.20E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
      0.40E+00, ...
     -0.50E+00, ...
     -0.40E+00, ...
     -0.30E+00, ...
     -0.20E+00, ...
     -0.10E+00, ...
      0.00E+00, ...
      0.10E+00, ...
      0.20E+00, ...
      0.30E+00, ...
      0.40E+00, ...
      0.50E+00, ...
      0.60E+00, ...
      0.70E+00, ...
      0.80E+00, ...
      0.90E+00, ...
      1.00E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    n = 0;
    a = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    n = n_vec(n_data);
    a = a_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


%%******************************************************************
%% mybicgstab
%%
%% [xx,resnrm,flag] = mybicgstab(A,b,M1,tol,maxit)
%%
%% iterate on  bb - (M1)*AA*x
%%
%% r = b-A*xtrue;
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************
  function [xx,resnrm,flag] = mybicgstab(A,b,M1,tol,maxit,printlevel)

  N = length(b);
  if (nargin < 6); printlevel = 1; end
  if (nargin < 5) | isempty(maxit); maxit = max(30,length(A.mat22)); end;
  if (nargin < 4) | isempty(tol); tol = 1e-10; end;
  tolb = min(1e-4,tol*norm(b));
  flag = 1;

  x = zeros(N,1);
  if (norm(x))
     if isstruct(A); r = b-matvec(A,x); else; r = b-mexMatvec(A,x); end;
  else
     r =b;
  end
  err = norm(r); resnrm(1) = err;  minresnrm = err; xx = x;
  %%if (err < 1e-3*tolb); return; end

  omega  = 1.0;
  r_tld = r;
%%
%%
%%
  breakyes = 0;
  smtol = 1e-40;
  for iter = 1:maxit,

     rho   = (r_tld'*r);
     if (abs(rho) < smtol)
        flag = 2;
        if (printlevel); fprintf('*'); end;
        breakyes = 1;
        break;
     end
     if (iter > 1)
        beta  = (rho/rho_1)* (alp/omega);
        p = r + beta*(p - omega*v);
     else
        p = r;
     end
     p_hat = precond(A,M1,p);
     if isstruct(A); v = matvec(A,p_hat); else; v = mexMatvec(A,p_hat); end;
     alp = rho / (r_tld'*v);
     s = r - alp*v;
     %%
     s_hat = precond(A,M1,s);
     if isstruct(A); t = matvec(A,s_hat); else; t = mexMatvec(A,s_hat); end;
     omega = (t'*s) / (t'*t);
     x = x + alp*p_hat + omega*s_hat;
     r = s - omega*t;
     rho_1 = rho;
     %%
     %% check convergence
     %%
     err = norm(r); resnrm(iter+1) = err;
     if (err < minresnrm);
        xx = x; minresnrm = err;
     end
     if (err < tolb)
        break;
     end
     if (err > 10*minresnrm)
        if (printlevel); fprintf('^'); end
        breakyes = 2;
        break;
     end
     if (abs(omega) < smtol)
        flag = 2;
        if (printlevel); fprintf('*'); end
        breakyes = 1;
        break;
     end
  end
  if (~breakyes) & (printlevel >=3); fprintf(' '); end
%%
%%*************************************************************************
%%*************************************************************************
%% precond:
%%*************************************************************************

   function Mx = precond(A,L,x)

   m = L.matdim; m2 = length(x)-m;
   Mx = zeros(length(x),1);

   for iter = 1
      if norm(Mx); r = x - matvec(A,Mx); else; r = x; end
      if (m2 > 0)
         r1 = full(r(1:m));
      else
         r1 = full(r);
      end
      if (m2 > 0)
         r2 = r(m+[1:m2]);
         w = linsysolvefun(L,r1);
         z = mexMatvec(A.mat12,w,1) - r2;
         z = L.Mu \ (L.Ml \ (L.Mp*z));
         r1 = r1 - mexMatvec(A.mat12,z);
      end
      d = linsysolvefun(L,r1);
      if (m2 > 0)
         d = [d; z];
      end
      Mx = Mx + d;
   end
%%*************************************************************************
%%*************************************************************************
%% matvec: matrix-vector multiply.
%% matrix = [A.mat11, A.mat12; A.mat12', A.mat22]
%%*************************************************************************

   function Ax = matvec(A,x);

   m = length(A.mat11); m2 = length(x)-m;
   if issparse(x); x = full(x); end
   if (m2 > 0)
      x1 = x(1:m);
   else
      x1 = x;
   end
   Ax = mexMatvec(A.mat11,x1);
   if (m2 > 0)
      x2 = x(m+[1:m2]);
      Ax = Ax + mexMatvec(A.mat12,x2);
      Ax2 = mexMatvec(A.mat12,x1,1) + mexMatvec(A.mat22,x2);
      Ax = [full(Ax); full(Ax2)];
   end
%%*************************************************************************


function failed_tutorial_natmeg_beamforming

% WALLTIME 00:20:00
% MEM 8gb
% DEPENDENCY

% this script executes the MATLAB content from
% http://www.fieldtriptoolbox.org/tutorial/natmeg/beamforming
%
% it corresponds to the wiki version of 7 October 2014

clear all
close all

cd(dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/natmeg'));

load timefrequency/data_clean_MEG_responselocked.mat;
load dipolefitting/headmodel_meg.mat;

% Select time window of interest
cfg = [];
cfg.toilim = [0.35 0.85];
data_timewindow = ft_redefinetrial(cfg,data_clean_MEG_responselocked);

% Freqanalysis for beamformer
cfg = [];
cfg.channel      = {'MEG*2', 'MEG*3'};
cfg.method       = 'mtmfft';
cfg.taper        = 'dpss';
cfg.output       = 'powandcsd';
cfg.keeptrials   = 'no';
cfg.foi          = 18;
cfg.tapsmofrq    = 4;

% for common filter over conditions
powcsd_all      = ft_freqanalysis(cfg, data_timewindow);

% for conditions
cfg.trials       = find(data_timewindow.trialinfo(:,1) == 256);
powcsd_left      = ft_freqanalysis(cfg, data_timewindow);
cfg.trials       = find(data_timewindow.trialinfo(:,1) == 4096);
powcsd_right     = ft_freqanalysis(cfg, data_timewindow);

% Create leadfield grid
cfg                 = [];
cfg.channel         = {'MEG*2', 'MEG*3'};
cfg.grad            = powcsd_all.grad;
cfg.headmodel       = headmodel_meg;
cfg.dics.reducerank = 2; % default for MEG is 2, for EEG is 3
cfg.sourcemodel.resolution = 0.5;   % use a 3-D grid with a 0.5 cm resolution
cfg.sourcemodel.unit       = 'cm';
cfg.sourcemodel.tight      = 'yes';
[grid] = ft_prepare_leadfield(cfg);

cfg              = [];
cfg.channel      = {'MEG*2', 'MEG*3'};
cfg.method       = 'dics';
cfg.frequency    = 18;
cfg.sourcemodel         = grid;
cfg.headmodel    = headmodel_meg;
cfg.senstype     = 'MEG'; % Must me 'MEG', although we only kept MEG channels, information on EEG channels is still present in data
cfg.dics.keepfilter   = 'yes'; % We wish to use the calculated filter later on
cfg.dics.projectnoise = 'yes';
cfg.dics.lambda  = '5%';
source_all = ft_sourceanalysis(cfg, powcsd_all);

cfg              = [];
cfg.channel      = {'MEG*2', 'MEG*3'};
cfg.method       = 'dics';
cfg.frequency    = 18;
cfg.sourcemodel         = grid;
cfg.sourcemodel.filter  = source_all.avg.filter;
cfg.headmodel    = headmodel_meg;
cfg.senstype     ='MEG';

source_left = ft_sourceanalysis(cfg, powcsd_left);
source_right = ft_sourceanalysis(cfg, powcsd_right);

load dipolefitting/mri_realigned2.mat;

mri_resliced = ft_volumereslice([], mri_realigned2);

cfg            = [];
cfg.parameter = 'avg.pow';
source_left_int  = ft_sourceinterpolate(cfg, source_left, mri_resliced);
source_right_int  = ft_sourceinterpolate(cfg, source_right, mri_resliced);

source_diff_int  = source_left_int;
source_diff_int.avg.pow  = (source_left_int.avg.pow - source_right_int.avg.pow) ./ (source_left_int.avg.pow + source_right_int.avg.pow);

% plot
cfg = [];
cfg.method        = 'ortho';
cfg.funparameter  = 'avg.pow';
cfg.funcolorlim   = 'maxabs';
cfg.opacitylim    = [0 1e-4];
cfg.opacitymap    = 'rampup';

ft_sourceplot(cfg, source_left_int);

cfg.location = [35 -13 76];
ft_sourceplot(cfg, source_diff_int);

load dipolefitting/headmodel_eeg.mat
load timefrequency/data_clean_EEG_responselocked.mat

% select time window
cfg = [];
cfg.toilim = [0.35 0.85];
data_timewindow = ft_redefinetrial(cfg,data_clean_EEG_responselocked);

% Freqanalysis for beamformer
cfg = [];
cfg.method       = 'mtmfft';
cfg.taper        = 'dpss';
cfg.output       = 'powandcsd';
cfg.keeptrials   = 'no';
cfg.foi          = 18;
cfg.tapsmofrq    = 4;

% for common filter over conditions and full duration
powcsd_all      = ft_freqanalysis(cfg, data_timewindow);

% for conditions
cfg.trials       = find(data_timewindow.trialinfo(:,1) == 256);
powcsd_left      = ft_freqanalysis(cfg, data_timewindow);
cfg.trials       = find(data_timewindow.trialinfo(:,1) == 4096);
powcsd_right     = ft_freqanalysis(cfg, data_timewindow);

% common grid/filter
cfg                 = [];
cfg.elec            = powcsd_all.elec;
cfg.headmodel       = headmodel_eeg;
cfg.reducerank      = 3; % default is 3 for EEG, 2 for MEG
cfg.sourcemodel.resolution = 0.5;   % use a 3-D grid with a 0.5 cm resolution
cfg.sourcemodel.unit       = 'cm';
cfg.sourcemodel.tight      = 'yes';
[grid] = ft_prepare_leadfield(cfg);

% beamform common filter
cfg              = [];
cfg.method       = 'dics';
cfg.frequency    = 18;
cfg.sourcemodel         = grid;
cfg.headmodel    = headmodel_eeg;
cfg.senstype     = 'EEG'; % Remember this must be specified as either EEG, or MEG
cfg.dics.keepfilter   = 'yes';
cfg.dics.lambda       = '15%';
source_all = ft_sourceanalysis(cfg, powcsd_all);

% beamform conditions
cfg              = [];
cfg.method       = 'dics';
cfg.frequency    = 18;
cfg.sourcemodel         = grid;
cfg.sourcemodel.filter  = source_all.avg.filter; % Use the common filter
cfg.headmodel    = headmodel_eeg;
cfg.senstype     = 'EEG';

source_left = ft_sourceanalysis(cfg, powcsd_left);
source_right = ft_sourceanalysis(cfg, powcsd_right);

cfg            = [];
cfg.parameter = 'avg.pow';
source_left_int  = ft_sourceinterpolate(cfg, source_left, mri_resliced);
source_right_int  = ft_sourceinterpolate(cfg, source_right, mri_resliced);

source_diff_int  = source_left_int;
source_diff_int.avg.pow  = (source_left_int.avg.pow - source_right_int.avg.pow) ./ (source_left_int.avg.pow + source_right_int.avg.pow);

cfg = [];
cfg.method        = 'ortho';
cfg.funparameter  = 'avg.pow';
cfg.funcolorlim   = 'maxabs';

ft_sourceplot(cfg, source_left_int);

cfg.location = [-19.5 -18.5 70.5];
ft_sourceplot(cfg, source_diff_int);


function output=SSBerouti79(signal,fs,IS)

% OUTPUT=SSBEROUTI79(S,FS,IS)
% Nonlinear Spectral Subtraction based on Berouti 79. Power spectral
% subtraction with adjusting subtraction factor. the adjustment is
% according to local a postriori SNR.
% S is the noisy signal, FS is the sampling frequency and IS is the initial
% silence (noise only) length in seconds (default value is .25 sec)
%
% Required functions:
% SEGMENT
% VAD
% Sep-04
% Esfandiar Zavarehei

if (nargin<3 | isstruct(IS))
    IS=.25; %seconds
end
W=fix(.025*fs); %Window length is 25 ms
nfft=W;
SP=.4; %Shift percentage is 40% (10ms) %Overlap-Add method works good with this value(.4)
wnd=hamming(W);

% IGNORE THIS SECTION FOR CAMPATIBALITY WITH ANOTHER PROGRAM FROM HERE.....
if (nargin>=3 & isstruct(IS))%This option is for compatibility with another programme
    W=IS.windowsize
    SP=IS.shiftsize/W;
    nfft=IS.nfft;
    wnd=IS.window;
    if isfield(IS,'IS')
        IS=IS.IS;
    else
        IS=.25;
    end
end
% .......IGNORE THIS SECTION FOR CAMPATIBALITY WITH ANOTHER PROGRAM T0 HERE

NIS=fix((IS*fs-W)/(SP*W) +1);%number of initial silence segments
Gamma=2;%Magnitude Power (1 for magnitude spectral subtraction 2 for power spectrum subtraction)
%Change Gamma to 1 to get a completely different performance
y=segment(signal,W,SP,wnd);
Y=fft(y,nfft);
YPhase=angle(Y(1:fix(end/2)+1,:)); %Noisy Speech Phase
Y=abs(Y(1:fix(end/2)+1,:)).^Gamma;%Specrogram
numberOfFrames=size(Y,2);
FreqResol=size(Y,1);

N=mean(Y(:,1:NIS)')'; %initial Noise Power Spectrum mean

NoiseCounter=0;
NoiseLength=9;%This is a smoothing factor for the noise updating

Beta=.03;
minalpha=1;
maxalpha=3;
minSNR=-5;
maxSNR=20;
alphaSlope=(minalpha-maxalpha)/(maxSNR-minSNR);
alphaShift=maxalpha-alphaSlope*minSNR;

BN=Beta*N;

for i=1:numberOfFrames
    [NoiseFlag, SpeechFlag, NoiseCounter, Dist]=vad(Y(:,i).^(1/Gamma),N.^(1/Gamma),NoiseCounter); %Magnitude Spectrum Distance VAD
    if SpeechFlag==0
        N=(NoiseLength*N+Y(:,i))/(NoiseLength+1); %Update and smooth noise
        BN=Beta*N;
    end

    SNR=10*log(Y(:,i)./N);
    alpha=alphaSlope*SNR+alphaShift;
    alpha=max(min(alpha,maxalpha),minalpha);

    D=Y(:,i)-alpha.*N; %Nonlinear (Non-uniform) Power Specrum Subtraction

    X(:,i)=max(D,BN); %if BY>D X=BY else X=D which sets very small values of subtraction result to an attenuated
                      %version of the input power spectrum.
end

output=OverlapAdd2(X.^(1/Gamma),YPhase,W,SP*W);

function ReconstructedSignal=OverlapAdd2(XNEW,yphase,windowLen,ShiftLen);

%Y=OverlapAdd(X,A,W,S);
%Y is the signal reconstructed signal from its spectrogram. X is a matrix
%with each column being the fft of a segment of signal. A is the phase
%angle of the spectrum which should have the same dimension as X. if it is
%not given the phase angle of X is used which in the case of real values is
%zero (assuming that its the magnitude). W is the window length of time
%domain segments if not given the length is assumed to be twice as long as
%fft window length. S is the shift length of the segmentation process ( for
%example in the case of non overlapping signals it is equal to W and in the
%case of %50 overlap is equal to W/2. if not givven W/2 is used. Y is the
%reconstructed time domain signal.
%Sep-04
%Esfandiar Zavarehei

if nargin<2
    yphase=angle(XNEW);
end
if nargin<3
    windowLen=size(XNEW,1)*2;
end
if nargin<4
    ShiftLen=windowLen/2;
end
if fix(ShiftLen)~=ShiftLen
    ShiftLen=fix(ShiftLen);
    disp('The shift length have to be an integer as it is the number of samples.')
    disp(['shift length is fixed to ' num2str(ShiftLen)])
end

[FreqRes FrameNum]=size(XNEW);

Spec=XNEW.*exp(j*yphase);

if mod(windowLen,2) %if FreqResol is odd
    Spec=[Spec;flipud(conj(Spec(2:end,:)))];
else
    Spec=[Spec;flipud(conj(Spec(2:end-1,:)))];
end
sig=zeros((FrameNum-1)*ShiftLen+windowLen,1);
weight=sig;
for i=1:FrameNum
    start=(i-1)*ShiftLen+1;
    spec=Spec(:,i);
    sig(start:start+windowLen-1)=sig(start:start+windowLen-1)+real(ifft(spec,windowLen));
end
ReconstructedSignal=sig;

function [NoiseFlag, SpeechFlag, NoiseCounter, Dist]=vad(signal,noise,NoiseCounter,NoiseMargin,Hangover)

%[NOISEFLAG, SPEECHFLAG, NOISECOUNTER, DIST]=vad(SIGNAL,NOISE,NOISECOUNTER,NOISEMARGIN,HANGOVER)
%Spectral Distance Voice Activity Detector
%SIGNAL is the the current frames magnitude spectrum which is to labeld as
%noise or speech, NOISE is noise magnitude spectrum template (estimation),
%NOISECOUNTER is the number of imediate previous noise frames, NOISEMARGIN
%(default 3)is the spectral distance threshold. HANGOVER ( default 8 )is
%the number of noise segments after which the SPEECHFLAG is reset (goes to
%zero). NOISEFLAG is set to one if the the segment is labeld as noise
%NOISECOUNTER returns the number of previous noise segments, this value is
%reset (to zero) whenever a speech segment is detected. DIST is the
%spectral distance.
%Saeed Vaseghi
%edited by Esfandiar Zavarehei
%Sep-04

if nargin<4
    NoiseMargin=3;
end
if nargin<5
    Hangover=8;
end
if nargin<3
    NoiseCounter=0;
end

FreqResol=length(signal);

SpectralDist= 20*(log10(signal)-log10(noise));
SpectralDist(find(SpectralDist<0))=0;

Dist=mean(SpectralDist);
if (Dist < NoiseMargin)
    NoiseFlag=1;
    NoiseCounter=NoiseCounter+1;
else
    NoiseFlag=0;
    NoiseCounter=0;
end

% Detect noise only periods and attenuate the signal
if (NoiseCounter > Hangover)
    SpeechFlag=0;
else
    SpeechFlag=1;
end

function Seg=segment(signal,W,SP,Window)

% SEGMENT chops a signal to overlapping windowed segments
% A= SEGMENT(X,W,SP,WIN) returns a matrix which its columns are segmented
% and windowed frames of the input one dimentional signal, X. W is the
% number of samples per window, default value W=256. SP is the shift
% percentage, default value SP=0.4. WIN is the window that is multiplied by
% each segment and its length should be W. the default window is hamming
% window.
% 06-Sep-04
% Esfandiar Zavarehei

if nargin<3
    SP=.4;
end
if nargin<2
    W=256;
end
if nargin<4
    Window=hamming(W);
end
Window=Window(:); %make it a column vector

L=length(signal);
SP=fix(W.*SP);
N=fix((L-W)/SP +1); %number of segments

Index=(repmat(1:W,N,1)+repmat((0:(N-1))'*SP,1,W))';
hw=repmat(Window,1,N);
Seg=signal(Index).*hw;


function a = le( x, y )

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

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

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


function err = get_cross_entropy(model, data, l)
% cross-entropy(reconstruction error, I use L2) of each layer. This is used
% to monitor the pretraining progress.
% data: can be real data(for rbm) or data filelist(crbm).

global kConv_backward  kConv_backward_c kConv_forward2 kConv_forward_c;

fraction = 5;
if l == 2
    n = length(data);
    batch_size = 32; batch_num = n / batch_size;
    assert(batch_num == floor(batch_num));
    stride = model.layers{l}.stride;

    batch_num = floor(batch_num / fraction); % evaluate 1/fraction% of data for speed
    shuffle_index = randperm(n);
    err = 0;
    for b = 1 : batch_num
        batch_index = shuffle_index((b-1)*batch_size + 1 : b * batch_size);
        batch = read_batch(model, data(batch_index), false);

        hidden_presigmoid = myConvolve2(kConv_forward2, batch, model.layers{l}.w, stride, 'forward');
        hidden_presigmoid = bsxfun(@plus, hidden_presigmoid, permute(model.layers{l}.c, [2,3,4,5,1]));
        hidden_prob = sigmoid(hidden_presigmoid);
        hidden_sample = single(hidden_prob > rand(size(hidden_prob)));

        visible_presigmoid = myConvolve(kConv_backward, hidden_sample, model.layers{l}.w, stride, 'backward');
        visible_presigmoid = bsxfun(@plus, visible_presigmoid, permute(model.layers{l}.b, [5,1,2,3,4]));
        visible_prob = sigmoid(visible_presigmoid);

        this_err = batch - visible_prob;
        err = err + sum(this_err(:).^2);
    end
    err = err / (batch_num * batch_size);
elseif strcmp(model.layers{l}.type, 'convolution')
    n = length(data);
    batch_size = 32; batch_num = n / batch_size;
    assert(batch_num == floor(batch_num));
    stride = model.layers{l}.stride;

    batch_num = floor(batch_num / fraction); % evaluate 1/fraction% of data for speed
    shuffle_index = randperm(n);
    err = 0;
    for b = 1 : batch_num
        batch_index = shuffle_index((b-1)*batch_size + 1 : b * batch_size);
        batch = read_batch(model, data(batch_index), false);
        batch = propagate_batch(model, batch, l);

        hidden_presigmoid = myConvolve(kConv_forward_c, batch, model.layers{l}.w, stride, 'forward');
        hidden_presigmoid = bsxfun(@plus, hidden_presigmoid, permute(model.layers{l}.c, [2,3,4,5,1]));
        hidden_prob = sigmoid(hidden_presigmoid);
        hidden_sample = single(hidden_prob > rand(size(hidden_prob)));

        visible_presigmoid = myConvolve(kConv_backward_c, hidden_sample, model.layers{l}.w, stride, 'backward');
        visible_presigmoid = bsxfun(@plus, visible_presigmoid, permute(model.layers{l}.b, [5,1,2,3,4]));
        visible_prob = sigmoid(visible_presigmoid);

        this_err = batch - visible_prob;
        err = err + sum(this_err(:).^2);
    end
    err = err / (batch_num * batch_size);
elseif model.classes > 0 && l == length(model.layers) % for the last layer
    n = size(data,1);
    batch_size = 32; batch_num = n / batch_size;
    err = 0;
    temp_w = model.layers{l}.w;
    temp_w(1:model.classes,:) = temp_w(1:model.classes,:) * model.duplicate;
    for b = 1 : batch_num
        batch = data((b-1)*batch_size + 1 : b * batch_size,:,:,:,:);
        hidden_presigmoid = bsxfun(@plus, ...
			batch * temp_w, model.layers{l}.c);
		hidden_prob = 1 ./ ( 1 + exp(- hidden_presigmoid) );
		hidden_sample = single(hidden_prob > rand(size(hidden_prob)));

        visible_presigmoid = bsxfun(@plus, ...
					hidden_sample * model.layers{l}.w', model.layers{l}.b);
        visible_prob_post = sigmoid(visible_presigmoid(:,model.classes+1:end));
        temp_exponential = exp(bsxfun(@minus,visible_presigmoid(:,1:model.classes),max(visible_presigmoid(:,1:model.classes),[],2)));
        visible_prob_pre = bsxfun(@rdivide, temp_exponential, sum(temp_exponential,2));

        data_err = batch(:,model.classes+1:end) - visible_prob_post;
        label_err = batch(:, 1:model.classes) - visible_prob_pre;
        err = err + model.duplicate * sum(label_err(:).^2) + sum(data_err(:).^2);
    end
    err = err / n;
else % for original rbm
    n = size(data,1);
    batch_size = 32; batch_num = n / batch_size;
    err = 0;
    for b = 1 : batch_num
        batch = data((b-1)*batch_size + 1 : b * batch_size,:,:,:,:);
        hidden_presigmoid = bsxfun(@plus, ...
			batch * model.layers{l}.w, model.layers{l}.c);
		hidden_prob = 1 ./ ( 1 + exp(- hidden_presigmoid) );
		hidden_sample = single(hidden_prob > rand(size(hidden_prob)));

        visible_presigmoid = bsxfun(@plus, ...
					hidden_sample * model.layers{l}.w', model.layers{l}.b);
        visible_prob = 1 ./ ( 1 + exp(-visible_presigmoid) );

        this_err = batch - visible_prob;
        err = err + sum(this_err(:).^2);
    end
    err = err / n;
end

function y = sigmoid(x)
    y = 1 ./ ( 1 + exp(-x) );


function K = sdlfmaXsdlfmaKernComputeBlock(lfmKern1, lfmKern2, t1, t2, ...
    kyy, kyv, kvy, kvv, i, j, generalConst)

% SDLFMAXSDLFMAKERNCOMPUTEBLOCK Computes SDLFM kernel matrix for block i,j
% FORMAT
% DESC computes the kernel matrix for the SDLFM kernel function in the
% block specified at indeces i,j. It assumes the computation for functions
% that describe accelerations (acceleration 1 and acceleration 2).
% ARG lfmKern1 : structure containing parameters for the system 1
% ARG lfmKern2 : structure containing parameters for the system 2
% ARG t1 : times at which the system 1 is evaluated
% ARG t2 : times at which the system 2 is evaluated
% ARG kyy : covariance for the initial conditions between position 1 and
% position 2 at block i,j
% ARG kyv : covariance for the initial conditions between position 1 and
% velocity 2 at block i,j
% ARG kvy : covariance for the initial conditions between velocity 1 and
% position 2 at block i,j
% ARG kvv : covariance for the initial conditions between velocity 1 and
% velocity 2 at block i,j
% ARG i : interval to be evaluated for system 1
% ARG j : interval to be evaluated for system 2
% ARG generalConstant : constants evaluated with sdlfmKernComputeConstant.m
% RETURN K : the kernel matrix portion of block i,j
%
% COPYRIGHT : Mauricio A. Alvarez, 2010.

% KERN

if nargin<11
    j = i;
    generalConst = [];
end

a1 = sdlfmaMeanCompute(lfmKern1(1), t1, 'Pos');
b1 = sdlfmaMeanCompute(lfmKern1(1), t1, 'Vel');
a2 = sdlfmaMeanCompute(lfmKern2(1), t2, 'Pos');
b2 = sdlfmaMeanCompute(lfmKern2(1), t2, 'Vel');

K = kyy*a1*a2.' + kyv*a1*b2.' + kvy*b1*a2.' + kvv*b1*b2.';

if i==j
    for k=1:length(lfmKern1)
        K  = K + lfmaXlfmaKernCompute(lfmKern1(k), lfmKern2(k), t1, t2);
    end
else
    if i>j
        AccelPos = zeros(1, length(t2));
        AccelVel = zeros(1, length(t2));
        for k=1:length(lfmKern1)
            AccelPos = AccelPos + lfmaXlfmKernCompute(lfmKern2(k), lfmKern1(k), t2, lfmKern2(k).limit).';
            AccelVel = AccelVel + lfmaXlfmvKernCompute(lfmKern2(k), lfmKern1(k), t2, lfmKern2(k).limit).';
        end
        if isempty(generalConst{i,j})
            K = K + a1*AccelPos + b1*AccelVel;
        else
            K = K + (generalConst{i,j}(1,1)*a1 + generalConst{i,j}(2,1)*b1)*AccelPos + ...
                (generalConst{i,j}(1,2)*a1 + generalConst{i,j}(2,2)*b1)*AccelVel;
        end
    else
        AccelPos = zeros(length(t1),1);
        AccelVel = zeros(length(t1),1);
        for k =1:length(lfmKern1)
            AccelPos = AccelPos + lfmaXlfmKernCompute(lfmKern1(k), lfmKern2(k), t1, lfmKern1(k).limit);
            AccelVel = AccelVel + lfmaXlfmvKernCompute(lfmKern1(k), lfmKern2(k), t1, lfmKern1(k).limit);
        end
        if isempty(generalConst{i,j})
            K = K + AccelPos*a2.' + AccelVel*b2.';
        else
            K = K + AccelPos*(generalConst{i,j}(1,1)*a2.' + generalConst{i,j}(2,1)*b2.') + ...
                AccelVel*(generalConst{i,j}(1,2)*a2.' + generalConst{i,j}(2,2)*b2.');
        end
    end
end


function C = train_RSLDAshrink(xTr, yTr, sublab, varargin)
% TRAIN_RSLDASHRINK - Relevance Subclass LDA with mean shrinkage to subclasses and automatic covariance shrinkage
%
%Synopsis:
%   C = train_RLSDAshrink(XTR, YTR, SUBLABELS)
%   C = train_RLSDAshrink(XTR, YTR, SUBLABELS, OPTS)
%
%Arguments:
%   XTR: DOUBLE [NxM] - Data matrix, with N feature dimensions, and M training points/examples.
%   YTR: INT [CxM] - Class membership labels of points in X_TR. C by M matrix of training
%                     labels, with C representing the number of classes and M the number of training examples/points.
%                     Y_TR(i,j)==1 if the point j belongs to class i.
%   SUBLABELS[Mx1] - Subclass membership for each data point
%   OPT: PROPLIST - Structure or property/value list of optional
%                   properties.
%     'Whitening' - BOOL (default 1): If true, mean shrinkage is performed
%       on whitened data. This is important for ERP data, as it has a
%       highly skewed eigenspectrum and the mean shrinkage is dominated by
%       directions of high variance.
%     'ReturnRegularizationProfile' - BOOL (default 1): If true, returns regularization
%       profile as matrix MxM for both classes, with M being the number of
%       unique subclasses.
%
%Returns:
%   C: STRUCT - Trained classifier structure, with the subclassifier
%     hyperplanes given by fields C.subC{k}.w and C.subC{k}.b for each
%     subclass k
%
% C includes the fields:
%    'sublab_unique' :      unique subclass labels
%    'pattern' :            pattern (i.e. mu2-mu1) of each subclassifier
%    'subC' :               LDA classifiers (w and b) for each subclass
%    'regularization_list': list of regularization weights for each class
%       and subclass, stacked for both classes
%    'regularization_profile' : (optional) regularization profile as matrix
%
%Description:
%   TRAIN_RLSDA trains a Relevance Subclass LDA classifier on data X with
%   class labels given in LABELS and Subclass labels given in SUBLABELS.
%   The mean shrinkage parameter is selected by sotm and the covariance
%   shrinkage parameter is selected by the function clsutil_shrinkage.
%
%
%Example 1:
%%perform standard ERP analysis (loads data, subsampling and divide into epochs)
% demo_analysis_ERPs
%
%
%%only the last digit codes for the stimulus identity
% subclass_lab = mod(epo.event.desc, 10);
%
%%extract features from the epochs by subsampling
% ivals = [140:30:260 300:50:600; 170:30:300 350:50:650]';
%%create features
% fv = proc_flaten(proc_jumpingMeans(epo, ivals));
%%train RSLDA
% C = train_RSLDAshrink(fv.x, fv.y, subclass_lab);
%
%%Example 2: compare RSLDAshrink with LDAshrink
% nfolds = 4;
% [divTr, divTe] = sample_KFold(epo.y, nfolds);
% loss_list_RSLDA = nan(1, nfolds);loss_list_RLDA = loss_list_RSLDA;
% for kk = 1:nfolds
%     %generate training and test features
%     fvTr = proc_flaten(proc_jumpingMeans(proc_selectEpochs(epo, divTr{1}{kk}), ivals));
%     fvTe = proc_flaten(proc_jumpingMeans(proc_selectEpochs(epo, divTe{1}{kk}), ivals));
%     %train cls on traing data
%     C_RSLDA = train_RSLDAshrink(fvTr.x, fvTr.y, subclass_lab(divTr{1}{kk}));
%     C_LDA = train_RLDAshrink(fvTr.x, fvTr.y);
%     %apply RSLDA on the test data
%     out_RSLDA = apply_RSLDAshrink(C_RSLDA, fvTe.x, subclass_lab(divTe{1}{kk}));
%     out_RLDA = apply_separatingHyperplane(C_LDA , fvTe.x);
%
%     loss_list_RSLDA(kk) = loss_rocArea(fvTe.y,out_RSLDA);
%     loss_list_RLDA(kk) = loss_rocArea(fvTe.y,out_RLDA);
% end
%%compare loss for each fold
% loss_list_RSLDA - loss_list_RLDA
%%plot regularization profile for the last fold
% figure, imagesc(C_RSLDA.regularization_profile{2})
%
%See also:
%   APPLY_SEPARATINGHYPERPLANE, CLSUTIL_MEANMTS, CLSUTIL_SHRINKAGE, TRAIN_RLDAshrink
%   TRAIN_LDA

% 02-2015 Johannes Hoehne
if size(yTr,1)==1, yTr= [yTr<0; yTr>0]; end
yTr = logical(yTr);
nClasses= size(yTr,1);
props= {'Whitening'        1                             'BOOL'
        'ReturnRegularizationProfile'  1                 'BOOL'
	    'Prior'            ones(nClasses, 1)/nClasses    'DOUBLE[- 1]'
       };

% get props list of the subfunction
props_shrinkage= clsutil_meanMTS;

if nargin==0,
  C= opt_catProps(props, props_shrinkage);
  return
end

opt= opt_proplistToStruct(varargin{:});
[opt, isdefault]= opt_setDefaults(opt, props);
opt_checkProplist(opt, props, props_shrinkage);

% preprocessing of input
yTr = logical(yTr);
if size(sublab,2)==1 && size(sublab,1)>1
    sublab = sublab'; %sublab is expected to be a row-vector of
end

% empirical class priors as an option, if nan (I leave 1/nClasses as default)
if isnan(opt.Prior)
  opt.Prior = sum(yTr, 2)/sum(sum(yTr));
end

C = {};

%start func
C.sublab_unique = unique(sublab);

% get meanfree X (without considereing subclass labels)
% compute Cov matrix
if opt.Whitening
    M1 = mean(xTr(:, yTr(1,:)),2);
    M2 = mean(xTr(:,yTr(2,:)),2);
    M = repmat(M1, 1, size(yTr,2));
    M(:, yTr(2,:)) = repmat(M2, 1, sum(yTr(2,:)));
    Xmeanfree_dum = xTr - M;
    globalCov = clsutil_shrinkage(Xmeanfree_dum);
    [ Eigvec, Eigval  ] = eig(globalCov);
    A_feat2white =  Eigvec * (Eigval^-.5) * Eigvec;
    A_white2feat = inv(A_feat2white);
else %do not perform whitening
%     --> this leads to a poor performance for data with a skewed eigen spectrum
    A_feat2white = eye(size(xTr,2));
    A_white2feat = eye(size(xTr,2));
end
Xwhite = A_feat2white' * xTr;

kk = 0;
M = nan(size(xTr)); %saves the classwise-shrinked means
for my_sublab = C.sublab_unique
    kk = kk+1;

    for ix_c = 1:nClasses %for each class
        %% estimate class means
        shrink_dat = {}; %initialize shrindat container
        shrink_dat.X = {Xwhite(:,((sublab == my_sublab) & yTr(ix_c,:)))' };

        %there might be sublabels without epoch in calss ix_c -> skip them as sublabel
        sublab_missing = [];
        for sublab_this = setdiff(C.sublab_unique, my_sublab)
            if sum((((sublab == sublab_this)) & yTr(ix_c,:)))>0
                shrink_dat.X{end+1} = Xwhite(:,(((sublab == sublab_this)) & yTr(ix_c,:)))';
            else
                sublab_missing(end+1) = sublab_this; %(only important for special cases)
            end
        end

        %PERFORM MULTI-TARGET SHINKAGE FOR CLASS 1
        % sublab=k & T   sublab~=k & T
        [Mest(ix_c,:), gamma_tmp] = clsutil_meanMTS(shrink_dat, 'convex', 1, 'variablewise', 0, 'conservative', 1);

        % (only important for special cases) deal with the arrangement of missing subclass labels
        for xx = sublab_missing %put missing subclasses as zeros in gamma to maintain the correct size
            if xx>=length(gamma_tmp)+1, gamma_tmp = [gamma_tmp; 0]; %the last one was missing
            else  gamma_tmp = [gamma_tmp(1:xx-1); 0; gamma_tmp(xx:end)]; %another one was missing
            end
        end
        gamma(ix_c,:) = gamma_tmp;

        %transform the Means back into feature space
        Mest_origSpace(ix_c,:) = A_white2feat' * Mest(ix_c,:)';

        % for each datapoint, save the dcorresponding subclass mean
        % --> build up Mean-template M with Target and NonTarget template
        M(:, find(yTr(ix_c,:) & sublab == my_sublab)) = repmat(Mest_origSpace(ix_c,:)', 1, length(find(yTr(ix_c,:) & sublab == my_sublab)));

        %save the mean estimators as we need them later to compute the cls
        list_M{kk}(ix_c,:) = Mest_origSpace(ix_c,:);
    end
    C.regularization_list(:,kk) =  gamma(:);
end

%% estimate Cov with optimized means
Xmeanfree = xTr - M;
Cest = clsutil_shrinkage(Xmeanfree);
C_invcov = pinv(Cest);

%% finalize cls
kk = 0;
for my_sublab = C.sublab_unique
    kk = kk+1;
    if nClasses>2
        C.subC{kk} =   comp_reg_cls(C_invcov, list_M{kk}', opt.Prior) ;
    else
        % get the Target and NonTartget Means for each subclass
        M1est = list_M{kk}(1,:)';
        M2est = list_M{kk}(2,:)';
        C.pattern(:,kk) = M2est - M1est;
        C.subC{kk} = comp_reg_cls2(C_invcov, M1est, M2est);
    end
    if ~isfinite(C.subC{kk}.b)
        error('sth went wrong')
    end
end

if opt.ReturnRegularizationProfile
    %reorganize the regularization_list such that we can interpret them
%                the regul parameters are reordered such that (dat)
%                    l1  l2
%                    l3  l4
%                    l5  l6
%                becomes a square Matrix A, which is better to interpret:
%                    (1-l1-l2)  l        l
%                      l3   (1-l3-l4)    l4
%                      l5       l6    (1-l5-l6)
    for i_class = 1:nClasses %for both classes
        nPara = size(C.regularization_list,1)/nClasses;
        dat = C.regularization_list((i_class-1)*nPara+1 : (i_class)*nPara,:)';
        A = zeros(size(dat,1));
        A(logical(eye(size(A)))) = 1-sum(dat,2);
        d = dat';
        A(~logical(eye(size(A)))) = d(:);
        C.regularization_profile{i_class} = A';
    end
end

return

end

function C = comp_reg_cls2(C_invcov, M1, M2)
C = [];
C.w= C_invcov*(M2 - M1);
C.b= -0.5*C.w'*(M2 + M1);
end

function C = comp_reg_cls(C_invcov, C_mean, Prior)
C = [];
C.w= C_invcov*C_mean;
C.b= -0.5*sum(C_mean.*C.w,1)' + log(Prior);
end


function [confusion_matrix, classes]=cosmo_confusion_matrix(ds, varargin)
% Returns a confusion matrix
%
% Usage 1: mx=cosmo_confusion_matrix(ds)
% Usage 2: mx=cosmo_confusion_matrix(targets, predicted)
%
%
% Inputs:
%   targets     Nx1 targets for N samples, or a dataset struct with
%               .sa.targets
%   predicted   NxM predicted labels (from a classifier), for N samples and
%               M predictions per set of samples
%
% Returns:
%   mx          PxPxM matrix assuming there are P unique targets.
%               mx(i,j,k)==c means that the i-th target class was classified
%               as the j-th target class c times for the k-th set of
%               samples.
%   classes     Px1 class labels.
%
% Example:
%     ds=cosmo_synthetic_dataset('ntargets',3,'nchunks',4);
%     args=struct();
%     args.partitions=cosmo_nchoosek_partitioner(ds,1);
%     args.output='winner_predictions';
%     args.classifier=@cosmo_classify_lda;
%     pred_ds=cosmo_crossvalidation_measure(ds,args);
%     confusion=cosmo_confusion_matrix(pred_ds.sa.targets,pred_ds.samples);
%     cosmo_disp(confusion)
%     %|| [ 3         0         1
%     %||   0         3         1
%     %||   1         0         3 ]
%     confusion_alt=cosmo_confusion_matrix(pred_ds);
%     isequal(confusion,confusion_alt)
%     %|| true
%     %
%     % run a searchlight with tiny radius of 1 voxel (3 is more common)
%     nbrhood=cosmo_spherical_neighborhood(ds,'radius',1,'progress',false);
%     measure=@cosmo_crossvalidation_measure;
%     sl_ds=cosmo_searchlight(ds,nbrhood,measure,args,'progress',false);
%     %
%     % the confusion matrix is 3x3x6, that is 6 3x3 confusion
%     % matrices. Here the dataset is passed directly
%     sl_confusion=cosmo_confusion_matrix(sl_ds);
%     cosmo_disp(sl_confusion)
%     %|| <double>@3x3x6
%     %||    (:,:,1) = [ 4         0         0
%     %||                0         4         0
%     %||                0         1         3 ]
%     %||    (:,:,2) = [ 4         0         0
%     %||                0         4         0
%     %||                0         1         3 ]
%     %||    (:,:,3) = [ 2         1         1
%     %||                0         4         0
%     %||                1         0         3 ]
%     %||    (:,:,4) = [ 4         0         0
%     %||                0         3         1
%     %||                0         1         3 ]
%     %||    (:,:,5) = [ 3         0         1
%     %||                0         4         0
%     %||                1         1         2 ]
%     %||    (:,:,6) = [ 3         0         1
%     %||                0         4         0
%     %||                1         1         2 ]
%
%     % using samples that are not predictions gives an error
%     ds=cosmo_synthetic_dataset('ntargets',3,'nchunks',4);
%     confusion=cosmo_confusion_matrix(ds)
%     %|| error('72 predictions mismatch targets, first is (1,1)=2.211999e+00')
%
% Notes:
%   - this function counts the number of times each sample was classified
%     as any target
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

    [targets,predicted]=get_data(ds,varargin{:});

    % see which classes there are
    [class_indices,classes]=cosmo_index_unique(targets);
    nclasses=numel(class_indices);

    % allocate space for output
    nfeatures=size(predicted,2);
    confusion_matrix=zeros([nclasses,nclasses,nfeatures]);

    % keep track which predicted samples were in targets
    visited=false(size(predicted));
    % >@@>
    for k=1:nclasses
        % rows for k-th class
        idxs=class_indices{k};
        for j=1:nclasses
            match_msk=bsxfun(@eq,classes(j),predicted(idxs,:));
            confusion_matrix(k,j,:)=sum(match_msk,1);
            visited(idxs,:)=visited(idxs,:) | match_msk;
        end
    end

    % <@@<

    missing=~(visited | isnan(predicted));
    if any(missing(:))
        n=sum(missing(:));
        [i,j]=find(missing,1);
        error(['%d predictions mismatch targets, '...
                'first is (%d,%d)=%d'],n,i,j,predicted(i,j));
    end

function [targets,predicted]=get_data(ds, predicted)
    has_predicted=nargin>=2;
    is_ds=isstruct(ds);
    if is_ds
        if has_predicted
            error('Need exactly one argument when input is struct');
        end
        % input is a dataset
        cosmo_isfield(ds,'sa.targets',true);
        cosmo_isfield(ds,'samples',true);

        predicted=ds.samples;
        targets=ds.sa.targets;
    elseif isnumeric(ds)
        if ~has_predicted
            error('Need two arguments when first argument is numeric');
        end
        targets=ds;
    else
        error('Illegal input: need struct or numeric vector');
    end

    if ~isvector(targets) || size(targets,2)~=1
        error('targets must be column vector');
    end

    if numel(size(predicted))~=2
        error('predictions must be matrix');
    end

    nsamples=numel(targets);
    if size(predicted,1)~=nsamples
        error(['Size mismatch: predictions has %d values on first '...
                'dimension, but targets has %d values'],...
                    size(predicted,1),nsamples);
    end


function p=drex_invlogit(a)
%DREX subfunction
%Written by Issam El Naqa 2003-2005
%Extracted for generalized use 2005, AJH
%
% Copyright 2010, Joseph O. Deasy, on behalf of the DREES development team.
%
% This file is part of the Dose Response Explorer System (DREES).
%
% DREES development has been led by:  Issam El Naqa, Aditya Apte, Gita Suneja, and Joseph O. Deasy.
%
% DREES has been financially supported by the US National Institutes of Health under multiple grants.
%
% DREES is distributed under the terms of the Lesser GNU Public License.
%
%     This version of DREES 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.
%
% DREES 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 DREES.  If not, see <http://www.gnu.org/licenses/>.

p = 1 ./ (1 + exp(-a));

return


function x = proxTVSquare(x,lambda,alpha,varargin)

[n,m] = size(x);
lambda = alpha * lambda; % only the product matters

% apply proxTV horizontally
mMax = 2*floor((m+1)/2)-1; % round down to an odd number
[x(:,2:2:mMax),x(:,3:2:mMax)] = proxTV(x(:,2:2:mMax),x(:,3:2:mMax),lambda,varargin{:});

mMax = 2*floor(m/2); % round down to an even number
[x(:,1:2:mMax),x(:,2:2:mMax)] = proxTV(x(:,1:2:mMax),x(:,2:2:mMax),lambda,varargin{:});

% apply proxTV vertically
nMax = 2*floor((n+1)/2)-1; % round down to an odd number
[x(2:2:nMax,:),x(3:2:nMax,:)] = proxTV(x(2:2:nMax,:),x(3:2:nMax,:),lambda,varargin{:});

nMax = 2*floor(n/2); % round down to an even number
[x(1:2:nMax,:),x(2:2:nMax,:)] = proxTV(x(1:2:nMax,:),x(2:2:nMax,:),lambda,varargin{:});


function box = mergeBoxes3d(box1, box2)
%MERGEBOXES3D Merge 3D boxes, by computing their greatest extent.
%
%   BOX = mergeBoxes3d(BOX1, BOX2);
%
%   Example
%   box1 = [5 20 5 30 10 50];
%   box2 = [0 15 0 15 0 20];
%   mergeBoxes3d(box1, box2)
%   ans =
%       0 20 0 30 0 50
%
%
%   See also
%   boxes3d, drawBox3d, intersectBoxes3d
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-07-26,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% unify sizes of data
if size(box1,1) == 1
    box1 = repmat(box1, size(box2,1), 1);
elseif size(box2, 1) == 1
    box2 = repmat(box2, size(box1,1), 1);
elseif size(box1,1) ~= size(box2,1)
    error('Bad size for inputs');
end

% compute extreme coords
mini = min(box1(:,1:2:end), box2(:,1:2:end));
maxi = max(box1(:,2:2:end), box2(:,2:2:end));

% concatenate result into a new box structure
box = [mini(:,1) maxi(:,1) mini(:,2) maxi(:,2) mini(:,3) maxi(:,3)];