function [p,ellipse]=phantom3dAniso(varargin)

%PHANTOM3D Three-dimensional analogue of MATLAB Shepp-Logan phantom
%   P = PHANTOM3D(DEF,N) generates a 3D head phantom that can
%   be used to test 3-D reconstruction algorithms.
%
%   DEF is a string that specifies the type of head phantom to generate.
%   Valid values are:
%
%      'Shepp-Logan'            A test image used widely by researchers in
%                               tomography
%      'Modified Shepp-Logan'   (default) A variant of the Shepp-Logan phantom
%                               in which the contrast is improved for better
%                               visual perception.
%      'yu-ye-wang'             Another version of the modified Shepp-Logan
%                               phantom from "Katsevich-Type Algorithms for
%                               Variable Radius Spiral Cone-BeamCT"
%
%   N specifies the 3D grid size of P
%   If N is a scalar, P will have isotropic size [N, N, N]
%   If N is a 3-vector, P will have size [N(1) N(2) N(3)]
%   If you omit the argument, N defaults to [64 64 64].
%
%   P = PHANTOM3D(E,N) generates a user-defined phantom, where each row
%   of the matrix E specifies an ellipsoid in the image.  E has ten columns,
%   with each column containing a different parameter for the ellipsoids:
%
%     Column 1:  A      the additive intensity value of the ellipsoid
%     Column 2:  a      the length of the x semi-axis of the ellipsoid
%     Column 3:  b      the length of the y semi-axis of the ellipsoid
%     Column 4:  c      the length of the z semi-axis of the ellipsoid
%     Column 5:  x0     the x-coordinate of the center of the ellipsoid
%     Column 6:  y0     the y-coordinate of the center of the ellipsoid
%     Column 7:  z0     the z-coordinate of the center of the ellipsoid
%     Column 8:  phi    phi Euler angle (in degrees) (rotation about z-axis)
%     Column 9:  theta  theta Euler angle (in degrees) (rotation about x-axis)
%     Column 10: psi    psi Euler angle (in degrees) (rotation about z-axis)
%
%   For purposes of generating the phantom, the domains for the x-, y-, and
%   z-axes span [-1,1].  Columns 2 through 7 must be specified in terms
%   of this range.
%
%   [P,E] = PHANTOM3D(...) returns the matrix E used to generate the phantom.
%
%   Class Support
%   -------------
%   All inputs must be of class double.  All outputs are of class double.
%
%   Remarks
%   -------
%   For any given voxel in the output image, the voxel's value is equal to the
%   sum of the additive intensity values of all ellipsoids that the voxel is a
%   part of.  If a voxel is not part of any ellipsoid, its value is 0.
%
%   The additive intensity value A for an ellipsoid can be positive or negative;
%   if it is negative, the ellipsoid will be darker than the surrounding pixels.
%   Note that, depending on the values of A, some voxels may have values outside
%   the range [0,1].
%
%   Example
%   -------
%        ph = phantom3d(128);
%        figure, imshow(squeeze(ph(64,:,:)))
%
%   Copyright 2005 Matthias Christian Schabel (matthias @ stanfordalumni . org)
%   University of Utah Department of Radiology
%   Utah Center for Advanced Imaging Research
%   729 Arapeen Drive
%   Salt Lake City, UT 84108-1218
%
%   This code is released under the Gnu Public License (GPL). For more information,
%   see : http://www.gnu.org/copyleft/gpl.html
%
%   Portions of this code are based on phantom.m, copyrighted by the Mathworks
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Modification May 25, 2015, by Patrick J. Bolan, University of Minnesota
% Added support for anisotropic phantom sizes: the phantom size can now be
% a vector.
%

[ellipse,n] = parse_inputs(varargin{:});

nx = n(1); ny = n(2); nz = n(3);
p = zeros([nx ny nz]);

rngx =  ( (0:nx-1)-(nx-1)/2 ) / ((nx-1)/2);
rngy =  ( (0:ny-1)-(ny-1)/2 ) / ((ny-1)/2);
rngz =  ( (0:nz-1)-(nz-1)/2 ) / ((nz-1)/2);

% PJB: Note the swap of the x and y with meshgrid parameters.
%[x,y,z] = meshgrid(rngx,rngy,rngz);
[x,y,z] = meshgrid(rngy,rngx,rngz);
x=single(x);y=single(y);z=single(z);
coord = [flatten(single(x)); flatten(single(y)); flatten(single(z))];
clear x y z;
p = flatten(p);

for k = 1:size(ellipse,1)
   A = ellipse(k,1);            % Amplitude change for this ellipsoid
   asq = ellipse(k,2)^2;        % a^2
   bsq = ellipse(k,3)^2;        % b^2
   csq = ellipse(k,4)^2;        % c^2
   x0 = ellipse(k,5);           % x offset
   y0 = ellipse(k,6);           % y offset
   z0 = ellipse(k,7);           % z offset
   phi = ellipse(k,8)*pi/180;   % first Euler angle in radians
   theta = ellipse(k,9)*pi/180; % second Euler angle in radians
   psi = ellipse(k,10)*pi/180;  % third Euler angle in radians

   cphi = cos(phi);
   sphi = sin(phi);
   ctheta = cos(theta);
   stheta = sin(theta);
   cpsi = cos(psi);
   spsi = sin(psi);

   % Euler rotation matrix
   alpha = [cpsi*cphi-ctheta*sphi*spsi   cpsi*sphi+ctheta*cphi*spsi  spsi*stheta;
            -spsi*cphi-ctheta*sphi*cpsi  -spsi*sphi+ctheta*cphi*cpsi cpsi*stheta;
            stheta*sphi                  -stheta*cphi                ctheta];

   % rotated ellipsoid coordinates
   coordp = alpha*coord;

   idx = find((coordp(1,:)-x0).^2./asq + (coordp(2,:)-y0).^2./bsq + (coordp(3,:)-z0).^2./csq <= 1);
   p(idx) = p(idx) + A;
end

%p = reshape(p,[nx ny nz]);
p = reshape(p, [nx ny nz]);

return;

function out = flatten(in)

out = reshape(in,[1 numel(in)]);

return;

function [e,n] = parse_inputs(varargin)
%  e is the m-by-10 array which defines ellipsoids
%  n is a 3-vector with the size of the phantom brain image, [nx ny nz]

n = [64 64 64];     % The default size
e = [];
defaults = {'shepp-logan', 'modified shepp-logan', 'yu-ye-wang'};

for i=1:nargin
   if ischar(varargin{i})         % Look for a default phantom
      def = varargin{i};
      idx = strcmpi(def, defaults);
      if isempty(idx)
         eid = sprintf('Images:%s:unknownPhantom',mfilename);
         msg = 'Unknown default phantom selected.';
         error(eid,'%s',msg);
      end
      switch defaults{idx}
      case 'shepp-logan'
         e = shepp_logan;
      case 'modified shepp-logan'
         e = modified_shepp_logan;
      case 'yu-ye-wang'
         e = yu_ye_wang;
      end
   elseif numel(varargin{i})==1
      n = [varargin{i} varargin{i} varargin{i}];   % a scalar is the image size
   elseif numel(varargin{i})==3
      siz = varargin{i};
      n = [siz(1) siz(2) siz(3)]; % 3 integers specify image dimensions
   elseif ndims(varargin{i})==2 && size(varargin{i},2)==10
      e = varargin{i};            % user specified phantom
   else
      eid = sprintf('Images:%s:invalidInputArgs',mfilename);
      msg = 'Invalid input arguments.';
      error(eid,'%s',msg);
   end
end

% ellipse is not yet defined
if isempty(e)
   e = modified_shepp_logan;
end

return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Default head phantoms:   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function e = shepp_logan

e = modified_shepp_logan;
e(:,1) = [1 -.98 -.02 -.02 .01 .01 .01 .01 .01 .01];

return;

function e = modified_shepp_logan
%
%   This head phantom is the same as the Shepp-Logan except
%   the intensities are changed to yield higher contrast in
%   the image.  Taken from Toft, 199-200.
%
%         A      a     b     c     x0      y0      z0    phi  theta    psi
%        -----------------------------------------------------------------
e =    [  1  .6900  .920  .810      0       0       0      0      0      0
        -.8  .6624  .874  .780      0  -.0184       0      0      0      0
        -.2  .1100  .310  .220    .22       0       0    -18      0     10
        -.2  .1600  .410  .280   -.22       0       0     18      0     10
         .1  .2100  .250  .410      0     .35    -.15      0      0      0
         .1  .0460  .046  .050      0      .1     .25      0      0      0
         .1  .0460  .046  .050      0     -.1     .25      0      0      0
         .1  .0460  .023  .050   -.08   -.605       0      0      0      0
         .1  .0230  .023  .020      0   -.606       0      0      0      0
         .1  .0230  .046  .020    .06   -.605       0      0      0      0 ];

return;

function e = yu_ye_wang
%
%   Yu H, Ye Y, Wang G, Katsevich-Type Algorithms for Variable Radius Spiral Cone-Beam CT
%
%         A      a     b     c     x0      y0      z0    phi  theta    psi
%        -----------------------------------------------------------------
e =    [  1  .6900  .920  .900      0       0       0      0      0      0
        -.8  .6624  .874  .880      0       0       0      0      0      0
        -.2  .4100  .160  .210   -.22       0    -.25    108      0      0
        -.2  .3100  .110  .220    .22       0    -.25     72      0      0
         .2  .2100  .250  .500      0     .35    -.25      0      0      0
         .2  .0460  .046  .046      0      .1    -.25      0      0      0
         .1  .0460  .023  .020   -.08    -.65    -.25      0      0      0
         .1  .0460  .023  .020    .06    -.65    -.25     90      0      0
         .2  .0560  .040  .100    .06   -.105    .625     90      0      0
        -.2  .0560  .056  .100      0    .100    .625      0      0      0 ];

return;


function x = reciprocal_cdf_inv ( cdf, a, b )

%*****************************************************************************80
%
%% RECIPROCAL_CDF_INV inverts the Reciprocal CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < A <= B.
%
%    Output, real X, the corresponding argument of the CDF.
%
  if ( cdf <= 0.0 )
    x = 0.0;
  elseif ( 0.0 < cdf )
    x = b^cdf / a^( cdf - 1.0 );
  end

  return
end


function [final_sound] = im2sound(filename, ext, f_sample, f_low, ...
    f_high, amp_mod, sample_t)

%INPUTS:
%'filename' - Name of the image to be encoded (not including extension
%ext' - Extension of the image (not including "." at the beginning).
%'f_sample' - Sampling frequency (Hz)
%'f_low' - Lowest frequency (Hz) (e.g. 40)
%'f_high' - Highest frequency (Hz) (e.g. 6000)
%'amp_mod' - Multiplication factor for the amplitude.  Decrease until
%image is clear. Too high and the waveform clips.  Too low and the image
%is very dark (e.g. 0.00002)
%'sample_t' - Duration of the sample in seconds.  Longer samples have
%better quality (e.g. 10)

%OUTPUTS:
%'final_sound' - the final sound containing the image.  This is
%automatically saved to a .wav file with the original image filename

%INITIALISING VARIABLES:
%The waveform at each time point.  This is reset at the beginning of each
%time point
temp_sound = 0;
%The final waveform
final_sound = 0;

%MAIN BODY
%Loading the sample image and calculating the image size
raw_im = imread(strcat(filename,'.',ext));
size_raw_im = size(raw_im);

%Making a frequency table for the height of the image.  Each row of the
%image is assigned a particular frequency from the corresponding row of
%this table.  The frequencies are linearly distributed between the highest and
%lowest user-definied frequencies.  "f_step" is the increment between each
%adjacent frequency
f_step = (f_high - f_low)/size_raw_im(1,1);
f_table = (f_high:-f_step:f_low);

%The final sound will dwell on each column of the image for a specific
%time.  This time is defined by "t_start" and "t_end".  It depends on how
%long the user determined the sound-clip should be and how wide (how many
%columns) the image is.
t_step = (sample_t/size_raw_im(1,2));

%Initial values for the start and end times.  These will be increased at
%the end of each loop iteration (when the script moves onto the next column
%of the image).
t_start = 0;
t_end = t_step;

%The loop which generates the sound file.  At each iteration it generates a
%segment of the final sound file, which is temporarily saved to
%"temp_sound".  This segment is built up of frequencies from that
%particular column of the image.
for j = 1:size_raw_im(1,2)
    %Initialising the variable (the sound for each frequency (row) is added
    %to the existing sound)
    temp_sound = 0;

    %Setting the time in matrix format
    t = t_start:1/f_sample:(t_end);

    %For each iteration of this loop, the script goes down the current
    %column of the image and generates a waveform of the frequency
    %specified in "f_table".  The amplitude of the waveform is determined
    %by the pixel intensity.  This generated waveform is added to all the
    %previously generated waveforms in that particular column
    for i = size_raw_im(1,1):-1:1
        temp_sound = temp_sound+ sin(2*pi*t*f_table(i))*...
            double(raw_im(i,j))*amp_mod;

    end

    %At the end of each column the segment of sound generated is added to
    %the end of the existing sound file ("final_sound").
    final_sound = cat(2,final_sound,temp_sound);

    %The temporary sound is cleared ready for the start of the next column
    clear temp_sound

    %Moving to the next time frame
    t_start = t_start + t_step;
    t_end = t_end + t_step;

end

%This saves "final_sound" to the '.wav' file of the same name as the input
%file
wavwrite(final_sound, f_sample, strcat(filename, '.wav'));


function [ gx ] = ObsRecGen(x,P,u,in)
% observation function for k-ToM's bet about her opponent's next move
% [ gx ] = ObsRecGen(x,P,u,in)
% Marie Devaine wrote this function in November 2015 (comments: JD).
% A k-ToM learner bases her decision (a=1 or a=0) upon her prediction of
% her opponent's next move, given the game payoff table. When k>1, k-ToM
% maintains more than one such prediction, which depends upon the possible
% level of her opponent. Let P(o=1) be the probability that k-ToM's
% opponent will pick the first alternative option. Then:
% P(o=1) = sum_k P(o=1|k)*P(k)
% where P(o=1|k) is the probability that k-ToM's opponent will pick
% the first alternative option if he was a k-ToM, and P(k) is the
% probability that k-ToM's opponent is a k-ToM.
% IN:
%   - x: hidden states (see indexing in inG.indlev)
%   - P: observation param:
%       P(1) = (log-) temperature
%       P(2) = bias [optional]
%   - u: [useless]
%   - inG: input structure (see prepare_kToM.m)
% OUT:
%   - gx: proba that the agent will pick the first option, i.e. gx=P(y=1).

player = in.player; % 1 or 2: role of the player
ntotPar = in.npara; % only for k-ToM with k>0
level = in.lev; % depth of k-ToM's recursive beliefs
game = in.game; % payoff table
a = 0.36; % for E[s(x)] when x~n(mu,Sig)
indlev = in.indlev; % hidden-states indexing [see defIndlev.m]

% Get the agent's prediction about her opponent's next move, ie P(o=1).
if level==0 % 0-ToM

    mx = x(1); % E[log-odds of P(o=1)]
    Vx = exp(x(2)); % V[log-odds of P(o=1)]
    Po = VBA_sigmoid(mx/(sqrt(1+a*Vx))); % P(o=1)

else

    % Get P(k'). Note: if the agent is k-ToM, then, by definition, she
    % considers that her opponent's sophistication is k' < k. In addition,
    % there is a constraint of normalization, ie sum_k' P(k') = 1. Thus,
    % one only needs to keep track of k'-1 probabilities (the last one is,
    % by construction, 1-sum_k' P(k')).
    Pk = VBA_sigmoid(x(1:(level-1))); % P(k'), with k'=0,...,k-1
    Pk = [Pk;max(0,1-sum(Pk))]; % insert last P(k'=k-1)

    % Get P(o=1|k'). Note: the agent's prediction P(o=1|k') depends upon
    % her estimate of her opponent's parameters (learning rate, tmperature,
    % bias...). Uncertainty Re: these parameters eventually results in
    % blurring her prediction.
    % Note: hidden states encode x(theta), the log-odds of P(o=1|k',theta)
    % evaluated at the agent's estimate of theta (mu). In addition, they
    % encode the gradient of x wrt to theta (dx/dtheta), and V[theta]. This
    % then serves to derive P(o=1|k') as follows:
    % P(o=1|k') = E[sigm(x(theta))]
    %           = sigm(E[x(theta)]/sqrt(1+a*V[x(theta)])
    %           = sigm(E[x(theta)]/sqrt(1+a*V[theta]*(dx/dtheta)^2)
    f = zeros(level,1); % E[x(theta)]
    Vx = zeros(level,1); % V[x(theta)]
    for j=1:level % loop over possible opponent's levels  (k'=j-1)
        f(j) = x(indlev(j).f); % E[x(theta)|k'=j-1]
        df = x(indlev(j).df); % d[x(theta)]/dtheta for k'=j-1
        Sig = exp(x(indlev(j).Par(2:2:2*ntotPar))); % V[theta|k'=j-1]
        Vx(j) = sum(Sig.*df.^2); % V[x(theta)|k'=j-1]
    end
    Es = VBA_sigmoid(f./sqrt(1+a*Vx)); % E[sigm(x(theta))]

    % Get P(o=1) = sum_k P(o=1|k')*P(k')
    Po = Pk'*Es; % k-ToM's belief about her opponent's next move

end

% Make decision based upon P(o=1)
DV = fplayer(Po,exp(P(1)),player,game); % incentive for a=1
if length(P)==1
    gx = VBA_sigmoid(DV); % P(a=1)
else % P(2) = bias
    gx = VBA_sigmoid(DV+P(2)); % P(a=1) with bias
end


function [Model, Info] = linear_sparse_stepwise_vec(X,Y,Model,parm)
%  Estimate linear weight matrix for input-output mapping
%     Automatic Relevance Prior for each input dimension
%     is imposed to get sparse weight matrix
%
% Notice !!
%     Delay embedding is done in this module,
%     then input vector should be original input data without embedding
%
%   [Model, Info] = linear_sparse_stepwise(X,Y,Model,parm)
%     Scalar iteration version
% --- Input
%  Y  : Output data ( N x T x Ntrial )
%  X  : Input data  ( M x (T + (Dtau-1)*Tau)  x Ntrial)
%  N  =  # of output
%  M  =  # of input (original input space dimension)
%  T  =  # of time sample
%
%  Estimate the following model
%    Y(t) = W * [X(:, t + (Dtau-1)*Tau ); ...; X(:, t)]
%
%  Model : Structure for estimated model
%  if Model is empty, initialization is done before training
%  if Model is previous training result, re-training us done
%
%  parm  : Structure for learning parameter
%  parm.Npre_train :  # of VB-update in initial training
%  parm.Ntrain :  # of training
%  parm.Nskip  :  skip # for print
%  parm.a_min  :  Min value for pruning small variance component
%  parm.Prune  :  = 1 : Prune small variance & irrelevant input dimension
%
%  parm.Tau       = Lag time
%  parm.Dtau      = Number of embedding dimension
%    Total input dimension in embedding space is (M * parm.Dtau)
% --- Output
%  Model : Structure for estimated model
%  Model.SY  :  Noise variance         ( 1 x 1 )
%  Model.W   :  Weight matrix          ( N x M*D ) , D = parm.Dtau
%  Model.A   :  Prior weight variance  ( 1 x M*D )
%  Model.ix_act : Active index for W after pruning
%
%  Info  : Structure for learning process history
%  Info.FE  = LP + H : Free energy
%  Info.LP  = Log likelihood
%  Info.H   = - Model entropy
%
% 2009-10-18 Made by M. Sato

MINVAL = 1.0e-15;

fprintf('linear_sparse_stepwise start\n')

% Dimension
[N ,T  ,Ntrial]  = size(Y); % N = # of output
[M ,Tx ,Ntrialx] = size(X); % M = # of input without embedding

Tall = T * Ntrial;

if Ntrial~=Ntrialx, error('# of trial is different in X and Y'); end;

Nskip  = 100;   % skip steps for display info
a_min = 1e-14;	% Minimum value for weight pruning
Fdiff = 1e-12; % Threshold for convergence
Ncheck = 100;   % Minimum number of training iteration
Fstep  = 5;     % Free energy convergence check step
Prune  = 1;     % Prune mode
space_ARD=1;	% if space_ARD=1, ARD is done only for space dimension

if isfield(parm,'Nskip'), Nskip  = parm.Nskip; end;
if isfield(parm,'Fdiff'), Fdiff   = parm.Fdiff; end;
if isfield(parm,'a_min'), a_min   = parm.a_min ; end;
if isfield(parm,'Prune'), Prune = parm.Prune; end;
if isfield(parm,'Ncheck'), Ncheck = parm.Ncheck; end
if isfield(parm,'Fstep'),Fstep  = parm.Fstep ; end
if isfield(parm,'Fstep'),Fstep  = parm.Fstep ; end
if isfield(parm,'space_ARD'),space_ARD  = parm.space_ARD ; end

% # of embedding dimension
if isfield(parm,'Dtau')
	D    = parm.Dtau;
	tau  = parm.Tau;
else
	D    = 1;
	tau  = 1;
end
% Parameters
Ntrain = parm.Ntrain;

if Ntrain < 1, Info = []; return; end;

if isfield(parm,'Npre_train')
	Npre_train = parm.Npre_train;
else
%	Npre_train = Ntrain;
	if Tall >= 2*M*D
		Npre_train = 0;
	elseif Tall >= M*D
		Npre_train = fix(Ntrain/2);
	else
		Npre_train = Ntrain;
	end
end

if Npre_train > Ntrain, Npre_train = Ntrain; end;

if isfield(parm,'Nupdate')
	Nupdate = parm.Nupdate;
else
	Nupdate = 1;
end

fprintf('--- Output Dimension  = %d\n',N)
fprintf('--- Input  Dimension  = %d\n',M)
fprintf('--- Embedding  Dimension  = %d\n',D)
fprintf('--- Number of trials  = %d\n',Ntrial)
fprintf('--- Number of training sample = %d\n',Tall)
fprintf('--- Total update iteration    = %d (%d)\n',Ntrain,Npre_train)

% original input dimension
Xdim  = M;
M = Xdim*D;
M_ALL = M;

%
% --- Initialization
%

% Input/Output variance
sx = mean((X(:) - mean(X(:))).^2);
sy = mean((Y(:) - mean(Y(:))).^2);

SY0 = mean(sy);
A0  = 1./mean(sx);

if isfield(parm,'Ta0') && parm.Ta0 > 0,
	Ta0 = parm.Ta0;
	a0  = parm.a0 * A0;
else
	Ta0 = 0;
	a0  = 1;
end

% Input covariance
XX  = sum(sum(X.^2,3),2)/(Tx*Ntrial);
XX  = repmat(XX', [1 D]);% 1 x M

if isfield(Model,'ix_act')
	SY  = Model.SY;  % 1 x 1

	% Active index
	ix_act = Model.ix_act;
	A = zeros(1,M_ALL);
	W = zeros(N,M_ALL);

	A(ix_act)   = sum(Model.A,1);
	W(:,ix_act) = Model.W;

	if M_ALL ~= Xdim*D,
		fprintf('M_ALL=%d,Xdim=%d,D=%d\n',M_ALL ,Xdim ,D)
		error('M_ALL ~= Xdim*D')
	end

	% Active index in input space without embedding
	IX_dim = find( sum(reshape(A,[Xdim,D]),2) > 0);

	IX_act = repmat( (0:(D-1))* Xdim ,[length(IX_dim) 1]) ...
	       + repmat(IX_dim, [1 D]);
	IX_act = IX_act(:);

	W  = W(:,IX_act) ;  % N x (M*D)
	A  = A(IX_act) ;    % 1 x (M*D)
	X  = X(IX_dim,:,:);
	XX = XX(IX_act);

	M     = length(IX_act);
	Xdim  = length(IX_dim);
else
	A = repmat(A0, [1,M_ALL]);
	W = zeros(N,M_ALL);
	SY = SY0;
	% Save original input for pruning
	IX_act  = 1:M_ALL;
	IX_dim  = 1:Xdim;
end

ix_act  = 1:M;
ix_dim  = 1:Xdim;
M_all   = M;

% Delay embedding index for W
Wid = [(0:(D-1))'* Xdim + 1 , (1:D)'* Xdim];

% Working variable
G_A = zeros(1,M);       % N x M

if size(A,1)==Xdim && size(A,2)==1
	A   = repmat(A',[1 D]);
elseif size(A,1)==1 && size(A,2)==Xdim
	A   = repmat(A,[1 D]);
end

if length(SY)==1, SY= repmat(SY,N,1); end;

SW = 1./(Tall*XX + 1./A );

fprintf('a_min = %g\n', a_min)
fprintf('SY0 = %g\n', SY0)
fprintf('SY  = %g\n', mean(SY))

% Free energy histry
FE  = zeros(Ntrain,1);
LP  = zeros(Ntrain,1);
H   = zeros(Ntrain,1);
Err = zeros(Ntrain,1);
Mhist = zeros(Ntrain,1);

% ARD hyper param. history
if isfield(parm,'Debug') && ~isempty(parm.Debug) && parm.Debug > 0
	Debug = 1;
	A_tmp = zeros(N*M_ALL, ceil(Ntrain/Nskip));
else
	Debug = 0;
end

% recover all component
A_all  = zeros(1,M_all);       % N x M

[dY] = error_delay_time(X,Y,W,tau);

k_save  = 0;

%%%%%% Learning Loop %%%%%%
for k=1:Ntrain
	% Ainv = alpha , A = 1/alpha
	Ainv = 1./A;

	% E = ( (Y-W*X)^2 +  W^2 * Ainv )/SY
    [W]  = weight_update_embed(X, dY, W, Tall*XX, Ainv, tau);
	[dY] = error_delay_time(X, Y, W, tau);

    dYY = sum(dY.^2,2)/(Tall); % N x 1
	WW  = W.^2;

    % Noise variance update
    SY  = dYY + WW * Ainv'/(Tall);
    % Prevent zero variance
    SY  = max( SY, MINVAL);

    % Weight variance
	SW = 1./(Tall*XX + Ainv );

    % Log variance
    SWA     = max( SW .* Ainv , MINVAL);
    log_sw  = N*(sum( log(SWA) - SWA + 1 ));
    log_sy  = sum( log(SY) );
    log_a   = Ta0*sum( log(Ainv) - a0.*Ainv + 1 );

    % Free energy
    LP(k)  = - (0.5) * (log_sy + N*sum(SW.*XX));
    H(k)   = 0.5*( log_sw + log_a  );
    FE(k)  = LP(k) + H(k)/Tall;
    Err(k) = sum(dYY)/(N*SY0);

	% E = ( (Y-W*X)^2 +  W^2 * Ainv )/SY

    % Hyper parameter for weight variance (ARD)
	if mod(k,Nupdate) == 0,
		% Ainv = 1./A;
	    % SW = 1./( Tall*XX + Ainv );

	    % G_A = 1 - (SW)./A;
	    %     = (Tall*XX + Ainv - 1/A) * SW
	    G_A = Tall.* ( SW ) .* XX;
	    G_A = max((G_A), MINVAL);

		% N*A = ( (W.^2) + SW )/SY ;
		if k <= Npre_train,
			% VB update rule (Stable)
			%  ARD for each weight
			A  = ((1./SY)'*WW + N*SW + 2*Ta0*a0)/( N + 2*Ta0 );
		else
			% Accelerated update rule (Unstable)
		    A  = sqrt( A.* ((1./SY)'*WW) ./(G_A * N) );
		    %A  = (WW + 2*Ta0*a0)./(G_A * N + 2*Ta0);
		end

		if space_ARD==1
			Am = mean(reshape(A,[M/D,D]),2);
			A  = repmat(Am', 1,D);
		end

	    % Prune small variance
	    if Prune == 1
		    % Find active input dimension
		    ix_act_old = ix_act;
		    ix_dim_old = ix_dim;

		    % recover all component
			A_all  = zeros(1,M_all);       % 1 x M
		    %  A_all(:,ix_act)  = A;
		    WWW = sum(WW,1);
		    A_all(ix_act) = WWW/max(WWW);

		    % find active input dimension (absolute index)
		    ix_dim = find( sum(reshape(A_all,[Xdim,D]),2) > a_min );
		    ix_act = repmat(ix_dim, [1 D]) ...
		           + repmat([0:D-1]*Xdim, [length(ix_dim) 1]);
		    ix_act = ix_act(:);

		    Mnew   = length(ix_act);  		% # of effective input

		    if Mnew < M,
			    % convert to relative index
			    jx_act = trans_index(ix_act,ix_act_old,M_all);
			    jx_dim = trans_index(ix_dim,ix_dim_old,Xdim);

			    M   = Mnew;
			    A   = A(jx_act) ;    % 1 x M
			    W   = W(:,jx_act) ;  % N x M
			    SW  = SW(:,jx_act);  % N x M

			    X	 = X(jx_dim,:,:);  	 	% Xdim x T
			    XX	 = XX(jx_act);  	 	% 1 x M
			end
	    end
	    % END of if Prune == 1

		A = max(A,MINVAL);

	end
	% END of if mod(k,Nupdate) == 0

	Mhist(k) = M;

    if mod(k, Nskip)==0
        % Save history
		if Debug == 1
        	k_save = k_save + 1;
        	A_tmp(:,k_save) = A_all(:);
		end

        fprintf('Iter = %4d, M = %4d, err = %g, F = %g, H = %g\n', ...
               k, length(ix_act), Err(k), FE(k), - 2*H(k)/log(T));
    end

    % Convergence check
	if k > Ncheck,
		Fdif = (FE(k) - FE(k-Fstep))/(abs(FE(k))+eps);
	else
		Fdif = Fdiff + 1;
	end

	if (Fdiff > abs(Fdif)),
		fprintf('Converged : Free energy change = %g\n',Fdif)
		break;
	end;
end

ix_act = IX_act(ix_act);

% Active index
Model.ix_act = ix_act;
Model.M_all  = M_ALL ;

% Save output variable
Model.A    = A ;
Model.W    = W ;
Model.SY   = SY;
Model.SW   = SW; % = 1./(Tall*XX + diag(Ainv))

Model.method = 'linear_sparse_stepwise';
Model.mode   = 'scalar';
Model.sparse = 'sparse';

% Save history
Info.FE  = FE(1:k);
Info.LP  = LP(1:k);
Info.H   = H(1:k) ;
Info.Err = Err(1:k);
Info.M   = Mhist(1:k);

if exist('A_tmp','var')
	Info.A   = A_tmp(:,1:k_save) ;
end

% recover all component
%W_all   = zeros(N,M_all);
%SW_all  = zeros(N,M_all);
%
%A_all(:,ix_act)  = A;
%W_all(:,ix_act)  = W ;  % N x M
%SW_all(:,ix_act) = SW;  % N x M

%%% ---- Index transformation from old active_index to current active_index
function	jx = trans_index(ix,ix_old,M)

N = length(ix_old);
Itrans = zeros(M,1);
Itrans(ix_old) = 1:N;

jx = Itrans(ix);


function I = mi_model_gd_vec(x, y, Ym, biascorrect, demeaned)
% MI_MODEL_GD_VEC Vectorized MI calculation between multiple Gaussian variables
%         and a common discrete variable in bits based on ANOVA style model comparison.
%   I = mi_model_gd_vec(x,y,Ym) returns the MI between the (possibly multidimensional)
%   Gaussian variables x and the discrete variable y.
%   size(x) = [Ntrl Nvec Ndim]
%   so each output I(i) = mi_model_gd(squeeze(x(:,i,:)), y, Ym);
%
%   For 1D x this is a lower bound to the mutual information.
%   y should contain integer values in the range [0 Ym-1] (inclusive).
%
%   biascorrect : true / false option (default true) which specifies whether
%   bias correction should be applied to the esimtated MI.
%   demeaned : false / true option (default false) which specifies whether the
%   input data already has zero mean (true if it has been copula-normalized)
%   See also: MI_MODEL_GD, MI_MIXTURE_GD_VEC

% ensure samples first axis for vectors
if isvector(x)
    x = x(:);
end
if ndims(x)>3
    error('mi_model_gd: input arrays should be 3d')
end
if isvector(y)
    y = y(:);
else
    error('mi_model_gd: only univariate discrete variable supported');
end

Ntrl = size(x,1);
Nvec = size(x,2);
Xdim = size(x,3);

if size(y,1) ~= Ntrl
    error('mi_model_gd: number of trials do not match');
end

% default option values
if nargin<4
    biascorrect = true;
end

if nargin<5
    demeaned = false;
end

% unconditional demean
if ~demeaned
    x = bsxfun(@minus,x,sum(x,1)/Ntrl);
end

I = zeros(Nvec,1);

% y = y-1;
% for vi=1:Nvec
%   I(vi) = mi_model_gd(squeeze(x(:,vi,:)),y,Ym,true,true);
% end
%
% return

% one-hot encoding of Y
Yhot = indexed2boolean(y);

% remove class means
[Xcen class_means] = removeclassmeans(x, Yhot);

% allocate memory for class-conditional entropies and covariances
Ntrl_y = sum(Yhot);
Hcond  = zeros(Nvec,Ym);
Cm     = zeros(Nvec,Xdim,Xdim,Ym);

% allocate memory for overall entropy and covariance
Hunc = zeros(Nvec,1);
Cx   = zeros(Nvec,Xdim,Xdim);

%  data is class-demeaned, this needs to be accounted for in the
% unconditional entropies
c = diag(sqrt(Ntrl_y))*class_means.';
c = reshape(c, [Ym Nvec Xdim]);

for vi1=1:Xdim
  % all voxels for this dimension
  x1 = Xcen(:,:,vi1);
  c1 = squeeze(c(:,:,vi1));

  Cx(:,vi1,vi1) = sum(x1.^2)+sum(c1.^2);
  for yi=1:Ym
    tmp = x1(Yhot(:,yi),:);
    Cm(:,vi1,vi1,yi) = sum(tmp.^2);
  end

  for vi2=(vi1+1):Xdim
    x2  = Xcen(:,:,vi2);
    c2  = squeeze(c(:,:,vi2));
    tmp = transpose(sum(x1.*x2) + sum(c1.*c2));

    Cx(:,vi1,vi2) = tmp;
    Cx(:,vi2,vi1) = tmp;
    for yi=1:Ym
      tmp = transpose(sum(x1(Yhot(:,yi),:).*x2(Yhot(:,yi),:)));
      Cm(:,vi1,vi2,yi) = tmp;
      Cm(:,vi2,vi1,yi) = tmp;
    end
  end
end

Cx = Cx / (Ntrl-1);
Cx = vecchol(Cx);
for vi=1:Xdim
  Hunc = Hunc + shiftdim(log(Cx(:,vi,vi)));
end

for yi=1:Ym
  Cm(:,:,:,yi) = Cm(:,:,:,yi) / (Ntrl_y(yi) - 1);
  Cm(:,:,:,yi) = vecchol(Cm(:,:,:,yi));
  for vi=1:Xdim
    Hcond(:,yi) = Hcond(:,yi)+shiftdim(log(Cm(:,vi,vi,yi)));
  end
end

% apply bias corrections
ln2 = log(2);
if biascorrect

  vars = 1:Xdim;

  psiterms_unc = psi((Ntrl - vars)/2) / 2;
  dterm_unc    = (ln2 - log(Ntrl-1)) / 2;
  bias_unc     = Xdim'*dterm_unc + sum(psiterms_unc);

  dterm_cond    = (ln2 - log(Ntrl_y-1)) / 2;
  psiterms_cond = zeros(1,Ym);
  for vi=vars
    idx = (Ntrl_y-vi);
    psiterms_cond = psiterms_cond + psi(idx/2);
  end
  bias_cond = Xdim*dterm_cond + (psiterms_cond/2);

  Hunc  = Hunc  - bias_unc;
  Hcond = Hcond - ones(Nvec,1)*bias_cond;
end

% class weights
w = Ntrl_y ./ Ntrl;

% compute mutual information
I = Hunc - Hcond*w';

% convert to bits
I = I / ln2;

function [Xcen, class_means] = removeclassmeans(X, design)
[Ntrl, Nvec, Ndim] = size(X);
Xcen = X(:,:);
class_means = zeros(Nvec*Ndim,size(design,2));
for k = 1:size(design,2)
  sel = design(:,k);
  tmp = Xcen(sel,:);
  class_means(:,k) = mean(tmp,1);
  Xcen(sel,:) = bsxfun(@minus,tmp,class_means(:,k).');
end
Xcen = reshape(Xcen,[Ntrl Nvec Ndim]);

function Y = indexed2boolean(X)
uX = unique(X);
Y  = false(numel(X),numel(uX));
for k = 1:size(Y,2)
  Y(X==uX(k),k) = true;
end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

% Test script for illustration

% base scenario
t=0; T=1;
f = 0.03;
alpha = 0.075;
beta = 0.5;
nu = 0.1;
rho = 0;

sabrdensity = @(x,y) psabr3(t,T,f,x,alpha,y,beta,nu,rho);

lcoeff = 1; ucoeff = 1;
lowerbound = f-lcoeff*f;
upperbound = f + ucoeff*f;

xvals = lowerbound:.0005:upperbound;

zvals1d = density1d(xvals,sabrdensity,0,1);

% change rho
rho = -.9;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dm = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dm)
rho = .9;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dp = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dp)
figure('Color',[1 1 1]);
hold on;
plot(xvals,zvals1dm,'r','LineWidth',2); plot(xvals,zvals1d,'b','LineWidth',2); plot(xvals,zvals1dp,'g','LineWidth',2);
title('Plot of SABR densities for different \rho')
xlabel('0\leq x \leq .1')
ylabel('density')
legend('\rho=-0.9','\rho=0','\rho=0.9')
hold off;
rho = 0.0;

% change nu
nu = 0.05;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dm = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dm)
nu = .15;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dp = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dp)
figure('Color',[1 1 1]);
hold on;
plot(xvals,zvals1dm,'r','LineWidth',2); plot(xvals,zvals1d,'b','LineWidth',2); plot(xvals,zvals1dp,'g','LineWidth',2);
title('Plot of SABR densities for different \nu')
xlabel('0\leq x \leq .1')
ylabel('density')
legend('\nu=0.05','\nu=0.4','\nu=0.2')
hold off;
nu = 0.1;

% change beta
beta = 0.4;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dm = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dm)
beta = 0.8;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dp = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dp)
figure('Color',[1 1 1]);
hold on;
plot(xvals,zvals1dm,'r','LineWidth',2); plot(xvals,zvals1d,'b','LineWidth',2); plot(xvals,zvals1dp,'g','LineWidth',2);
title('Plot of SABR densities for different \beta')
xlabel('0\leq x \leq .1')
ylabel('density')
legend('\beta=0.4','\beta=0.5','\beta=0.6')
hold off;
beta = 0.5;

% change alpha
alpha = 0.05;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dm = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dm)
alpha = .1;
sabrdensity = @(x,y) psabr30(t,T,f,x,alpha,y,beta,nu,rho);
zvals1dp = density1d(xvals,sabrdensity,0,1);
intdensity(xvals,zvals1dp)
figure('Color',[1 1 1]);
hold on;
plot(xvals,zvals1dm,'r','LineWidth',2); plot(xvals,zvals1d,'b','LineWidth',2); plot(xvals,zvals1dp,'g','LineWidth',2);
title('Plot of SABR densities for different \alpha')
xlabel('0\leq x \leq .1')
ylabel('density')
legend('\alpha=0.05','\alpha=0.075','\alpha=0.1')
hold off;
alpha = 0.075;


function [Q] = CouetteBC2D(xin, yin, nxin, nyin, mapI, mapO, mapW, mapC, Q, time);

% function [Q] = CouetteBC2D(xin, yin, nxin, nyin, mapI, mapO, mapW, mapC, Q, time);
% Purpose: evaluate solution for Couette flow

% Couette flow (mach .2 at inner cylinder)
gamma = 1.4;

% extract conserved variables
rho = Q(:,:,1); rhou = Q(:,:,2); rhov = Q(:,:,3); Ener = Q(:,:,4);

mapB = [mapI;mapO];

rad = sqrt(xin(mapB).^2 + yin(mapB).^2);
theta = atan2(yin(mapB), xin(mapB));

utheta = (-rad + 16./rad)/75;
p = 1 + (1./(75^2))*( (rad.^2)/2 - 32*log(rad) - 128./rad.^2 );

rho (mapB) = 1;
rhou(mapB) = -sin(theta).*utheta;
rhov(mapB) =  cos(theta).*utheta;
Ener(mapB) = p/(gamma-1) + 0.5*(rhou(mapB).^2 + rhov(mapB).^2)./rho(mapB);

% pack modified conserved variables
Q(:,:,1) = rho; Q(:,:,2) = rhou; Q(:,:,3) = rhov; Q(:,:,4) = Ener;
return


function [E,L,G]=GenPCBasis(S,A)
% this function computes the conditional principal portfolios
% see A. Meucci - "Managing Diversification", Risk Magazine, June 2009
% available at www.ssrn.com

% Code by A. Meucci. This version March 2009.
% Last version available at MATLAB central as "Managing Diversification"

% inputs
% S : covariance matrix
% A : conditioning matrix

% outputs
% E : conditional principal portfolios composition
% L : conditional principal portfolios variances
% G : map weights -> conditional diversification distribution (square root of, not normalized)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if isempty(A)
    N=size(S,1);
    K=0;
    [E_,L_]=eig(S);
    E=E_;
    for n=1:N
        E(:,n)=E_(:,N-n+1);
        L(n)=L_(N-n+1,N-n+1);
    end

else

    [K,N]=size(A);
    E=[];
    B=A;
    for n=1:N-K
        if ~isempty(E)
            B=[A
                E'*S];
        end
        e=GenFirstEigVect(S,B);
        E=[E e];
    end

    for n=N-K+1:N
        B=E'*S;
        e=GenFirstEigVect(S,B);
        E=[E e];
    end

    % swap order
    E=[E(:,N-K+1:N) E(:,1:N-K)];
end

L=diag(E'*S*E);

G=diag(sqrt(L))*inv(E);
G=G(K+1:N,:);


%TROTY Rotation about Y axis
%
% T = TROTY(THETA) is a homogeneous transformation (4x4) representing a rotation
% of THETA radians about the y-axis.
%
% T = TROTY(THETA, 'deg') as above but THETA is in degrees.
%
% Notes::
% - Translational component is zero.
%
% See also ROTY, TROTX, TROTZ, TROT2.

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

function T = troty(t, varargin)
	T =    [roty(t, varargin{:}) [0 0 0]'; 0 0 0 1];


%% DEMO 7:  Algorithms 02. SART
%
%
% In this demo the usage of the algorithms on the SART family among with
% their options are presented.
%
%
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------
%% Initialize
clear;
close all;

%% Define Geometry
geo=defaultGeometry('nVoxel',[128;128;128]);

%% Load data and generate projections
% see previous demo for explanation
angles=linspace(0,2*pi,100);
head=headPhantom(geo.nVoxel);
projections=Ax(head,geo,angles,'interpolated');
noise_projections=addCTnoise(projections);
%% SART family of algorithms
%
% There are 3 algorithms in this damily included in TIGRE: SART,SIRT and
% OS-SART.
%
% The main difference between them is the update process.
%   SART: Updates the image projection by projection
%   SIRT: Updates the image using the whole set of projections at once
%   OS-SART: middle ground. Updates the image using a subset of the
%            projections
%
%  Of these algorithms, SART is generally the one reaching a better image
%  (less L2 error) for the same amount of iterations, and SIRT is the
%  worst (still relatively similar). However, SART needs increased
%  computational time per iteration, as it needs to update the image very often,
%  while SIRT only updates the emage ones for the whole sets of projections.
%  OS-SART lies in the middle, reaching similar convergence (L2 error per
%  iteration) than SART but with less computational time than SART.
%
%% Usage, with optional parameters.
% In the three algorithms, there are 4 mandatory input arguments:
% Projections, geometry, angles and number of iterations.
%
%
% Optional arguments for all of them
%==========================================================================
% 'lambda': hyperparameter. The update will be multiplied by this number
% every iteration, to make the steps bigger or smaller. Default: 1
%
lambda=1;

% 'lambdared': reduction multiplier for the hyperparameter.
% lambda=lambda*lambdared every iterations, so the steps can be smaller
% the further the update. Default=0.99
lambdared=0.999;

% 'Init' : Initialization method. Possible options are
%          'none' (default). There will be no initialization method, just
%                 the algorithm
%
%          'FDK'  Initialize the image with the result of FDK algorithm
%
%          'multigrid' Initialize using the multigrid method. The image
%                      will be solved in a small scale, and the size of it
%                      will increase until the desired size is reached.
%
%          'image'     Initialzies with a user given image. Not recoomended
%                      unless you really know what you are doing.

initmode='none';

% 'InitImg' : related to init. The image to use for initializing the
% algorithm.

% 'verbose': boolean to make the algorithm display (or not) running state.
%            default true.

verbose=true;
% 'QualMeas'     Asks the algorithm for a set of quality measurement
%                parameters. Input should contain a cell array of desired
%                quality measurement names. Example: {'CC','RMSE','MSSIM'}
%                These will be computed in each iteration.
qualmeas={'RMSE'};

% SIRT and SART both have no extra input parameters.
% =========================================================================
[imgSIRT,errL2SIRT,qualitySIRT]=SIRT(projections,geo,angles,20,...
                            'lambda',lambda,'lambda_red',lambdared,'verbose',verbose,'QualMeas',qualmeas);
[imgSART,errL2SART,qualitySART]=SART(projections,geo,angles,20,...
                            'lambda',lambda,'lambda_red',lambdared,'verbose',verbose,'QualMeas',qualmeas);
% OS-SART
% ========================================================================
% Additionally OS-SART includes a couple of other parameters, related to
% the subsets.
%
%   'BlockSize':   Sets the projection block size used simultaneously. If
%                  BlockSize = 1 OS-SART becomes SART and if  BlockSize = size(angles,2)
%                  then OS-SART becomes SIRT. Default is 20.
blcks=8;
% 'OrderStrategy':  Chooses the subset ordering strategy. Options are
%                  'ordered' :uses them in the input order, but divided
%                  'random'  : orders them randomply
%                  'angularDistance': chooses the next subset with the
%                                     biggest angular distance with the
%                                     ones used.  (default)
order='angularDistance';
[imgOSSART,errL2OSSART,qualityOSSART]=OS_SART(projections,geo,angles,20,...
                            'lambda',lambda,'lambda_red',lambdared,'verbose',verbose,'QualMeas',qualmeas,...
                             'BlockSize',blcks,'OrderStrategy',order);
%% Lets have a brief show of the results
% set(0,'DefaultTextInterpreter', 'latex')

subplot(211)
plot(log10([errL2SIRT;[errL2OSSART nan(1,length(errL2SIRT)-length(errL2OSSART))];[errL2SART nan(1,length(errL2SIRT)-length(errL2SART))]]'));
title('Convergence')
xlabel('Iteration')
ylabel('$ log_{10}(|Ax-b|) $','interpreter','latex')
legend('SIRT','OS-SART','SART')
subplot(212)
plot(log10([qualitySIRT;[qualityOSSART nan(1,length(qualitySIRT)-length(qualityOSSART))];[qualitySART nan(1,length(qualitySIRT)-length(qualitySART))]]'));
title('Evolution of RMSE')
legend('SIRT','OS-SART','SART')
xlabel('Iteration')
ylabel('$ log_{10}(RMSE) $','interpreter','latex')

%% plot the results

% It is clear that SART will get to better results for the same amoutn of
% iterations, however, it takes x7 more time to run.

% SART
% OS-SART
% SIRT

plotImg([imgSIRT;  imgOSSART; imgSART;],'Dim','Z','Savegif','sarts.gif');

% plot error
plotImg(abs([head-imgSIRT; head-imgOSSART; head-imgSART; ]),'Dim','Z');


function hermite_cubic_test10 ( )

%*****************************************************************************80
%
%% HERMITE_CUBIC_TEST10 tests HERMITE_CUBIC_SPLINE_INTEGRAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 February 2011
%
%  Author:
%
%    John Burkardt
%
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'HERMITE_CUBIC_TEST10:\n' );
  fprintf ( 1, '  HERMITE_CUBIC_SPLINE_INTEGRAL integrates a Hermite\n' );
  fprintf ( 1, '  cubic spline over the definition interval [X1,XNN].\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  If the subintervals are equally spaced, the derivative\n' );
  fprintf ( 1, '  information has no effect on the result, except for\n' );
  fprintf ( 1, '  the first and last values, DN(1) and DN(NN).\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '                            Exact       Computed\n' );
  fprintf ( 1, '     X1          XNN        Integral    Integral  Comment\n' );
  fprintf ( 1, '\n' );

  for test = 1 : 5
%
%  Equal spacing.
%
    if ( test == 1 )
      nn = 11;
      xn = linspace ( 0.0, pi, nn );
      fn = sin ( xn );
      dn = cos ( xn );
      integral_exact = - cos ( xn(nn) ) + cos ( xn(1) );
      comment = 'Equal spacing, correct DN';
%
%  Equal spacing, reset DN(2:NN-1) to random numbers.
%
    elseif ( test == 2 )
      nn = 11;
      xn = linspace ( 0.0, pi, nn );
      fn = sin ( xn );
      dn = cos ( xn );
      [ dn(2:nn-1), seed ] = r8vec_uniform_01 ( nn - 2, seed );
      dn(2:nn-1) = 1000.0 * dn(2:nn-1);
      integral_exact = - cos ( xn(nn) ) + cos ( xn(1) );
      comment = 'Equal spacing, DN(2:N-1) random';
%
%  Equal spacing, now reset all of DN to random numbers.
%
    elseif ( test == 3 )

      nn = 11;
      xn = linspace ( 0.0, pi, nn );
      fn = sin ( xn );
      [ dn, seed ] = r8vec_uniform_01 ( nn, seed );
      dn(1:nn) = 1000.0 * dn(1:nn);
      integral_exact = - cos ( xn(nn) ) + cos ( xn(1) );
      comment = 'Equal spacing, DN(1:N) random';
%
%  Variable spacing, correct data.
%
    elseif ( test == 4 )
      nn = 11;
      xn = linspace ( 0.0, pi^2, nn );
      xn = sqrt ( xn );
      fn = sin ( xn );
      dn = cos ( xn );
      integral_exact = - cos ( xn(nn) ) + cos ( xn(1) );
      comment = 'Variable spacing, correct DN';
%
%  Variable spacing, change one entry in DN.
%
    elseif ( test == 5 )
      nn = 11;
      xn = linspace ( 0.0, pi^2, nn );
      xn = sqrt ( xn );
      fn = sin ( xn );
      dn = cos ( xn );
      [ r, seed ] = r8_uniform_01 ( seed );
      dn( floor ( nn + 1 ) / 2 ) = 1000.0 * r;
      integral_exact = - cos ( xn(nn) ) + cos ( xn(1) );
      comment = 'Variable spacing, a single internal DN randomized.';
    end

    integral_computed = hermite_cubic_spline_integral ( nn, xn, fn, dn );

    fprintf ( 1, '  %10f  %10f  %10.6g  %10.6g  %s\n', ...
      xn(1), xn(nn), integral_exact, integral_computed, comment );

  end

  return
end


function [x_best, psi_best, out] = CSDF(mapp, x0, options)
% CSDF is a derivative-free algorithm for solving systems of nonlinear
% equations :math:`f(x) = 0`, x in :math:`R^m`
% using the nonlinear unconstrained minimization :math:`\textrm{min}\ \psi(x) = 1/2 ||f(x)||^2` s.t. `x` in :math:`R^m`.
%
% USAGE:
%
%    [x_best, psi_best, out] = CSDF(mapp, x0, options)
%
% INPUTS:
%    mapp:        function handle provides `f(x)` and gradient `f(x)`
%    x0:          initial point
%    options:     structure including the parameteres of scheme
%
%                   * .MaxNumIter - maximum number of iterations
%                   * .MaxNumMapEval - maximum number of function evaluations
%                   * .TimeLimit - maximum running time
%                   * .epsilon - accuracy parameter
%                   * .x_opt - optimizer
%                   * .psi_opt - optimum
%                   * .sigma - strong duplomonotone parameter
%                   * .l - Lipschitz continuity constant of `f`
%                   * .tauBar - a constant for determining the step-size
%                   * .flag_x_error - 1: saves :math:`x_{error}`, 0: do not saves :math:`x_{error}` (default)
%                   * .flag_psi_error - 1:saves :math:`\psi_{error}`, 0: do not saves :math:`\psi_{error}` (default)
%                   * .flag_time - 1: saves :math:`\psi_{error}`, 0: do not saves :math:`\psi_{error}` (default)
%                   * .Stopping_Crit - stopping criterion:
%
%                     1. stop if :math:`||nfxk|| \leq \epsilon`
%                     2. stop if `MaxNumIter` is reached
%                     3. stop if `MaxNumMapEval` is reached
%                     4. stop if `TimeLimit` is reached
%                     5. stop if (default) :math:`||hxk|| \leq \epsilon` or `MaxNumIter` is reached
%
% OUTPUTS:
%    x_best:      the best approximation of the optimizer
%    psi_best:    the best approximation of the optimum
%    out:         structure including more output information
%
%                   * .T - running time
%                   * .Niter - total number of iterations
%                   * .Nmap - total number of mapping evaluations
%                   * .merit_func - array including all merit function values
%                   * .x_error - relative error :math:`norm(x_k(:)-x_{opt}(:))/norm(x_{opt})`
%                   * .psi_error - relative error :math:`(\psi_k-\psi_{opt})/(\psi_0-\psi_{opt}))`
%                   * .Status - reason of termination
%
% .. REFERENCE:
% .. Algorithm 2 of [1]: F.J. Aragon Artacho, R.M.T. Fleming, Globally convergent algorithms for finding zeros of duplomonotone mappings, Optimization Letter, 9, 569-584 (2015)
% .. Author: - Masoud Ahookhosh, System Biochemistry Group, Luxembourg Center for System Biomedicine, University of Luxembourg, Luxembourg
%            - Update July 2017 - M. Ahookhosh

format longG ;

% ================ Error messages for input and output =================
if nargin > 3
    error('The number of input arguments is more than what is needed');
elseif nargin < 3
    error('The number of input arguments is not enough');
end;

if isempty(mapp)
    error('the function handle mapp has to be defined');
elseif ~isa(mapp,'function_handle')
    error('mapp should be a function handle');
end

if isempty(x0)
    error('The starting point x0 has to be defined');
elseif ~isa(x0,'numeric')
    error('x0 should be a numeric vector');
end

% =================== initializing the parameters ======================
% ===== user has requested viewing the default values of "options" =====
[MaxNumIter,MaxNumMapEval,TimeLimit,epsilon,alpha, ...
         beta,sigma,l,tauBar,lambda_min,lambda_max,flag_x_error, ...
         flag_psi_error,flag_time,Stopping_Crit] = InitialDuplo(options);

if isfield(options,'x_opt')
    x_opt=options.x_opt;
elseif flag_x_error==1
    error('x_error requires to x_opt be specified');
end

if flag_x_error == 1
    Nxopt      = sqrt(sum(x_opt(:).^2));
    x_error(1) = sqrt(sum((x0(:)-x_opt(:)).^2))/Nxopt;
end

if flag_psi_error == 1
    psi_error(1) = 1;
end

if flag_time == 1
    Time(1) = 0;
end

xk         = x0;
Xk         = xk;
Niter      = 1;
fx0        = mapp(x0);
Nmap       = 1;
nfx0       = norm(fx0);
fxk        = fx0;
nfxk       = nfx0;
merit_func = 0.5*nfxk^2;
lambda     = min(sigma/l^2,tauBar);
StopFlag   = 0;

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%% Main body of CSDF.m %%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T0 = tic;

% ======================= start of the main loop =======================
while ~StopFlag

    xk    = xk-lambda*fxk;
    Niter = Niter+1
    fxk   = mapp(xk);
    Nmap  = Nmap+1;
    nfxk2 = norm(fxk)^2;

    % ================= Gathering output information ===================
    psik              = 0.5*nfxk2;
    merit_func(Niter) = psik;
    if flag_time == 1
        Time(Niter+1) = toc(T0);
    end

    if flag_x_error == 1
        Nx_opt = norm(x_opt);
        x_error(Niter+1) = sqrt(sum((xk(:)-x_opt(:)).^2))/Nx_opt;
    end

    if flag_psi_error == 1
        psi_error(Niter+1) = (psik-psi_opt)/(psi0-psi_opt);
    end

    % ================== checking stopping criteria ====================
    T = toc(T0);

    [StopFlag,Status] = StopCritDuplo(nfxk,Niter,Nmap,T, ...
              MaxNumIter,MaxNumMapEval,TimeLimit,epsilon,Stopping_Crit);

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Outputs %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Status
x_best         = xk;
psi_best       = psik;
out.Xk         = Xk;
out.T          = T;
out.nhx        = nfxk;
out.merit_func = merit_func';
out.Niter      = Niter;
out.Nmap       = Nmap;
out.Status     = Status;

if flag_x_error == 1
    out.x_error = x_error;
end
if flag_psi_error == 1
    out.psi_error = psi_error;
end
if flag_time == 1
    out.Time = Time;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%% End of CSDF.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [xi, em, gx, gy] = surface_interpolation(x, PARAM, INTERP, res, KLIM, nlev)
% surface_interpolation  Interpolate a surface from a scattered set of points
%
% [XI, EM, GX, GY] = surface_interpolation(X)
%
%   X is a 3-row matrix. Each column has the coordinates of a point that
%   belongs to the surface we want to interpolate.
%
%   XI is a 3-row matrix with the coordinates of the interpolated points.
%
%   EM is a 2-row matrix with the coordinates of the X points projected
%   onto the interpolation domain. Note that the interpolation domain may
%   change between methods, so don't expect results to be aligned if you do
%   e.g. plot3(gx(:), gy(:), y(:, 3), 'o').
%
%   GX, GY are the grid for the box that contains EM.
%
% ... = surface_interpolation(X, PARAM, INTERP, RES, KLIM, NLEV)
%
%   PARAM is a string with the method used to parametrize the surface and
%   X:
%
%     'xy' (default): No change, the X coordinates are kept the same.
%
%     'pca': X points are rotated according to their eigenvectors to make
%     the dominant plane of the points X as horizontal as possible before
%     interpolating.
%
%     'isomap': Use the Isomap method by [1] to "unfold" the curved surface
%     defined by X before interpolating. (This option requires function
%     IsomapII).
%
%   INTERP is a string with the interpolation method:
%
%      'tps' (default): Thin-plate spline. Global support.
%
%      'tsi': Matlab's TriScatteredInterp() function. Local support,
%      limited to the convex hull of the scattered points 2D projection on
%      the interpolation domain.
%
%      'gridfit': John D'Errico's gridfit() function [3] (note:
%      approximation, rather than interpolation). Local support with
%      extrapolation outside the convex hull.
%
%      'mba': Multilevel B-Spline Approximation Library by SINTEF ICT [4].
%      Local support, limited to a rectangle that tighly contains the
%      scattered points 2D projection on the interpolation domain.
%
%      'mbae': Like the 'mba' method, but first a thin-plate spline is used
%      to extrapolate values on the interpolation domain boundary. Then MBA
%      is used for the local support interpolation of X and the boundary
%      values set by the thin-plate spline.
%
%   RES is a 2-vector with the grid spacing in the x- and y-directions. By
%   default, RES=[1 1].
%
%   KLIM is a scalar factor for the extension of the interpolation domain.
%   By default, KLIM=1 and the interpolation domain is a rectangle that
%   tightly contains X. Sections of the interpolated surface that protude
%   from the image volume are removed.
%
%   NLEV is the number of levels in the hierarchical construction of 'mba'
%   and 'mbae'. For other INTERP options, it will be ignored.  By default,
%   NLEV = 7.
%
%
% [1] J.B. Tenenbaum, V. de Silva and J.C. Langford, "A Global Geometric
% Framework for Nonlinear Dimensionality Reduction", Science 290(5500):
% 2319-2323, 2000.
%
% [2] Isomap Homepage, http://isomap.stanford.edu/
%
% [3] Surface Fitting using gridfit by John D'Errico 11 Nov 2005 (Updated
% 29 Jul 2010). Code covered by the BSD License
% http://www.mathworks.com/matlabcentral/fileexchange/8998-surface-fitting-using-gridfit
%
% [4] MBA - Multilevel B-Spline Approximation Library
% http://www.sintef.no/Projectweb/Geometry-Toolkits/MBA/

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2010-2011 University of Oxford
% Version: 0.2.0
% $Rev: 782 $
% $Date: 2012-06-02 22:55:03 +0100 (Sat, 02 Jun 2012) $
%
% 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/>.

% check arguments
error(nargchk(2, 6, nargin, 'struct'));
error(nargoutchk(0, 4, nargout, 'struct'));

% defaults
if (nargin < 2 || isempty(PARAM))
    PARAM = 'xy';
end
if (nargin < 3 || isempty(INTERP))
    INTERP = 'tps';
end
if (nargin < 4 || isempty(res))
    res = [1 1];
end
if (nargin < 5 || isempty(KLIM))
    KLIM = 1;
end
if (nargin > 5 && ~isempty(nlev) ...
        && ~(strcmp(INTERP, 'mba') || strcmp(INTERP, 'mbae')))
    warning('NLEV input argument ignored for this INTERP option')
end
if (nargin < 6 || isempty(nlev))
    nlev = 7;
end

%% map the 3D points (x,y,z) to a 2D domain (u,v)

% this is analogous to computing the knot vector for a curve interpolation.
% The idea is that (x,y)->z is not necessarily a function, due to the valve
% folding over. However, we hope that (u,v)->(x,y,z) is a function
switch PARAM

    case 'xy'

        % (u,v) is simply (x,y)
        em = x(1:2, :);

    case 'pca'

        % rotate valve points to make the valve surface as horizontal as
        % possible
        m = mean(x, 2);
        em = x - m(:, ones(1, size(x, 2)));
        eigv = pts_pca(em);
        if any(isnan(eigv(:)))
            error('Cannot interpolate surface');
        end
        em = eigv' * em;
        em = em(1:2, :);

    case 'isomap'

        % compute distance matrix
        d = dmatrix(x, x, 'euclidean');

        % compute 2-d projection of the 3-d data
        options.dims = 2;
        options.display = 0;
        options.overlay = 0;
        options.verbose = 0;
        em = IsomapII(d, 'k', round(size(x, 2)/3), options);
        em = em.coords{1};

    otherwise
        error('Parametrization method not implemented')
end

%% compute interpolation domain

% find box that contains embedded coordinates
emmin = min(em, [], 2);
emmax = max(em, [], 2);

% box size and centroid
delta = emmax - emmin;
boxm = mean([emmax emmin], 2);

% extend the box
emmin = boxm - delta/2*KLIM;
emmax = boxm + delta/2*KLIM;

% generate grid for the embedding box
[gy, gx] = ndgrid(emmin(2):res(1):emmax(2), emmin(1):res(2):emmax(1));

%% compute interpolating surface

% source and target points that will define the warp
%s = em; % don't duplicate data in memory
%t = x; % don't duplicate data in memory

% interpolate
switch INTERP
    case 'tps' % thin-plate spline
        xi = pts_tps_map(em', x', [gx(:) gy(:)]);
    case 'tsi' % Matlab's TriScatteredInterp
        fx = TriScatteredInterp(em(1, :)', em(2, :)', x(1, :)', 'natural');
        fy = TriScatteredInterp(em(1, :)', em(2, :)', x(2, :)', 'natural');
        fz = TriScatteredInterp(em(1, :)', em(2, :)', x(3, :)', 'natural');
        fx = fx(gx, gy);
        fy = fy(gx, gy);
        fz = fz(gx, gy);
        xi = [fx(:) fy(:) fz(:)];
    case 'gridfit'
        fx = gridfit(em(1, :)', em(2, :)', x(1, :)', ...
            emmin(1):res(2):emmax(1), emmin(2):res(1):emmax(2), ...
            'tilesize', 150);
        fy = gridfit(em(1, :)', em(2, :)', x(2, :)', ...
            emmin(1):res(2):emmax(1), emmin(2):res(1):emmax(2), ...
            'tilesize', 150);
        fz = gridfit(em(1, :)', em(2, :)', x(3, :)', ...
            emmin(1):res(2):emmax(1), emmin(2):res(1):emmax(2), ...
            'tilesize', 150);
        xi = [fx(:) fy(:) fz(:)];
    case 'mba' % Multilevel B-Spline Approximation Library
        xi = [...
            mba_surface_interpolation(em(1, :)', em(2, :)', x(1, :)', gx(:), gy(:), nlev) ...
            mba_surface_interpolation(em(1, :)', em(2, :)', x(2, :)', gx(:), gy(:), nlev) ...
            mba_surface_interpolation(em(1, :)', em(2, :)', x(3, :)', gx(:), gy(:), nlev)];
    case 'mbae' % MBA extrapolated using a TPS
        % use TPS to interpolate the points, but we are only interested in
        % the edges and corners of the interpolation, where the TPS is
        % extrapolating linearly; also, we decimate the points in the edges
        em2 = [gx(1, 1:end)' gy(1, 1:end)' ; ... % top edge
            gx(2:end, end) gy(2:end, end) ; ... % right edge
            gx(end, 1:end-1)' gy(end, 1:end-1)' ; ... % bottom edge
            gx(2:end-1, 1) gy(2:end-1, 1) ; ... % left edge
            ]';
        xi = pts_tps_map(em', x', em2')';
        x = [x xi(:, 1:10:end)];
        % local support interpolation with boundary conditions provided by
        % the TPS
        em = [em em2(:, 1:10:end)];
        xi = [...
            mba_surface_interpolation(em(1, :)', em(2, :)', x(1, :)', gx(:), gy(:), nlev) ...
            mba_surface_interpolation(em(1, :)', em(2, :)', x(2, :)', gx(:), gy(:), nlev) ...
            mba_surface_interpolation(em(1, :)', em(2, :)', x(3, :)', gx(:), gy(:), nlev)];
    otherwise
        error('Interpolation method not implemented')
end


function ttf = friedman_test(n_problem, r, n)
%FRIEDMAN regression

fun = @(i, j, x) x^(j-1);

if n_problem == 1
    %Friedman 1
    d = 10;
    N = 20000;
    x = rand(d,N);
    y = 10*sin(pi*x(1,:).*x(2,:)) + 20*(x(3,:) - 0.5*ones(1,N)).^2 + 10*x(4,:) + 5*x(5,:);

    tt_rank = [1 ; r*ones(d-1,1) ; 1];
    n_basis = n*ones(d,1);
    basis_ps = cumsum([1 ; n_basis*N]);
    basis_cr = zeros(basis_ps(d+1) - basis_ps(1), 1);
    for dim = 1: d
        t = zeros(n, N);
        for i = 1: n
            t(i,:) = x(dim,:).^(i-1);
        end
        basis_cr(basis_ps(dim):basis_ps(dim+1)-1) = reshape(t, [n*N 1]);
    end
elseif n_problem == 2
    %Friedman 2
    d = 4;
    N = 20000;
    x = rand(d,N);
    x1 = x(1,:)*100;
    x2 = x(2,:)*520*pi + 40*pi*ones(1,N);
    x3 = x(3,:);
    x4 = x(4,:)*10 + ones(1,N);
    y = sqrt(x1.^2 + (x2.*x3 - 1./(x2.*x4)).^2);

    tt_rank = [1 ; r*ones(d-1,1) ; 1];
    n_basis = n*ones(d,1);
    basis_ps = cumsum([1 ; n_basis*N]);
    basis_cr = zeros(basis_ps(d+1) - basis_ps(1), 1);
    for dim = 1: d
        t = zeros(n, N);
        for i = 1: n
            t(i,:) = x(dim,:).^(i-1);
        end
        basis_cr(basis_ps(dim):basis_ps(dim+1)-1) = reshape(t, [n*N 1]);
    end
else
    %Friedman 3
    d = 4;
    N = 20000;
    x = rand(d,N);
    x1 = x(1,:)*100;
    x2 = x(2,:)*520*pi + 40*pi*ones(1,N);
    x3 = x(3,:);
    x4 = x(4,:)*10 + ones(1,N);
    y = atan((x2.*x3 - 1./(x2.*x4))./x1);

    tt_rank = [1 ; r*ones(d-1,1) ; 1];
    n_basis = n*ones(d,1);
    basis_ps = cumsum([1 ; n_basis*N]);
    basis_cr = zeros(basis_ps(d+1) - basis_ps(1), 1);
    for dim = 1: d
        t = zeros(n, N);
        for i = 1: n
            t(i,:) = x(dim,:).^(i-1);
        end
        basis_cr(basis_ps(dim):basis_ps(dim+1)-1) = reshape(t, [n*N 1]);
    end
end

coeff = reg_als(basis_cr, y, tt_rank, n_basis);
ttf = tt_function(fun, coeff);

end


%converts frequency to mel
%>
%> @param fMel: frequency
%> @param cModel: 'Fant','Shaughnessy', or 'Umesh'
%>
%> @retval Hertz value
% ======================================================================
function [fInHz] = ToolMel2Freq(fMel, cModel)

    if (nargin < 2)
        cModel  = 'Fant';
    end

    % set function handle
    hPitchFunc  = str2func (['aca' cModel '_I']);

    fInHz       = hPitchFunc(fMel);
end

function [f] = acaFant_I(m)
    %mel         = 1000 * log2(1 + f/1000);
    f   = 1000 * (2.^(m/1000)-1);
end

function [f] = acaShaughnessy_I(m)
    %mel         = 2595 * log10(1 + f/700);
    f   = 700 * (10.^(m/2595)-1);
end

function [f] = acaUmesh_I(m)
    %mel         = f./(2.4e-4*f + 0.741);
    f   = m*.741 ./ (1 - m * 2.4e-4);
end


function [nQb, pos, vel, q] = ins(w_b, f_b, nQb, pos, vel, gravity, dt)
%% 捷联更新
rotate_vector = w_b*dt;
rotate_vector_norm = norm(rotate_vector);
if(rotate_vector_norm <1e-10) % fix nan issue
    q = [1 0 0 0];
else
    q = [cos(rotate_vector_norm/2); rotate_vector/rotate_vector_norm*sin(rotate_vector_norm/2)]';
end

% 姿态更新
nQb = ch_qmul(nQb, q); %四元数更新（秦永元《惯性导航（第二版）》P260公式9.3.3）
nQb = ch_qnormlz(nQb); %单位化四元数

% 速度更新
f_n = ch_qmulv(nQb, f_b);
dv = (f_n + [0; 0; -gravity]); %比力方程
vel = vel + dv*dt;

% 位置更新
pos = pos + vel*dt;


function [z_Tji_Ground_m,r_I_IRji_m,r_I_ITji_m] = Rotation_Position_ToTireFixed(z_I_Ground_m,A_K_I,A_F_I,r_I_IAji_m,h_ji_m,re_ji_m)
%% Rotation ground displacement from inertial fixed coordinate system into tire fixed coordinate systems
% Input parameters:
% A_K_I          [---]    Rotation Matrix from Inertial Frame to VehicleFixed Frame
% A_F_I          [---]    Rotation Matrix from Inertial Frame to RearAxleFixed Frame
% h_ji_m         [m]      Suspension Height [SuspHFL SuspHFR SuspHRL SuspHRR]
% re_ji_m        [m]      Effective Wheel Radii [ReFL ReFR ReRL ReRR]
% r_I_IAji       [m]      Absolut Position of Axles in Inertial Reference Frame [r_I_IAFL r_I_IAFR r_I_IARL r_I_IARR] [3x4]
% z_Ground_I_m   [---]    z-Coordinate of Road Displacement in Inertial Reference Frame [1x4]
% Output parameters:
% z_Ground_Tji_m [---]    Transformed Displacement Vector [r_TFL_ITFL r_TFR_ITFR r_TRL_ITRL r_TRR_ITRR] [1x4]

%% Wheel center points in inertial reference frame

% Quarter Vehicle Model along VehicleFixed z-Axis: Rotation from VehicleFixed Axis System into Inertial Frame
% r_I_IRji_m = [(r_I_IAji_m(:,1) + transpose(A_K_I)*[0;0;1]*(h_ji_m(1))) (r_I_IAji_m(:,2) + transpose(A_K_I)*[0;0;1]*(h_ji_m(2))) (r_I_IAji_m(:,3) + transpose(A_K_I)*[0;0;1]*(h_ji_m(3))) (r_I_IAji_m(:,4) + transpose(A_K_I)*[0;0;1]*(h_ji_m(4)))];

% Quarter Vehicle Model along RearAxleFixed z-Axis: Rotation from RearAxleFixed Axis System into Inertial Frame
r_I_IRji_m = [(r_I_IAji_m(:,1) + transpose(A_F_I)*[0;0;1]*(h_ji_m(1))) (r_I_IAji_m(:,2) + transpose(A_F_I)*[0;0;1]*(h_ji_m(2))) (r_I_IAji_m(:,3) + transpose(A_F_I)*[0;0;1]*(h_ji_m(3))) (r_I_IAji_m(:,4) + transpose(A_F_I)*[0;0;1]*(h_ji_m(4)))];

%% Contact points of road-tire-interface in inertial reference frame

% Quarter Vehicle Model along VehicleFixed z-Axis: Rotation from VehicleFixed Axis System into Inertial Frame
% r_I_ITji_m = [(r_I_IAji_m(:,1) + transpose(A_K_I)*[0;0;1]*(re_ji_m(1)+h_ji_m(1))) (r_I_IAji_m(:,2) + transpose(A_K_I)*[0;0;1]*(re_ji_m(2)+h_ji_m(2))) (r_I_IAji_m(:,3) + transpose(A_K_I)*[0;0;1]*(re_ji_m(3)+h_ji_m(3))) (r_I_IAji_m(:,4) + transpose(A_K_I)*[0;0;1]*(re_ji_m(4)+h_ji_m(4)))];

% Quarter Vehicle Model along RearAxleFixed z-Axis: Rotation from RearAxleFixed Axis System into Inertial Frame
r_I_ITji_m = [(r_I_IAji_m(:,1) + transpose(A_F_I)*[0;0;1]*(re_ji_m(1)+h_ji_m(1))) (r_I_IAji_m(:,2) + transpose(A_F_I)*[0;0;1]*(re_ji_m(2)+h_ji_m(2))) (r_I_IAji_m(:,3) + transpose(A_F_I)*[0;0;1]*(re_ji_m(3)+h_ji_m(3))) (r_I_IAji_m(:,4) + transpose(A_F_I)*[0;0;1]*(re_ji_m(4)+h_ji_m(4)))];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %% Elaborate Computation of Vertical Displacement
% % The computation of the position vector of the road-tire interface
% % requires information about the rx_I_ITji_m and ry_I_ITji_m coordinates of the
% % road-tire-interface relative to the inertial frame and the vertical
% % displacement z_Ground_I_m.
%
% %% Position Vector of Road-Tire-Interface around Orientation of the Quarter Vehicle Model
%
% % Extracting (x,y)-Coordinates of r_I_ITji_m
% % rx_I_ITji_m = r_I_ITji_m(1,:);
% % ry_I_ITji_m = r_I_ITji_m(2,:);
% %
% % Quarter Vehicle Model along VehicleFixed z-Axis: Rotation from Inertial Frame into VehicleFixed Axis System
% r_Ground_m = A_K_I*vertcat(rx_I_ITji_m,ry_I_ITji_m,+z_Ground_I_m);
%
% % Quarter Vehicle Model along RearAxleFixed z-Axis: Rotation from Inertial Frame into RearAxleFixed Axis System
% r_Ground_m = A_F_I*vertcat(rx_I_ITji_m,ry_I_ITji_m,+z_Ground_I_m);
%
% %% Orientation Change from z-down-Orientation to z-up-Orientation
% z_Ground_Tji_m = -r_Ground_m(3,:);
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Robust Computation of Vertical Displacement
% A more robust computation of the Vertical Displacement is possible with
% the simplified z-displacement vector [0;0;1]*z_Ground_I_m under
% neglection of the rx_I_ITji_m and ry_I_ITji_m coordinates of the
% road-tire-interface relative to the inertial frame.

%% Vertical z-Displacement of Road-Tire-Interface around Orientation of the Quarter Vehicle Model

% Quarter Vehicle Model along VehicleFixed z-Axis: Rotation from Inertial Frame into VehicleFixed Axis System
% r_Ground_m = A_K_I*[0;0;1]*z_Ground_I_m;

% Quarter Vehicle Model along RearAxleFixed z-Axis: Rotation from Inertial Frame into RearAxleFixed Axis System
r_Ground_m = A_F_I*[0;0;1]*z_I_Ground_m;

%% Orientation Change from z-down-Orientation to z-up-Orientation
z_Tji_Ground_m = -r_Ground_m(3,:);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end


function  [F, G] = F_update(upd, DCMbn, imu)
% F_update: updates F and G matrices before the execution of Kalman filter.
%
% INPUT
%   upd, 1x8 vector with data from the INS.
%   DCMbn, DCM body-to-nav.
%   imu, IMU data structure.
%
% OUTPUT
%   F,  15x15 state transition matrix.
%   G,  15x12 control-input matrix.
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%   Groves, P.D. (2013), Principles of GNSS, Inertial, and
% Multisensor Integrated Navigation Systems (2nd Ed.). Artech House.
% Matrix F from Eq. 14.63.
%
% 	Farrell, J. (2008). Aided Navigation: GPS With High Rate
% Sensors. McGraw-Hill Professional, USA. Matrix G from Eq. 11.108.
%
% Version: 010
% Date:    2022/03/06
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

Vn =  upd(1);
Ve =  upd(2);
Vd =  upd(3);
lat = upd(4);
h  =  upd(5);

fn =  upd(6:8);
% wn =  upd(9:11);

Om = 7.292115e-5;
I = eye(3);
Z = zeros(3);

[RM,RN] = radius(lat);

RO = sqrt(RN*RM) + h;

%% ATTITUDE MATRICES

a11 = 0;
a12 = -( (Om * sin(lat)) + (Ve / RO * tan(lat)) );
a13 = Vn / RO;
a21 = (Om * sin(lat)) + (Ve / RO * tan(lat));
a22 = 0 ;
a23 = (Om * cos(lat)) + (Ve / RO) ;
a31 = -Vn / RO;
a32 = -Om * cos(lat) - (Ve / RO);
a33 = 0;
F11 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

% Groves, 14.64
% F11 = skewm(wn);

a11 = 0;
a12 = 1 / RO;
a13 = 0;
a21 = -1 / RO;
a22 = 0;
a23 = 0;
a31 = 0;
a32 = -tan(lat) / RO;
a33 = 0;
F12 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

a11 = -Om * sin(lat);
a12 = 0;
a13 = -Ve / (RO^2);
a21 = 0 ;
a22 = 0 ;
a23 = Vn / (RO^2);
a31 =  -Om * cos(lat) - (Ve / ((RO) * (cos(lat))^2));
a32 = 0 ;
a33 = (Ve * tan(lat)) / (RO^2) ;
F13 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

%% VELOCITY MATRICES

F21 = skewm(fn);

a11 = Vd / RO;
a12 = -2 * ((Om * sin(lat)) + ((Ve / RO) * tan(lat))) ;
a13 = Vn / RO ;
a21 = (2 * Om * sin(lat)) + ( (Ve / RO) * tan(lat) );
a22 = (1 / RO) * ((Vn * tan(lat)) + Vd) ;
a23 = 2 * Om * cos(lat) + (Ve / RO);
a31 = (-2 * Vn) / RO;
a32 = -2 * (Om * cos(lat) +  (Ve / RO)) ;
a33 = 0;
F22 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

e = 0.0818191908425;        % WGS84 eccentricity
res = RN * sqrt( cos(lat)^2 + (1-e^2)^2 * sin(lat)^2);
g = gravity(lat,h);
g0 = g(3);

a11 = -Ve * ((2 * Om * cos(lat)) + (Ve / (RO * (cos(lat))^2)));
a12 = 0 ;
a13 = (1 / RO^2) * ( (Ve^2 * tan(lat)) - (Vn * Vd) );
a21 = 2 * Om * ( (Vn * cos(lat)) - (Vd * sin(lat)) ) + ( (Vn * Ve) / (RO * (cos(lat))^2) ) ;
a22 = 0 ;
a23 = -(Ve / RO^2) * (Vn * tan(lat) + Vd);
a31 = 2 * Om * Ve * sin(lat);
a32 = 0;
% a33 = (1 / RO^2) * (Vn^2 + Ve^2);
a33 = Ve^2 / (RN+h)^2 + Vn^2 / (RM+h)^2 - 2 * g0 / res;
F23 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

%% POSITIONING MATRICES

F31 = zeros(3);

a11 = 1 / RO;
a12 = 0;
a13 = 0;
a21 = 0;
a22 = 1 / (RO * cos(lat));
a23 = 0;
a31 = 0;
a32 = 0;
a33 = -1;
F32 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

a11 = 0;
a12 = 0;
a13 = -Vn / RO^2;
a21 = (Ve * tan(lat)) / (RO * cos(lat));
a22 = 0;
a23 = -Ve / (RO^2 * cos(lat));
a31 = 0;
a32 = 0;
a33 = 0;
F33 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];

Fbg = I;
Fba = I;

if (isinf(imu.gb_corr))
    Fgg = Z;
else
    Fgg = -diag( 1./ imu.gb_corr);
    %     Fbg = -diag(sqrt (2 ./ imu.gb_corr .* imu.gb_dyn.^2));
end

if (isinf(imu.ab_corr))
    Faa = Z;
else
    Faa = -diag(1 ./ imu.ab_corr);
    %     Fba = -diag(sqrt (2 ./ imu.ab_corr .* imu.ab_dyn.^2));
end

% Eq. 14.63 from Groves
F = [F11 F12 F13 DCMbn Z  ;
    F21  F22 F23 Z     DCMbn  ;
    F31  F32 F33 Z     Z      ;
    Z    Z   Z   Fgg   Z      ;
    Z    Z   Z   Z     Faa    ;
    ];

% Eq. 11.108 from Farrell
G = [DCMbn Z     Z   Z ;
    Z      DCMbn Z   Z ;
    Z      Z     Z   Z ;
    Z      Z     Fbg Z ;
    Z      Z     Z   Fba ;
    ];

end


function [y,e] = powspec(x, sr, wintime, steptime, dither)
%[y,e] = powspec(x, sr, wintime, steptime, sumlin, dither)
%
% compute the powerspectrum and frame energy of the input signal.
% basically outputs a power spectrogram
%
% each column represents a power spectrum for a given frame
% each row represents a frequency
%
% default values:
% sr = 8000Hz
% wintime = 25ms (200 samps)
% steptime = 10ms (80 samps)
% which means use 256 point fft
% hamming window
%
% $Header: /Users/dpwe/matlab/rastamat/RCS/powspec.m,v 1.3 2012/09/03 14:02:01 dpwe Exp dpwe $

% for sr = 8000
%NFFT = 256;
%NOVERLAP = 120;
%SAMPRATE = 8000;
%WINDOW = hamming(200);

if nargin < 2
  sr = 8000;
end
if nargin < 3
  wintime = 0.025;
end
if nargin < 4
  steptime = 0.010;
end
if nargin < 5
  dither = 1;
end

winpts = round(wintime*sr);
steppts = round(steptime*sr);

NFFT = 2^(ceil(log(winpts)/log(2)));
%WINDOW = hamming(winpts);
%WINDOW = [0,hanning(winpts)'];
WINDOW = [hanning(winpts)'];
% hanning gives much less noisy sidelobes
NOVERLAP = winpts - steppts;
SAMPRATE = sr;

% Values coming out of rasta treat samples as integers,
% not range -1..1, hence scale up here to match (approx)
y = abs(specgram(x*32768,NFFT,SAMPRATE,WINDOW,NOVERLAP)).^2;

% imagine we had random dither that had a variance of 1 sample
% step and a white spectrum.  That's like (in expectation, anyway)
% adding a constant value to every bin (to avoid digital zero)
if (dither)
  y = y + winpts;
end
% ignoring the hamming window, total power would be = #pts
% I think this doesn't quite make sense, but it's what rasta/powspec.c does

% that's all she wrote

% 2012-09-03 Calculate log energy - after windowing, by parseval
e = log(sum(y));


function p = lbfgsbprod(H,g)
%lbfgshprod  computes products with the L-BFGS matrix B.
%
%   p = lbfgsbprod(H,g)  returns  p = B*g.
%
%   The product is computed using the factoriation
%   [(9.15),p.231] described in Nocedal and Wright, 1999.

%   See also lbfgsadd, lbfgsdel, lbfgsupdate, lbfgsinit.

% $Id$

% ----------------------------------------------------------------------
% Explicit matrix formulation
% ----------------------------------------------------------------------

%jMax = H.jMax;
%jNew = H.jNew;
%jOld = H.jOld;

% |---|---|---|---|---|
%       ^   ^
%       |   |
%      old new
%   4   5   1   2   3    1=newest, 2=2nd newest,... 5=oldest

%if false
%   Sk = H.S(:,[jNew-1:-1:1,jMax:-1:jNew]);
%   Yk = H.Y(:,[jNew-1:-1:1,jMax:-1:jNew]);
%   Lk = (Sk' * Yk) .* (repmat((1:jMax)',1,jMax) > repmat((1:jMax),jMax,1)) ;
%   Dk = diag(sum(Sk.*Yk));

%   deltak = H.delta;

%   M = [deltak*Sk'*Sk, Lk; Lk', -Dk];

%   p = [deltak*Sk'; Yk'] * g;
%   p = M \ p;
%   p = [deltak*Sk, Yk] * p;
%   p = deltak * g - p;
%end

% ----------------------------------------------------------------------
% Formulation directly based on arrays in permuted form :-)
% ----------------------------------------------------------------------

if ~isempty(H.ML)

   % This code works much faster for larger problems
   v1 = H.delta * (g' * H.S)';
   v2 = (g' * H.Y)';
   p  = [v1(H.valid); v2(H.valid)];

%  v1 = H.delta * (g' * H.S(:,H.valid))';
%  v2 = (g' * H.Y(:,H.valid))';
%  q = [v1; v2];

   p = H.MU \ (H.ML \ p); % H.M \ p (optionally: use linsolve)

%  v1 = H.S(:,H.valid) * p(1:H.rank) * H.delta;
%  v2 = H.Y(:,H.valid) * p(H.rank+1:2*H.rank);
%  q = v1 + v2;

   % This code works much faster for larger problems
   pe = zeros(size(H.S,2),1); pe(H.valid) = p(1:H.rank);
   v1 = H.S * (pe * H.delta);
   pe = zeros(size(H.Y,2),1); pe(H.valid) = p(H.rank+1:2*H.rank);
   v2 = H.Y * pe;
   p  = v1 + v2;
else
   p = 0;
end

p = H.delta * g - p;


function zz=lpcss2zz(ss)
%LPCSS2ZZ Convert s-place poles to z-plane poles ZZ=(SS)
%the s-plane is in units of Normalized Hz and so the imaginary part
% of each ss() value is in the range +-0.5
%
% If you multiply ss by the sample frequency, a formant with
% frequency f and bandwidth b will give an s-plane pole-pair
% of approximately -b/2 +- j f

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: lpcss2zz.m,v 1.4 2007/05/04 07:01:39 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 2 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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

zz=exp(2*pi*ss);


function [Q, R, E] = qr(f, varargin)
%QR   QR factorisation of an array-valued BNDFUN.
%   [Q, R] = QR(F) returns a QR factorisation of F such that F = Q*R, where the
%   BNDFUN Q is orthogonal (with respect to the continuous L^2 norm on the
%   domain of F) and of the same size as F and R is an m x m upper-triangular
%   matrix when F has m columns.
%
%   [Q, R, E] = QR(F) produces unitary Q, upper-triangular R, and a permutation
%   matrix E so that F*E = Q*R. The column permutation E is chosen to reduce
%   fill-in in R.
%
%   [Q, R, E] = QR(F, 'vector') returns the permutation information as a vector
%   instead of a matrix.  That is, E is a row vector such that F(:,E) = Q*R.
%   Similarly, [Q, R, E] = QR(F, 'matrix') returns a permutation matrix E. This
%   is the default behavior.

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

% Deal with empty case:
if ( isempty(f) )
    Q = [];
    R = [];
    E = [];
    return
end

% Initialise Q to be a BNDFUN:
Q = f;

% Rescaling factor, (b - a)/2.
rescaleFactor = .5*diff(f.domain);

% Call QR on the ONEFUN of f:
if ( nargout == 3 )
    [Q.onefun, R, E] = qr(f.onefun, varargin{:});
else
    [Q.onefun, R] = qr(f.onefun, varargin{:});
end

% Rescale so that columns of Q will be orthonormal (rather than orthogonal):
Q = Q/sqrt(rescaleFactor);

% Rescale R so that f = QR:
R = R*sqrt(rescaleFactor);

end


function [Model, Info] = linear_map_sparse_cov_pinv(X,Y,Model,parm)
%  Estimate linear weight matrix for input-output mapping
%     Automatic Relevance Prior for each input dimension
%     is imposed to get sparse weight matrix
%
%   [Model, Info] = linear_map_sparse_cov_pinv(X,Y,Model,parm)
%
% --- Input
%  X  : Input data  ( M x T )
%  Y  : Output data ( N x T )
%  N  =  # of output
%  M  =  # of input
%  T  =  # of data
%
%  Model : Structure for estimated model
%  Model.SY0 :  Output data variance                 ( 1 x 1 )
%  Model.A0  :  (Output data var)/(Input data var)   ( 1 x 1 )
%
%  parm  : Structure for learning parameter
%  parm.Ntrain :  # of training
%  parm.Nskip  :  skip # for print
%  parm.a_min  :  Min value for pruning small variance component
%  parm.Prune  :  = 1 : Prune small variance & irrelevant input dimension
%
% --- Output
%  Model : Structure for estimated model
%  Model.SY  :  Noise variance         ( 1 x 1 )
%  Model.SW  :  Weight variance        ( M x M )
%  Model.W   :  Weight matrix          ( N x M )
%  Model.A   :  Prior weight variance  ( N x M ) ARD hyper parameter
%
%  Info  : Structure for learning process history
%  Info.FE  = LP + H : Free energy
%  Info.LP  = - (Log error)
%  Info.H   = - (# of effective weight parameters)
%
% 2007/1/26 Made by M. Sato

% Constants
MINVAL  = 1.0e-15;
MinCond = 1.0e-10;

% # of total training iteration
Ntrain = parm.Ntrain;

Nskip  = 100;   % skip steps for display info
a_min  = 1e-10; % Minimum value for weight pruning
Fdiff  = 1e-10; % Threshold for convergence
Ncheck = 100;   % Minimum number of training iteration
Fstep  = 5;     % Free energy convergence check step
Prune  = 1;     % Prune mode

if isfield(parm,'Nskip'), Nskip  = parm.Nskip; end;
if isfield(parm,'Fdiff'), Fdiff   = parm.Fdiff; end;
if isfield(parm,'a_min'), a_min   = parm.a_min ; end;
if isfield(parm,'Prune'), Prune = parm.Prune; end;
if isfield(parm,'Ncheck'), Ncheck = parm.Ncheck; end
if isfield(parm,'Fstep'),Fstep  = parm.Fstep ; end

% # of embedding dimension
if isfield(parm,'Dtau')
	D    = parm.Dtau;
	tau  = parm.Tau;
else
	D    = 1;
	tau  = 1;
end

% Dimension
[M ,Tx ,Nx ]= size(X); % input dim
[N ,Ty ,Ny ]= size(Y); % output dim

if Nx~=Ny, error('Trial number is different for input & output'); end
if Tx~=Ty, error('Time sample is different for input & output'); end

% Reshape into 2D matrix
T = Tx*Nx; % # of data
X = reshape(X, [M T]);
Y = reshape(Y, [N T]);

% # of stable VB-update in initial training
if isfield(parm,'Npre_train')
	Npre_train = parm.Npre_train;
else
%	Npre_train = Ntrain;
%	Npre_train = 0;
	if T >= 2*M*D
		Npre_train = 0;
	elseif T >= M*D
		Npre_train = fix(Ntrain/2);
	else
		Npre_train = Ntrain;
	end
end
if Npre_train > Ntrain, Npre_train = Ntrain; end;

fprintf('linear map sparse covariance_pinv start\n')
fprintf('--- Output Dimension  = %d\n',N)
fprintf('--- Input  Dimension  = %d\n',M)
fprintf('--- Embedding  Dimension  = %d\n',D)
fprintf('--- Number of trials  = %d\n',Nx)
fprintf('--- Number of training sample = %d\n',Tx)
fprintf('--- Total update iteration    = %d (%d)\n',Ntrain,Npre_train)

%
% --- Initialization
%  A  : Initial variable to use 1st update
%     : 1 x M

% Input/Output variance
sx = mean(repadd(X, - mean(X,2)).^2, 2);
sy = mean(repadd(Y, - mean(Y,2)).^2, 2);

A0  = 1./mean(sx);
SY0 = mean(sy);

if isempty(Model)
	A   = repmat(A0, [1,M]);
	W   = zeros(N,M);
	SY  = SY0;
else
	A   = Model.A ;	 % 1 x M
	W   = Model.W ;  % N x M
	SY  = mean(Model.SY);  % 1 x 1
end

if isfield(parm,'Ta0') && parm.Ta0 > 0,
	Ta0 = parm.Ta0;
	a0  = parm.a0 * A0;
else
	Ta0 = 0;
	a0  = 1;
end

%  --- Initialization by other method ---
% Model.mode = 'scalar': ARD term = alpha * W^2
% Model.mode = 'cov'   : ARD term = alpha * W^2 * SY(^-1)
%
if isfield(Model, 'mode') &&  strcmp(Model.mode,'scalar')==1,
	fprintf('Old result is used as initial value\n')
	fprintf('Old method = %s\n', Model.method)
	A  = sum(A,1)./(sum(SY));
end

A = max(A,MINVAL);

% Original input dimension
M_ALL = M;

if isfield(Model,'ix_act')
	% Active index
	IX_act = Model.ix_act;

	X = X(IX_act,:); 	% M x T
	M = length(IX_act);
else
	IX_act = 1:M;
end

% Initial active index
M_all  = M;
A_all  = A/max(A) ;
ix_act_old = 1:M;

ix_act = find( A_all > a_min );   % effective indices
Mnew   = length(ix_act);  		% # of effective input

if Mnew < M,
    % convert to relative index
    jx_act = trans_index(ix_act,ix_act_old,M_all);

    M   = Mnew;
    A   = A(jx_act) ;  			% 1 x M
    W   = W(:,jx_act) ;  		% N x M
	X 	= X(jx_act,:);			% M x T
end

% Input variance
%XX  = (X * X')/T;   	% M x M
%YX  = (Y * X')/T;       % N x M
%YY  = sum(Y.^2,2)/T;    % N x 1
% Covariance matrix (not normalised)
YX  = (Y * X');       % N x M
YY  = sum(Y.^2,2);    % N x 1

fprintf('a_min = %g\n', a_min)
fprintf('SY0   = %g\n', SY0)
fprintf('SY    = %g\n', SY)

% Working variable
if T <= M
	XX  = [];
	Cinv= zeros(T,T);
	SW  = zeros(T,T);
else
	XX  = (X * X');   	  % M x M
	Cinv= zeros(M,M);
	SW  = zeros(M,M);
end

G_A = zeros(1,M);       % 1 x M
log_a = 0;
A_old = A;

% Free energy histry
FE  = zeros(Ntrain,1);
LP  = zeros(Ntrain,1);
H   = zeros(Ntrain,1);
MM  = zeros(Ntrain,1);
Err = zeros(Ntrain,1);

% ARD hyper param. history
if isfield(parm,'Debug') & ~isempty(parm.Debug) & parm.Debug > 0
	Debug = 1;
	A_tmp = zeros(M_all, ceil(Ntrain/Nskip));
else
	Debug = 0;
end

k_save  = 0;

%%%%%% Learning Loop %%%%%%
for k=1:Ntrain
	% ARD hyper variance parameter
	% A = 1/alpha

	if T < M
	    % Weight variance
	    % inv(X*X' + 1./A) = A - A * X * inv(X'*A*X + 1) * X' * A
		%  C = ( X' *A* X + eye(T) );
		XA   = repmultiply(X' , A); % T x M
	    Cinv = XA * X + eye(T);  % T x T

    	if rcond(Cinv) > MinCond,
			Cinv  = inv( Cinv );  % M x M
		else
			Cinv  = pinv( Cinv );
		end

		% Weight update
	    % inv(X*X' + 1./A) = A - A * X * Cinv * X' * A
		% W0 = YX .* A;
		% W  = W0 - (((W0 * X) * Cinv) * X') .* A;
		% W  = W0 - ((W0 * X) * Cinv) * (X'.* A);
		W  = repmultiply(YX , A);
		W  = W - ((W * X) * Cinv) * XA;
%		W  = W - ((W * X) / C ) * XA;
%		W  = W - repmultiply( ((W * X) * Cinv) * X' , A);

		%  C = ( X' *A* X + eye(T) );
		%  G_A = diag( X * inv(C) * X' *A )
		% G_A  = A .* sum(X .* (X / C), 2)';
		G_A  = A .* sum(X .* (X * Cinv), 2)';

		% Log variance
		log_sw  = log_det(Cinv) ;
		if mod(k, Nskip)==0, fprintf('- '); end
	else
		if isempty(XX)
			% covariance matrix in reduced space
			XX = X * X';
			% save original index
			IX_act = IX_act(ix_act);
			% new active index in reduced space
			ix_act = 1:M;
			M_all  = M;
			A_all  = A;
		end
    	SW  = XX + diag(1./A);

    	if rcond(SW) > MinCond,
			SW  = inv( SW );  % M x M
		else
			SW  = pinv( SW );
		end

		% Weight update
		W  = YX * SW;

		% SW  = X*X' + diag(1./A)
		% G_A = diag(inv(SW) * X*X')
		%     = 1 - diag(inv(SW)) ./A
		% G_A = diag( XX /SW );
		G_A = sum( SW .* XX ,1);
		%G_A = 1 - diag(SW)' ./A;

		log_sw  = log_det(SW) - sum(log(A));
		if mod(k, Nskip)==0, fprintf('+ '); end
	end

	WW = sum(W.^2, 1);
    % Noise variance update
%    SY = (sum(YY) - sum(sum(W.*YX)))/(N*T);
%
%    if (SY/SY0) <= MINVAL,
	    % Error
	    dY  = Y - W * X;        % N x T
	    dYY = sum(dY.^2, 2);  	% N x 1

	    SY  = (sum(dYY) + sum( WW./A ))/(N*T);
	    % Prevent zero variance
	    SY  = max( SY, MINVAL);
%	    fprintf('*')
%	end

	% Log variance
    log_sy  = N * log(SY) ;
    if Ta0 > 0,
	    log_a   = Ta0 * sum( - log(A./a0) - a0./A + 1);
	end

    % Free energy
    H(k)   =   0.5*N * (log_sw - M);
    LP(k)  = - 0.5*(T * log_sy) ;
    FE(k)  = LP(k) + H(k);
    Err(k) = (SY)./(SY0);
    MM(k)  = M;

    % Hyper parameter for weight variance (ARD)
    G_A = max((G_A), MINVAL);

	if k <= Npre_train,
		% VB update rule
		%  	N * A  = (W.^2)./SY + N * (A - A.*G_A)  ;
   		%A  = WW./SY + N * (A - A.*G_A)  ;
		if T < M
			A  = (WW./SY + N * A.*(1 - G_A) + 2*Ta0*a0)./( N + 2*Ta0 );
		else
			A  = (WW./SY + N * diag(SW)' + 2*Ta0*a0)./( N + 2*Ta0 );
		end
		%	A^2  = A .* (1./SY)' * (W.^2) ./ (G_A * N);
	else
	    % Accelerated update rule
		%	A  = (1./SY)' * (W.^2) ./ (G_A * N);
	    A  = sqrt(A.*(WW./SY)./(G_A * N));
	    %A  = ((WW./SY) + 2*Ta0*a0)./(G_A * N + 2*Ta0);
	end

    % Prune small variance
    if Prune == 1
	    ix_act_old = ix_act;

	    % Recover all component
	    switch	Prune
	    case	1
		    A_all(ix_act) = WW/max(WW);    % Prune by Weight
	    case	2
		    A_all(ix_act) = A /max(A);	   % Prune by Alpha
	    case	3
		    A_all(ix_act) = A * (1/SX);    % Prune by Alpha
	    end

	    % Find active input dimension (absolute index)
	    ix_act = find( A_all > a_min ); % effective indices
	    Mnew   = length(ix_act);  		% # of effective input

	    if Mnew < M,
		    % convert to relative index
		    jx_act = trans_index(ix_act,ix_act_old,M_all);

		    M   = Mnew;
		    A   = A(jx_act) ;  			% 1 x M
		    W   = W(:,jx_act) ;  		% N x M
			X 	= X(jx_act,:);			% M x T
			YX  = YX(:,jx_act);  	 	% N x M
			if ~isempty(XX)
				XX	= XX(jx_act,jx_act);	% M x M
			end
		end
    end

	A = max(A,MINVAL);

    if mod(k, Nskip)==0
        % Save history
		if Debug == 1
        	k_save = k_save + 1;
        	A_tmp(:,k_save) = A_all(:);
		end

        fprintf('Iter = %4d, M = %4d, err = %g, F = %g, H = %g\n', ...
               k, M, Err(k), FE(k), - H(k));
    end

	if k > Ncheck && M == MM(k-1)
		Adif = max(abs(A - A_old));
	else
		Adif = 1;
	end
	if Adif < Fdiff,
		fprintf('Converged : Alpha change = %g\n',Adif)
		break;
	end;

	A_old = A;

%		Fdif = (FE(k) - FE(k-Fstep))/abs(FE(k));
%	else
%		Fdif = Fdiff + 1;
%	end
end

% convert to relative index
ix_act = IX_act(ix_act);

% Active index
Model.ix_act = ix_act;
Model.M_all  = M_ALL ;

% Save trained variable
%  W & A is sufficient for cov-method initialization
Model.A  = A ;
Model.W  = W ;
Model.SY = SY;

Model.method = 'linear_map_sparse_cov';
Model.mode   = 'cov';
Model.sparse = 'sparse';

% Save history
Info.FE  = FE(1:k);
Info.LP  = LP(1:k);
Info.H   = H(1:k) ;
Info.Err = Err(1:k);
Info.M   = MM(1:k);

if exist('A_tmp','var')
	Info.A   = A_tmp(:,1:k_save) ;
end

%%% ---- Index transformation from old active_index to current active_index
function	jx = trans_index(ix,ix_old,M)

N = length(ix_old);
Itrans = zeros(M,1);
Itrans(ix_old) = 1:N;

jx = Itrans(ix);


%==============================================================================
% 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/
%==============================================================================
%
% function [y,dy] = affine3D(w,x,varargin)
%
% computes y = Q*w and the derivative wrt. w.
% x = reshape(x,[],3);
% Q = [x(:,1),x(:,2),x(:,3),1, 0      0        0      0  0      0        0
%      0      0      0,     0, x(:,1),x(:,2,1),x(:,3),1, 0      0        0
%      0      0      0,     0, 0      0        0      0, x(:,1),x(:,2,1),x(:,3),1]
% dy = Q:
% if no arguments are given, the parameters for the identity map are returned.
%
% see also transformations/contents.m, trafo.m
%==============================================================================

function [y,dy] = affine3D(w,x,varargin)

% the persitent variable stores the matrix
% Q(x) = kron( I_2 , [x(:,1),x(:,2),1] );
persistent Q

if nargin==0
    help(mfilename)
    runMinimalExample;
    return;
else
  y = mfilename('fullfile');
  dy = reshape(eye(4,3),[],1);         % parameterization of identity
  if ischar(w),  Q  = []; w = []; end; % reset Q
  if isempty(w), return;          end;
end;

if isempty(w) || (size(Q,1) ~= numel(x)),
  n = length(x)/3; x = reshape(x,n,3);
  Q = sparse(kron(speye(3),[x,ones(n,1)]));
  if nargout == 0, return; end;
end;
y  = Q*w;
dy = Q;

%------------------------------------------------------------------------------
function runMinimalExample
fprintf('%s: minimal example\n',mfilename)

omega = [0,10,0,8,0,6]; m = [8,7,6];
w = 22/pi;c = (omega(2:2:end)-omega(1:2:end))'/2;
R = [ cos(w),-sin(w),0;sin(w),cos(w),0;0,0,1];
g = (eye(3)-R)*reshape(c,[],1);
w = reshape([R,g]',[],1)
x = getNodalGrid(omega,m);
z = feval(mfilename,w,x);
FAIRfigure(1); clf;
plotGrid(x,omega,m,'color','r'); axis image; hold on;
plotGrid(z,omega,m,'color','b'); axis image; hold off;

fctn = @(w) feval(mfilename,w,x);
w = w + randn(size(w));
checkDerivative(fctn,w,'fig',2);

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


function value = legendre_integral ( d )

%*****************************************************************************80
%
%% LEGENDRE_INTEGRAL returns the Legendre integral of a test function.
%
%  Discussion:
%
%    The same function, integrated over [-1,+1]^D, has an integral that
%    is a factor of 2^D times as large as this result.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 May 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer D, the spatial dimension.
%
%    Output, real VALUE, the value of the integral.
%
  value = ( 0.5 * erf ( 0.5 / sqrt ( 2.0 ) ) ) .^ d;

  return
end


function dz = singlePendulumRhs(~,z,g,l)
% This function is used inside of ode45 for running a simulation of a
% single pendulum. The first argument (time) is not used.

th = z(1,:);
w = z(2,:);

dth = w;
dw = singlePendulumDynamics(th,g,l);

dz = [dth;dw];

end


function [L,U,P] = lu_rightp (A)
%LU_RIGHTP right-looking LU factorization, with partial pivoting.
%
% Example:
%   [L,U,P] = lu_rightp (A)
% See also: cs_demo

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

n = size (A,1) ;
P = eye (n) ;
for k = 1:n
    [x,i] = max (abs (A (k:n,k))) ;                           % partial pivoting
    i = i+k-1 ;
    P ([k i],:) = P ([i k], :) ;
    A ([k i],:) = A ([i k], :) ;                              % (6.10), (6.11)
    A (k+1:n,k) = A (k+1:n,k) / A (k,k) ;                               % (6.12)
    A (k+1:n,k+1:n) = A (k+1:n,k+1:n) - A (k+1:n,k) * A (k,k+1:n) ;     % (6.9)
end
L = tril (A,-1) + eye (n) ;
U = triu (A) ;


function [ f, rank ] = rgf_successor ( m, f, rank )

%*****************************************************************************80
%
%% RGF_SUCCESSOR generates the next restricted growth function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer M, the domain of the RGF is the integers
%    from 1 to M.  M must be positive.
%
%    Input/output, integer F(M), the restricted growth function.
%
%    Input/output, integer RANK, the rank.
%    If RANK = -1 on input, then the routine understands that this is
%    the first call, and that the user wishes the routine to supply
%    the first element in the ordering, which has RANK = 0.
%    In general, the input value of RANK is increased by 1 for output,
%    unless the very last element of the ordering was input, in which
%    case the output value of RANK is 0.
%

%
%  Return the first element.
%
  if ( rank == -1 )
    f(1:m) = 1;
    rank = 0;
    return
  end
%
%  Check.
%
  ierror = rgf_check ( m, f );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'RGF_SUCCESSOR - Fatal error!\n' );
    fprintf ( 1, '  The input array is illegal!\n' );
    fprintf ( 1, '  IERROR = %d\n', ierror );
    error ( 'RGF_SUCCESSOR - Fatal error!\n' );
  end
%
%  Find the first position from the right which can be incremented.
%
  for i = m : -1 : 2

    fmax = 1;
    for j = 2 : i - 1
      fmax = max ( fmax, f(j) );
    end
%
%  Increment the function at this position, and set later entries to 1.
%
    if ( f(i) ~= fmax + 1 )
      f(i) = f(i) + 1;
      f(i+1:m) = 1;
      rank = rank + 1;
      return
    end

  end
%
%  The final element was input.
%  Return the first element.
%
  f(1:m) = 1;
  rank = 0;

  return
end