%FEATSELO Branch and bound feature selection
%
%   [W,R] = FEATSELO(A,CRIT,K,T)
%   [W,R] = A*FEATSELO([],CRIT,K,T)
%   [W,R] = A*FEATSELO(CRIT,K,T)
%   [W,R] = FEATSELO(A,CRIT,K,N)
%   [W,R] = A*FEATSELO([],CRIT,K,N)
%   [W,R] = A*FEATSELO(CRIT,K,N)
%
% INPUT
%   A     Input dataset
%   CRIT  String name of the criterion or untrained mapping
%           (optional, def= 'maha-s')
%   K     Numner of features to select (optional, def: K=2)
%   T     Validation set (optional)
%   N     Number of cross-validations (optional)
%
% OUTPUT
%   W     Output feature selection mapping
%   R     Matrix with step-by-step results
%
% DESCRIPTION
% Backward selection of K features by baktracking using the branch
% and bound procedure on the data set A. CRIT sets the criterion
% used by the feature evaluation routine FEATEVAL. If the data set T
% is given, it is used as test set for FEATEVAL. Alternatively a number
% of cross-validations N may be supplied. The resulting W can be used for
% the selecting features of a dataset B by B*W.
% The selected features are stored in W.DATA and can be found by +W.
%
% This procedure finds the optimum feature set if a monotoneous
% criterion is used. The use of a testset does not guarantee that.
%
% REFERENCE
% P. M. Narendra and K. Fukunaga
% A Branch and Bound Algorithm for Feature Subset Selection,
% IEEE Trans. Computer, 26(9), pp. 917-922, September 1977
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, FEATEVAL, FEATSELF, FEATSELB, FEATSELI,
% FEATSEL, FEATSELP, FEATSELM

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: featselo.m,v 1.7 2009/07/01 09:33:23 duin Exp $

function [W,R] = featselo(varargin)

  varargin = shiftargin(varargin,{'char','prmapping'});
  argin = setdefaults(varargin,[],'maha-s',2,[],[]);
  if mapping_task(argin,'definition')
    W = define_mapping(argin,'untrained','B&B FeatSel');
    return
  end

  [A,crit,kmin,T,fid] = deal(argin{:});

	isvaldfile(A,1,2); % at least 1 object per class, 2 classes
	A = testdatasize(A);
	if isdataset(T), iscomdset(A,T); end

	[m,k,c] = getsize(A);
	featlist = getfeatlab(A);
  A = setprior(A,getprior(A));

	if ((kmin < 1) | (kmin >= k))
		error('The desired feature size should be > 0 and < dataset feature size')
	end

	% space for criteria values
	feat = zeros(1,k);

   % Get performance of the individual features:
	if isempty(T)
		for j=1:k
			feat(j) = feateval(A(:,j),crit);
		end
	elseif is_scalar(T)
		for j=1:k
			feat(j) = feateval(A(:,j),crit,T);
		end
	else
		for j=1:k
			feat(j) = feateval(A(:,j),crit,T(:,j));
		end
	end

   % Get the kmin worst(?) individual features according to their
   % individual performance:
	[F,S] = sort(feat);

	%sometimes the above line is bad compared to the following two
	%w = featselb(A,crit,[]);
	%S = fliplr(+w);

	Iopt = [k-kmin+1:k];

	I = [1:k];
	J = [zeros(1,kmin),1:(k-kmin-1),k-kmin+1,k+1];
	level = k;

   % Get the performance of Iopt
	if isdataset(T)
		bound = feateval(A(:,S(Iopt)),crit,T(:,S(Iopt)));
	elseif is_scalar(T)
		bound = feateval(A(:,S(Iopt)),crit,T);
	else
		bound = feateval(A(:,S(Iopt)),crit);
	end

	C = inf;
	prwaitbar(100,'Branch & Bound Feature Selection')
	iter = 0;
	while numel(I) > 0 && J(k+1) == k+1;
		iter = iter+1;
		prwaitbar(100,100-100*exp(-iter/25),['Branch & Bound Feature Selection: ' num2str(iter)]);
		if J(level) == J(level+1) | level <= kmin | C <= bound
			J(level) = level - kmin;
			level = level + 1;
			I = sort([I,J(level)]);
			J(level) = J(level) + 1;
			C = inf;
		else
			I(J(level)) = [];
			level = level - 1;
			if J(level+1) < 3 & level == kmin+1 & 0 % never happens ??
				;
			else
				if isdataset(T)
					C = feateval(A(:,S(I)),crit,T(:,S(I)));
				elseif is_scalar(T)
					C = feateval(A(:,S(I)),crit,T);
				else
					C = feateval(A(:,S(I)),crit);
        end
				if level == kmin & C > bound
					bound = C;
					Iopt = I;
          disp([bound,iter,numel(I)])
        end
			end
		end
  end
	prwaitbar(0);

   % Store the optimal features in the mapping:
	W = featsel(k,S(Iopt));
  W = setmapping_type(W,'trained');
  W = setsize(W,[k length(S(Iopt))]);
	if ~isempty(featlist)
		W = setlabels(W,featlist(S(Iopt),:));
	end
	W = setname(W,'B&B FeatSel');

	R = [];  %DXD I'm still not sure what to return

return


function [r, R_r, R_v, R_a] = rpredict(r, v, a, dt)

% RPREDICT Position prediction.
%   RPREDICT(R,V,DT) performs the time update R = R + V*DT.
%
%   RPREDICT(R,V,A,DT) considers R = R + V*DT + 1/2*A*DT instead.
%
%   [R,R_r,R_v,R_a] = ... returns Jacobian matrices wrt position R,
%   velocity V and acceleration A.
%
%   See also VPREDICT, QPREDICT.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

if nargin == 3
    dt  = a;
    r   = r + v*dt;
    R_r = eye(length(r));
    R_v = dt*R_r;
    R_a = zeros(3);

elseif nargin == 4
    r   = r + v*dt + .5*a*dt^2;
    R_r = eye(length(r));
    R_v = R_r*dt;
    R_a = 0.5*R_r*dt^2;

end

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


 function ir_mri_rf_spsp_plot(b, gz, d, f, z, mm, t)
%function ir_mri_rf_spsp_plot(b, gz, d, f, z, mm, t)
%|
%| plot SPSP pulses etc.
%| code extracted from compute_rf_spsp_mgh

%printm('Displaying SPSP pulse design results...')

pl = @(i) subplot(340+i);

pl(1)
plot(t*1000,gz)
axis tight
grid
xlabel('Time (ms)')
ylabel('g/cm')
title('z gradient')

pl(5)
plot(t*1000,real(b),'b-', t*1000,imag(b),'r-')
axis tight
grid
xlabel('Time (ms)')
ylabel('g')
title('SPSP pulse')
legend('Real','Imaginary')

%Display desired pattern of the SPSP recovery pulse.
pl(2)
imagesc(f,z,abs(d))
colormap default;colorbar
xlabel('Frequency offset (Hz)')
ylabel('z (cm)')
title('Desired SPSP pattern (magnitude)')

pl(6)
imagesc(f,z,angle(d).*abs(d),[-pi,pi]);colorbar
colormap(gca, hsv)
xlabel('Frequency offset (Hz)')
ylabel('z (cm)')
title('Desired SPSP pattern (phase)')

pl(3)
imagesc(f,z,abs(mm))
xlabel('frequency (Hz)')
ylabel('z (cm)')
title('Resulting pattern (linear regime prediction)')

pl(7)
imagesc(f,z,angle(mm))
colormap(gca, hsv)
xlabel('frequency (Hz)')
ylabel('z (cm)')
title('Resulting phase pattern (linear regime prediction)')


function [CrowdDis]=Crowding(Pop)
%Harmonic average distance of each solution in the decision space
%Return: the crowding distance of each individual

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

% This function is written by Wenhua Li

    [N, ~]=size(Pop);
    K=N-1;
    Z = min(Pop,[],1);
    Zmax = max(Pop,[],1);
    pop=(Pop-repmat(Z,N,1))./repmat(Zmax-Z,N,1);
    distance=pdist2(pop,pop);
    [value,~]=sort(distance,2);
    CrowdDis=K./sum(1./value(:,2:N),2);
end


function [rv,rvSS]=realized_threshold_multipower_variation(price,time,timeType,samplingType,samplingInterval,gamma,thresholdScale,thresholdType,subsamples)

c = thresholdScale;

logPrice =log(price);
% Filter prices and compute the RV
filteredLogPrice = realized_price_filter(logPrice,time,timeType,samplingType,samplingInterval);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
filteredLogPrice  = log(price);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

returns = diff(filteredLogPrice);

L = 25;
V = inf*ones(size(returns));
K = -L:L;
K = 1/sqrt(2*pi)*exp(-(K/L).^2/2);
K(L+(-1:1)) = 0;
m = size(returns,1);
returns2 = returns.^2;

finished = false;
while ~finished
    ind = returns2<(c^2.*V);
    Vold  = V;
    for i=1:m
        pl = i-L:i+L;
        valid = pl>1 & pl<=m;
        tempInd = ind(pl(valid));
        w = K(valid);
        V(i) = w*(returns2(pl(valid)).*tempInd)/(w*tempInd);
    end
    if all(V==Vold)
        finished = true;
    end
end

expectedValue = 1./(2*normcdf(-c)*sqrt(pi))*(2/c^2) * gamma(3/2).*gammainc(c^2/2,3/2,'upper') * c^2 *V;

rv = returns(ind)'*returns(ind) + sum(expectedValue(~ind));


%%*****************************************************************
%% NTscaling: Compute NT scaling matrix
%%
%% compute SVD of Xchol*Zchol via eigenvalue decompostion of
%%     Zchol * X * Zchol' = V * diag(sv2) * V'.
%% compute W satisfying W*Z*W = X.
%%     W = G'*G,  where G = diag(sqrt(sv)) * (invZchol*V)'
%%     important to keep W symmertic.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

 function [W,G,sv,gamx,gamz,dd,ee,ff] = ...
          NTscaling(blk,X,Z,Zchol,invZchol);

    numblk = size(blk,1);
    W = cell(numblk,1); G = cell(numblk,1); sv = cell(numblk,1);
    gamx = cell(numblk,1); gamz = cell(numblk,1);
    dd = cell(numblk,1); ee = cell(numblk,1); ff = cell(numblk,1);
%%
    for p = 1:size(blk,1)
        pblk = blk(p,:);
        numblk = length(pblk{2});
        n = sum(pblk{2});
        if strcmp(pblk{1},'l')
           dd{p} = X{p}./Z{p}; %% do not add perturbation, it badly affects cre-a
        elseif strcmp(pblk{1},'q');
           gamx{p} = sqrt(qops(pblk,X{p},X{p},2));
           gamz{p} = sqrt(qops(pblk,Z{p},Z{p},2));
           w2 = gamz{p}./gamx{p};  w = sqrt(w2);
           dd{p} = qops(pblk,1./w2,ones(n,1),4);
           tt = qops(pblk,1./w,Z{p},3) - qops(pblk,w,X{p},4);
           gamtt = sqrt(qops(pblk,tt,tt,2));
           ff{p} = qops(pblk,1./gamtt,tt,3);
           ee{p} = qops(pblk,sqrt(2)./w,ff{p},4);
        elseif strcmp(pblk{1},'s')
           tmp = Prod2(pblk,Zchol{p},X{p},0);
           tmp = Prod2(pblk,tmp,Zchol{p}',1);
           [sv2,V] = blkeig(pblk,tmp);
           sv2 = max(1e-20,sv2);
           sv{p} = sqrt(sv2);
           tmp  = Prod2(pblk,invZchol{p},V);
           G{p} = Prod2(pblk,spdiags(sqrt(sv{p}),0,n,n),tmp');
           W{p} = Prod2(pblk,G{p}',G{p},1);
        end
    end
%%**********************************************************************


function [m3] = cl2m3(cl)
% Convert volume from centiliters to cubic meters.
% Chad Greene 2012
m3 = cl*0.00001;


function [H s phi T] = RSTLS(X1, X2, normalization)

% [H s phi T] = RSTLS(X1, X2, normalization)
%
% DESC:
% computes the RST transformation between the point pairs X1, X2
%
% VERSION:
% 1.0.1
%
% INPUT:
% X1, X2        = point matches (cartesian coordinates)
% normalization = true (default) or false to enable/disable point
%                 normalzation
%
% OUTPUT:
% H             = homography representing the RST transformation
% s             = scaling
% phi           = rotation angle
% T             = translation vector

% AUTHOR:
% Marco Zuliani, email: marco.zuliani@gmail.com
% Copyright (C) 2011 by Marco Zuliani
%
% LICENSE:
% This toolbox is distributed under the terms of the GNU GPL.
% Please refer to the files COPYING.txt for more information.

% HISTORY
% 1.0.0         08/27/08 - intial version
% 1.0.1         06/09/09 - implemented closed form for the LS estimation
%                          routines

if (nargin < 3)
    normalization = true;
end;

N = size(X1, 2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% checks
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (size(X2, 2) ~= N)
    error('RSTLS:inputError', ...
        'The set of input points should have the same cardinality')
end;
if N < 2
    error('RSTLS:inputError', ...
        'At least 2 point correspondences are needed')
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% normalize the input
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (normalization) && (N > 2)
    % fprintf('\nNormalizing...')
    [X1, T1] = normalize_points(X1);
    [X2, T2] = normalize_points(X2);
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% estimation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (N == 2)

    % fast estimation
    Theta = zeros(4,1);

    % $\mbox{\texttt{MM}} \eqdef M_{:,1} = \vct{y}^{(1)}-\vct{y}^{(2)} = \left[\begin{array}{c} y_1^{(1)}-y_1^{(2)} \\ y_2^{(1)}-y_2^{(2)} \end{array}\right]$
    % 2 additions
    MM = X1(:,1) - X1(:,2);
    % $ \mbox{\texttt{detMM}} \eqdef |M|$
    % 1 additions, 2 multiplication
    detMM = MM(1)*MM(1) + MM(2)*MM(2);
    % $ \mbox{\texttt{MMi}} \eqdef \left[ \begin{array}{c} \left[M^{-1}\right]_{1,1} \\ -\left[M^{-1}\right]_{2,1}\end{array}\right]$
    % 2 multiplications
    MMi = MM / detMM;

    % $ \mbox{\texttt{Delta}} \eqdef \vct{T}_{\vct{\theta}} (\vct{y}^{(1)})-\vct{T}_{\vct{\theta}} (\vct{y}^{(2)})$
    % 2 additions
    Delta = X2(:,1) - X2(:,2);

    % $ \mbox{\texttt{Theta(1:2)}} = M^{-1}\left(\vct{T}_{\vct{\theta}} (\vct{y}^{(1)})-\vct{T}_{\vct{\theta}} (\vct{y}^{(2)})\right)$
    % 1 additions, 2 multiplications
    Theta(1) = MMi(1)*Delta(1) + MMi(2)*Delta(2);
    % 1 additions, 2 multiplications
    Theta(2) = MMi(1)*Delta(2) - MMi(2)*Delta(1);
    % $ \mbox{\texttt{Theta(3:4)}} = -S^{(2)}\vct{\theta}_{1:2}+\vct{T}_{\vct{\theta}} (\vct{y}^{(2)})$
    % 2 additions, 2 multiplications

    Theta(3) = X2(1,2) - Theta(1)*X1(1,2) + Theta(2)*X1(2,2);
    % 2 additions, 2 multiplications
    Theta(4) = X2(2,2) - Theta(1)*X1(2,2) - Theta(2)*X1(1,2);

    % total: 11 additions, 12 multiplications
else

    % Closed form LS solution. Using the tutorial notation.

    % Notation semplification:
    % $\vct{p}^{(i)} = \bar{\vct{y}}^{(i)}$ and $\vct{q}^{(i)} = \overline{\vct{T}_{\vct{\theta}} (\vct{y}^{(i)})}$
    % $a = \sum_{i=1}^N\left( (p_1^{(i)})^2 + (p_2^{(i)})^2 \right)$
    a = sum(X1(:).^2);

    % Explicit LS expansion:
    % $  \theta_1 = \frac{1}{a}\sum_{i=1}^N p_1^{(i)} q_1^{(i)} + p_2^{(i)} q_2^{(i)} $
    % $  \theta_2 = \frac{1}{a}\sum_{i=1}^N -p_2^{(i)} q_1^{(i)} + p_1^{(i)} q_2^{(i)} $
    % $  \theta_3 = 0 $
    % $  \theta_4 = 0 $
    Theta(1) = sum( X1(1, :).*X2(1, :) + X1(2, :).*X2(2, :) ) / a;
    Theta(2) = sum( -X1(2, :).*X2(1, :) + X1(1, :).*X2(2, :) ) / a;
    Theta(3) = 0;
    Theta(4) = 0;

    % Traditional LS
    %
    %     A = zeros(2*N, 4);
    %     b = zeros(2*N, 1);
    %
    %     ind = 1:2;
    %     for n = 1:N
    %
    %         A(ind, 1:2) = [X1(1,n) -X1(2,n); X1(2,n) X1(1,n)];
    %         A(ind, 3:4) = eye(2);
    %
    %         b(ind) = X2(1:2, n);
    %
    %         ind = ind + 2;
    %
    %     end;
    %
    %     % solve the linear system in a least square sense
    %     Theta = A\b;

end;

% compute the corresponding homography
H = [Theta(1) -Theta(2) Theta(3); Theta(2) Theta(1) Theta(4); 0 0 1];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% de-normalize the parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (normalization) && (N > 2)
    H = T2\H*T1;
end;
H = H/H(9);

% prepare the output
if nargout > 1

    s       = sqrt(H(1,1)*H(1,1) + H(2,1)*H(2,1));
    phi     = atan2(H(2,1), H(1,1));
    T       = H(1:2, 3);

end;

return


function [M,R] = spm_get_closest_affine(x,y,w1,w2)
% Determine the affine transform mapping x to y
% FORMAT [M,R] = spm_get_closest_affine(X,Y,W1,W2)
% X  - n1*n2*n3*3 array of floats representing coordinates.
% Y  - n1*n2*n3*3 array of floats representing coordinates.
% W1 - n1*n2*n3   array of floats representing weights.
% W2 - n1*n2*n3   array of floats representing weights.
%
% M  - an affine transform
% R  - a rigid-body transform
%
% The code treats X and Y as reshaped versions (n1*n2*n3) x 3,
% and W1 and W2 as column vectors.
%
% It generates XX = [diag(W1)*X W1]'*diag(W2)*[diag(W1)*X W1]
% and          XY = [diag(W1)*X W1]'*diag(W2)*[Y W1]
%
% These can then be used to compute an affine transform (M),
% by M = (XX\XY)'
% A weighted procrustes decomposition is also performed,
% so that a rigid-body transform matrix (R) is returned.
%
% If W1 or W2 are empty or not passed, then they are assumed
% to be all ones.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_get_closest_affine.m 6137 2014-08-19 12:43:11Z john $

XX = zeros(4);
XY = zeros(4);
d  = size(x);
o  = ones(d(1)*d(2),1);
for k=1:size(x,3),
    xk  = reshape(x(:,:,k,:),[d(1)*d(2),3]);
    if (nargin<3 || isempty(w1)) && (nargin<4 || isempty(w2)),
        ox = o;
        oy = o;
    else
        if nargin>=4 && ~isempty(w1) && ~isempty(w2),
            oy = reshape(w2(:,:,k), [d(1)*d(2),1]);
            ox = reshape(w1(:,:,k), [d(1)*d(2),1]).*oy;
        elseif nargin>=3 && ~isempty(w1),
            ox = reshape(w1(:,:,k), [d(1)*d(2),1]);
            oy = ox;
        elseif nargin>=4 && ~isempty(w2),
            ox = reshape(w2(:,:,k), [d(1)*d(2),1]);
        end
        xk(:,1) = xk(:,1).*ox;
        xk(:,2) = xk(:,2).*ox;
        xk(:,3) = xk(:,3).*ox;
    end
    yk  = reshape(y(:,:,k,:),[d(1)*d(2),3]);
    msk = find(all(isfinite(xk),2) & all(isfinite(yk),2));
    X   = [xk(msk,:), ox(msk)];
    Y   = [yk(msk,:), oy(msk)];
    XX  = XX + double(X'*X);
    XY  = XY + double(X'*Y);
end
M = (XX\XY)';

if nargout>1,
    % Procrustes decomposition
    XX1 = XX - XX(:,4)*XX(:,4)'/XX(4,4);
    XY1 = XY - XY(:,4)*XY(4,:) /XY(4,4);
    Z   = (XX1(1:3,1:3)\XY1(1:3,1:3))';
    [U,S,V] = svd(Z);                   % Decompose into rotate, zoom and rotate.
    R   = [U*V' zeros(3,1);0 0 0 1];    % Pure rotation (by taking out the zoom)
    T1  = [eye(4,3) -XY(:,4) /XY(4,4)]; % Initial translation of centre of mass to origin.
    T2  = [eye(4,3) -XY(4,:)'/XY(4,4)]; % Final translation of origin to centre of mass.
    R   = T2 * R * T1;
end


function Out   = RiemannExpMap(P,X)

[U Delta] = eig(P);
G = U*sqrt(Delta);
Y = inv(G)*X*inv(G)';
[V Sigma] = eig(Y);
Out = (G*V)*diag(exp(diag(Sigma)))*(G*V)';


function [points3d, covariance] = myEsimatePosAmers(pointTracks, ...
    camPoses, cameraParams,ParamFilter)
chiC = ParamFilter.chiC;
RotC = chiC(1:3,1:3);
xC = chiC(1:3,4);
R = eye(2);
k = 2*length(camPoses);
q = 6*length(camPoses);
N_aug = q+k;
Rc = chol(kron(eye(k/2),R));

S = zeros(q);
chi = cell(k/2,1);
for i = 1:k/2
    S(6*i-5:6*i,6*i-5:6*i) = camPoses(i).S([1:3 7:9],[1:3 7:9]);
    chi{i} = [camPoses(i).Orientation camPoses(i).Location;0 0 0 1];
end

S_aug = blkdiag(S,Rc);

% scaled unsented transform
W0 = 1-N_aug/3;
Wj = (1-W0)/(2*N_aug);
gamma = sqrt(N_aug/(1-W0));
alpha = 1;
beta = 2;

% Compute transformed measurement
X = zeros(N_aug,2*N_aug+1); %sigma points
Y = zeros(3,2*N_aug+1);
ybar = zeros(3,1); % Measurement mean
for j = 1:2*N_aug+1
    if j == 1
    elseif j < N_aug+2
        X(:,j) = gamma*S_aug(j-1,:)';
    else
        X(:,j) = -gamma*S_aug(j-N_aug-1,:)';
    end
    camPoses_j = camPoses;
    pointTracks_j = pointTracks;
    xi_j = X(1:q,j);
    v_j = X(q+1:N_aug,j);
    for i = 1:k/2
        chi_j = expSE3(xi_j(6*i-5:6*i))*chi{i};
        camPoses_j(i).Orientation = RotC'*chi_j(1:3,1:3)';
        camPoses_j(i).Location = chi_j(1:3,4) +chi_j(1:3,1:3)*RotC*xC;
        pointTracks_j.Points(i,:) = pointTracks.Points(i,:) + v_j(2*i-1:2*i)';
    end

    Y(:,j) = EsimatePosAmers(pointTracks_j,camPoses_j,cameraParams)';
    if j == 1
        ybar = ybar + W0*Y(:,j);
    else
        ybar = ybar + Wj*Y(:,j);
    end
end

Y(:,1) = sqrt(abs(W0+(1-alpha^2+beta)))*(Y(:,1)-ybar);
YY = sqrt(Wj)*(Y(:,2:2*N_aug+1)-ybar*ones(1,2*N_aug));
[~,Rs] = qr(YY');
Ss = Rs(1:3,1:3);
[Sy,~] = cholupdate(Ss,Y(:,1),'-'); % Sy'*Sy = Pyy

Pxy = zeros(q,3);
for j = 2:2*N_aug+1
    Pxy = Pxy + Wj*X(1:q,j)*(Y(:,j)-ybar)';
end

points3d = ybar;
S = S(end-5:end,end-5:end);
covariance = [S'*S Pxy(end-5:end,:); Pxy(end-5:end,:)' Sy'*Sy];
end


%This Matlab script can be used to reproduce Figure 3.4 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Define the range of BS antennas
Mvalues = [1 10 100];

%Angular standard deviation in the local scattering model (in degrees)
ASD = 10;

%Nominal angle of desired UE
thetaDesired = pi/6;

%Range of nominal angles of the interfering UE
varphiInterfererDegrees = -180:1:180;
varphiInterfererRadians = varphiInterfererDegrees*(pi/180);

%Define the antenna spacing (in number of wavelengths)
antennaSpacing = 1/2; %Half wavelength distance

%Define the effective SNR in (3.13) for the desired UE
SNR1dB = 10;
SNR1 = 10.^(SNR1dB/10);

%Define the effective SNR in (3.13) for the interfering UE
SNR2dB = 0;
SNR2 = 10.^(SNR2dB/10);

%Preallocate matrices for storing the simulation results
correlationcoeff = zeros(length(varphiInterfererRadians),length(thetaDesired),length(Mvalues));

%Compute the spatial correlation matrix of the desired UE
R1 = functionRlocalscattering(max(Mvalues),thetaDesired,ASD,antennaSpacing);

%% Go through all angles of interfering UE
for n = 1:length(varphiInterfererRadians)

    %Output simulation progress
    disp([num2str(n) ' angles out of ' num2str(length(varphiInterfererRadians))]);

    %Compute the spatial correlation matrix of the interfering UE
    R2 = functionRlocalscattering(max(Mvalues),varphiInterfererRadians(n),ASD,antennaSpacing);

    %Go through all number of antennas
    for m = 1:length(Mvalues)

        %Extract correlation matrices of the specified dimension
        R1m = R1(1:Mvalues(m),1:Mvalues(m));
        R2m = R2(1:Mvalues(m),1:Mvalues(m));

        %Compute the denominator in (3.18)
        normalization = sqrt(SNR1*SNR2*abs(trace(R1m*((SNR1*R1m+SNR2*R2m+eye(Mvalues(m)))\R1m)))*abs(trace(R2m*((SNR1*R1m+SNR2*R2m+eye(Mvalues(m)))\R2m))));

        %Compute absolute value of antenna-averaged correlation coefficient in (3.18)
        correlationcoeff(n,m) = sqrt(SNR1*SNR2)*abs(trace(R1m*((SNR1*R1m+SNR2*R2m+eye(Mvalues(m)))\R2m)))/normalization;

    end

end

%% Plot the simulation results
figure;
hold on; box on;

plot(varphiInterfererDegrees,correlationcoeff(:,1),'k-','LineWidth',1);
plot(varphiInterfererDegrees,correlationcoeff(:,2),'r--','LineWidth',1);
plot(varphiInterfererDegrees,correlationcoeff(:,3),'b-.','LineWidth',1);

xlabel('Angle of interfering UE [degree]');
ylabel('Antenna-averaged correlation coefficient');
xlim([-180 180]);
ylim([0 1.1]);

legend('M=1','M=10','M=100','Location','Best');


function grad = B_logistic(input_layers, CostLayer)

[~, output, target] = prepareCostEvaluation(input_layers, CostLayer);
m = length(output);
output = sigmoid(output);

grad = -(target - output)/m;

end


function [Dps_eff, Dns_eff] = solidPhaseDiffusionCoefficients(T,param)
% solidPhaseDiffusionCoefficients evaluates diffusion coefficients of the solid phase [m^2 /s].
% The user may modify the script to meet specific requirements.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

if(param.TemperatureEnabled>=1)
    Dps_eff     = param.Dps*exp(-param.EaDps/param.R*(1./T(param.Nal+1:param.Nal+param.Np)-1/param.Tref));
else
    Dps_eff     = param.Dps*ones(param.Np,1);
end

if(param.TemperatureEnabled>=1)
    Dns_eff     = param.Dns*exp(-param.EaDns/param.R*(1./T(param.Nal+param.Np+param.Ns+1:param.Nal+param.Np+param.Ns+param.Nn)-1/param.Tref));
else
    Dns_eff     = param.Dns*ones(param.Nn,1);
end

end


function detail_layer_out = StevensonDetailEnhancement(detail_layer, F_L)
%
%       detail_layer_out = StevensonDetailEnhancement(detail_layer, F_L)
%
%       This function adjusts the detail layer for taking into account the
%       Stevenson effect.
%
%       input:
%           -detail_layer: the detail layer of an image
%           -F_L: a luminance dependent factor
%
%       output:
%           -detail_layer_out: processed detail layer
%
%     Copyright (C) 2014-15  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

exponent = (F_L + 0.8).^0.25;

[~, ~, col] = size(detail_layer);

detail_layer_out = zeros(size(detail_layer));

for i=1:col
    detail_layer_out(:,:,i) = detail_layer(:,:,i).^exponent;
end

end


function J = CalcJacobian_rot(idx)
% Jacobian matrix of current configration in World frame
global uLINK

jsize = length(idx);
target = uLINK(idx(end)).p;   % absolute target position
J = zeros(3,jsize);

for n=1:jsize
    j = idx(n);
    a = uLINK(j).R * uLINK(j).a;  % joint axis vector in world frame
    J(:,n) = cross(a, target - uLINK(j).p);
end


% Parallel HYperslab Projection along Affine SubSpace (PHYPASS) algorithm
%
% M. Takizawa and M. Yukawa, "An Efficient Data-Reusing Kernel Adaptive
% Filtering Algorithm Based on Parallel Hyperslab Projection Along Affine
% Subspace," in Proc. ICASSP, May. 2013, pp.3557-3561.
%
% Remark: version01, August 2013
% Contributor for this code: Masa-aki Takizawa
%
% This file is part of the Kernel Adaptive Filtering Toolbox for Matlab.
% https://github.com/steven2358/kafbox/

classdef phypass < kernel_adaptive_filter

    properties (GetAccess = 'public', SetAccess = 'private')
        mu = 0.5; % step size
        s = 1; % number of update coefficients (dictionary size)
        sigma = 0.95; % threshold for dictionary
        p = 8; % number of hyper slabs
        kerneltype = 'gauss'; % kernel type
        kernelpar = 1; % kernel parameter
        omega = 1/8; % weight
    end

    properties (GetAccess = 'public', SetAccess = 'private')
        mem = []; % memory
        dict = []; % dictionary
        alpha = []; % expansion coefficients
        d = []; % output window
    end

    methods
        function kaf = phypass(parameters) % constructor
            if (nargin > 0) % copy valid parameters
                for fn = fieldnames(parameters)'
                    if ismember(fn,fieldnames(kaf))
                        kaf.(fn{1}) = parameters.(fn{1});
                    end
                end
            end
        end

        function y_est = evaluate(kaf,x) % evaluate the algorithm
            if size(kaf.dict,1)>0
                k = kernel(kaf.dict,x,kaf.kerneltype,kaf.kernelpar);
                y_est = k'*kaf.alpha;
            else
                y_est = zeros(size(x,1),1);
            end
        end

        function train(kaf,x,y) % train the algorithm
            if (length(kaf.d) < kaf.p) % grow the memory
                kaf.mem = [kaf.mem; x];
                kaf.d = [kaf.d; y];
            else % sliding window
                kaf.mem = [kaf.mem(2:end,:); x];
                kaf.d = [kaf.d(2:end); y];
            end
            if size(kaf.dict,1) == 0 % initialize
                kaf.dict = x;
                kaf.alpha = 0;
            else
                k = kernel(x,kaf.dict,kaf.kerneltype,kaf.kernelpar);
                if max(k) < kaf.sigma % coherence criterion
                    kaf.dict = [kaf.dict ; x]; % add the current input x
                    kaf.alpha = [kaf.alpha ; 0];
                end
            end
            num = length(kaf.d); % length of memory

            num_d = min(size(kaf.dict,1),kaf.s); % number of dictionary elements to update

            dict_id = zeros(num_d,num); % memory of dictionary index

            Kmemdict = kernel(kaf.mem,kaf.dict,kaf.kerneltype,kaf.kernelpar);
            Kdict = kernel(kaf.dict,kaf.dict,kaf.kerneltype,kaf.kernelpar);

            for k=1:num
                d_check = Kmemdict(k,:); % kernel between k'th memory element and full dictionary
                [mm,ii] = sort(d_check,'descend'); %#ok<ASGLU>
                dict_id(:,k) = ii(1:num_d); % memory indexes
            end

            k_n = zeros(num_d,num_d,num);
            y_n = zeros(num_d,num);
            alpha_new = zeros(num_d,num);
            for k=1:num
                k_n(:,:,k) = Kdict(dict_id(:,k),dict_id(:,k));
                y_n(:,k) = Kmemdict(k,dict_id(:,k))';
                alpha_new(:,k) = k_n(:,:,k)\y_n(:,k); % compute projections onto dictionary subspaces
            end
            ln_nume = 0;
            numerator = zeros(num,1);
            denominator = zeros(num,1);
            beta = zeros(num,1);

            for k=1:num
                numerator(k) = Kmemdict(k,:)*kaf.alpha;
                denominator(k) = Kmemdict(k,dict_id(:,k))*alpha_new(:,k);
                beta(k) = (kaf.d(k) - numerator(k))/(denominator(k)); %  progress of the projection onto hyperplanes
                ln_nume = ln_nume + kaf.omega * beta(k)^2 *...
                    alpha_new(:,k)' * k_n(:,:,k) * alpha_new(:,k); % numerator of the extrapolation coefficcient
            end

            G = zeros(num,num);
            for k = 1:num
                Grow =  alpha_new(:,k)'*Kdict(dict_id(:,k),dict_id);
                sti = 1;
                ndi = num_d;
                for l = 1:num
                    G(k,l) = Grow(1,sti:ndi)*alpha_new(:,l);
                    sti = ndi + 1;
                    ndi = ndi + num_d;
                end
            end

            ln_deno = kaf.omega^2 * beta' * G * beta; % denominator of the extrapolation coefficient
            ln = ln_nume/(ln_deno); % extrapolation coefficient
            alpha_new2 = zeros(length(kaf.alpha),num);
            for k=1:num
                alpha_new2(dict_id(:,k),k) = alpha_new(:,k);
            end
            for k=1:num
                kaf.alpha = kaf.alpha + kaf.mu * ln *...
                    kaf.omega * beta(k) * alpha_new2(:,k); % update expansion coefficients
            end

        end
    end
end


classdef L1_ADMM_YZ < handle
% Solver for various L1 minimization problems
% based on the 2011 paper by
% Junfeng Yang and Yin Zhang

properties
    % weight for the quadratic penalty term
    rho
     % verbosity
    verbose
    % Maximum number of ADMM iterations
    max_iterations
    % relative tolerance
    tolerance
    % additional scaling factor for x updates
    gamma
end % properties

properties(SetAccess=private)
    % The sensing operator
    A
    % The signals being recovered
    B
    % The solution representation vectors
    X
    % The dimension of signal space
    M
    % The dimension of representation space
    N
    % Details of execution (intermediate values)
    details
end % properties

% Following properties are meant for implementation only
properties(Access=private)
    X0
    Y0
    Z0
end

methods

function self = L1_ADMM_YZ(A, options)
    if nargin < 1
        error('A must be specified.');
    end
    if isa(A, 'spx.dict.Operator')
        self.A = A;
    elseif ismatrix(A)
        self.A = spx.dict.MatrixOperator(A);
    else
        error('Unsupported operator.');
    end
    [self.M, self.N] = size(self.A);
    if nargin < 2
        options = struct;
    end
    self.init_options();
    self.process_options(options);
end % function

function [X] = solve_bp(self, B, options)
    % Solves the problem min ||x||_1 s.t. Ax = b
    if nargin < 2
        error('B must be specified.');
    end
    if nargin < 3
        options = struct;
    end
    self.process_options(options);
    num_problems = size(B, 2);
    if self.verbose > 0
        fprintf('Solving l1 minimization problem: BP.\n');
    end
    self.init_details(num_problems);
    max_iterations = self.max_iterations;
    A  = self.A;
    X = zeros(self.N, num_problems);
    % import relevant functions from other modules
    import spx.opt.projections.proj_linf_ball;
    % iterate over problems
    for prob=1:num_problems
        tstart = tic;
        b = B(:, prob);
        b_max = norm(b, 'inf');
        terminated = false;
        % Check for zero solution condition
        if b_max < self.tolerance
            % There is no need to proceed further
            % zero solution is best solution
            self.details.terminated(prob) = true;
            self.details.iterations(prob) = 0;
            self.details.elapsed_times(prob) = toc(tstart);
            X(:, prob) = zeros(self.N, 1);
            continue;
        end
        if self.rho > 0
            % we will use user defined quadratic term weight
            rho = self.rho;
        else
            rho = mean(abs(b));
        end
        gamma = self.gamma;
        % scale the problem
        b = b / b_max;
        % Initialize solution
        x = A.adjoint(b);
        z = zeros(self.N, 1);
        y = zeros(self.M, 1);
        % compute current primary residual
        r_primal = A.apply(x) - b;
        for iter=1:max_iterations
            % check if we need to iterate further
            if terminated; break; end;
            % update z
            z_prev = z;
            z = proj_linf_ball(A.adjoint(y) + (x / rho));
            % update y
            y_prev = y;
            Az = A.apply(z);
            y = (Az - r_primal / rho);
            % update dual residual
            Aty = A.adjoint(y);
            r_dual =  Aty - z;
            % update x
            x_prev = x;
            x = x + (gamma * rho)*r_dual;
            % update primary residual
            r_primal = A.apply(x) - b;
            % primal objective
            primal_obj = sum(abs(x));
            % dual objective
            dual_obj = real(b' * y);
            terminated = self.check_termination(x, x_prev, y, z, ...
                r_primal, r_dual, primal_obj, dual_obj, iter, prob);
        end % iteration loop
        self.details.terminated(prob) = terminated;
        self.details.iterations(prob) = iter-1;
        self.details.elapsed_times(prob) = toc(tstart);
        % put the final solution back in result
        X(:, prob) = x * b_max;
    end % problem loop
end % function

function [X] = solve_bpic(self, B, delta, options)
    % Solves the problem min ||x||_1 s.t. ||Ax - b||_2 <= delta
    if nargin < 2
        error('B must be specified.');
    end
    if nargin < 3
        error('delta must be specified.');
    end
    if nargin < 4
        options = struct;
    end
    self.process_options(options);
    num_problems = size(B, 2);
    if self.verbose > 0
        fprintf('Solving constrained l1 minimization problem: BPIC.\n');
    end
    self.init_details(num_problems);
    max_iterations = self.max_iterations;
    A  = self.A;
    X = zeros(self.N, num_problems);
    % import relevant functions from other modules
    import spx.opt.projections.proj_linf_ball;
    import spx.opt.projections.proj_l2_ball;
    % iterate over problems
    for prob=1:num_problems
        tstart = tic;
        b = B(:, prob);
        b_max = norm(b, 'inf');
        terminated = false;
        % Check for zero solution condition
        if norm(b) < delta
            % There is no need to proceed further
            % zero solution is best solution
            self.details.terminated(prob) = true;
            self.details.iterations(prob) = 0;
            self.details.elapsed_times(prob) = toc(tstart);
            X(:, prob) = zeros(self.N, 1);
            continue;
        end
        if self.rho > 0
            % we will use user defined quadratic term weight
            rho = self.rho;
        else
            rho = mean(abs(b));
        end
        gamma = self.gamma;
        % scale the problem
        b = b / b_max;
        % Initialize solution
        x = A.adjoint(b);
        z = zeros(self.N, 1);
        y = zeros(self.M, 1);
        % compute current primary residual
        r_primal = A.apply(x) - b;
        delta_by_rho = delta / rho;
        for iter=1:max_iterations
            % check if we need to iterate further
            if terminated; break; end;
            % update z
            z_prev = z;
            z = proj_linf_ball(A.adjoint(y) + (x / rho));
            % update y
            y_prev = y;
            Az = A.apply(z);
            y = (Az - r_primal / rho);
            y = y - proj_l2_ball(y, delta_by_rho);
            % update dual residual
            Aty = A.adjoint(y);
            r_dual =  Aty - z;
            % update x
            x_prev = x;
            x = x + (gamma * rho)*r_dual;
            % update primary residual
            r_primal = A.apply(x) - b;
            % primal objective
            primal_obj = sum(abs(x));
            % dual objective
            dual_obj = real(b' * y) - delta * norm(y);
            terminated = self.check_termination(x, x_prev, y, z, ...
                r_primal, r_dual, primal_obj, dual_obj, iter, prob);
        end % iteration loop
        self.details.terminated(prob) = terminated;
        self.details.iterations(prob) = iter-1;
        self.details.elapsed_times(prob) = toc(tstart);
        % put the final solution back in result
        X(:, prob) = x * b_max;
    end % problem loop
end % function

function [X] = solve_bpdn_l2(self, B, mu, options)
    % Solves the problem min ||x||_1  + ||Ax - b||_2 / (2 mu)
    if nargin < 2
        error('B must be specified.');
    end
    if nargin < 3
        error('mu must be specified.');
    end
    if nargin < 4
        options = struct;
    end
    self.process_options(options);
    num_problems = size(B, 2);
    if self.verbose > 0
        fprintf('Solving l1-l2 unconstrained minimization problem: BPDN l2.\n');
    end

    self.init_details(num_problems);
    max_iterations = self.max_iterations;
    A  = self.A;
    X = zeros(self.N, num_problems);
    % import relevant functions from other modules
    import spx.opt.projections.proj_linf_ball;
    % iterate over problems
    for prob=1:num_problems
        tstart = tic;
        b = B(:, prob);
        %  Compute A' b
        Atb = A.adjoint(b);
        terminated = false;
        % Check for zero solution condition
        if norm(Atb, 'inf') <= mu
            % There is no need to proceed further
            % zero solution is best solution
            self.details.terminated(prob) = true;
            self.details.iterations(prob) = 0;
            self.details.elapsed_times(prob) = toc(tstart);
            X(:, prob) = zeros(self.N, 1);
            continue;
        end
        if self.rho > 0
            % we will use user defined quadratic term weight
            rho = self.rho;
        else
            rho = mean(abs(b));
        end
        gamma = self.gamma;
        % scale the problem
        b_max = norm(b, 'inf');
        b = b / b_max;
        mu = mu / b_max;
        % Initialize solution
        x = A.adjoint(b);
        z = zeros(self.N, 1);
        y = zeros(self.M, 1);
        % compute current primary residual
        r_primal = A.apply(x) - b;
        for iter=1:max_iterations
            % check if we need to iterate further
            if terminated; break; end;
            % update z
            z_prev = z;
            z = proj_linf_ball(A.adjoint(y) + (x / rho));
            % update y
            y_prev = y;
            Az = A.apply(z);
            y = (rho/(mu +  rho)) * (Az - r_primal / rho);
            % update dual residual
            Aty = A.adjoint(y);
            r_dual =  Aty - z;
            % update x
            x_prev = x;
            x = x + (gamma * rho)*r_dual;
            % update primary residual
            r_primal = A.apply(x) - b;
            % primal objective
            primal_obj = sum(abs(x)) + (1/(2*mu))*(r_primal'*r_primal);
            % dual objective
            dual_obj = real(b' * y) - (mu / 2) * (y' * y);
            terminated = self.check_termination(x, x_prev, y, z, ...
                r_primal, r_dual, primal_obj, dual_obj, iter, prob);
        end % iteration loop
        self.details.terminated(prob) = terminated;
        self.details.iterations(prob) = iter-1;
        self.details.elapsed_times(prob) = toc(tstart);
        % put the final solution back in result
        X(:, prob) = x * b_max;
    end % problem loop
end % function

function [X] = solve_bpdn_l1(self, B, nu, options)
    % Solves the problem min ||x||_1  + ||Ax - b||_1 / (nu)
end % function

end % methods

methods(Access=private)

function init_options(self)
    self.rho = 0;
    self.verbose = 0;
    self.max_iterations = 100;
    % self.eps_abs = 1e-4;
    self.tolerance = 1e-2;
    self.gamma = 1;
end % function

function process_options(self, options)
    % weight for the quadratic penalty term
    if isfield(options, 'rho')
        self.rho = options.rho;
    end
    % verbosity
    if isfield(options, 'verbose')
        self.verbose = options.verbose;
    end
    % Maximum number of ADMM iterations
    if isfield(options, 'max_iterations')
        self.max_iterations = options.max_iterations;
    end
    % gamma for primal variable update
    if isfield(options, 'gamma')
        self.gamma = options.gamma;
    end
    % relative tolerance
    if isfield(options, 'tolerance')
        self.tolerance = options.tolerance;
    end
end % function

function init_xyz(self, num_problems, options)
    if isfield(options, 'X0')
        self.X0 = X0;
    else
        self.X0 = zeros(self.N, num_problems);
    end
    if isfield(options, 'Y0')
        self.Y0 = Y0;
    else
        self.Y0 = zeros(self.N, num_problems);
    end
    if isfield(options, 'Z0')
        self.Z0 = Z0;
    else
        self.Z0 = zeros(self.N, num_problems);
    end
end % function

function init_details(self, num_problems)
    max_iterations = self.max_iterations;
    self.details.iterations = zeros(1, num_problems);
    self.details.terminated = zeros(1, num_problems);
    self.details.elapsed_times = zeros(1, num_problems);
    self.details.x_norms = zeros(max_iterations, num_problems);
    self.details.z_norms = zeros(max_iterations, num_problems);
    self.details.r_primal_norms = zeros(max_iterations, num_problems);
    self.details.r_dual_norms = zeros(max_iterations, num_problems);
    self.details.primal_objectives = zeros(max_iterations, num_problems);
    self.details.dual_objectives = zeros(max_iterations, num_problems);
end % function

function terminate = check_termination(self, x, x_prev, y, z, ...
    r_primal, r_dual, primal_obj, dual_obj, iter, prob)
    terminate = false;
    % norm of primal variable x
    x_norm = norm(x);
    % norm of change in x
    x_diff_norm = norm(x - x_prev);
    % relative change in x
    x_rel_change = x_diff_norm / x_norm;
    % norm of dual variable z
    z_norm = norm(z);
    % primal residual norm
    r_primal_norm  = norm(r_primal);
    % dual residual norm
    r_dual_norm = norm(r_dual);
    % relative change in dual norm
    r_dual_rel_change = r_dual_norm / z_norm;
    % gap in objectives
    gap = abs(primal_obj  - dual_obj);
    relative_gap = gap / abs(primal_obj);

    self.details.x_norms(iter, prob) = x_norm;
    self.details.z_norms(iter, prob) = z_norm;
    self.details.r_primal_norms(iter, prob) = r_primal_norm;
    self.details.r_dual_norms(iter, prob) = r_dual_norm;
    self.details.primal_objectives(iter, prob) = primal_obj;
    self.details.dual_objectives(iter, prob) = dual_obj;

    if self.verbose > 1
        fprintf('Objective primal: %.4f, dual: %.4f\n', primal_obj, dual_obj);
    end

    tolerance = self.tolerance;
    if x_rel_change < tolerance
        terminate = true;
    end
    if x_rel_change >= tolerance*1.1
        % we will continue for a while
        return;
    end
    if relative_gap < tolerance
        terminate = true;
    end
    if r_dual_rel_change < tolerance
        terminate = true;
    end
end % function

end % methods

end % class


classdef MatrixVectorizedInverter_2x2 < MatrixVectorizedInverter_Interface

    methods (Access = public)

        function B = computeInverse(obj,A)
            d(1,1,:) = A(2,2,:);
            d(1,2,:) = -A(1,2,:);
            d(2,1,:) = -A(2,1,:);
            d(2,2,:) = A(1,1,:);

            det = obj.computeDeterminant(A);

            B = zeros(size(A));
            for i = 1:2
                for j = 1:2
                    B(i,j,:) = squeeze(d(i,j,:))./det;
                end
            end
        end

        function detA = computeDeterminant(~,A)
            detA = squeeze(A(1,1,:).*A(2,2,:)-A(1,2,:).*A(2,1,:));
        end

    end

end


function [gx] = g_metaToM(x,P,u,inG)
% observation function for meta-learner (k-ToM vs sequence)
% function [gx] = g_metaToM(x,P,u,inF)
% The meta-learner bases her decision (a=1 or a=0) upon her prediction of
% her opponent's next move, given the game payoff table. The specific
% difficulty of the meta-learner is that she does not know with absolute
% certainty whether she faces an intentional agent (k-ToM) or an inanimate
% patter (sequence). In fact, she holds a belief about this, which is
% quantified in terms of the probability Pi that she is facing an
% intentional agent (the probability that the agent faces an inanimate
% pattern is 1-Pi).
% IN:
%   - x: hidden states (see indexing in inF.indlev)
%   - P: observation params: phi(1) = log-temperature and phi(2) = bias
%   - u: sequence of past actions:
%       u(1)= opponent's last move
%       u(2)= learner's last move
%       u(3:K+2) = sequence of K past opponent's moves
%   - inG: input structure (see prepare_metaToM.m)
% OUT:
%   - gx: updated hidden states
% [see RecToMfunction.m and f_BSL.m]

% 1- get k-ToM probabilistic decision P(a=1|k-ToM)
xktom = x(inG.ktom.indx);
inG_ktom = inG.ktom.inG;
gx_ktom = ObsRecGen(xktom,P,u,inG_ktom);

% 2- get BSL probabilistic decision P(a=1|seq)
xseq = x(inG.seq.indx);
inG_seq = inG.seq.inG;
gx_bsl = g_BSLinGame(xseq,P,u,inG_seq);

% 3- derive meta-ToM probabilistic decision P(a=1)
Pi = VBA_sigmoid(x(inG.meta.indx)); % prior P(agent=kToM)
gx = Pi*gx_ktom + (1-Pi)*gx_bsl;


function mapVals = rmOverlapMap(model, mask, X, Y);
% Produce a single pRF / stimulus overlap mapVals given some low-level inputs.
%
%  mapVals = rmOverlapMap(model, mask, X, Y);
%
% model should be a pRF / RM Model struct (or a path to an RM file).
%
% mask should be a single, 2D, binary stimulus mask, sampled at the
% retinotopic points represented in X and Y.
%
% X and Y can either be the range of x and y points at which the mask was
% sampled, or matrices (e.g. produced my MESHGRID) matching the size of
% mask. Direction conventions: +x is right, -x is left; +y is up, -y is
% down.
%
% Returns a vector of mapVals values representing the proportion of each
% voxel's pRF which is contained within the mask.
%
% Currently implemented only for Volume/Gray views models.
%
% SEE ALSO: rmComputeOverlapMaps.
%
% ras, 11/2007.
if nargin < 4, error('Need all 4 input arguments.');	end
if iscell(model),	model = model{1};		end

if ~isequal( size(mask), size(X) ) | ~isequal( size(mask), size(X) )
	% assume x and y ranges were input
	xRange = unique(X);
	yRange = unique(Y);
	[X Y] = meshgrid(xRange, yRange);
end

% get indices in the mask
I = find(mask);

%% main loop, create pRF for each voxel, convolve w/ mask
% Doing this one voxel at a time takes a LONG time (hours).
% But trying to make all the pRFs at once may cause an
% out-of-memory error. So, we split the difference: do it
% in batches of size n (chosen to work on my ~2005 Windows box;
% adjust as needed).
n = 50;  % compute n pRFs at once
nVoxels = length(model.x0);
verbose = prefsVerboseCheck;
if verbose
	hwait = mrvWaitbar(0, 'Computing pRF Overlap Map...');
end

% go
for v = 1:n:nVoxels
	rng = [0:n-1] + v;  % take this bunch of voxels first
	rng = rng( rng < nVoxels );  % out-of-range check

	% grab params for this batch
	x0 = model.x0(rng);
	y0 = model.y0(rng);
	sigma = model.sigma.major(rng);

	% make pRFs for this batch
	pRFs = rfGaussian2D(X, Y, sigma, sigma, 0, x0, y0);

	% loop across pRFs, convolving each with the mask
	for ii = 1:length(rng)
		mapVals(rng(ii)) = sum(pRFs(I,ii)) ./ sum(pRFs(:,ii));
	end

	if verbose, mrvWaitbar(v/nVoxels, hwait); end
end

if verbose,	close(hwait); end

return


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% cam.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function f = cam(x,y)
% Four-hump camel
function f = cam(x,y)
if nargin == 1
  x1 = x(1);
  x2 = x(2);
else
  x1 = x;
  x2 = y;
end
f=(4-2.1.*x1.^2+x1.^4./3).*x1.^2+x1.*x2+(-4+4.*x2.^2).*x2.^2;


function [engine, loglik] = enter_evidence(engine, evidence, varargin)
% ENTER_EVIDENCE Add the specified evidence to the network (kalman)
% [engine, loglik] = enter_evidence(engine, evidence, ...)
%
% evidence{i,t} = [] if if X(i,t) is hidden, and otherwise contains its observed value (scalar or column vector)
%
% The following optional arguments can be specified in the form of name/value pairs:
% [default value in brackets]
%
% maximize - if 1, does max-product (same as sum-product for Gaussians!), else sum-product [0]
% filter -   if 1, do filtering, else smoothing [0]
%
% e.g., engine = enter_evidence(engine, ev, 'maximize', 1)

maximize = 0;
filter = 0;

% parse optional params
args = varargin;
nargs = length(args);
if nargs > 0
  for i=1:2:nargs
    switch args{i},
     case 'maximize', maximize = args{i+1};
     case 'filter', filter = args{i+1};
     otherwise,
      error(['invalid argument name ' args{i}]);
    end
  end
end

assert(~maximize);

bnet = bnet_from_engine(engine);
n = length(bnet.intra);
onodes = bnet.observed;
hnodes = mysetdiff(1:n, onodes);
T = size(evidence, 2);
ns = bnet.node_sizes;
O = sum(ns(onodes));
data = reshape(cat(1, evidence{onodes,:}), [O T]);

A = engine.trans_mat;
C = engine.obs_mat;
Q = engine.trans_cov;
R = engine.obs_cov;
init_x = engine.init_state;
init_V = engine.init_cov;

if filter
  [x, V, VV, loglik] = kalman_filter(data, A, C, Q, R, init_x, init_V);
else
  [x, V, VV, loglik] = kalman_smoother(data, A, C, Q, R, init_x, init_V);
end

% Wrap the posterior inside a potential, so it can be marginalized easily
engine.one_slice_marginal = cell(1,T);
engine.two_slice_marginal = cell(1,T);
ns(onodes) = 0;
ns(onodes+n) = 0;
ss = length(bnet.intra);
for t=1:T
  dom = (1:n);
  engine.one_slice_marginal{t} = mpot(dom+(t-1)*ss, ns(dom), 1, x(:,t), V(:,:,t));
end
% for t=1:T-1
%   dom = (1:(2*n));
%   mu = [x(:,t); x(:,t)];
%   Sigma = [V(:,:,t) VV(:,:,t+1)';
% 	   VV(:,:,t+1) V(:,:,t+1)];
%   engine.two_slice_marginal{t} = mpot(dom+(t-1)*ss, ns(dom), 1, mu, Sigma);
% end
for t=2:T
  %dom = (1:(2*n));
  current_slice = hnodes;
  next_slice = hnodes + ss;
  dom = [current_slice next_slice];
  mu = [x(:,t-1); x(:,t)];
  Sigma = [V(:,:,t-1) VV(:,:,t)';
	   VV(:,:,t) V(:,:,t)];
  engine.two_slice_marginal{t-1} = mpot(dom+(t-2)*ss, ns(dom), 1, mu, Sigma);
end


function [bankAng,angOfAttack,angOfSideslip] = computeInertialAeroAnglesFromBodyAxes(ut, rVect, vVect, bodyInfo, bodyX, bodyY, bodyZ)
    %Source: http://www.dept.aoe.vt.edu/~cdhall/courses/aoe5204/AircraftMotion.pdf

    [R_wind_2_inert, ~, ~, ~] = computeWindFrame(rVect,vVect);
    Rtotal = horzcat(bodyX, bodyY, bodyZ);

    angles = rotm2eulARH(R_wind_2_inert' * Rtotal, 'zyx');

    angles = real(angles);

    bankAng = angles(3);
	angOfAttack = angles(2);
	angOfSideslip = angles(1);
end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2019.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtBmmAlgebra.m                               |
%|    #    |   VERSION    : 0.61                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 05.09.2019                                    |
%| ( === ) |   SYNOPSIS   : Block matrix algebra                          |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Library path
run('../../addpathGypsilab')

% Dimensions
N = 200;

% Accuracy
tol = 1e-3;

% Particles receptors X
mesh  = mshSphere(N,1);
omega = dom(mesh,3);
phi0  = fem(mesh,'P0');
phi1  = fem(mesh,'P1');

% Wave number or frequency (Hz)
k = 5;

% Green kernel -> exp(1i*k*r)/r
green = '[exp(ikr)/r]';
Gxy   = @(X,Y) femGreenKernel(X,Y,green,k) + ...
    sqrt(N).*(X(:,1)==Y(:,1)).*(X(:,2)==Y(:,2)).*(X(:,3)==Y(:,3))  ;

% Particles charges (multiples)
V0 = (-1+2*rand(length(phi0),2)) + (-1+2i*rand(length(phi0),2));
V1 = (-1+2*rand(length(phi1),2)) + (-1+2i*rand(length(phi1),2));

% Spatial representation of particles
figure
plot(mesh)
axis equal

% All forms
tic
Ah = integral(omega,omega,phi0,Gxy,phi0,tol);
Bv = integral(omega,omega,phi0,green,k,phi1,tol);
Cs = integral(omega,phi1,phi0);
Df = integral(omega,omega,phi1,Gxy,phi1);
toc

%%% Single Builder
disp('~~~~~~~~~~~~~ SINGLE BUILDERS ~~~~~~~~~~~~~')
Mb  = bmm(Ah);
sol = Mb * V0;
ref = Ah * V0;
norm(ref-sol,'inf')/norm(ref,'inf')

Mb  = bmm(Bv);
sol = Mb * V1;
ref = Bv * V1;
norm(ref-sol,'inf')/norm(ref,'inf')

Mb  = bmm(Cs);
sol = Mb * V0;
ref = Cs * V0;
norm(ref-sol,'inf')/norm(ref,'inf')

Mb  = bmm(Df);
sol = Mb * V1;
ref = Df * V1;
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Multi Builder
disp('~~~~~~~~~~~~~ MULTI BUILDERS ~~~~~~~~~~~~~')
Mb  = bmm({Ah,Cs';Cs,Df});
Mr  = [full(Ah) Cs' ; Cs Df];
Vb  = [V0;V1];
sol = Mb * Vb;
ref = Mr * Vb;
norm(ref-sol,'inf')/norm(ref,'inf')

figure
spy(Mb)

disp(' ')

%%% Full conversion
disp('~~~~~~~~~~~~~ FULL CONVERSION ~~~~~~~~~~~~~')
sol = full(Mb) * Vb;
ref = Mr * Vb;
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Sparse conversion
disp('~~~~~~~~~~~~~ SPARSE CONVERSION ~~~~~~~~~~~~~')
sol = sparse(Mb) * Vb;
ref = Mr * Vb;
norm(ref-sol,'inf')/norm(ref,'inf')

figure
spy(sparse(Mb))

disp(' ')

%%% Transposition
disp('~~~~~~~~~~~~~ TRANSPOSITION ~~~~~~~~~~~~~')
sol = Vb.' * Mb.';
ref = (Mr * Vb).';
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Transposition
disp('~~~~~~~~~~~~~ CONJUGATE TRANSPOSITION ~~~~~~~~~~~~~')
sol = Vb' * Mb';
ref = (Mr * Vb)';
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Concatenation
disp('~~~~~~~~~~~~~ CONCATENATION ~~~~~~~~~~~~~')
Ab  = Mb;
Bb  = bmm({Bv;Df});
Cb  = bmm({Ah,Bv});
Db  = bmm(Cs');
Mb2 = [Ab , Bb ; Cb , Db];
sol = Mb2 * [Vb;V1];
ref = [Ab*Vb + Bb*V1;Cb*Vb+Db*V1];
norm(ref-sol,'inf')/norm(ref,'inf')

spy(Mb2)

disp(' ')

%%% Scalar product
disp('~~~~~~~~~~~~~ SCALAR PRODUCT ~~~~~~~~~~~~~')
sol = (-(3.*Mb.*2i)) * Vb;
ref = (-3*2i) * (Mr*Vb);
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Addition
disp('~~~~~~~~~~~~~ ADDITION ~~~~~~~~~~~~~')
sol = (Mb+Mb) * Vb;
ref = 2*(Mr*Vb);
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Multiplication
disp('~~~~~~~~~~~~~ MULTIPLICATION ~~~~~~~~~~~~~')
sol = (Mb.'*Mb) * Vb;
ref = Mr.'*(Mr*Vb);
norm(ref-sol,'inf')/norm(ref,'inf')

figure
spy(Mb.'*Mb)

disp(' ')

%%% LU factorization
disp('~~~~~~~~~~~~~ LU FACTORISATION ~~~~~~~~~~~~~')
[Lb,Ub] = lu(Mb);
sol = Lb * (Ub * Vb);
ref = Mb * Vb;
norm(ref-sol,'inf')/norm(ref,'inf')

figure
subplot(1,2,1); spy(Lb)
subplot(1,2,2); spy(Ub)

disp(' ')

%%% Solve LU
disp('~~~~~~~~~~~~~ SOLVE LU SYSTEM ~~~~~~~~~~~~~')
sol = Mb \ Vb;
ref = Mr \ Vb;
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Inversion
disp('~~~~~~~~~~~~~ INVERSION ~~~~~~~~~~~~~')
Mbm1 = inv(Mb);
sol  = Mbm1 * Vb;
ref  = Mr \ Vb;
norm(ref-sol,'inf')/norm(ref,'inf')

figure
spy(Mbm1)

disp(' ')

disp('~~> Michto gypsilab !')


function route = perturb(route_old, method)
% PERTURB
% route = PERTURB(route_old, method) generate randomly a neighbouring route by
% perturb old route. perturb methods:
%                        ___________            ___________
%     1. reverse:   [1 2 3 4 5 6 7 8 9] -> [1 2 8 7 6 5 4 3 9]
%                        _         _            _         _
%     2. swap:      [1 2 3 4 5 6 7 8 9] -> [1 2 8 4 5 6 7 3 9]

route = route_old;
numbercities = length(route);
city1 = ceil(numbercities*rand);
city2 = ceil(numbercities*rand);
switch method
    case 'reverse'
        citymin = min(city1,city2);
        citymax = max(city1,city2);
        route(citymin:citymax) = route(citymax:-1:citymin);
    case 'swap'
        route([city1, city2]) = route([city2, city1]);
end


function triangulation_test33 ( )

%*****************************************************************************80
%
%% TEST33 tests VORONOI_POLYGON_VERTICES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 August 2006
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;
  neighbor_num = 4;
  node_num = 5;

  center = 5;
  neighbor_index = [ 1, 2, 3, 4 ];
  node_xy = [ ...
    0.0, 0.0; ...
    1.0, 0.0; ...
    1.0, 1.0; ...
    0.0, 1.0; ...
    0.5, 0.5 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST33\n' );
  fprintf ( 1, '  VORONOI_POLYGON_VERTICES computes the vertices of\n' );
  fprintf ( 1, '  a finite Voronoi polygon.\n' );

  v = voronoi_polygon_vertices ( center, neighbor_num, neighbor_index, ...
    node_num, node_xy );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Voronoi Polygon Vertex coordinates:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : neighbor_num
    fprintf ( 1, '  %4d  %14f  %14f\n', i, v(1:2,i) );
  end

  return
end


% Normalizes all of the field components so that the mode has
% unity power (Poynting vector integrated over the cross section.)

function [ex,ey,ez,hx,hy,hz] = normalize(dx,dy,EX,EY,EZ,HX,HY,HZ)

Z0 = 119.9169832*pi; % vacuum impedance

[nx,ny] = size(EX);

if (length(dx) ~= nx),
  dx = dx*ones(nx,1);
end
if (length(dy) ~= ny),
  dy = dy*ones(1,ny);
end

ii = zeros(nx+1,ny+1);
ii(:) = (1:(nx+1)*(ny+1));

i1 = ii(1:nx,2:ny+1);
i2 = ii(1:nx,1:ny);
i3 = ii(2:nx+1,1:ny);
i4 = ii(2:nx+1,2:ny+1);

HXp = (HX(i1) + HX(i2)+ HX(i3) + HX(i4))/4;
HYp = (HY(i1) + HY(i2)+ HY(i3) + HY(i4))/4;

SZ = Z0*(conj(EX).*HYp - conj(EY).*HXp + EX.*conj(HYp) - EY.*conj(HXp))/4;
dA = dx*dy;
N = sqrt(sum(SZ(:).*dA(:)));
ex = Z0*EX/N;
ey = Z0*EY/N;
ez = Z0*EZ/N;
hx = HX/N;
hy = HY/N;
hz = HZ/N;


function sd_estimate = sdest3(x)
 wc=dwt3(x,'sym4','mode','sym');
s=wc.dec{2,2,2};
sd_estimate = mad(s(:),1)/0.6745;


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: compact version of matrix-free regularization
%
%  S(y) = alpha/2 * norm(B*(y-yRef))^2,
%
% where
%  alpha regularization parameter, weights regularization versus
%        distance in the joint objective function, alpha = 1 here
%  yRef  is a reference configuration, e.g. yRef = x or a
%        pre-registration result
%  B     a discrete partial differential operator either in explicit
%        matrix form or as a structure containing the necessary
%        parameters to compute B*y
% see also regularizer E8_regularization_MB
%==============================================================================

clear, close all, help(mfilename);

% initialize the regularization and create a starting  point
regularizer('reset','regularizer','mfElastic','alpha',1,'mu',1,'lambda',0);
y0 = @(omega,m) randn(size(getStaggeredGrid(omega,m)));

% 2D example, initialize physical domain and number of discretization points
omega = [0,1,0,1]; m = [16,12];   %

% test derivative of 2D implementation
fctn = @(yc) regularizer(yc,omega,m);  checkDerivative(fctn,y0(omega,m));

% 3D example, initialize physical domain and number of discretization points
omega = [0,1,0,1,0,1]; m  = [16,12,8];

% test derivative of 3D implementation
fctn = @(yc) regularizer(yc,omega,m);
checkDerivative(fctn,y0(omega,m));

%==============================================================================


function H = displ_field_gradient_Nd(D, pix_resolution)
    Nd = size(D, 4);
    H = zeros([Nd,Nd, size(D, 1),size(D, 2), size(D, 3)]);
    for i = 1 : Nd
%         GS = cell(2,1);
        GS = {};
        if Nd == 2
            [GS{1}, GS{2}] = my_gradient( squeeze(D(:,:, 1, i)), pix_resolution, 0, 2);
        elseif Nd == 3
            [GS{1}, GS{2}, GS{3}] = my_gradient( squeeze(D(:,:,:, i)), pix_resolution, 0, 2);
        end

        for j = 1 : Nd
%             tmpD = imgaussfilt(D(:,:,i), 1.5);
%             tmpD = imgaussfilt(D(:,:,i), 1.9);
%             tmpD = D(:,:,i);
%             G = DGradient(tmpD, [], j) / pix_resolution(j);
              G = GS{j};
%             G = medfilt2(G, [3,3]);
%             G = imgaussfilt(G, 1.5);
            H(i, j, :, :, :) = G;
        end
    end
end


function spm_dcm_local_minima(DCM)
% evaluates the free energy landscape around the posterior
% FORMAT: spm_dcm_local_minima(DCM)
% DCM - (invert) model structure
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_dcm_local_minima.m 5892 2014-02-23 11:00:16Z karl $

% find dimension of greatest curvature
%==========================================================================
U  = spm_svd(DCM.Cp);
U  = U(:,end);
Cu = U'*DCM.Cp*U;
Su = spm_sqrtm(Cu);

% Free energy landscape
%==========================================================================
DCM.options.DATA = 0;
DCM.M.Nmax = 1;
DCM.name   = 'test';

N = 128;
s = linspace(-8,8,N);
for i = 1:N

    Ep           = spm_vec(DCM.Ep);
    Ep           = Ep + s(i)*U*Su*(U'*Ep);
    DCM.M.P      = spm_unvec(Ep,DCM.Ep);
    try
        [Qp,Cp,Eh,F] = spm_nlsi_GN(DCM.M,DCM.xU,DCM.xY);
        FF(i)        = F;
    catch
        FF(i)    = NaN;
    end
end

plot(s,FF)
title('Free energy','FontSize',16);
xlabel('parameter 1')
ylabel('parameter 2')
axis square


function nextpop = extractPop(opt, combinepop)
% Function: nextpop = extractPop(opt, combinepop)
% Description: Extract the best n individuals in 'combinepop'(population
%   size is 2n).
%
%         LSSSSWC, NWPU
%    Revision: 1.1  Data: 2011-07-12
%*************************************************************************

popsize = length(combinepop) / 2;
nextpop = combinepop(1:popsize);    %just for initializing

rankVector = vertcat(combinepop.rank);

n = 0;          % individuals number of next population
rank = 1;       % current rank number
idx = find(rankVector == rank);
numInd = length(idx);       % number of individuals in current front
while( n + numInd <= popsize )
    nextpop( n+1 : n+numInd ) = combinepop( idx );

    n = n + numInd;
    rank = rank + 1;

    idx = find(rankVector == rank);
    numInd = length(idx);
end

% If the number of individuals in the next front plus the number of individuals
% in the current front is greater than the population size, then select the
% best individuals by corwding distance(NSGA-II) or preference distance(R-NSGA-II).
if( n < popsize )
    if(~isempty(opt.refPoints))
        prefDistance = vertcat(combinepop(idx).prefDistance);
        prefDistance = [prefDistance, idx];
        prefDistance = sortrows( prefDistance, 1);
        idxSelect  = prefDistance( 1:popsize-n, 2);       % Select the individuals with smallest preference distance
        nextpop(n+1 : popsize) = combinepop(idxSelect);
    else
        distance = vertcat(combinepop(idx).distance);
        distance = [distance, idx];
        distance = flipud( sortrows( distance, 1) );      % Sort the individuals in descending order of crowding distance in the front.
        idxSelect  = distance( 1:popsize-n, 2);           % Select the (popsize-n) individuals with largest crowding distance.
        nextpop(n+1 : popsize) = combinepop(idxSelect);
    end
end


function gd=gabdual(g,a,M,varargin)
%GABDUAL  Canonical dual window of Gabor frame
%   Usage:  gd=gabdual(g,a,M);
%           gd=gabdual(g,a,M,L);
%           gd=gabdual(g,a,M,'lt',lt);
%
%   Input parameters:
%         g     : Gabor window.
%         a     : Length of time shift.
%         M     : Number of channels.
%         L     : Length of window. (optional)
%         lt    : Lattice type (for non-separable lattices).
%   Output parameters:
%         gd : Canonical dual window.
%
%   `gabdual(g,a,M)` computes the canonical dual window of the discrete Gabor
%   frame with window *g* and parameters *a*, *M*.
%
%   The window *g* may be a vector of numerical values, a text string or a
%   cell array. See the help of |gabwin| for more details.
%
%   If the length of *g* is equal to *M*, then the input window is assumed
%   to be an FIR window. In this case, the canonical dual window also has
%   length of *M*. Otherwise the smallest possible transform length is chosen
%   as the window length.
%
%   `gabdual(g,a,M,L)` returns a window that is the dual window for a system
%   of length *L*. Unless the dual window is a FIR window, the dual window
%   will have length *L*.
%
%   `gabdual(g,a,M,'lt',lt)` does the same for a non-separable lattice
%   specified by *lt*. Please see the help of |matrix2latticetype| for a
%   precise description of the parameter *lt*.
%
%   If $a>M$ then the dual window of the Gabor Riesz sequence with window
%   *g* and parameters *a* and *M* will be calculated.
%
%   Examples:
%   ---------
%
%   The following example shows the canonical dual window of the Gaussian
%   window:::
%
%     a=20;
%     M=30;
%     L=300;
%     g=pgauss(L,a*M/L);
%     gd=gabdual(g,a,M);
%
%     % Simple plot in the time-domain
%     figure(1);
%     plot(gd);
%
%     % Frequency domain
%     figure(2);
%     magresp(gd,'dynrange',100);
%
%   See also:  gabtight, gabwin, fir2long, dgt

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: TEST_DGT
%   REFERENCE: REF_GABDUAL.

%% ---------- Assert correct input.

if nargin<3
  error('%s: Too few input parameters.',upper(mfilename));
end;

definput.keyvals.L=[];
definput.keyvals.lt=[0 1];
definput.keyvals.nsalg=0;
[flags,kv,L]=ltfatarghelper({'L'},definput,varargin);

%% ------ step 2: Verify a, M and L
if isempty(L)
    if isnumeric(g)
        % Use the window length
        Ls=length(g);
    else
        % Use the smallest possible length
        Ls=1;
    end;

    % ----- step 2b : Verify a, M and get L from the window length ----------
    L=dgtlength(Ls,a,M,kv.lt);

else

    % ----- step 2a : Verify a, M and get L

    Luser=dgtlength(L,a,M,kv.lt);
    if Luser~=L
        error(['%s: Incorrect transform length L=%i specified. Next valid length ' ...
               'is L=%i. See the help of DGTLENGTH for the requirements.'],...
              upper(mfilename),L,Luser);
    end;

end;

%% ----- step 3 : Determine the window

[g,info]=gabwin(g,a,M,L,kv.lt,'callfun',upper(mfilename));

if L<info.gl
  error('%s: Window is too long.',upper(mfilename));
end;

R=size(g,2);
% -------- Are we in the Riesz sequence of in the frame case

scale=1;
if a>M*R
  % Handle the Riesz basis (dual lattice) case.
  % Swap a and M, and scale differently.
  scale=a/M;
  tmp=a;
  a=M;
  M=tmp;
end;

% -------- Compute -------------

if kv.lt(2)==1
    % Rectangular case
    if (info.gl<=M) && (R==1)

        % Diagonal of the frame operator
        d = gabframediag(g,a,M,L);
        gd=g./long2fir(d,info.gl);

    else

        % Long window case

        % Just in case, otherwise the call is harmless.
        g=fir2long(g,L);

        gd=comp_gabdual_long(g,a,M)*scale;

    end;

else
    % Non-separable case
    g=fir2long(g,L);

    if (kv.nsalg==1) || (kv.nsalg==0 && kv.lt(2)<=2)

        mwin=comp_nonsepwin2multi(g,a,M,kv.lt,L);

        gdfull=comp_gabdual_long(mwin,a*kv.lt(2),M)*scale;

        % We need just the first vector
        gd=gdfull(:,1);

    else

        [s0,s1,br] = shearfind(L,a,M,kv.lt);

        if s1 ~= 0
            p1 = comp_pchirp(L,s1);
            g = p1.*g;
        end

        b=L/M;
        Mr = L/br;
        ar = a*b/br;

        if s0 == 0
            gd=comp_gabdual_long(g,ar,Mr);
        else
            p0=comp_pchirp(L,-s0);
            g = p0.*fft(g);
            gd=comp_gabdual_long(g,L/Mr,L/ar)*L;
            gd = ifft(conj(p0).*gd);
        end

        if s1 ~= 0
            gd = conj(p1).*gd;
        end

    end;

    if (info.gl<=M) && (R==1)
        gd=long2fir(gd,M);
    end;

end;

% --------- post process result -------

if isreal(g) && (kv.lt(2)==1 || kv.lt(2)==2)
  % If g is real and the lattice is either rectangular or quinqux, then
  % the output is known to be real.
  gd=real(gd);
end;

if info.wasrow
  gd=gd.';
end;


function [kW] = MW2kW(MW)
% Convert power from megawatts to kilowatts.
% Chad A. Greene 2012
kW = MW*1000;


function sparse_grid_composite_test ( )

%*****************************************************************************80
%
%% SPARSE_GRID_COMPOSITE_TEST tests the SPARSE_GRID_COMPOSITE library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 August 2009
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_GRID_COMPOSITE_TEST\n' );
  fprintf ( 1, '  MATLAB version.\n' );
  fprintf ( 1, '  Test the SPARSE_GRID_COMPOSITE library.\n' );
%
%  Count number of points in sparse rule from DIM_MIN to DIM_MAX, LEVEL_MAX_MAX.
%
  sparse_grid_composite_test01 ( 1, 6, 0, 5 );
  sparse_grid_composite_test01 ( 6, 10, 0, 4 );
  sparse_grid_composite_test01 ( 100, 100, 0, 1 );
%
%  Compute abstract grid indices of sparse grid points as selected from
%  product grid for DIMENSION, LEVEL_MAX.
%
  dim_num = 2;
  level_max = 3;
  sparse_grid_composite_test02 ( dim_num, level_max );

  dim_num = 2;
  level_max = 4;
  sparse_grid_composite_test02 ( dim_num, level_max );

  dim_num = 3;
  level_max = 0;
  sparse_grid_composite_test02 ( dim_num, level_max );

  dim_num = 3;
  level_max = 2;
  sparse_grid_composite_test02 ( dim_num, level_max );

  dim_num = 6;
  level_max = 2;
  sparse_grid_composite_test02 ( dim_num, level_max );
%
%  Compute sparse composite rule for DIMENSION, LEVEL_MAX.
%
  dim_num = 2;
  level_max = 3;
  sparse_grid_composite_test03 ( dim_num, level_max );

  dim_num = 3;
  level_max = 0;
  sparse_grid_composite_test03 ( dim_num, level_max );

  dim_num = 3;
  level_max = 1;
  sparse_grid_composite_test03 ( dim_num, level_max );
%
%  Test sum of weights for DIMENSION, LEVEL_MAX.
%
  sparse_grid_composite_test04 ( 2, 4 );
  sparse_grid_composite_test04 ( 3, 0 );
  sparse_grid_composite_test04 ( 3, 1 );
  sparse_grid_composite_test04 ( 3, 6 );
  sparse_grid_composite_test04 ( 10, 3 );
%
%  Test monomial exactness for DIMENSION, LEVEL_MAX, DEGREE_MAX.
%
  sparse_grid_composite_test05 ( 2, 0, 2 );
  sparse_grid_composite_test05 ( 2, 1, 2 );
  sparse_grid_composite_test05 ( 2, 2, 2 );
  sparse_grid_composite_test05 ( 2, 3, 2 );
  sparse_grid_composite_test05 ( 2, 4, 2 );
  sparse_grid_composite_test05 ( 2, 5, 2 );

  sparse_grid_composite_test05 ( 3, 0, 2 );
  sparse_grid_composite_test05 ( 3, 1, 2 );
  sparse_grid_composite_test05 ( 3, 2, 2 );
  sparse_grid_composite_test05 ( 3, 3, 2 );
%
%  Show how to write a rule to a file.
%
  dim_num = 2;
  level_max = 3;
  sparse_grid_composite_test06 ( dim_num, level_max );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_GRID_COMPOSITE_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function [ y, m, d, ierror ] = ymd_check_islamic ( y, m, d )

%*****************************************************************************80
%
%% YMD_CHECK_ISLAMIC checks an Islamic YMD date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, M, D, the YMD date, which may
%    be corrected if necessary and possible.
%
%    Output, integer IERROR, is 0 if the date is legal.
%

%
%  Check the year.
%
  if ( y <= 0 )
    ierror = 1;
    return
  end
%
%  Check the month.
%
  [ y, m, ierror ] = ym_check_islamic ( y, m );

  if ( ierror ~= 0 )
    return
  end
%
%  Check the day.
%
  [ y, m, d ] = day_borrow_islamic ( y, m, d );

  [ y, m, d ] = day_carry_islamic ( y, m, d );

  return
end


function B = rowFirstNonZero(A)
%DESCRIPTION: get first non zero element of each column of a matrix
[~, c] = max( A ~=0, [], 2 );
d = (c-1)*size(A,2) +[1:size(A,1)]';
B=A(d);
end