function [y1, y2] = vl_nnpdist(x, x0, p, varargin)
%VL_NNPDIST CNN p-distance from target.
%   VL_NNPDIST(X, X0, P) computes the P distance raised of each feature
%   vector in X to the corresponding feature vector in X0:
%
%     Y(i,j,1) = (SUM_d (X(i,j,d) - X0(i,j,d))^P)^(1/P)
%
%   X0 should have the same size as X; the outoput Y has the same
%   height and width as X, but depth equal to 1. Optionally, X0 can
%   be a 1 x 1 x D x N array, in which case the same target feature
%   vector in X0 is compared to all feature vectors in X. In that case,
%   however, the DZDX0 are of size of X.
%
%   Setting the `noRoot` option to `true` does not take the 1/P power
%   in the formula, computing instead
%
%     Y(i,j,1) = SUM_d (X(i,j,d) - X0(i,j,d))^P
%
%   For example, `vl_nnpdist(x, x0, 2, 'noRoot', true)` computes the
%   squared L2 distance.
%
%   [DZDX, DZDX0] = VL_NNPDISTP(X, X0, P, DZDY) computes the derivative
%   of the block inputs projected onto DZDY. DZDX, DZDX0 and DZDY have the
%   same dimensions as X and Y, respectively.
%
%   VL_NNPDIST(___, 'OPT', VAL, ...) accepts the following options:
%
%   `NoRoot`:: `false`
%      If set to true, compute the P-distance to the P-th power.
%
%   `Epsilon`:: 1e-6
%      When computing derivatives, quantities that are divided in are
%      lower boudned by this value. For example, the L2 distance is
%      not smooth at the origin; this option prevents the
%      derivative from diverging.
%
%   `Aggregate`:: false
%      Instead of returning one scalar for each spatial location in
%      the inputs, sum all of them into a single scalar.
%
%   `InstanceWeights``:: `[]`
%      Optionally weight individual instances. This parameter can be
%      eigther a scalar or a weight mask, one for each pixel in the
%      input tensor.

% Copyright (C) 2015  Karel Lenc and Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

% -------------------------------------------------------------------------
%                                                             Parse options
% -------------------------------------------------------------------------

opts.noRoot = false ;
opts.epsilon = 1e-6 ;
opts.aggregate = false ;
opts.instanceWeights = [] ;
backMode = numel(varargin) > 0 && ~ischar(varargin{1}) ;
if backMode
  dzdy = varargin{1} ;
  opts = vl_argparse(opts, varargin(2:end), 'nonrecursive') ;
else
  dzdy = [] ;
  opts = vl_argparse(opts, varargin, 'nonrecursive') ;
end

% -------------------------------------------------------------------------
%                                                             Parse options
% -------------------------------------------------------------------------

d = bsxfun(@minus, x, x0) ;

if ~isempty(dzdy) && ~isempty(opts.instanceWeights)
  dzdy = bsxfun(@times, opts.instanceWeights, dzdy) ;
end

if ~opts.noRoot
  if isempty(dzdy)
    if p == 1
      y1 = sum(abs(d),3) ;
    elseif p == 2
      y1 = sqrt(sum(d.*d,3)) ;
    else
      y1 = sum(abs(d).^p,3).^(1/p) ;
    end
  else
    if p == 1
      y1 = bsxfun(@times, dzdy, sign(d)) ;
    elseif p == 2
      y1 = max(sum(d.*d,3), opts.epsilon).^(-0.5) ;
      y1 = bsxfun(@times, bsxfun(@times, dzdy, y1),  d) ;
    elseif p < 1
      y1 = sum(abs(d).^p,3).^((1-p)/p) ;
      y1 = bsxfun(@times, bsxfun(@times, dzdy, y1), max(abs(d), opts.epsilon).^(p-1) .* sign(d)) ;
    else
      y1 = max(sum(abs(d).^p,3), opts.epsilon).^((1-p)/p) ;
      y1 = bsxfun(@times, bsxfun(@times, dzdy, y1), abs(d).^(p-1) .* sign(d)) ;
    end
  end
else
  if isempty(dzdy)
    if p == 1
      y1 = sum(abs(d),3) ;
    elseif p == 2
      y1 = sum(d.*d,3) ;
    else
      y1 = sum(abs(d).^p,3) ;
    end
  else
    if p == 1
      y1 = bsxfun(@times, dzdy, sign(d)) ;
    elseif p == 2
      y1 = bsxfun(@times, 2 * dzdy, d) ;
    elseif p < 1
      y1 = bsxfun(@times, p * dzdy, max(abs(d), opts.epsilon).^(p-1) .* sign(d)) ;
    else
      y1 = bsxfun(@times, p * dzdy, abs(d).^(p-1) .* sign(d)) ;
    end
  end
end

if isempty(dzdy)
  if ~isempty(opts.instanceWeights)
    y1 = bsxfun(@times, opts.instanceWeights, y1) ;
  end
  if opts.aggregate
    y1 = sum(sum(y1)) ;
  end
end
if ~isempty(dzdy), y2 = -y1; end


%PARZENC Optimisation of the Parzen classifier
%
%  [W,H] = PARZENC(A,H)
%  [W,H] = A*PARZENC([],H)
%  [W,H] = A*PARZENC(H)
%
% INPUT
%  A    dataset
%  H    smoothing parameter (may be scalar, vector of per-class
%       parameters, or matrix with parameters for each class (rows) and
%       dimension (columns))
%
% OUTPUT
%  W    trained mapping
%  H    estimated smoothing (scalar value)
%
% DESCRIPTION
% Computation of the optimum smoothing parameter H for the Parzen
% classifier between the classes in the dataset A. The leave-one-out
% Lissack & Fu estimate is used for the classification error E. The
% final classifier is stored as a mapping in W. It may be converted
% into a classifier by W*CLASSC. PARZENC cannot be used for density
% estimation.
%
% In case smoothing H is specified, no learning is performed, just the
% discriminant W is produced for the given smoothing parameters H.
% Smoothing parameters may be scalar, vector of per-class parameters, or
% a matrix with individual smoothing for each class (rows) and feature
% directions (columns)
%
% EXAMPLES
% See PREX_DENSITY for densities and PREX_PARZEN for differences between
% PARZENC, PARZENDC and PARZENM.
%
% REFERENCES
% T. Lissack and K.S. Fu, Error estimation in pattern recognition via
% L-distance between posterior density functions, IEEE Trans. Inform.
% Theory, vol. 22, pp. 34-45, 1976.
%
% SEE ALSO (PRTools Guide)
% DATASETS, MAPPINGS, PARZEN_MAP, PARZENML, PARZENDC, PARZENM, CLASSC,
% PREX_PARZEN

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

% $Id: parzenc.m,v 1.6 2008/07/03 09:11:44 duin Exp $

function [W,h] = parzenc(varargin)

	mapname = 'ParzenC';
  argin = shiftargin(varargin,'scalar');
  argin = setdefaults(argin,[],[]);

  if mapping_task(argin,'definition')
    W = define_mapping(argin,'untrained',mapname);

  elseif mapping_task(argin,'training')			% Train a mapping.

    [a,h] = deal(argin{:});
    islabtype(a,'crisp','soft');
    isvaldfile(a,2,2); % at least 2 objects per class, 2 classes
    a = testdatasize(a);
    a = testdatasize(a,'objects');

    [m,k,c] = getsize(a);
    nlab = getnlab(a);

    if ~isempty(h)       % take user setting for smoothing parameter

      if size(h,1) == 1, h = repmat(h,c,1); end
      if size(h,2) == 1, h = repmat(h,1,k); end
      if any(size(h) ~= [c,k])
        error('Array with smoothing parameters has wrong size');
      end
      W = prmapping('parzen_map','trained',{a,h},getlablist(a),k,c);
      W = setname(W,'Parzen Classifier');
      return

    end

    % compute all object distances
    % make diagonal inf to exclude objects own contribution
    D = +distm(a) + diag(inf*ones(1,m));

    % find object frequencies
    if islabtype(a,'crisp')
      csize = classsizes(a);
      of = csize(nlab);
    else
      csize = sum(gettargets(a),1);
    end

    % find object weights q
    p = getprior(a);
    a = setprior(a,p);
    q = p(nlab)./csize(nlab);

    % initialise
    h = max(std(a)); % for sure a too high value
    L = -inf;
    Ln = 0;
    z = 0.1^(1/k); % initial step size

    % iterate

    iter = 0;
    prwaitbar(100,'parzenc: Optimizing smoothing parameter',m > 100);
    while abs(Ln-L) > 0.001 & z < 1

      % In L we store the best performance estimate found so far.
      % Ln is the actual performance (for the actual h)
      % If Ln > L we improve the bound L, and so we rest it.

      if Ln > L, L = Ln; end
      iter = iter+1;
      prwaitbar(100,100-100*exp(-iter/10));

      r = -0.5/(h^2);
      F = q(ones(1,m),:)'.*exp(D*r);           % density contributions
      FS = sum(F)*((m-1)/m); IFS = find(FS>0); % joint density distribution
      if islabtype(a,'crisp');
        G = sum(F .* (nlab(:,ones(1,m)) == nlab(:,ones(1,m))'));
        G = G.*(of-1)./of;                     % true-class densities
      else
        % here we are for soft labels (stored in targets)
        G = zeros(1,m);
        for j=1:c
          G = G + sum(F .* (a.targets(:,j) * a.targets(:,j)'));
          % to be corrected for bias?
        end
      end

      % performance estimate
      en = max(p)*ones(1,m);
      en(IFS) = (G(IFS))./FS(IFS);
      Ln = exp(sum(log(en))/m);

      if Ln < L            % compute next estimate
        z = sqrt(z);       % adjust stepsize up (recall: 0 < z < 1)
        h = h / z;         % if we don't improve, increase h (approach h_opt from below)
      else
        h = h * z;         % if we improve, decrease h (approach h_opt from above)
      end
    end
    prwaitbar(0);
    W = prmapping('parzen_map','trained',{a,repmat(h,c,k);},getlablist(a),k,c);
    W = setname(W,mapname);
    W = setcost(W,a);

  end

return


function out = gpuBlockSmallXtY(X, Y)

g = gpuDevice;
bytesGPU = g.AvailableMemory;
infoX = whos('X');
infoY = whos('Y');
[hX, wX] = size(X);
[hY, wY] = size(Y); % hY should be equal to hX
% the dimensions of out will be wX-by-wY
bytesPerRowX = infoX.bytes/hX;
bytesPerRowY = infoY.bytes/hY;
% theoretically, to keep the result and {X', X, and Y} in GPU memory
% bytesRequired = bytesPerRowX*wY + batchSize*(2*bytesPerRowX+bytesPerRowY);
batchSize = floor((bytesGPU-bytesPerRowX*wY)/(2*bytesPerRowX+bytesPerRowY));
batchSize = floor(batchSize/8); % just to be on the safe side
% also, interestingly with smaller batches the code runs slightly faster
nBatches = ceil(hX/batchSize);

startIdx = 1:batchSize:hX;
endIdx = startIdx + batchSize-1;
endIdx = min(endIdx, hX);

out = gpuArray(zeros([wX, wY], 'like', X));
for iBatch = 1:nBatches
%     fprintf('Batch %d/%d\n', iBatch, nBatches);
    batchX = gpuArray(X(startIdx(iBatch):endIdx(iBatch), :));
    batchY = gpuArray(Y(startIdx(iBatch):endIdx(iBatch), :));
    out = out + batchX'*batchY;

% MK - this is an annoying line of code, but it helps Matlab to do memory
% management on the GPU properly. Otherwise I get OUT OF MEMORY errors
% maybe it is just my GPU is a bit unstable
    wait(gpuDevice);
end

out = gather(out);

return


function hypersphere_integrals_test ( )

%*****************************************************************************80
%
%% HYPERSPHERE_INTEGRALS_TEST tests the HYPERSPHERE_INTEGRALS library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 January 2014
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HYPERSPHERE_INTEGRALS_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the HYPERSPHERE_INTEGRALS library.\n' );

  hypersphere_integrals_test01 ( );
  hypersphere_integrals_test02 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HYPERSPHERE_INTEGRALS_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function [f_unk_1]=rlse(yk_1,hk_1,G_un,rk)

% Estimate sk_1: A matrix with dimension rxr
Sk_1=inv(G_un'*inv(rk)*G_un);

% Recusive solution for unknown excitation
f_unk_1=Sk_1*G_un'*inv(rk)*(yk_1-hk_1);


function final_score = aggregation_attention(score, attention, T)

if size(score, 1)
    score = cat(1, score, score);
    attention = cat(1, attention, attention);
end

if size(score, 3) > 1
score = double(squeeze(mean(score, 2)));
attention = double(squeeze(mean(attention, 2)));
end

attention = attention/T;
softmax_attention = exp(attention);
softmax_attention = bsxfun(@rdivide, softmax_attention, sum(softmax_attention));

final_score = bsxfun(@times, score, softmax_attention);
final_score = sum(final_score, 1);

end


function [dP] = GradJacobiP(r, alpha, beta, N);

% function [dP] = GradJacobiP(r, alpha, beta, N);
% Purpose: Evaluate the derivative of the Jacobi polynomial of type (alpha,beta)>-1,
%	       at points r for order N and returns dP[1:length(r))]

dP = zeros(length(r), 1);
if(N == 0)
  dP(:,:) = 0.0;
else
  dP = sqrt(N*(N+alpha+beta+1))*JacobiP(r(:),alpha+1,beta+1, N-1);
end;
return


function res = immedian(img, filtre, varargin)
%IMMEDIAN Compute median value in the neighboorhood of each pixel
%
%   RES = immedian(IMG, SE)
%   Compute the median filter of image IMG, using structuring element SE.
%   The goal of this function is to provide the same interface as for
%   other image filters (imopen, imerode ...), and to allow the use of
%   median filter with user-defined structuring element.
%   This function can be used for directional filtering.
%
%
%   RES = immedian(IMG, SE, PADOPT) also specify padding option. PADOPT can
%   be either 'zeros' or 'SYMMETRIC', see medfilt2 or ordfilt2 for details.
%
%   See Also: IMMEAN, IMDIRFILTER, MEDFILT2, ORDFILT2
%
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 16/02/2004.
%

%   HISTORY
%   17/02/2004: add support for 'strel' objects.
%   20/02/2004: add 'padopt' option, and documentation
%   2011-11-05 deprecate

warning('imael:deprecatedFunction', ...
    'function "immedian" has been deprecated and replaced by "imMedianFilter"');

% transform STREL object into single array
if strcmp(class(filtre), 'strel') %#ok<STISA>
    filtre = getnhood(filtre);
end

% get padopt option.
padopt = 'zeros'; % default for standard median filtering in matlab
if ~isempty(varargin)
    padopt = varargin{1};
end

% perform filtering
order = ceil(sum(filtre(:))/2);
res = ordfilt2(img, order, filtre, padopt);


% Hoer's code (P. O. Hoyer. Non-negative Matrix Factorization with sparseness constraints.
% Journal of Machine Learning Research  5:1457-1469, 2004.) is modified

function [W, H, objhistory, objhistory_v] = nmf( V, rdim, showflag, ttt )
%% INPUT
% V: data matrix
% rdim : matrix factorization rank
% showflag: if it is 1. than it plots the change of reconstruction error
% for each iteration
% ttt: maximum number of iterations allowed

%% OUTPUT
% W: Mixing matrix (V=WH)
% H: Encoding matrix (V=WH)
% objhistory: change of total reconstruction error
% objhistory_v: reconstruction error for each individual sample

% Check that we have non-negative data
if min(V(:))<0, error('Negative values in data!'); end

% Dimensions
vdim = size(V,1);
samples = size(V,2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% initialization %%%
fname2=['initials/Initials' num2str(vdim) 'x' num2str(rdim) '.txt'];
fid=fopen(fname2,'r');
if (fid==-1)
    ssbinitial(vdim,rdim);
end
fidW2 = fopen(fname2,'r');
W = fscanf(fidW2,'%f', [vdim inf]);
fclose(fidW2);

clear fname2

fname2=['initials/Initials' num2str(rdim) 'x' num2str(samples) '.txt'];
fid=fopen(fname2,'r');
if (fid==-1)
    ssbinitial(rdim,samples);
end
fidW2 = fopen(fname2,'r');
H = fscanf(fidW2,'%f', [rdim inf]);
fclose(fidW2);

clear fname2

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

if (showflag==1)
    objhistory = ((sum(sum((V-W*H).^2))))/(vdim*samples);
    figure(2); clf;
    drawnow;
end

% Start iteration
iter = 0;
while 1,

    if (iter==ttt)
        break
    end
    iter = iter+1;
    % Compute new W and H (Lee and Seung; NIPS*2000)
    H = H.*(W'*V)./(W'*W*H + 1e-9);
    W = W.*(V*H')./(W*H*H' + 1e-9);

%     norms = sqrt(sum(W.^2));
%     H = H.*(norms'*ones(1,samples));
%     W = W./(ones(vdim,1)*norms);

    newobj = ((sum(sum((V-W*H).^2))))/(vdim*samples);
    newobj_v = ((sum(sum((V(:,end)-W*H(:,end)).^2))))/(vdim);
    if iter==1
        objhistory = [newobj];
        objhistory_v = [newobj_v];
    else
        objhistory = [objhistory newobj];
        objhistory_v = [objhistory_v newobj_v];
    end

    if(showflag==1)
        if (iter>1)
            plot(objhistory(2:end));
        end
        drawnow;
    end
end


function nfigures = NumTotFigures(row,col,nvar)
% =======================================================================
% Determine the number of figures for a given number of subplots, rows
% and columns
% =======================================================================
% nfigures = NumTotFigures(row,col,nvar)
% -----------------------------------------------------------------------
% INPUT
%	- row: number of rows of the subplot
%	- col: number of columns of the subplot
%	- nvar: number of variables
% -----------------------------------------------------------------------
% OUTPUT
%	- nfigures: number of charts per subplot
% =======================================================================
% VAR Toolbox 3.0
% Ambrogio Cesa-Bianchi
% ambrogiocesabianchi@gmail.com
% March 2015. Updated November 2020
% -----------------------------------------------------------------------

NumGraphXPage = row*col;

p = floor(nvar./NumGraphXPage);
q = (nvar./NumGraphXPage);

if NumGraphXPage>=nvar
    nfigures=1;
elseif p==q
    nfigures=p;
else
    nfigures=p+1;
end
clear p q


function b = gray2bi( g )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                                                                %
%%   GRAY2BI converts Gray encoded sequence into the binary       %
%%   sequence. It is assumed that the most significant bit is     %
%%   the left hand side bit.                                      %
%%                                                                %
%%   Comments and suggestions to: batlles@gmail.com               %
%%                                                                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % copy the msb:
    b(:,1) = g(:,1);

    for i = 2:size(g,2),
        b(:,i) = xor( b(:,i-1), g(:,i) );
    end

return;


function [ n_data, n, x, fx ] = laguerre_polynomial_values ( n_data )

%*****************************************************************************80
%
%% LAGUERRE_POLYNOMIAL_VALUES returns some values of the Laguerre polynomial.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      LaguerreL[n,x]
%
%  Differential equation:
%
%    X * Y'' + (1-X) * Y' + N * Y = 0
%
%  First terms:
%
%      1
%     -X   +  1
%   (  X^2 -  4 X   +   2 ) / 2
%   ( -X^3 +  9 X^2 -  18 X   +    6 ) / 6
%   (  X^4 - 16 X^3 +  72 X^2 -   96 X +      24 ) / 24
%   ( -X^5 + 25 X^4 - 200 X^3 +  600 X^2 -   600 X   +   120 ) / 120
%   (  X^6 - 36 X^5 + 450 X^4 - 2400 X^3 +  5400 X^2 -  4320 X   +   720 ) / 720
%   ( -X^7 + 49 X^6 - 882 X^5 + 7350 X^4 - 29400 X^3 + 52920 X^2 - 35280 X + 5040 ) / 5040
%
%  Recursion:
%
%    L(0)(X) = 1,
%    L(1)(X) = 1-X,
%    N * L(N)(X) = (2*N-1-X) * L(N-1)(X) - (N-1) * L(N-2)(X)
%
%  Orthogonality:
%
%    Integral ( 0 <= X < oo ) exp ( - X ) * L(N)(X) * L(M)(X) dX
%    = 0 if N /= M
%    = 1 if N == M
%
%  Special values:
%
%    L(N)(0) = 1.
%
%  Relations:
%
%    L(N)(X) = (-1)^N / N! * exp ( x ) * (d/dx)^n ( exp ( - x ) * X^n )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the order of the polynomial.
%
%    Output, real X, the point where the polynomial is evaluated.
%
%    Output, real FX, the value of the function.
%
  n_max = 17;

  fx_vec = [ ...
      0.1000000000000000E+01, ...
      0.0000000000000000E+00, ...
     -0.5000000000000000E+00, ...
     -0.6666666666666667E+00, ...
     -0.6250000000000000E+00, ...
     -0.4666666666666667E+00, ...
     -0.2569444444444444E+00, ...
     -0.4047619047619048E-01, ...
      0.1539930555555556E+00, ...
      0.3097442680776014E+00, ...
      0.4189459325396825E+00, ...
      0.4801341790925124E+00, ...
      0.4962122235082305E+00, ...
     -0.4455729166666667E+00, ...
      0.8500000000000000E+00, ...
     -0.3166666666666667E+01, ...
      0.3433333333333333E+02  ];

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

  x_vec = [ ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     0.5E+00, ...
     3.0E+00, ...
     5.0E+00, ...
     1.0E+01 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function [handles, key_points] = draw_pie_wedge(T1, T2, myradius, varargin)
% Draws a pie wedge with color fill, for polar plots and circular charts
%
% :Usage:
% ::
%
%     handles = draw_pie_wedge(T1, T2, myradius, ['linecolor'], [r g b], ['fillcolor'], [r g b])
%
% ..
%     Dec 2017, Tor Wager
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
% ..
%
% :Inputs:
%
%   **T1:**
%        Starting location, in radians. 0 = to the right on plot; 2*pi is
%        full-circle, also to the right on plot.
%
%   **T2:**
%        Ending location, in radians. 0 = to the right on plot; 2*pi is
%        full-circle, also to the right on plot.
%
% :Optional Inputs:
%   **'linecolor':**
%        Followed by [r g b] triplet specifying line color, or 'none'
%
%   **'fillcolor:**
%        Followed by [r g b] triplet specifying fill color, or 'none'
%
%   **'outside_radius_offset'**
%        Followed by outside_radius_offset value. See below.
%
%   **'linewidth':**
%       Followed by line width
%
%   **'stripes':**
%       Turn on radial striping; can be followed by value true or false
%
%   **'stripedensity':**
%       Number of stripes/lines to draw in wedge
%
% :Outputs:
%
%   **handles:**
%        A structure of line and fill handles
%
%   **key_points:**
%        x and y locations for the mid-point of the arc, inner mid-point of the
%        wedge, and outer mid-point offset by a constant value. This is
%        set by outside_radius_offset, and is useful for text labels
%
% :Examples:
% ::
%
% myradius = 3;
% T1 = 1; % start, in radians
% T2 = 4; % end, in radians
%
% handles = draw_pie_wedge(1, 3, 2); % Draw wedge from radian=1 to 3, radius = 2
%
% Draw reference circle with radius = 2
% handles = draw_pie_wedge(0, 2*pi, 2, 'linecolor', [0 0 0], 'fillcolor', 'none');
%
% Now draw a wedge on top of that:
% handles = draw_pie_wedge(1, 1.5, 2.5, 'linecolor', [.7 .7 0], 'fillcolor', [.3 .7 .7]);
%
% Draw with dense striping pattern:
% handles = draw_pie_wedge(1, 1.5, 2.5, 'linecolor', [.7 .7 0], 'fillcolor', [.3 .7 .7], 'stripes', 'stripedensity', 40);
%
% Draw a series of wedges in different colors:
% n_categories = 7;
% st = linspace(0, 2*pi, n_categories+1); % starting vals in radians
%
% en = st(2:end);
% st = st(1:end-1);
%
% colors = seaborn_colors(n_categories + 1); % add one to make colors more diff
% clear handles
%
% figure; hold on;
%
% for i = 1:n_categories
%
%     handles(i) = draw_pie_wedge(st(i), en(i), i, 'linecolor', colors{i} ./ 2, 'fillcolor', colors{i});
%
% end
%
% axis equal
%
% For another extended example with text labels, see tor_wedge_plot.m

linecolor = [1 0 0];
fillcolor = [.7 .3 .7];
linewidth = 2;
outside_radius_offset = .04; % Value to offset by when calculating outside-wedge midpoint for labels
dostripes = false;
stripedensity = 12;

for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}

            case 'linecolor', linecolor = varargin{i+1}; varargin{i+1} = [];
            case 'fillcolor', fillcolor = varargin{i+1}; varargin{i+1} = [];

            case 'linewidth', linewidth = varargin{i+1}; varargin{i+1} = [];

            case 'stripes'
                dostripes = true;
                % Optional argument true or false following keyword
                if length(varargin) > i && ~ischar(varargin{i+1}) % param value entered in next arg
                    dostripes = varargin{i+1}; varargin{i+1} = [];
                end

            case 'stripedensity', stripedensity = varargin{i+1}; varargin{i+1} = [];

            case 'outside_radius_offset', outside_radius_offset = varargin{i+1}; varargin{i+1} = [];
            otherwise, warning(['Unknown input string option:' varargin{i}]);
        end
    end
end

% This would draw a reference circle
%hold on
%h0 = drawCircle(0, 0, myradius);
%set(h0, 'Color', 'k');

% Circle Arc
% ------------------------------------------
%h = drawCircleArc(0, 0, myradius, T1, T2);

[h xt yt] = drawArc(T1, T2, myradius);

% get midpoint of arc
% ------------------------------------------
tmid = (T1+T2)/2;
xmid_arc = 0 + myradius*cos(tmid);
ymid_arc = 0 + myradius*sin(tmid);

% Middle of pie
xmid_pie = 0 + myradius/2*cos(tmid);
ymid_pie = 0 + myradius/2*sin(tmid);

% outside arc
rad2 = myradius + outside_radius_offset; %.1 * myradius;
xmid_outside = 0 + rad2*cos(tmid);
ymid_outside = 0 + rad2*sin(tmid);

% others
radialangle = rad2deg(tmid); % radial angle in degrees
tangentangle = rad2deg(tmid + (pi/2)); % angle of tangent, for text

key_points = struct('tmid_radians', tmid, 'radialangle', radialangle, 'tangentangle', tangentangle, ...
    'xmid_arc', xmid_arc, 'ymid_arc', ymid_arc, 'xmid_outside', xmid_outside, ...
    'ymid_outside', ymid_outside, 'xmid_pie', xmid_pie);

set(h, 'Color', linecolor, 'LineWidth', linewidth)

% Pie wedge
% ------------------------------------------
hold on
[x1,y1] = pol2cart(T1, myradius);
h(2) = plot([0 x1], [0 y1], 'Color', linecolor, 'LineWidth', linewidth);

[x2,y2] = pol2cart(T2, myradius);
h(3) = plot([0 x2], [0 y2], 'Color', linecolor, 'LineWidth', linewidth);

% Fill
% ------------------------------------------

if ischar(fillcolor) && strcmp(fillcolor, 'none')
    % skip it
    hfill = [];
else
    hfill = fill([xt 0], [yt 0], fillcolor, 'FaceAlpha', .65);
end

handles = struct('line_han', h, 'fill_han', hfill);

% Striping
% ------------------------------------------
if dostripes

    handles.stripe_han = drawStripes(T1, T2, myradius, stripedensity);
    set(handles.stripe_han, 'Color', linecolor, 'LineWidth', linewidth)

else

    handles.stripe_han = [];

end

end % main function

function [h xt yt] = drawArc(T1, T2, myradius)

% key bits
t = T1:.01:T2;          % Arc
xt = 0 + myradius*cos(t);
yt = 0 + myradius*sin(t);
xt = [xt 0+myradius*cos(T2)];
yt = [yt 0+myradius*sin(T2)];

h = line(xt, yt);

end

function h = drawStripes(T1, T2, myradius, stripedensity)

r = linspace(0, myradius, stripedensity);

for i = 1:length(r)

    h(i) = drawArc(T1, T2, r(i));

end

end


function uscita = deg2rad(ingresso)
uscita = 2*pi*ingresso/360;


[datapoints, numpoints] = px4_read_binary_file('e4_ekf_A.bin');
roll_ekf = datapoints(1, :);
pitch_ekf = datapoints(2, :);
roll_cf = datapoints(3, :);
pitch_cf = datapoints(4, :);
roll_px4 = datapoints(5, :);
pitch_px4 = datapoints(6, :);
t = datapoints(7, :);
rad2deg = 180/pi;
figure(1)
plot(t, roll_ekf*rad2deg, 'g', t, roll_cf*rad2deg, 'b', t, roll_px4*rad2deg, 'r')
legend('ekf', 'cf', 'px4');
xlabel('time/s');
ylabel('roll/deg')
title('roll-Filter effect comparison')


function X = slpcarecon(S, Y)
%SLPCARECON Reconstructs the samples in original space
%
% $ Syntax $
%   - Xr = slpcarecon(S, Y)
%
% $ Arguments $
%   - S:        the PCA model struct
%   - Y:        the principal component features
%   - Xr:       the reconstructed samples
%
% $ Description $
%   - Xr = slpcarecon(S, Y) reconstructs the original samples approximately
%     using the principal components Y. If the dimension of Y is less than
%     the subspace dimension, the leading space dimensions will be used.
%
% $ History $
%   - Created by Dahua Lin, on Aug 17, 2006
%   - Modified by Dahua Lin, on Sep 10, 2006
%       - replace sladd by sladdvec to increase efficiency
%

%% parse and verify input

if ~isstruct(S)
    error('sltoolbox:invalidarg', ...
        'S should be a PCA model struct');
end

if ~isnumeric(Y) || ndims(Y) ~= 2
    error('sltoolbox:invalidarg', ...
        'The features Y should be a 2D numeric matrix');
end

dy = size(Y, 1);
if dy > S.feadim
    error('sltoolbox:sizmismatch', ...
        'The feature dimension of Y exceeds the subspace dimension preserved in model');
end

%% reconstruct

if dy == S.feadim
    X = S.P * Y;
else
    X = S.P(:, 1:dy) * Y;
end

X = sladdvec(X, S.vmean, 1);


% Uncomment individual Example lines.

%---- Example 1: E5 (nyquist frequency)
fs = 120*1.023e6;
n_E5_periods = 1;

%---- Example 2: E5 (nyquist frequency, several periods)
% fs = 120*1.023e6;
% n_E5_periods = 8.2;

%---- Example 3: E5 (sub-nyquist frequency, several periods)
% fs = 10*1.023e6;
% n_E5_periods = 2.2;

%---- Example 4: E5 (higher frequency, several periods)
% fs = 130*1.023e6;
% n_E5_periods = 1.2;

%---- Example 4: E5 (higher frequency non multiple, several periods)
% fs = 130*1e6;
% n_E5_periods = 1.2;

%---- Example 4: E5 (lower frequency non multiple, several periods)
% fs = 10*1e6;
% n_E5_periods = 1.2;

coh_time = n_E5_periods*1e-3;
fh = BOCgen;
[sd1, sd2, sp1 sp2]  = fh.BOC_E5(coh_time,fs);

%==========================================================================
%% Plots
%==========================================================================
t_boc_chips = linspace(0,1.023e6*120*coh_time,length(sd1));

figure;
        hold on;
        stairs(t_boc_chips,sd1,'b');
        stairs(t_boc_chips,sd2,'r');
        grid on;
        title('Data spread codes');
        xlabel('BOC symbol period')
        ylabel('amplitude');
        legend('Data spread code', 'Data spread code delayed');

figure; hold on;
        stairs(t_boc_chips,sp1,'b');
        stairs(t_boc_chips,sp2,'r');
        grid on;
        title('Pilot spread codes');
        xlabel('BOC symbol period')
        ylabel('amplitude');
        legend('Pilot spread code', 'Pilot spread code delayed');

% Next plots just shows that consecutive BOC symbols are identical
sub_period = round(fs/1.023e6*120);
n_periods = floor(length(sd1)/sub_period);
figure, hold on;
for i=1:n_periods-1
     plot(sd2(sub_period*(i-1)+1:sub_period*i)-sd2(sub_period*i+1:sub_period*(i+1)));
end

figure, hold on;
for i=1:n_periods-1
     plot(sd1(sub_period*(i-1)+1:sub_period*i)-sd1(sub_period*i+1:sub_period*(i+1)));
end

figure, hold on;
for i=1:n_periods-1
     plot(sp1(sub_period*(i-1)+1:sub_period*i)-sp1(sub_period*i+1:sub_period*(i+1)));
end

figure, hold on;
for i=1:n_periods-1
     plot(sp2(sub_period*(i-1)+1:sub_period*i)-sp2(sub_period*i+1:sub_period*(i+1)));
end


function [u,v,I]=localcross(Y,tol)
% Full-pivoted cross for truncating one ket TT block instead of SVD

if (exist('localcross_mex','file')>0)&&(isreal(Y))
    % Check for a faster mex
    if (nargout>2)
        [u,v,I]=localcross_mex(Y,tol);
    else
        [u,v]=localcross_mex(Y,tol);
    end;
    return;
end;

[n,m,b]=size(Y);

minsz = min(n, m*b);
u = zeros(n, minsz);
v = zeros(minsz, m*b);
res = reshape(Y, n, m*b);
% Return also the indices
I = zeros(1, minsz);
val_max = max(abs(Y(:)));
for r=1:minsz
    res = reshape(res, [], 1);
    [val,piv]=max(abs(res));
    piv = tt_ind2sub([n, m*b], piv);
    if (val<=tol*val_max)
        break;
    end;
    res = reshape(res, n, m*b);
    u(:,r) = res(:, piv(2));
    v(r,:) = res(piv(1), :)/res(piv(1),piv(2));
    res = res - u(:,r)*v(r,:);
    I(r)=piv(1);
end;
% Return indices
r = find(I==0,1);
if (isempty(r))
    r=minsz;
else
    r=r-1;
end;
I = I(1:r);
u = u(:, 1:r);
v = v(1:r, :);
if (r==0)
    u = zeros(n,1);
    v = zeros(1,m);
    I = 1;
end;
% qr u, in case we don't have enrichment
[u,rv]=qr(u,0);
v = rv*v;
end


function Res = dfm(X,Spec,threshold)
%DFM()    Runs the dynamic factor model
%
%  Syntax:
%    Res = DFM(X,Par)
%
%  Description:
%   DFM() inputs the organized and transformed data X and parameter structure Par.
%   Then, the function outputs dynamic factor model structure Res and data
%   summary statistics (mean and standard deviation).
%
%  Input arguments:
%    X: Kalman-smoothed data where missing values are replaced by their expectation
%    Par: A structure containing the following parameters:
%      Par.blocks: Block loadings.
%      Par.nQ: Number of quarterly series
%      Par.p: Number of lags in transition matrix
%      Par.r: Number of common factors for each block
%
% Output Arguments:
%
%   Res - structure of model results with the following fields
%       . X_sm | Kalman-smoothed data where missing values are replaced by their expectation
%       . Z | Smoothed states. Rows give time, and columns are organized according to Res.C.
%       . C | Observation matrix. The rows correspond
%          to each series, and the columns are organized as shown below:
%         - 1-20: These columns give the factor loadings. For example, 1-5
%              give loadings for the first block and are organized in
%              reverse-chronological order (f^G_t, f^G_t-1, f^G_t-2, f^G_t-3,
%              f^G_t-4). Columns 6-10, 11-15, and 16-20 give loadings for
%              the second, third, and fourth blocks respectively.
%       .R: Covariance for observation matrix residuals
%       .A: Transition matrix. This is a square matrix that follows the
%      same organization scheme as Res.C's columns. Identity matrices are
%      used to account for matching terms on the left and righthand side.
%      For example, we place an I4 matrix to account for matching
%      (f_t-1; f_t-2; f_t-3; f_t-4) terms.
%       .Q: Covariance for transition equation residuals.
%       .Mx: Series mean
%       .Wx: Series standard deviation
%       .Z_0: Initial value of state
%       .V_0: Initial value of covariance matrix
%       .r: Number of common factors for each block
%       .p: Number of lags in transition equation
%
% References:
%
%   Marta Banbura, Domenico Giannone and Lucrezia Reichlin
%   Nowcasting (2010)
%   Michael P. Clements and David F. Hendry, editors,
%   Oxford Handbook on Economic Forecasting.

%% Store model parameters ------------------------------------------------

% DFM input specifications: See documentation for details
Par.blocks = Spec.Blocks;                  % Block loading structure
Par.nQ = sum(strcmp('q',Spec.Frequency));  % Number of quarterly series
Par.p = 1;                                 % Number of lags in autoregressive of factor (same for all factors)
Par.r = ones(1,size(Spec.Blocks,2));       % Number of common factors for each block
%Par.r(1) =2;
% Display blocks
try
    fprintf('\n\n\n');
    disp('Table 3: Block Loading Structure')
    disp(array2table(Spec.Blocks,...
         'RowNames', strrep(Spec.SeriesName,' ','_'),...
         'VariableNames',Spec.BlockNames));
    fprintf('\n\n\n');
catch
end

fprintf('Estimating the dynamic factor model (DFM) ... \n\n');

[T,N] = size(X);
r = Par.r;
p = Par.p;
nQ = Par.nQ;
blocks = Par.blocks;

i_idio = logical([ones(N-nQ,1);zeros(nQ,1)]);

%R*Lambda = q; Contraints on the loadings of the quartrly variables

R_mat = [  2 -1  0  0  0;...
           3  0 -1  0  0;...
           2  0  0 -1  0;...
           1  0  0  0 -1];

q = zeros(4,1);

if(nargin < 3)
    threshold = 1e-5;  % EM loop threshold (default value)
end

max_iter = 5000;  % EM loop maximum number of iterations

%% Prepare data -----------------------------------------------------------

Mx = mean(X,'omitnan');
Wx = std(X,'omitnan');
xNaN = (X-repmat(Mx,T,1))./repmat(Wx,T,1);  % Standardize series

%% Initial Conditions -----------------------------------------------------

optNaN.method = 2; % Remove leading and closing zeros
optNaN.k = 3;      % Setting for filter(): See remNaN_spline

[A, C, Q, R, Z_0, V_0] = InitCond(xNaN,r,p,blocks,optNaN,R_mat,q,nQ,i_idio);

% Initialize EM loop values
previous_loglik = -inf;
num_iter = 0;
LL = -inf;
converged = 0;

% y for the estimation is WITH missing data
y = xNaN';

%% EM LOOP ----------------------------------------------------------------

%The model can be written as
%y = C*Z + e;
%Z = A*Z(-1) + v
%where y is NxT, Z is (pr)xT, etc

% Remove the leading and ending nans
optNaN.method = 3;
y_est = remNaNs_spline(xNaN,optNaN)';

while (num_iter < max_iter) & ~converged % Loop until converges or max iter.

    [C_new, R_new, A_new, Q_new, Z_0, V_0, loglik] = ...  % Applying EM algorithm
        EMstep(y_est, A, C, Q, R, Z_0, V_0, r,p,R_mat,q,nQ,i_idio,blocks);

    C = C_new;
    R = R_new;
    A = A_new;
    Q = Q_new;

    if num_iter > 2  % Checking convergence
        [converged, decrease(num_iter + 1)] = ...
            em_converged(loglik, previous_loglik, threshold, 1);
    end

    if (mod(num_iter,10) == 0) && (num_iter > 0)  % Print updates to command window
        disp(['Now running the ',num2str(num_iter),...
              'th iteration of max ', num2str(max_iter)]);
        disp(['  Loglik','   (% Change)'])
        disp([num2str(loglik),'   (', sprintf('%6.2f',100*((loglik-previous_loglik)/previous_loglik)) '%)'])
    end

    LL = [LL loglik];
    previous_loglik = loglik;
    num_iter =  num_iter + 1;

end

if(num_iter < max_iter)
    disp(['Successful: Convergence at ', num2str(num_iter), ' iterations'])
else
   disp('Stopped because maximum iterations reached')
end

% Final run of the Kalman filter
Zsmooth = runKF(y, A, C, Q, R, Z_0, V_0)';

x_sm = Zsmooth(2:end,:) * C';  % Get smoothed X

%%  Loading the structure with the results --------------------------------
Res.x_sm = x_sm;
Res.X_sm = repmat(Wx,T,1) .* x_sm + repmat(Mx,T,1);  % Unstandardized, smoothed
Res.Z = Zsmooth(2:end,:);
Res.C = C;
Res.R = R;
Res.A = A;
Res.Q = Q;
Res.Mx = Mx;
Res.Wx = Wx;
Res.Z_0 = Z_0;
Res.V_0 = V_0;
Res.r = r;
Res.p = p;

%% Display output
% Table with names and factor loadings

nQ       = Par.nQ;                      % Number of quarterly series
nM       = size(Spec.SeriesID,1) - nQ;  % Number monthly series
nLags    = max(Par.p, 5);               % 5 comes from monthly-quarterly aggregation
nFactors = sum(Par.r);

fprintf('\n\n\n');

try
disp('Table 4: Factor Loadings for Monthly Series');
disp(array2table(Res.C(1:nM, 1:5:nFactors*5),...  % Only select lag(0) terms
     'RowNames', strrep(Spec.SeriesName(1:nM),' ','_'), ...
     'VariableNames', Spec.BlockNames));
fprintf('\n\n\n');
disp('Table 5: Quarterly Loadings Sample (Global Factor)')
disp(array2table(Res.C(end-nQ+1:end, 1:5), ...  % Select only quarterly series
     'RowNames', strrep(Spec.SeriesName(end-nQ+1:end),' ','_'), ...
     'VariableNames', {'f1_lag0', 'f1_lag1', 'f1_lag2', 'f1_lag3', 'f1_lag4'}));
fprintf('\n\n\n');
catch
end

% Table with AR model on factors (factors with AR parameter and variance of residuals)

A_terms = diag(Res.A);  % Transition equation terms
Q_terms = diag(Res.Q);  % Covariance matrix terms

try
disp('Table 6: Autoregressive Coefficients on Factors')
disp(table(A_terms(1:5:nFactors*5), ...  % Only select lag(0) terms
           Q_terms(1:5:nFactors*5), ...
           'VariableNames', {'AR_Coefficient', 'Variance_Residual'}, ...
           'RowNames',      strrep(Spec.BlockNames,' ','_')));
fprintf('\n\n\n');
catch
end

% Table with AR model idiosyncratic errors (factors with AR parameter and variance of residuals)
try
disp('Table 7: Autoregressive Coefficients on Idiosyncratic Component')
disp(table(A_terms([nFactors*5+1:nFactors*5+nM nFactors*5+nM+1:5:end]),...  % 21:50 give monthly, 51:5:61 give quarterly
           Q_terms([nFactors*5+1:nFactors*5+nM nFactors*5+nM+1:5:end]), ...
           'VariableNames', {'AR_Coefficient', 'Variance_Residual'}, ...
           'RowNames', strrep(Spec.SeriesName,' ','_')));
catch
end

end

%% PROCEDURES -------------------------------------------------------------
% Note: Kalman filter (runKF()) is in the 'functions' folder

function  [C_new, R_new, A_new, Q_new, Z_0, V_0, loglik] = EMstep(y, A, C, Q, R, Z_0, V_0, r,p,R_mat,q,nQ,i_idio,blocks)
%EMstep    Applies EM algorithm for parameter reestimation
%
%  Syntax:
%    [C_new, R_new, A_new, Q_new, Z_0, V_0, loglik]
%    = EMstep(y, A, C, Q, R, Z_0, V_0, r, p, R_mat, q, nQ, i_idio, blocks)
%
%  Description:
%    EMstep reestimates parameters based on the Estimation Maximization (EM)
%    algorithm. This is a two-step procedure:
%    (1) E-step: the expectation of the log-likelihood is calculated using
%        previous parameter estimates.
%    (2) M-step: Parameters are re-estimated through the maximisation of
%        the log-likelihood (maximize result from (1)).
%
%    See "Maximum likelihood estimation of factor models on data sets with
%    arbitrary pattern of missing data" for details about parameter
%    derivation (Banbura & Modugno, 2010). This procedure is in much the
%    same spirit.
%
%  Input:
%    y:      Series data
%    A:      Transition matrix
%    C:      Observation matrix
%    Q:      Covariance for transition equation residuals
%    R:      Covariance for observation matrix residuals
%    Z_0:    Initial values of factors
%    V_0:    Initial value of factor covariance matrix
%    r:      Number of common factors for each block (e.g. vector [1 1 1 1])
%    p:      Number of lags in transition equation
%    R_mat:  Estimation structure for quarterly variables (i.e. "tent")
%    q:      Constraints on loadings
%    nQ:     Number of quarterly series
%    i_idio: Indices for monthly variables
%    blocks: Block structure for each series (i.e. for a series, the structure
%            [1 0 0 1] indicates loadings on the first and fourth factors)
%
%  Output:
%    C_new: Updated observation matrix
%    R_new: Updated covariance matrix for residuals of observation matrix
%    A_new: Updated transition matrix
%    Q_new: Updated covariance matrix for residuals for transition matrix
%    Z_0:   Initial value of state
%    V_0:   Initial value of covariance matrix
%    loglik: Log likelihood
%
% References:
%   "Maximum likelihood estimation of factor models on data sets with
%   arbitrary pattern of missing data" by Banbura & Modugno (2010).
%   Abbreviated as BM2010
%
%

%% Initialize preliminary values

% Store series/model values
[n, T] = size(y);
nM = n - nQ;  % Number of monthly series
pC = size(R_mat,2);
ppC = max(p,pC);
num_blocks = size(blocks,2);  % Number of blocks

%% ESTIMATION STEP: Compute the (expected) sufficient statistics for a single
%Kalman filter sequence

% Running the Kalman filter and smoother with current parameters
% Note that log-liklihood is NOT re-estimated after the runKF step: This
% effectively gives the previous iteration's log-likelihood
% For more information on output, see runKF
[Zsmooth, Vsmooth, VVsmooth, loglik] = runKF(y, A, C, Q, R, Z_0, V_0);

%% MAXIMIZATION STEP (TRANSITION EQUATION)
% See (Banbura & Modugno, 2010) for details.

% Initialize output
A_new = A;
Q_new = Q;
V_0_new = V_0;

%%% 2A. UPDATE FACTOR PARAMETERS INDIVIDUALLY ----------------------------

for i = 1:num_blocks  % Loop for each block: factors are uncorrelated

    % SETUP INDEXING
    r_i = r(i);  % r_i = 1 if block is loaded
    rp = r_i*p;
    rp1 = sum(r(1:i-1))*ppC;
    b_subset = rp1+1:rp1+rp;  % Subset blocks: Helps for subsetting Zsmooth, Vsmooth
    t_start = rp1+1;          % Transition matrix factor idx start
    t_end = rp1+r_i*ppC;      % Transition matrix factor idx end

    % ESTIMATE FACTOR PORTION OF Q, A
    % Note: EZZ, EZZ_BB, EZZ_FB are parts of equations 6 and 8 in BM 2010

    % E[f_t*f_t' | Omega_T]
    EZZ = Zsmooth(b_subset, 2:end) * Zsmooth(b_subset, 2:end)'...
        +sum(Vsmooth(b_subset, b_subset, 2:end) ,3);

    % E[f_{t-1}*f_{t-1}' | Omega_T]
    EZZ_BB = Zsmooth(b_subset, 1:end-1)*Zsmooth(b_subset, 1:end-1)'...
            +sum(Vsmooth(b_subset, b_subset, 1:end-1), 3);

    % E[f_t*f_{t-1}' | Omega_T]
    EZZ_FB = Zsmooth(b_subset, 2:end)*Zsmooth(b_subset, 1:end-1)'...
        +sum(VVsmooth(b_subset, b_subset, :), 3);

    % Select transition matrix/covariance matrix for block i
    A_i = A(t_start:t_end, t_start:t_end);
    Q_i = Q(t_start:t_end, t_start:t_end);

    % Equation 6: Estimate VAR(p) for factor
    A_i(1:r_i,1:rp) = EZZ_FB(1:r_i,1:rp) * inv(EZZ_BB(1:rp,1:rp));

    % Equation 8: Covariance matrix of residuals of VAR
    Q_i(1:r_i,1:r_i) = (EZZ(1:r_i,1:r_i) - A_i(1:r_i,1:rp)* EZZ_FB(1:r_i,1:rp)') / T;

    % Place updated results in output matrix
    A_new(t_start:t_end, t_start:t_end) = A_i;
    Q_new(t_start:t_end, t_start:t_end) = Q_i;
    V_0_new(t_start:t_end, t_start:t_end) =...
        Vsmooth(t_start:t_end, t_start:t_end,1);
end

%%% 2B. UPDATING PARAMETERS FOR IDIOSYNCRATIC COMPONENT ------------------

rp1 = sum(r)*ppC;           % Col size of factor portion
niM = sum(i_idio(1:nM));    % Number of monthly values
t_start = rp1+1;            % Start of idiosyncratic component index
i_subset = t_start:rp1+niM; % Gives indices for monthly idiosyncratic component values

% Below 3 estimate the idiosyncratic component (for eqns 6, 8 BM 2010)

% E[f_t*f_t' | \Omega_T]
EZZ = diag(diag(Zsmooth(t_start:end, 2:end) * Zsmooth(t_start:end, 2:end)'))...
    + diag(diag(sum(Vsmooth(t_start:end, t_start:end, 2:end), 3)));

% E[f_{t-1}*f_{t-1}' | \Omega_T]
EZZ_BB = diag(diag(Zsmooth(t_start:end, 1:end-1)* Zsmooth(t_start:end, 1:end-1)'))...
       + diag(diag(sum(Vsmooth(t_start:end, t_start:end, 1:end-1), 3)));

% E[f_t*f_{t-1}' | \Omega_T]
EZZ_FB = diag(diag(Zsmooth(t_start:end, 2:end)*Zsmooth(t_start:end, 1:end-1)'))...
       + diag(diag(sum(VVsmooth(t_start:end, t_start:end, :), 3)));

A_i = EZZ_FB * diag(1./diag((EZZ_BB)));  % Equation 6
Q_i = (EZZ - A_i*EZZ_FB') / T;           % Equation 8

% Place updated results in output matrix
A_new(i_subset, i_subset) = A_i(1:niM,1:niM);
Q_new(i_subset, i_subset) = Q_i(1:niM,1:niM);
V_0_new(i_subset, i_subset) = diag(diag(Vsmooth(i_subset, i_subset, 1)));

%% 3 MAXIMIZATION STEP (observation equation)

%%% INITIALIZATION AND SETUP ----------------------------------------------
Z_0 = Zsmooth(:,1); %zeros(size(Zsmooth,1),1); %

% Set missing data series values to 0
nanY = isnan(y);
y(nanY) = 0;

% LOADINGS
C_new = C;

% Blocks
bl = unique(blocks,'rows');  % Gives unique loadings
n_bl = size(bl,1);           % Number of unique loadings

% Initialize indices: These later help with subsetting
bl_idxM = [];  % Indicator for monthly factor loadings
bl_idxQ = [];  % Indicator for quarterly factor loadings
R_con = [];    % Block diagonal matrix giving monthly-quarterly aggreg scheme
q_con = [];

% Loop through each block
for i = 1:num_blocks
    bl_idxQ = [bl_idxQ repmat(bl(:,i),1,r(i)*ppC)];
    bl_idxM = [bl_idxM repmat(bl(:,i),1,r(i)) zeros(n_bl,r(i)*(ppC-1))];
    R_con = blkdiag(R_con, kron(R_mat,eye(r(i))));
    q_con = [q_con;zeros(r(i)*size(R_mat,1),1)];
end

% Indicator for monthly/quarterly blocks in observation matrix
bl_idxM = logical(bl_idxM);
bl_idxQ = logical(bl_idxQ);

i_idio_M = i_idio(1:nM);            % Gives 1 for monthly series
n_idio_M = length(find(i_idio_M));  % Number of monthly series
c_i_idio = cumsum(i_idio);          % Cumulative number of monthly series

for i = 1:n_bl  % Loop through unique loadings (e.g. [1 0 0 0], [1 1 0 0])

    bl_i = bl(i,:);
    rs = sum(r(logical(bl_i)));                    % Total num of blocks loaded
    idx_i = find(ismember(blocks, bl_i, 'rows'));  % Indices for bl_i
    idx_iM = idx_i(idx_i<nM+1);                    % Only monthly
    n_i = length(idx_iM);                          % Number of monthly series

    % Initialize sums in equation 13 of BGR 2010
    denom = zeros(n_i*rs,n_i*rs);
    nom = zeros(n_i,rs);

    % Stores monthly indicies. These are done for input robustness
    i_idio_i = i_idio_M(idx_iM);
    i_idio_ii = c_i_idio(idx_iM);
    i_idio_ii = i_idio_ii(i_idio_i);

    %%% UPDATE MONTHLY VARIABLES: Loop through each period ----------------
    for t = 1:T
        Wt = diag(~nanY(idx_iM, t));  % Gives selection matrix (1 for nonmissing values)

        denom = denom +...  % E[f_t*t_t' | Omega_T]
                kron(Zsmooth(bl_idxM(i, :), t+1) * Zsmooth(bl_idxM(i, :), t+1)' + ...
                Vsmooth(bl_idxM(i, :), bl_idxM(i, :), t+1), Wt);

        nom = nom + ...  E[y_t*f_t' | \Omega_T]
              y(idx_iM, t) * Zsmooth(bl_idxM(i, :), t+1)' - ...
              Wt(:, i_idio_i) * (Zsmooth(rp1 + i_idio_ii, t+1) * ...
              Zsmooth(bl_idxM(i, :), t+1)' + ...
              Vsmooth(rp1 + i_idio_ii, bl_idxM(i, :), t+1));
    end

    vec_C = inv(denom)*nom(:);  % Eqn 13 BGR 2010

    % Place updated monthly results in output matrix
    C_new(idx_iM,bl_idxM(i,:)) = reshape(vec_C, n_i, rs);

   %%% UPDATE QUARTERLY VARIABLES -----------------------------------------

   idx_iQ = idx_i(idx_i > nM);  % Index for quarterly series
   rps = rs * ppC;

   % Monthly-quarterly aggregation scheme
   R_con_i = R_con(:,bl_idxQ(i,:));
   q_con_i = q_con;

   no_c = ~(any(R_con_i,2));
   R_con_i(no_c,:) = [];
   q_con_i(no_c,:) = [];

   % Loop through quarterly series in loading. This parallels monthly code
   for j = idx_iQ'
       % Initialization
       denom = zeros(rps,rps);
       nom = zeros(1,rps);

       idx_jQ = j-nM;  % Ordinal position of quarterly variable
       % Loc of factor structure corresponding to quarterly var residuals
       i_idio_jQ = (rp1 + n_idio_M + 5*(idx_jQ-1)+1:rp1+ n_idio_M + 5*idx_jQ);

       % Place quarterly values in output matrix
       V_0_new(i_idio_jQ, i_idio_jQ) = Vsmooth(i_idio_jQ, i_idio_jQ,1);
       A_new(i_idio_jQ(1), i_idio_jQ(1)) = A_i(i_idio_jQ(1)-rp1, i_idio_jQ(1)-rp1);
       Q_new(i_idio_jQ(1), i_idio_jQ(1)) = Q_i(i_idio_jQ(1)-rp1, i_idio_jQ(1)-rp1);

       for t=1:T
           Wt = diag(~nanY(j,t));  % Selection matrix for quarterly values

           % Intermediate steps in BGR equation 13
           denom = denom + ...
                   kron(Zsmooth(bl_idxQ(i,:), t+1) * Zsmooth(bl_idxQ(i,:), t+1)'...
                 + Vsmooth(bl_idxQ(i,:), bl_idxQ(i,:), t+1), Wt);
           nom = nom + y(j, t)*Zsmooth(bl_idxQ(i,:), t+1)';
           nom = nom -...
                Wt * ([1 2 3 2 1] * Zsmooth(i_idio_jQ,t+1) * ...
                Zsmooth(bl_idxQ(i,:),t+1)'+...
                [1 2 3 2 1]*Vsmooth(i_idio_jQ,bl_idxQ(i,:),t+1));
       end

        C_i = inv(denom) * nom';
        C_i_constr = C_i - ...  % BGR equation 13
                     inv(denom) * R_con_i'*inv(R_con_i*inv(denom)*R_con_i') * (R_con_i*C_i-q_con_i);

        % Place updated values in output structure
        C_new(j,bl_idxQ(i,:)) = C_i_constr;
   end
end

%%% 3B. UPDATE COVARIANCE OF RESIDUALS FOR OBSERVATION EQUATION -----------
% Initialize covariance of residuals of observation equation
R_new = zeros(n,n);
for t=1:T
    Wt = diag(~nanY(:,t));  % Selection matrix
    R_new = R_new + (y(:,t) - ...  % BGR equation 15
            Wt * C_new * Zsmooth(:, t+1)) * (y(:,t) - Wt*C_new*Zsmooth(:,t+1))'...
           + Wt*C_new*Vsmooth(:,:,t+1)*C_new'*Wt + (eye(n)-Wt)*R*(eye(n)-Wt);
end

R_new = R_new/T;
RR = diag(R_new); %RR(RR<1e-2) = 1e-2;
RR(i_idio_M) = 1e-04;  % Ensure non-zero measurement error. See Doz, Giannone, Reichlin (2012) for reference.
RR(nM+1:end) = 1e-04;
R_new = diag(RR);

end

%--------------------------------------------------------------------------

function [converged, decrease] = em_converged(loglik, previous_loglik, threshold, check_decreased)
%em_converged    checks whether EM algorithm has converged
%
%  Syntax:
%    [converged, decrease] = em_converged(loglik, previous_loglik, threshold, check_increased)
%
%  Description:
%    em_converged() checks whether EM has converged. Convergence occurs if
%    the slope of the log-likelihood function falls below 'threshold'(i.e.
%    f(t) - f(t-1)| / avg < threshold) where avg = (|f(t)| + |f(t-1)|)/2
%    and f(t) is log lik at iteration t. 'threshold' defaults to 1e-4.
%
%    This stopping criterion is from Numerical Recipes in C (pg. 423).
%    With MAP estimation (using priors), the likelihood can decrease
%    even if the mode of the posterior increases.
%
%  Input arguments:
%    loglik: Log-likelihood from current EM iteration
%    previous_loglik: Log-likelihood from previous EM iteration
%    threshold: Convergence threshhold. The default is 1e-4.
%    check_decreased: Returns text output if log-likelihood decreases.
%
%  Output:
%    converged (numeric): Returns 1 if convergence criteria satisfied, and 0 otherwise.
%    decrease (numeric): Returns 1 if loglikelihood decreased.

%% Instantiate variables

% Threshhold arguments: Checks default behavior
if nargin < 3, threshold = 1e-4; end
if nargin < 4, check_decreased = 1; end

% Initialize output
converged = 0;
decrease = 0;

%% Check if log-likelihood decreases (optional)

if check_decreased
    if loglik - previous_loglik < -1e-3 % allow for a little imprecision
        fprintf(1, '******likelihood decreased from %6.4f to %6.4f!\n', previous_loglik, loglik);
        decrease = 1;
    end
end

%% Check convergence criteria

delta_loglik = abs(loglik - previous_loglik);  % Difference in loglik
avg_loglik = (abs(loglik) + abs(previous_loglik) + eps)/2;

if (delta_loglik / avg_loglik) < threshold,
    converged = 1;  % Check convergence
end

end

%--------------------------------------------------------------------------

%InitCond()      Calculates initial conditions for parameter estimation
%
%  Description:
%    Given standardized data and model information, InitCond() creates
%    initial parameter estimates. These are intial inputs in the EM
%    algorithm, which re-estimates these parameters using Kalman filtering
%    techniques.
%
%Inputs:
%  - x:      Standardized data
%  - r:      Number of common factors for each block
%  - p:      Number of lags in transition equation
%  - blocks: Gives series loadings
%  - optNaN: Option for missing values in spline. See remNaNs_spline() for details.
%  - Rcon:   Incorporates estimation for quarterly series (i.e. "tent structure")
%  - q:      Constraints on loadings for quarterly variables
%  - NQ:     Number of quarterly variables
%  - i_idio: Logical. Gives index for monthly variables (1) and quarterly (0)
%
%Output:
%  - A:   Transition matrix
%  - C:   Observation matrix
%  - Q:   Covariance for transition equation residuals
%  - R:   Covariance for observation equation residuals
%  - Z_0: Initial value of state
%  - V_0: Initial value of covariance matrix

function [ A, C, Q, R, Z_0, V_0] = InitCond(x,r,p,blocks,optNaN,Rcon,q,nQ,i_idio)

pC = size(Rcon,2);  % Gives 'tent' structure size (quarterly to monthly)
ppC = max(p,pC);
n_b = size(blocks,2);  % Number of blocks

OPTS.disp=0;  % Turns off diagnostic information for eigenvalue computation
[xBal,indNaN] = remNaNs_spline(x,optNaN);  % Spline without NaNs

[T,N] = size(xBal);  % Time T series number N
nM = N-nQ;           % Number of monthly series

xNaN = xBal;
xNaN(indNaN) = nan;  % Set missing values equal to NaNs
res = xBal;          % Spline output equal to res: Later this is used for residuals
resNaN = xNaN;       % Later used for residuals

% Initialize model coefficient output
C = [];
A = [];
Q = [];
V_0 = [];

% Set the first observations as NaNs: For quarterly-monthly aggreg. scheme
indNaN(1:pC-1,:) = true;

for i = 1:n_b  % Loop for each block

    r_i = r(i);  % r_i = 1 when block is loaded

    %% Observation equation -----------------------------------------------

    C_i = zeros(N,r_i*ppC);     % Initialize state variable matrix helper
    idx_i = find(blocks(:,i));  % Returns series index loading block i
    idx_iM = idx_i(idx_i<nM+1); % Monthly series indicies for loaded blocks
    idx_iQ = idx_i(idx_i>nM);   % Quarterly series indicies for loaded blocks

    % Returns eigenvector v w/largest eigenvalue d
    [v, d] = eigs(cov(res(:,idx_iM)), r_i, 'lm');

    % Flip sign for cleaner output. Gives equivalent results without this section
    if(sum(v) < 0)
        v = -v;
    end

    % For monthly series with loaded blocks (rows), replace with eigenvector
    % This gives the loading
    C_i(idx_iM,1:r_i) = v;
    f = res(:,idx_iM)*v;  % Data projection for eigenvector direction
    F = [];

    % Lag matrix using loading. This is later used for quarterly series
    for kk = 0:max(p+1,pC)-1
        F = [F f(pC-kk:end-kk,:)];
    end

    Rcon_i = kron(Rcon,eye(r_i));  % Quarterly-monthly aggregation scheme
    q_i = kron(q,zeros(r_i,1));

    % Produces projected data with lag structure (so pC-1 fewer entries)
    ff = F(:, 1:r_i*pC);

    for j = idx_iQ'      % Loop for quarterly variables

        % For series j, values are dropped to accommodate lag structure
        xx_j = resNaN(pC:end,j);

        if sum(~isnan(xx_j)) < size(ff,2)+2
            xx_j = res(pC:end,j);  % Replaces xx_j with spline if too many NaNs

        end

        ff_j = ff(~isnan(xx_j),:);
        xx_j = xx_j(~isnan(xx_j));

        iff_j = inv(ff_j'*ff_j);
        Cc = iff_j*ff_j'*xx_j;  % Least squares

        % Spline data monthly to quarterly conversion
        Cc = Cc - iff_j*Rcon_i'*inv(Rcon_i*iff_j*Rcon_i')*(Rcon_i*Cc-q_i);

        C_i(j,1:pC*r_i)=Cc';  % Place in output matrix
    end

    ff = [zeros(pC-1,pC*r_i);ff];  % Zeros in first pC-1 entries (replace dropped from lag)

    % Residual calculations
    res = res - ff*C_i';
    resNaN = res;
    resNaN(indNaN) = nan;

    C = [C C_i];  % Combine past loadings together

    %% Transition equation ------------------------------------------------

    z = F(:,1:r_i);            % Projected data (no lag)
    Z = F(:,r_i+1:r_i*(p+1));  % Data with lag 1

    A_i = zeros(r_i*ppC,r_i*ppC)';  % Initialize transition matrix

    A_temp = inv(Z'*Z)*Z'*z;  % OLS: gives coefficient value AR(p) process
    A_i(1:r_i,1:r_i*p) = A_temp';
    A_i(r_i+1:end,1:r_i*(ppC-1)) = eye(r_i*(ppC-1));

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    Q_i = zeros(ppC*r_i,ppC*r_i);
    e = z  - Z*A_temp;         % VAR residuals
    Q_i(1:r_i,1:r_i) = cov(e); % VAR covariance matrix

    initV_i = reshape(inv(eye((r_i*ppC)^2)-kron(A_i,A_i))*Q_i(:),r_i*ppC,r_i*ppC);

    % Gives top left block for the transition matrix
    A = blkdiag(A,A_i);
    Q = blkdiag(Q,Q_i);
    V_0 = blkdiag(V_0,initV_i);
end

eyeN = eye(N);  % Used inside observation matrix
eyeN(:,~i_idio) = [];

C=[C eyeN];
C = [C [zeros(nM,5*nQ); kron(eye(nQ),[1 2 3 2 1])]];  % Monthly-quarterly agreggation scheme
R = diag(var(resNaN,'omitnan'));  % Initialize covariance matrix for transition matrix

ii_idio = find(i_idio);    % Indicies for monthly variables
n_idio = length(ii_idio);  % Number of monthly variables
BM = zeros(n_idio);        % Initialize monthly transition matrix values
SM = zeros(n_idio);        % Initialize monthly residual covariance matrix values

for i = 1:n_idio;  % Loop for monthly variables
    % Set observation equation residual covariance matrix diagonal
    R(ii_idio(i),ii_idio(i)) = 1e-04;

    % Subsetting series residuals for series i
    res_i = resNaN(:,ii_idio(i));

    % Returns number of leading/ending zeros
    leadZero = max( find( (1:T)' == cumsum(isnan(res_i)) ) );
    endZero  = max( find( (1:T)' == cumsum(isnan(res_i(end:-1:1))) ) );

    % Truncate leading and ending zeros
    res_i = res(:,ii_idio(i));
    res_i(end-endZero + 1:end) = [];
    res_i(1:leadZero) = [];

    % Linear regression: AR 1 process for monthly series residuals
    BM(i,i) = inv(res_i(1:end-1)'*res_i(1:end-1))*res_i(1:end-1)'*res_i(2:end,:);
    SM(i,i) = cov(res_i(2:end)-res_i(1:end-1)*BM(i,i));  % Residual covariance matrix

end

Rdiag = diag(R);
sig_e = Rdiag(nM+1:N)/19;
Rdiag(nM+1:N) = 1e-04;
R = diag(Rdiag);  % Covariance for obs matrix residuals

% For BQ, SQ
rho0 = 0.1;
temp = zeros(5);
temp(1,1) = 1;

% Blocks for covariance matrices
SQ = kron(diag((1-rho0^2)*sig_e),temp);
BQ = kron(eye(nQ),[[rho0 zeros(1,4)];[eye(4),zeros(4,1)]]);
initViQ = reshape(inv(eye((5*nQ)^2)-kron(BQ,BQ))*SQ(:),5*nQ,5*nQ);
initViM = diag(1./diag(eye(size(BM,1))-BM.^2)).*SM;

% Output
A = blkdiag(A, BM, BQ);                % Observation matrix
Q = blkdiag(Q, SM, SQ);                % Residual covariance matrix (transition)
Z_0 = zeros(size(A,1),1);              % States
V_0 = blkdiag(V_0, initViM, initViQ);  % Covariance of states

end

function [zsmooth, Vsmooth, VVsmooth, loglik] = runKF(Y, A, C, Q, R, Z_0, V_0);
%runKF()    Applies Kalman filter and fixed-interval smoother
%
%  Syntax:
%    [zsmooth, Vsmooth, VVsmooth, loglik] = runKF(Y, A, C, Q, R, Z_0, V_0)
%
%  Description:
%    runKF() applies a Kalman filter and fixed-interval smoother. The
%    script uses the following model:
%           Y_t = C_t Z_t + e_t for e_t ~ N(0, R)
%           Z_t = A Z_{t-1} + mu_t for mu_t ~ N(0, Q)

%  Throughout this file:
%    'm' denotes the number of elements in the state vector Z_t.
%    'k' denotes the number of elements (observed variables) in Y_t.
%    'nobs' denotes the number of time periods for which data are observed.
%
%  Input parameters:
%    Y: k-by-nobs matrix of input data
%    A: m-by-m transition matrix
%    C: k-by-m observation matrix
%    Q: m-by-m covariance matrix for transition equation residuals (mu_t)
%    R: k-by-k covariance for observation matrix residuals (e_t)
%    Z_0: 1-by-m vector, initial value of state
%    V_0: m-by-m matrix, initial value of state covariance matrix
%
%  Output parameters:
%    zsmooth: k-by-(nobs+1) matrix, smoothed factor estimates
%             (i.e. zsmooth(:,t+1) = Z_t|T)
%    Vsmooth: k-by-k-by-(nobs+1) array, smoothed factor covariance matrices
%             (i.e. Vsmooth(:,:,t+1) = Cov(Z_t|T))
%    VVsmooth: k-by-k-by-nobs array, lag 1 factor covariance matrices
%              (i.e. Cov(Z_t,Z_t-1|T))
%    loglik: scalar, log-likelihood
%
%  References:
%  - QuantEcon's "A First Look at the Kalman Filter"
%  - Adapted from replication files for:
%    "Nowcasting", 2010, (by Marta Banbura, Domenico Giannone and Lucrezia
%    Reichlin), in Michael P. Clements and David F. Hendry, editors, Oxford
%    Handbook on Economic Forecasting.
%
% The software can be freely used in applications.
% Users are kindly requested to add acknowledgements to published work and
% to cite the above reference in any resulting publications

S = SKF(Y, A, C, Q, R, Z_0, V_0);  % Kalman filter
S = FIS(A, S);                     % Fixed interval smoother

% Organize output
zsmooth = S.ZmT;
Vsmooth = S.VmT;
VVsmooth = S.VmT_1;
loglik = S.loglik;

end

%______________________________________________________________________
function S = SKF(Y, A, C, Q, R, Z_0, V_0)
%SKF    Applies Kalman filter
%
%  Syntax:
%    S = SKF(Y, A, C, Q, R, Z_0, V_0)
%
%  Description:
%    SKF() applies the Kalman filter

%  Input parameters:
%    Y: k-by-nobs matrix of input data
%    A: m-by-m transition matrix
%    C: k-by-m observation matrix
%    Q: m-by-m covariance matrix for transition equation residuals (mu_t)
%    R: k-by-k covariance for observation matrix residuals (e_t)
%    Z_0: 1-by-m vector, initial value of state
%    V_0: m-by-m matrix, initial value of state covariance matrix
%
%  Output parameters:
%    S.Zm: m-by-nobs matrix, prior/predicted factor state vector
%          (S.Zm(:,t) = Z_t|t-1)
%    S.ZmU: m-by-(nobs+1) matrix, posterior/updated state vector
%           (S.Zm(t+1) = Z_t|t)
%    S.Vm: m-by-m-by-nobs array, prior/predicted covariance of factor
%          state vector (S.Vm(:,:,t) = V_t|t-1)
%    S.VmU: m-by-m-by-(nobs+1) array, posterior/updated covariance of
%           factor state vector (S.VmU(:,:,t+1) = V_t|t)
%    S.loglik: scalar, value of likelihood function
%    S.k_t: k-by-m Kalman gain

%% INITIALIZE OUTPUT VALUES ---------------------------------------------
  % Output structure & dimensions of state space matrix
  [~, m] = size(C);

  % Outputs time for data matrix. "number of observations"
  nobs  = size(Y,2);

  % Instantiate output
  S.Zm  = nan(m, nobs);       % Z_t | t-1 (prior)
  S.Vm  = nan(m, m, nobs);    % V_t | t-1 (prior)
  S.ZmU = nan(m, nobs+1);     % Z_t | t (posterior/updated)
  S.VmU = nan(m, m, nobs+1);  % V_t | t (posterior/updated)
  S.loglik = 0;

%% SET INITIAL VALUES ----------------------------------------------------
  Zu = Z_0;  % Z_0|0 (In below loop, Zu gives Z_t | t)
  Vu = V_0;  % V_0|0 (In below loop, Vu guvse V_t | t)

  % Store initial values
  S.ZmU(:,1)    = Zu;
  S.VmU(:,:,1)  = Vu;

%% KALMAN FILTER PROCEDURE ----------------------------------------------
  for t = 1:nobs
      %%% CALCULATING PRIOR DISTIBUTION----------------------------------

      % Use transition eqn to create prior estimate for factor
      % i.e. Z = Z_t|t-1
      Z   = A * Zu;

      % Prior covariance matrix of Z (i.e. V = V_t|t-1)
      %   Var(Z) = Var(A*Z + u_t) = Var(A*Z) + Var(\epsilon) =
      %   A*Vu*A' + Q
      V   = A * Vu* A' + Q;
      V   =  0.5 * (V+V');  % Trick to make symmetric

      %%% CALCULATING POSTERIOR DISTRIBUTION ----------------------------

      % Removes missing series: These are removed from Y, C, and R
      [Y_t, C_t, R_t, ~] = MissData(Y(:,t), C, R);

      % Check if y_t contains no data. If so, replace Zu and Vu with prior.
      if isempty(Y_t)
          Zu = Z;
          Vu = V;
      else
          % Steps for variance and population regression coefficients:
          % Var(c_t*Z_t + e_t) = c_t Var(A) c_t' + Var(u) = c_t*V *c_t' + R
          VC  = V * C_t';
          iF  = inv(C_t * VC + R_t);

          % Matrix of population regression coefficients (QuantEcon eqn #4)
          VCF = VC*iF;

          % Gives difference between actual and predicted observation
          % matrix values
          innov  = Y_t - C_t*Z;

          % Update estimate of factor values (posterior)
          Zu  = Z  + VCF * innov;

          % Update covariance matrix (posterior) for time t
          Vu  = V  - VCF * VC';
          Vu   =  0.5 * (Vu+Vu'); % Approximation trick to make symmetric

          % Update log likelihood
          S.loglik = S.loglik + 0.5*(log(det(iF))  - innov'*iF*innov);
      end

      %%% STORE OUTPUT----------------------------------------------------

      % Store covariance and observation values for t-1 (priors)
      S.Zm(:,t)   = Z;
      S.Vm(:,:,t) = V;

      % Store covariance and state values for t (posteriors)
      % i.e. Zu = Z_t|t   & Vu = V_t|t
      S.ZmU(:,t+1)    = Zu;
      S.VmU(:,:,t+1)  = Vu;
  end

  % Store Kalman gain k_t
  if isempty(Y_t)
      S.k_t = zeros(m,m);
  else
      S.k_t = VCF * C_t;
  end

end

%______________________________________________________________________
function S = FIS(A, S)
%FIS()    Applies fixed-interval smoother
%
%  Syntax:
%    S = FIS(A, S)
%
%  Description:
%    SKF() applies a fixed-interval smoother, and is used in conjunction
%    with SKF(). See  page 154 of 'Forecasting, structural time series models
%    and the Kalman filter' for more details (Harvey, 1990).
%
%  Input parameters:
%    A: m-by-m transition matrix
%    S: structure returned by SKF()
%
%  Output parameters:
%    S: FIS() adds the following smoothed estimates to the S structure:
%    - S.ZmT: m-by-(nobs+1) matrix, smoothed states
%             (S.ZmT(:,t+1) = Z_t|T)
%    - S.VmT: m-by-m-by-(nobs+1) array, smoothed factor covariance
%             matrices (S.VmT(:,:,t+1) = V_t|T = Cov(Z_t|T))
%    - S.VmT_1: m-by-m-by-nobs array, smoothed lag 1 factor covariance
%               matrices (S.VmT_1(:,:,t) = Cov(Z_t Z_t-1|T))
%
%  Model:
%   Y_t = C_t Z_t + e_t for e_t ~ N(0, R)
%   Z_t = A Z_{t-1} + mu_t for mu_t ~ N(0, Q)

%% ORGANIZE INPUT ---------------------------------------------------------

% Initialize output matrices
  [m, nobs] = size(S.Zm);
  S.ZmT = zeros(m,nobs+1);
  S.VmT = zeros(m,m,nobs+1);

  % Fill the final period of ZmT, VmT with SKF() posterior values
  S.ZmT(:,nobs+1)   = squeeze(S.ZmU(:, nobs+1));
  S.VmT(:,:,nobs+1) = squeeze(S.VmU(:,:, nobs+1));

  % Initialize VmT_1 lag 1 covariance matrix for final period
  S.VmT_1(:,:,nobs) = (eye(m)-S.k_t) *A*squeeze(S.VmU(:,:,nobs));

  % Used for recursion process. See companion file for details
  J_2 = squeeze(S.VmU(:,:,nobs)) * A' * pinv(squeeze(S.Vm(:,:,nobs)));

  %% RUN SMOOTHING ALGORITHM ----------------------------------------------

  % Loop through time reverse-chronologically (starting at final period nobs)
    for t = nobs:-1:1

        % Store posterior and prior factor covariance values
        VmU = squeeze(S.VmU(:,:,t));
        Vm1 = squeeze(S.Vm(:,:,t));

        % Store previous period smoothed factor covariance and lag-1 covariance
        V_T = squeeze(S.VmT(:,:,t+1));
        V_T1 = squeeze(S.VmT_1(:,:,t));

        J_1 = J_2;

        % Update smoothed factor estimate
        S.ZmT(:,t) = S.ZmU(:,t) + J_1 * (S.ZmT(:,t+1) - A * S.ZmU(:,t)) ;

        % Update smoothed factor covariance matrix
        S.VmT(:,:,t) = VmU + J_1 * (V_T - Vm1) * J_1';

        if t>1
            % Update weight
            J_2 = squeeze(S.VmU(:, :, t-1)) * A' * pinv(squeeze(S.Vm(:,:,t-1)));

            % Update lag 1 factor covariance matrix
            S.VmT_1(:,:,t-1) = VmU * J_2'+J_1 * (V_T1 - A * VmU) * J_2';
        end
    end

end

function [y,C,R,L]  = MissData(y,C,R)
% Syntax:
% Description:
%   Eliminates the rows in y & matrices C, R that correspond to missing
%   data (NaN) in y
%
% Input:
%   y: Vector of observations at time t
%   C: Observation matrix
%   R: Covariance for observation matrix residuals
%
% Output:
%   y: Vector of observations at time t (reduced)
%   C: Observation matrix (reduced)
%   R: Covariance for observation matrix residuals
%   L: Used to restore standard dimensions(n x #) where # is the nr of
%      available data in y

  % Returns 1 for nonmissing series
  ix = ~isnan(y);

  % Index for columns with nonmissing variables
  e  = eye(size(y,1));
  L  = e(:,ix);

  % Removes missing series
  y  = y(ix);

  % Removes missing series from observation matrix
  C  =  C(ix,:);

  % Removes missing series from transition matrix
  R  =  R(ix,ix);

end


function imgOut = pyrGaussGenAux(img)
%
%
%        imgOut = pyrGaussGenAux(img)
%
%
%        Input:
%           -img: an image
%
%        Output:
%           -imgOut: filtered img using 5x5 Gaussian filter
%           -imgB: downsampled img at half the size
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

%5x5 Gaussian Kernel
kernel = [1, 4, 6, 4, 1];
mtx = kernel' * kernel;
mtx = mtx / sum(mtx(:));

%Convolution
imgB = imfilter(img, mtx, 'replicate');

%Downsampling
[r, c] = size(img);
imgOut = imgB(1:2:r, 1:2:c); %imresize(imgB, 0.5, 'bilinear');

end


close all
clear all
image = im2double(imread('..\2.tif'));

%% proof that txt files and stage 4 size is not equal.
prophoto2 = im2double(imread('..\3.tif'));
%center aligned
eucliudian_error1 = sqrt((image(:,:,1)-...
    prophoto2(:,:,1)).^2 + ...
    (image(:,:,2)-...
    prophoto2(:,:,2)).^2 + ...
    (image(:,:,3)-...
    prophoto2(:,:,3)).^2);
sum(eucliudian_error1(:))
figure,
imagesc(eucliudian_error1, [0 0.01]);

% abs()


function [grad, grad_W_raw] = B_weighted_average(prev_layers, curr_layer, future_layers)

% raw_weights = prev_layers{1}.a;
input = prev_layers{2}.a;
[dim, nFr] = size(input);

% raw_weights2 = exp(raw_weights);
% weights = raw_weights2 / sum(raw_weights2);

weights = curr_layer.weights;

future_grad = 0;
for i=1:length(future_layers)
    future_grad = future_grad + future_layers{i}.grad;
end

% gradient of the input
% grad_y = sum(future_grad,2);
grad_y = future_grad;
grad = grad_y * weights;

% gradient of the weights
grad_W = input' * grad_y;

if 1
    grad_W_raw = weights .* grad_W' - weights * (weights * grad_W);
else
    grad_W_raw = (diag(weights) - weights' * weights) * grad_W;
    grad_W_raw = grad_W_raw';
end

end


function output = FeaturePipe(input, processing, input2)

output = input;
for i=1:length(processing)
    switch processing{i}.name

        % data manipulations

        case 'CPU2GPU'
            output = gpuArray(output);
        case 'GPU2CPU'
            output = gather(output);
        case 'selectDim'
            output = output(processing{i}.transform,:);
        case {'splice', 'Splice'}
            output = ExpandContext_v2(output, processing{i}.transform);
        case 'stream2'  % extra input
            output = [output; input2];
        case 'segmentation'     % this is similar to enframe. It is usually used for high dimensional input, while enframe is usually for single/multichannel waveforms
            seglen = processing{i}.transform(1);
            segshift = processing{i}.transform(2);
            % if input is 1D array, output is 2D matrix. If input is 2D matrix, output is 3D tensor.The last dimension is the number of segments
            % Any remaining frames that is shorter than seglen is discarded.
            output = DivideSent2Segments(output, seglen, segshift, 0);
        case 'transpose'
            output = output';
        case 'permute'
            output = permute(output, processing{i}.transform);
        case 'vectorize'
            output = reshape(output,numel(output),1);

        case 'removeNonspeechFrame' % assume the input is something related to energy. Do energy based VAD first, then discard those frames classified as nonspeech

        case 'upsample'

        case 'downsample'

            % signal processing

        case 'addNoise'
            output = output + randn(size(output)) * processing{i}.transform;
        case 'removeDC'
            output = DC_remove(output,0.999);
        case 'preemphasis'
            output = filter([1 -0.97],1,output);
        case 'enframe'
            output = my_enframe(output, processing{i}.transform(1), processing{i}.transform(2));
        case 'windowing'
            switch processing{i}.transform
                case 'hamming'
                    window = hamming(frame_size);
                case 'hanning'
                    window = hanning(frame_size);
                otherwise
                    window = [];
            end
            if ~isempty(window)
                output = bsxfun(@times, output, window);
            end
        case 'fft'
            output = fft(output,processing{i}.transform);
        case 'log'
            if isfield(processing{i}, 'transform')
                output = log(output+processing{i}.transform);
            else
                output = log(output+eps);
            end
        case 'power'
            output = real(output .* conj(output));
        case 'complex2realImag'
            [D,nFr] = size(output);
            output = output(1:D/2,:) + sqrt(-1)*output(D/2+1:end,:);
        case 'realImag2complex'
            output = [real(output); imag(output)];

            % language processing

        case 'seq2ngram'
            output = seq2ngram(output, processing{i}.ngram, processing{i}.vocab);
        case 'idx2vec'
            output2 = sparse(processing{i}.outputDim, length(output));
            for j=1:length(output)
                output2(output(j),j)= 1;
            end
            output = output2;
        case {'context_sum', 'Context_sum'}
            context = processing{i}.transform;
            output = SumContext(output, context);
        case 'fulltify'
            output = full(output);

            % temporal processing

        case 'delta'
            output = comp_dynamic_feature(output', processing{i}.delta_order, processing{i}.delta_order)';
        case 'dynamic'
            D = genDeltaTransform(size(output,2), 2);
            A = D*D;
            output = [output; output*D'; output*A'];

            % neural networks steps

        case {'affinetransform','AffineTransform'};
            output = processing{i}.transform * output;
            if isfield(processing{i}, 'bias')
                bias = processing{i}.bias;
                output = bsxfun(@plus, output, bias(:));
            end
        case {'sigmoid','Sigmoid'}
            output = sigmoid(output);
        case {'softmax', 'Softmax'}
            output = softmax(output);
        case {'addshift', 'AddShift'}
            output = bsxfun(@minus, output, processing{i}.transform');
        case {'rescale', 'Rescale'}
            if issparse(output)
                visible_nonzero_idx = find(sum(abs(output),2)>0);
                visible_nonzero = full(output(visible_nonzero_idx,:));
                output(visible_nonzero_idx) = bsxfun(@times, visible_nonzero, processing{i}.transform(visible_nonzero_idx)');
            else
                output = bsxfun(@times, output, processing{i}.transform');
            end
        case 'linear'
            % linear activation node, do nothings
        case 'phoneID2posterior'
            output = phoneID2posterior(output, length(processing{i}.classID), processing{i}.classID);
        case 'phoneID2classID'
            output = phoneID2classID(output, processing{i}.classID);

            % normalization methods

        case 'MVN'
            output = MVN(output')';
        case 'CMN'
            output = CMN(output')';
        case 'length_norm'
            scale = 1./sqrt(sum(output.^2,1));
            output = bsxfun(@times, output, scale);

        otherwise
            fprintf('Error: unknown processing step: %s\n', processing{i}.name);
            break;
    end
end
end


function stroud_test2075 ( )

%*****************************************************************************80
%
%% STROUD_TEST2075 tests the rules for EPN with GLG weight on monomials.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2010
%
%  Author:
%
%    John Burkardt
%
  test_num = 5;

  alpha_test = [ - 0.5, 0.0, 0.5, 1.0, 2.0 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'STROUD_TEST2075\n' );
  fprintf ( 1, '  Demonstrate the use of quadrature rules for the region\n' );
  fprintf ( 1, '  EPN_GLG, that is, the positive half space [0,+oo)^N, with the\n' );
  fprintf ( 1, '  weight W(ALPHA;X) = product ( 1 <= I <= N ) X(I)^ALPHA exp ( -X(I) )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  We use the formulas to integrate various monomials of\n' );
  fprintf ( 1, '  the form X(1)^E(1) * X(2)^E(2) * ... X(N)^E(N)\n' );
  fprintf ( 1, '  and compare to the exact integral.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The precision of each formula is known, and we only use\n' );
  fprintf ( 1, '  a formula if its precision indicates it should be able to\n' );
  fprintf ( 1, '  produce an exact result.\n' );

  for n = 1 : 6

    expon = zeros ( n, 1 );
    for test = 1 : test_num
      alpha = alpha_test(test);
      epn_glg_test ( n, expon, alpha );
    end

    expon = zeros ( n, 1 );
    expon(n) = 1;
    for test = 1 : test_num
      alpha = alpha_test(test);
      epn_glg_test ( n, expon, alpha );
    end

    if ( 2 <= n )
      expon = zeros ( n, 1 );
      expon(1) = 1;
      expon(2) = 1;
      for test = 1 : test_num
        alpha = alpha_test(test);
        epn_glg_test ( n, expon, alpha );
      end
    end

    expon = zeros ( n, 1 );
    expon(1) = 2;
    for test = 1 : test_num
      alpha = alpha_test(test);
      epn_glg_test ( n, expon, alpha );
    end

  end

  return
end


function b = ismonom(a)
% function B = ismonom(A)
%
% DESCRIPTION
%   Returns true for a monomial or a vector or matrix of monomials.
%
% INPUTS
%   A: polynomial
%
% OUTPUTS
%   B: 1 if A is a monomial or a vector or matrix of monomials.  Returns
%      0 otherwise.
%
% SYNTAX
%   B = ismonom(A);

% 1/29/2008: PJS  Initial Coding

if isa(a,'double') && a==1
    b = true;
    return;
elseif ~isa(a,'polynomial')
    b = false;
    return;
end

% acoef will be Nterms-by-(Nrows*Ncols)
acoef = a.coefficient;

if all(nonzeros(acoef)==1) && all(sum(acoef,1)==1)
    b = true;
else
    b = false;
end


function [symbol,labelx,labely] = sectionLabels(type)

switch lower(type)

  case 'alpha'

    symbol = '\alpha';
    labelx = '$\gamma$';
    labely = '$\beta$';

  case 'gamma'

    symbol = '\gamma';
    labelx = '$\alpha$';
    labely = '$\beta$';

  case 'phi1'

    symbol = '\varphi_1';
    labelx = '$\varphi_2$';
    labely = '$\Phi$';

  case 'phi2'

    symbol = '\varphi_2';
    labelx = '$\varphi_1$';
    labely = '$\Phi$';

  case 'sigma'

    symbol = '\sigma';
    labelx = '$\sigma_2$';
    labely = '$\Phi$';

  case 'omega'

    symbol = '\omega';
    labelx = '$\theta$';
    labely = '$\rho$';

  case 'axisangle'

    symbol = '\theta';
    labelx = '$\r$';
    labely = '$\Theta$';

end


function example4 ( )

%*****************************************************************************80
%
%% EXAMPLE4 uses BVP4C to solve the EXAMPLE4 problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2013
%
%  Author:
%
%    Original MATLAB version by Shampine, Kierzenka, Reichelt.
%    This version by John Burkardt.
%
%  Reference:
%
%    Lawrence Shampine, Jacek Kierzenka, Mark Reichelt,
%    Solving boundary value problems for ordinary differential equations
%    in MATLAB with bvp4c.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'EXAMPLE4:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Use BVP4C to solve a boundary value problem with\n' );
  fprintf ( 1, '  a periodic solution of unknown period P.\n' );
  fprintf ( 1, '  We solve this on [0,1], with P an additional unknown.\n' );
  fprintf ( 1, '  y'' = 3 ( y + z - y^3/3 - 1.3\n' );
  fprintf ( 1, '  z'' = - ( y - 0.7 + 0.8 * z ) / 3\n' );
  fprintf ( 1, '  y(0) = y(P), z(0) = z(P)\n' );
  fprintf ( 1, '  Use initial guess y = sin ( 2 pi x ), z = cos ( 2 pi x ).\n' );
%
%  Set SOLINIT, the structure defining the initial guess.
%
  x_init = linspace ( 0.0, 1.0, 5 );
  p_init = 2.0 * pi;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial estimate for P is %g\n', p_init );
  solinit = bvpinit ( x_init, @example4_init, p_init );
%
%  Have BVP4C solve the problem.
%
  sol = bvp4c ( @example4_ode, @example4_bc, solinit );
%
%  Retrieve the updated value of the period.
%
  p = sol.parameters;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Period P = %g\n', p );
%
%  Use DEVAL to evaluate the solution.
%
  x = linspace ( 0.0, 1.0, 101 );
  px = p * x;
  sol = deval ( sol, x );
%
%  Evaluate the initial guess on the initial grid [0,1].
%
  y_init = example4_init ( x );
%
%  Display a plot of Y versus initial guess.
%
  plot ( px, sol(1,:), 'r-', ...
         px, y_init(1,:), 'g-', 'Linewidth', 2 );
  xlabel ( '<--- X --->', 'Fontsize', 16 );
  ylabel ( '<--- Y --->', 'Fontsize', 16 );
  title ( 'EXAMPLE4: Y(red) versus initial guess (green)', 'Fontsize', 16 )
  grid on
  filename = 'example4.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saving plot file as "%s"\n', filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'EXAMPLE4:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function y = example4_init ( x )

%*****************************************************************************80
%
%% EXAMPLE4_INIT evaluates the initial guess for the solution.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the point at which the ODE is to be evaluated.
%
%    Output, real Y(M,1), the value of the initial guess.
%
  y(1,:) = sin ( 2.0 * pi * x );
  y(2,:) = cos ( 2.0 * pi * x );

  return
end
function dydx = example4_ode ( x, y, p )

%*****************************************************************************80
%
%% EXAMPLE4_ODE evaluates the right hand side of the ODE.
%
%  Discussion:
%
%    We assume that the differential equation has been rewritten as a
%    system of first order equations of the form
%
%      dydx = f(x,y)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the point at which the ODE is to be evaluated.
%
%    Input, real Y(M), the value of the solution at X.
%
%    Input, real P, the estimate for the period.
%
%    Output, real DYDX(M), the value of the right hand side given X and Y.
%
  dydx(1,1) = p * 3.0 * ( y(1) + y(2) - ( y(1) )^3 / 3.0 - 1.3 );
  dydx(2,1) =  - p * ( y(1) - 0.7 + 0.8 * y(2) ) / 3.0;

  return
end
function bc = example4_bc ( ya, yb, p )

%*****************************************************************************80
%
%% EXAMPLE4_BC evaluates the boundary conditions.
%
%  Discussion:
%
%    The third boundary condition applies the phase condition, and eliminates
%    the spurious solution y'(x) = 0, and solutions with p = 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real YA(M), YB(M), the solution value at the left and right endpoints.
%
%    Input, real P, the estimate for the period.
%
%    Output, real BC(M), the value of the boundary conditions.
%
  bc(1,1) = ya(1) - yb(1);
  bc(2,1) = ya(2) - yb(2);
  bc(3,1) = p * ( ya(1) - 0.7 + 0.8 * ya(2) ) / 3.0 - 1.0;

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


function [ x, seed ] = r8vec_uniform_01 ( n, seed )

%*****************************************************************************80
%
%% R8VEC_UNIFORM_01 returns a unit pseudorandom R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Second Edition,
%    Springer, 1987,
%    ISBN: 0387964673,
%    LC: QA76.9.C65.B73.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, December 1986, pages 362-376.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley, 1998,
%    ISBN: 0471134031,
%    LC: T57.62.H37.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, Number 2, 1969, pages 136-143.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X(N,1), the vector of pseudorandom values.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  x = zeros ( n, 1 );

  i4_huge = 2147483647;

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8VEC_UNIFORM_01 - Fatal error!' );
  end

  r = zeros ( n, 1 );

  for i = 1 : n

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + i4_huge;
    end

    x(i) = seed * 4.656612875E-10;

  end

  return
end


function [ x, seed ] = r4vec_uniform_ab ( n, a, b, seed )

%*****************************************************************************80
%
%% R4VEC_UNIFORM_AB returns a scaled pseudorandom R4VEC.
%
%  Discussion:
%
%    Each dimension ranges from A to B.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, real A, B, the range of the pseudorandom values.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X(N), the vector of pseudorandom values.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  x = zeros ( n, 1 );

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4VEC_UNIFORM_AB - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R4VEC_UNIFORM_AB - Fatal error!' );
  end

  for i = 1 : n

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + 2147483647;
    end

    x(i) = a + ( b - a ) * seed * 4.656612875E-10;

  end

  return
end


function [a, time_in, time_out, el] = test_resample(a_init, sfreq_out, sfreq_in)
% TEST_RESAMPLE: Test all the methods available in Brainstorm for resamlping.
%
% USAGE:  test_resample(a_init, sfreq_out, sfreq_in)
%         test_resample(a_init)
%         test_resample()
%         [a, time_in, time_out, el] = test_resample();
%
% INPUT:
%     - a_init    : Signal to resample
%                   Default: [cos(t);sin(t)], with t=-pi:.0001:pi
%     - sfreq_out : Output sampling frequency
%                   Default: 4217
%     - sfreq_in  : Initial sampling frequency
%                   Default: 5000
%
% OUTPUT:
%     - a        : Cell-array oo the resampled signals for each method
%     - time_in  : Input time vector (scalar)
%     - time_out : Resampled time vector for each method
%     - el       : Computation time for each method

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2011

% Define input signal
if (nargin < 1)
    t = -pi:.0001:pi;
    a_init = [cos(t);sin(t)];
end
if (nargin < 3)
    sfreq_in  = 5000;
    sfreq_out = 4217;
end
time_in = (1:size(a_init,2)) / sfreq_in;

% List of available methods
list_methods = {'resample', 'resample-rational', 'resample-cascade', 'interp-decimate-cascade', 'fft-spline'};

% Intialize arrays
a        = cell(1,length(list_methods));
time_out = a;
el       = a;
isFigCreated = 0;
% Loop on all the methods
for i = 1:length(list_methods)
    % Method selection
    method = list_methods{i};
    % Resample
    tic;
    [a{i}, time_out{i}] = process_resample('Compute', a_init, time_in, sfreq_out, method);
    el{i} = toc;
    % Plot initial signal
    if ~isFigCreated
        isFigCreated = 1;
        % Create figure: signal
        hFigSignal = figure('Name', 'Resample: signal', 'NumberTitle', 'off', 'Toolbar', 'figure', 'Units', 'normalized', 'Position', [0 0 1 1]);
        zoom on
        % Plot signal
        hAxesSignal(1) = PlotSignal(hFigSignal, 1, sprintf('Initial signal (%5.3fHz)', sfreq_in), time_in, a_init);
        % Create figure: signal
        hFigSpect(1) = figure('Name', 'Resample: |fft|', 'NumberTitle', 'off', 'Toolbar', 'figure', 'Units', 'normalized', 'Position', [0 0 1 1]);
        zoom on
        % Plot spectrum
        hAxesSpect(1) = PlotSpectrum(hFigSpect, 1, sprintf('Initial signal (%5.3fHz)', sfreq_in), sfreq_in, a_init);
    end
    % Plot resampled signal
    sfreq_out_effective = 1./diff(time_out{i}([1,2]));
    axesTitle = sprintf('%s (%5.3fHz, %3.4fs)', method, sfreq_out_effective, el{i});
    hAxesSignal(i+1) = PlotSignal(hFigSignal, i+1, axesTitle, time_out{i}, a{i});
    % Plot resampled spectrum
    hAxesSpect(i+1) = PlotSpectrum(hFigSpect, i+1, axesTitle, sfreq_out_effective, a{i});
end
% Link all axes
linkaxes(hAxesSignal);
linkaxes(hAxesSpect(2:end));

function hAxes = PlotSignal(hFig, iPlot, axesTitle, t, x)
    hAxes = subplot(2, 4, iPlot, 'Parent', hFig);
    plot(hAxes, t, x);
    title(hAxes, axesTitle);

function hAxes1 = PlotSpectrum(hFig, iPlot, axesTitle, sfreq_in, x)
    % Compute FFT
    ntime = size(x,2);
    nfft = 2^nextpow2(ntime);
    Y = fft(x',nfft)' / ntime;
    f = sfreq_in * linspace(0, 1, nfft);
    % Plot |FFT|
    hAxes1 = subplot(2, 4, iPlot, 'Parent', hFig(1));
    plot(hAxes1, f, 2 * log(max(abs(Y'),1e-5)));
    title(hAxes1, axesTitle);


function [widths, maxDepth, nodePositions] = treeGetWidths(tree)

% TREEGETWIDTHS give width of each level of tree.
% FORMAT
% DESC gives the width of a tree at each level of the hierarchy and
% the maximum depth of a tree as well as the node stored at a give
% depth and breadth.
% ARG tree : the tree for which the dimensions are required.
% RETURN widths : stores the width at each depth level.
% RETURN maxDepth : the maximum depth of the tree.
% RETURN nodePositions : stores the nodeIndex present at depth i
% and breadth j.
%
% COPYRIGHT : Andrew J. Moore, 2006
%
% SEEALSO : treeFindParents, treeFindChildren

% NDLUTIL

maxDepth = 0;
%widths stores the width at each depth level. Since the max depth isn't yet
%known, allocate enough memory to widths for the worst case scenario where
%each node has only 1 child and the depth is equal to the number of nodes.
widths = zeros(length(tree), 1);
%nodePositions
nodePositions = zeros(length(tree), length(tree));
rootInd = treeFindRoots(tree);
indAlreadySeen = [];
for i = 1:length(rootInd)
  traverseTree(rootInd(i), 1);
end
widths = widths(1:maxDepth, :); %trim off excess rows

  function traverseTree(nodeIndex, depthLevel)
    if ~any(indAlreadySeen == nodeIndex)
      widths(depthLevel) = widths(depthLevel) + 1;
    end
    indAlreadySeen = [indAlreadySeen nodeIndex];

    nodePositions(depthLevel, widths(depthLevel)) = nodeIndex;
    if length(tree(nodeIndex).children) > 0
      for i=1:length(tree(nodeIndex).children)
        traverseTree(tree(nodeIndex).children(i), depthLevel + 1);
      end
    else
      if depthLevel > maxDepth
        maxDepth = depthLevel;
      end
    end
  end

end


function a = daub10_inverse ( n )

%*****************************************************************************80
%
%% DAUB10_INVERSE returns the inverse of the DAUB10 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(N,N), the matrix.
%
  a = ( daub10 ( n ) )';

  return
end


function niederreiter2_test01 ( )

%*****************************************************************************80
%
%% NIEDERREITER2_TEST01 tests SETFLD2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 November 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'NIEDERREITER2_TEST01\n' );
  fprintf ( 1, '  SETFLD2 returns the addition, multiplication, and\n' );
  fprintf ( 1, '  subtraction tables for base 2.\n' );

  [ add, mul, sub ] = setfld2 ( 0 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Addition table:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '+  0  1\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '0  %d  %d\n', add(1,1), add(1,2) );
  fprintf ( 1, '1  %d  %d\n', add(2,1), add(2,2) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Multiplication table:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '*  0  1\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '0  %d  %d\n', mul(1,1), mul(1,2) );
  fprintf ( 1, '1  %d  %d\n', mul(2,1), mul(2,2) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Subtraction table:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '-  0  1\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '0  %d  %d\n', sub(1,1), sub(1,2) );
  fprintf ( 1, '1  %d  %d\n', sub(2,1), sub(2,2) );

  return
end


function x = mogSample(model, numSamples)

% MOGSAMPLE Sample from a mixture of Gaussians model.
% FORMAT
% DESC samples from a mixture of Gaussians.
% ARG model  : the model that you want to sample from.
% ARG numSamples : the number of samples required.
% RETURN x : the samples from the model.
%
% COPYRIGHT : Neil D. Lawrence, 2008
%
% SEEALSO : mogCreate

% MLTOOLS

p = rand(numSamples, 1);
bins = cumsum(model.prior);
compNo = sum(repmat(p, 1, model.m)<repmat(bins, numSamples, 1), 2);
x = zeros(numSamples, model.d);

for i = 1:model.m
  ind = find(compNo == i);
  x(ind, :) = repmat(model.mean(i, :), length(ind), 1);
  switch model.covtype
   case 'ppca'
    samps = randn(length(ind), model.q);
    samps = samps*model.W{i}';
    samps = samps + randn(length(ind), model.d)*sqrt(model.sigma2(i));
    x(ind, :) = x(ind, :) + samps;
   case 'spherical'
    x(ind, :) = x(ind, :) + randn(length(ind), model.d)* ...
        sqrt(model.sigma(i));
  end
end