function [n,edges,nbedges,xechan] = hist_ic(x,crit)

%HIST_IC  optimal Histogram based on IC information criterion
%
%   [N,EDGES,NBEDGES,XECHAN] = HIST-IC(X,CRIT)
%	bins the elements of X into an optimal number of bins according
%	to a cost function based on Akaike's Criterion.
%
%
%   CRIT = 1 | 2 | 3  (choose one of the 3 possible criterium)  (default 3)
%          4          (returns the initial histogram instead of the optimal one)
%
%
%   N = cell array containing the distribution of each column of X
%   (or a vector if X is a column vector)
%   EDGES = cell array containing the bin edges of each column of X
%   (or a vector if X is a column vector)
%   NBEDGES = vector containing the number of bin edges for each column of X
%   (or a number if X is a column vector)
%   XECHAN = discretized version of X
%
%   Ref : O. Colot et al., Information Criteria and Abrupt Changes in
%         Probability Laws, Signal Processing VII: Theory and Applications
%	  pp.1855-18858, September 1994
%
%   F. El-Matouat, O. Colot 2000 (first version)
%   Revised 01-06-2001 by Ph. Leray - philippe.leray@univ-nantes.fr
%
%
%   Things to do :
%	* Call criteron by a name ('aic','xxx', ...) instead of a number
%

if nargin == 0
    error('Requires one or two arguments.')
end

if nargin == 1
    crit = 3;
end;

if min(size(x))==1, x = x(:); end

if isstr(x)
    error('Input argument must be numeric.')
end

if isempty(x),
	error('No elements to count')
end

[nb_l,nb_c]=size(x);

% Outputs declaration
xechan=zeros(nb_l,nb_c);

edges=cell(nb_c,1);
% Local variables
maxi = max(x);
mini = min(x);

%% Erreur ? ancien code :
%% nb_clas_ini=2*round(sqrt(nb_l)-1);	% article Fatima

nb_clas_ini=round(2*sqrt(nb_l)-1);

pas_ini=(maxi-mini)/nb_clas_ini;	% initial step

for j=1:nb_c,

	% optimal histogram for each column of X
	histo_ini =hist(x(:,j),nb_clas_ini);		% initial histogram

	if (crit~=4)
		[hist_opt,pas_opt]=hist1_ic(histo_ini,nb_l,pas_ini(j),nb_clas_ini,crit);
	else
		fprintf('Histo initial\n');
		hist_opt=histo_ini;
		pas_opt=ones(1,nb_clas_ini)*pas_ini(j);
	end;
	nbedges(j)=size(hist_opt,2);
	edges{j}=mini(j)+cumsum(pas_opt(1:nbedges(j)-1)); %+1e-7;
        [n{j} xechan(:,j)]=histc(x(:,j),[-inf edges{j} inf]);
	n{j}=n{j}(1:end-1);
end

if (nb_c==1)
	n=n{1}; edges=edges{1};
end

% ============================== subfunctions

function [hist_opt,step_opt]=hist1_ic(histo,nb,step_ini,m,critere);

%HIST1_IC  optimal Histogram based on IC information criterion
%
%   [HIST_OPT, STEP_OPT] = HIST1_IC(HISTO, NB, STEP_INI, NBSTEP_INI, CRIT)
%       fusion of an 1D histogramme (HISTO) according to an IC criterion (CRIT)
%
%   This function is mainly an internal function used by HIST_IC
%
%   Ref : O. Colot et al., Information Criteria and Abrupt Changes in
%         Probability Laws, Signal Processing VII: Theory and Applications
%         pp.1855-18858, September 1994
%
%   F. El-Matouat, O. Colot 2000 (first version)
%   Revised 11-06-2001 by Ph. Leray
%
%
%   Things to do :
%       * Call criteron by a name ('aic','xxx', ...) instead of a number
%

aic=[];
aic2=[];

% Initialisation
histt=histo;
teta = histt/nb;
pas = step_ini*ones(1,m);

% Calcul de l'ensemble des histogrammes optimaux

for z=1:m

	% Calcul de AIC pour l'union entre hist(indice,u) et hist(indice,u+1)
	aic2 = [aic2 cal_aic(nb,teta,pas,m+1-z,critere)];

	if (z~=m)
		% Calcul des couples de classes adjacentes
		if critere==1
			penalite=(2*(m-z)-1)/nb;
		elseif critere==2
			penalite=(m-z-1)*(1+log(nb))/nb;
		else
			penalite=(m-z)*(1+log(log(nb)))/nb;
		end
 		aic=cla_adj(nb,teta,pas,step_ini,m-z+1,histt,penalite,aic);
		% Recherche de la valeur min du crit�re pour les classes adjacentes
		[min_aic classe]=min(aic(1:(m-z)));

		% Fusion de hist(classe) et hist(classe+1)
		nb_pas1=pas(classe)/step_ini;
		nb_pas2=pas(classe+1)/step_ini;

		ess=round( nb_pas1*histt(classe)+nb_pas2*histt(classe+1) );
		teta(classe)=ess / nb;
		histt(classe)=ess / (nb_pas1+nb_pas2);
		pas(classe)=pas(classe)+pas(classe+1);

		% Cr�ation du nouvel histogramme
		itemp = setdiff(1:m+1-z,classe+1);
		histt = histt(itemp);
		pas = pas(itemp);
		teta = teta(itemp);
	end
end

% Recherche du crit�re minimun AIC
[min_AIC fusion]=min(aic2(1:m));

% Initialisation de histo
histt=histo;
teta = histt/nb;
pas = step_ini*ones(1,m);

% Calcul de l'histogramme optimal

for z=1:fusion-1

	% Calcul des couples de classes adjacentes
	if critere==1
		penalite=(2*(m-1)-1)/nb;
	elseif critere==2
		penalite=(m-2)*(1+log(nb))/nb;
	else
		penalite=(m-1)*(1+log(log(nb)))/nb;
	end

	aic=cla_adj(nb,teta,pas,step_ini,m,histt,penalite,aic);

	% Recherche de la valeur min du crit�re pour les classes adjacentes
	[min_aic classe]=min(aic(1:m-1));

	% Fusion de hist(indice,classe) et hist(indice,classe+1)

	nb_pas1=pas(classe)/step_ini;
	nb_pas2=pas(classe+1)/step_ini;

	teta(classe)=(round(nb_pas1*histt(classe)+nb_pas2*histt(classe+1)))/nb;
	histt(classe)=(nb_pas1*histt(classe)+nb_pas2*histt(classe+1))/(nb_pas1+nb_pas2);
	pas(classe)=pas(classe)+pas(classe+1);

	% Cr�ation du nouvel histogramme

	itemp=setdiff(1:m,classe+1);
	histt = histt(itemp);
	pas = pas(itemp);
	teta = teta(itemp);
	%aic=zeros(1,m-1);

	m=m-1;
end
hist_opt=histt;
step_opt=pas;

%=====================================================
% Calcul du Critere pour l'ensemble des classes

function akaike=cal_aic(size_ech,teta,pas,m,critere);

if critere==1
	a=(2*m-1)/size_ech;
elseif critere==2
	a=(m-1)*(1+log(size_ech))/size_ech;
else
	a=m*(1+log(log(size_ech)))/size_ech;
end

indu = find(teta);
akaike = a - 2*sum(teta(indu).*log(teta(indu)./pas(indu)));

%=====================================================
% Cla_adj.m
% aic=cla_adj(taille,indice,teta,pas,pas_ini,m,hist,penalite,aic)
% taille=nombre d'�l�ments dans chacune des hypotheses;
% indice=numero de la classe;
% Calcul du critere de Akaike pour l'histogramme totale avec
% fusion de deux classes adjacentes u et (u+1).

function aic=cla_adj(size_ech,teta,pas,pas_ini,m,hist,penalite,aic);

for u=1:m-1

	cumul=0;

	% This loop is faster than a sum of a vectorised computation !
	for x=1:m
		if x~=u & x~=u+1 & teta(x)~=0
			cumul=cumul+teta(x)*log(teta(x)/pas(x));
		end
	end

	nb_pas1=pas(u)/pas_ini;
	nb_pas2=pas(u+1)/pas_ini;

	b=( round(nb_pas1*hist(u)+nb_pas2*hist(u+1) ) ) / size_ech;

	if b~=0
		c=2*b*log( b / ( pas(u) + pas(u+1) ) );
	else
		c=0;
	end

	aic(u)=penalite-2*cumul-c;
end


%FASTHOUGHTRANSFORM  Calculates 2D Fast Hough transform of an image
%
%     dst = cv.FastHoughTransform(src)
%     dst = cv.FastHoughTransform(src, 'OptionName',optionValue, ...)
%
% ## Input
% * __src__ The source (input) image.
%
% ## Output
% * __dst__ The destination image, result of transformation.
%
% ## Options
% * __DDepth__ The depth of destination image. Default `int32`.
% * __Op__ The operation to be applied. This specifies binary operations, that
%   is such ones which involve two operands. Formally, a binary operation `f`
%   on a set `S` is a binary relation that maps elements of the Cartesian
%   product `SxS` to `S`: `f: SxS -> S`. Default 'Addition'. One of
%   * __Minimum__ Binary minimum operation. The constant specifies the binary
%     minimum operation `f` that is defined as follows: `f(x, y) = min(x, y)`.
%   * __Maximum__ Binary maximum operation. The constant specifies the binary
%     maximum operation `f` that is defined as follows: `f(x, y) = max(x, y)`.
%   * __Addition__ Binary addition operation. The constant specifies the binary
%     addition operation `f` that is defined as follows: `f(x, y) = x + y`.
%   * __Average__ Binary average operation. The constant specifies the binary
%     average operation `f` that is defined as follows: `f(x, y) = (x + y)/2`.
% * __AngleRange__ The part of Hough space to calculate. Each member specifies
%   primarily direction of lines (horizontal or vertical) and the direction of
%   angle changes. Direction of angle changes is from multiples of 90 to odd
%   multiples of 45. The image considered to be written top-down and
%   left-to-right. Angles are started from vertical line and go clockwise.
%   Separate quarters and halves are written in orientation they should be in
%   full Hough space. Default `ARO_315_135`. One of:
%   * **ARO_0_45** Vertical primarily direction and clockwise angle changes.
%   * **ARO_45_90** Horizontal primarily direction and counterclockwise angle
%     changes.
%   * **ARO_90_135** Horizontal primarily direction and clockwise angle
%     changes.
%   * **ARO_315_0** Vertical primarily direction and counterclockwise angle
%     changes.
%   * **ARO_315_45** Vertical primarily direction.
%   * **ARO_45_135** Horizontal primarily direction.
%   * **ARO_315_135** Full set of directions.
%   * **ARO_CTR_HOR** `90 +/- atan(0.5)`, interval approximately from `64.5`
%     to `116.5` degrees. It is used for calculating Fast Hough Transform for
%     images skewed by `atan(0.5)`.
%   * **ARO_CTR_VER** `0 +/- atan(0.5)`, interval approximately from `333.5`
%     (`-26.5`) to `26.5` degrees. It is used for calculating Fast Hough
%     Transform for images skewed by `atan(0.5)`.
% * __MakeSkew__ Specifies to do or not to do skewing of Hough transform
%   image. The enum specifies to do or not to do skewing of Hough transform
%   image so it would be no cycling in Hough transform image through borders
%   of image. Default 'Deskew'. One of:
%   * __Raw__ Use raw cyclic image.
%   * __Deskew__ Prepare deskewed image.
%
% The function calculates the fast Hough transform for full, half or quarter
% range of angles.
%
% See also: cv.HoughPoint2Line, cv.HoughLines, hough, houghlines, houghpeaks
%


function bpab_test ( )

%*****************************************************************************80
%
%% BPAB_TEST tests BPAB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BPAB_TEST\n' );
  fprintf ( 1, '  BPAB evaluates Bernstein polynomials.\n' );
  fprintf ( 1, '\n' );

  x = 0.3;
  a = 0.0;
  b = 1.0;
  bern = bpab ( n, x, a, b );

  fprintf ( 1, '  The Bernstein polynomials of degree %d\n', n );
  fprintf ( 1, '  based on the interval from %f\n', a );
  fprintf ( 1, '  to %f\n', b );
  fprintf ( 1, '  evaluated at X = %f\n', x );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   I        Bern(I,X)\n' );
  fprintf ( 1, '\n' );

  for i = 0 : n
    fprintf ( 1, '  %2d  %12f\n', i, bern(i+1) );
  end

  return
end


function days = month_length_common ( y, m )

%*****************************************************************************80
%
%% MONTH_LENGTH_COMMON returns the number of days in a Common month.
%
%  Discussion:
%
%    The "common" calendar is meant to be the calendar which is Julian up to
%    day JED = 2299160, and Gregorian from day JED = 2299161 and after.
%
%    The routine knows that February has 28 days, except in leap years,
%    when it has 29.
%
%    In the Common calendar, October 1582 had only 21 days
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year in which the month occurred.
%
%    Input, integer M, the number of the month.
%
%    Output, integer DAYS, the number of days
%    in the month.
%
  mdays = [ 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 ];
%
%  Check the input.
%
  [ y2, m2, ierror ] = ym_check_common ( y, m );

  if ( ierror ~= 0 )
    days = 0;
    return
  end
%
%  Take care of the special case.
%
  if ( y2 == 1582 )
    if ( m2 == 10 )
      days = 21;
      return
    end
  end
%
%  Get the number of days in the month.
%
  days = mdays(m2);
%
%  If necessary, add 1 day for February 29.
%
  if ( m2 == 2 && year_is_leap_common ( y2 ) )
    days = days + 1;
  end

  return
end


function CrowdDis = CrowdingDistance(PopObj,FrontNo)
%CrowdingDistance - Calculate the crowding distances of solutions front
%by front.
%
%   CD = CrowdingDistance(F) calculates the crowding distances of solutions
%   according to their objective values in F.
%
%   CD = CrowdingDistance(F,FrontNo) calculates the crowding distances of
%   solutions in each non-dominated front, where FrontNo is the front
%   numbers of solutions.
%
%   Example:
%       CrowdDis = CrowdingDistance(PopObj,FrontNo)

%------------------------------- Reference --------------------------------
% S. Kukkonen and K. Deb, Improved pruning of non-dominated solutions based
% on crowding distance for bi-objective optimization problems, Proceedings
% of the IEEE Congress on Evolutionary Computation, 2006, 1179-1186.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    [N,M] = size(PopObj);
    if nargin < 2
        FrontNo = ones(1,N);
    end
    CrowdDis = zeros(1,N);
    Fronts   = setdiff(unique(FrontNo),inf);
    for f = 1 : length(Fronts)
        Front = find(FrontNo==Fronts(f));
        Fmax  = max(PopObj(Front,:),[],1);
        Fmin  = min(PopObj(Front,:),[],1);
        for i = 1 : M
            [~,Rank] = sortrows(PopObj(Front,i));
            CrowdDis(Front(Rank(1)))   = inf;
            CrowdDis(Front(Rank(end))) = inf;
            for j = 2 : length(Front)-1
                CrowdDis(Front(Rank(j))) = CrowdDis(Front(Rank(j)))+(PopObj(Front(Rank(j+1)),i)-PopObj(Front(Rank(j-1)),i))/(Fmax(i)-Fmin(i));
            end
        end
    end
end


%FIGURE_Critical_Angle

MoI = logspace(-3,0,100);
Mu = [0,0.05,0.1,0.5,1,5,10,inf];
P.m = 1;
P.g = 1;
P.L = 1;

Data = cell(length(Mu),0);
for i=1:length(Mu)
    Data(i).idxF = false(size(MoI));
    Data(i).idxB = false(size(MoI));
    Data(i).fail = false(size(MoI));
    Data(i).th = zeros(size(MoI));
    P.u = Mu(i);
    disp(['Running u = ' num2str(P.u)]);
    for j=1:length(MoI)
        P.I = MoI(j);
        C = ToppleFromRest(P);
        if ~isempty(C.th)
            if strcmp(C.exit,'SlipForwards')
               Data(i).idxF(j) = true;
            elseif strcmp(C.exit,'SlipBackwards')
                Data(i).idxB(j) = true;
            end
            Data(i).th(j) = C.th;
        else
            Data(i).fail(j) = true;
        end
    end
end

style = {'k--','b--','r--','m--','g--',...
    'k-','b-','r-','m-','g-'...
    'k:','b:','r:','m:','g:'};
LINEWIDTH = 3;
FontSize.Title = 16;
FontSize.label = 12;
names = cell(1,length(Mu));
for i=1:length(Mu)
    names{i} = num2str(['u = ' num2str(Mu(i))]);
end

figH = figure(400); clf;
set(figH,'Name','CriticalAngleLim','NumberTitle','off')
IDX = false(2,length(Mu));
for i=1:length(Mu)
    sty = style{mod(i-1,length(style))+1};
    idx = Data(i).idxB;  IDX(1,i) = sum(idx)~=0;
    subplot(2,1,1);  hold on;
    semilogx(MoI(idx),Data(i).th(idx)*180/pi,sty,'LineWidth',LINEWIDTH);
    idx = Data(i).idxF;  IDX(2,i) = sum(idx)~=0;
    subplot(2,1,2);  hold on;
    semilogx(MoI(idx),Data(i).th(idx)*180/pi,sty,'LineWidth',LINEWIDTH);
end

subplot(2,1,1);
 title('Backwards Slip (falling forward)','FontSize',FontSize.Title)
    xlabel('Moment of Inertia','FontSize',FontSize.label)
    ylabel('Critical Angle (deg)','FontSize',FontSize.label)
    legend(names(IDX(1,:)),'Location','NorthEast');
    set(gca,'Xscale','log')

    subplot(2,1,2);
 title('Forwards Slip (falling forward)','FontSize',FontSize.Title)
    xlabel('Moment of Inertia','FontSize',FontSize.label)
    ylabel('Critical Angle (deg)','FontSize',FontSize.label)
    legend(names(IDX(2,:)),'Location','NorthWest');
    set(gca,'Xscale','log')

save('DATA_Critical_Fall.mat','Data');
save2pdf('../WriteUp/Figures/Critical_Fall.pdf',figH,600);

%Later on, compute the acceleration of the tip at the onset of sliding.


function [ less, equal, more ] = r8r8r8vec_index_search ( n, x, y, z, ...
  indx, xval, yval, zval )

%*****************************************************************************80
%
%% R8R8R8VEC_INDEX_SEARCH searches for an R8R8R8 value in an indexed sorted list.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the size of the current list.
%
%    Input, real X(N), Y(N), Z(N), the list.
%
%    Input, integer INDX(N), the sort index of the list.
%
%    Input, real XVAL, YVAL, ZVAL, the value to be sought.
%
%    Output, integer LESS, EQUAL, MORE, the indexes in INDX of the
%    entries of X that are just less than, equal to, and just greater
%    than XVAL.  If XVAL does not occur in X, then EQUAL is zero.
%    If XVAL is the minimum entry of X, then LESS is 0.  If XVAL
%    is the greatest entry of X, then MORE is N+1.
%
  if ( n <= 0 )
    less = 0;
    equal = 0;
    more = 0;
    return
  end

  lo = 1;
  hi = n;

  xlo = x(indx(lo));
  ylo = y(indx(lo));
  zlo = z(indx(lo));

  xhi = x(indx(hi));
  yhi = y(indx(hi));
  zhi = z(indx(hi));

  compare = r8r8r8_compare ( xval, yval, zval, xlo, ylo, zlo );

  if ( compare == -1 )
    less = 0;
    equal = 0;
    more = 1;
    return
  elseif ( compare == 0 )
    less = 0;
    equal = 1;
    more = 2;
    return
  end

  compare = r8r8r8_compare ( xval, yval, zval, xhi, yhi, zhi );

  if ( compare == 1 )
    less = n;
    equal = 0;
    more = n + 1;
    return
  elseif ( compare == 0 )
    less = n - 1;
    equal = n;
    more = n + 1;
    return
  end

  while ( 1 )

    if ( lo + 1 == hi )
      less = lo;
      equal = 0;
      more = hi;
      return
    end

    mid = round ( ( lo + hi ) / 2 );
    xmid = x(indx(mid));
    ymid = y(indx(mid));
    zmid = z(indx(mid));

    compare = r8r8r8_compare ( xval, yval, zval, xmid, ymid, zmid );

    if ( compare == 0 )
      equal = mid;
      less = equal - 1;
      more = equal + 1;
      return
    elseif ( compare == -1 )
      hi = mid;
    elseif ( compare == +1 )
      lo = mid;
    end

  end

  return
end


function order = strong_elim_order(G, node_sizes, partial_order)
% STRONG_ELIM_ORDER Find an elimination order to produce a strongly triangulated graph.
% order = strong_elim_order(moral_graph, node_sizes, partial_order)
%
% partial_order(i,j)=1 if we must marginalize i *after* j
% (so i will be nearer the strong root).
% e.g., if j is a decision node and i is its information set:
%   we cannot maximize j if we have marginalized out some of i
% e.g., if j is a continuous child and i is its discrete parent:
%   we want to integrate out the cts nodes before the discrete ones,
%   so that the marginal is strong.
%
% For details, see
% - Jensen, Jensen and Dittmer, "From influence diagrams to junction trees", UAI 94.
% - Lauritzen, "Propgation of probabilities, means, and variances in mixed graphical
%   association models", JASA 87(420):1098--1108, 1992.
%
% On p369 of the Jensen paper, they state "the reverse of the elimination order must be some
% extension of [the partial order] to a total order".
% We make no attempt to find the best such total ordering, in the sense of minimizing the weight
% of the resulting cliques.

% Example from the Jensen paper:
% Let us number the nodes in Fig 1 from top to bottom, left to right,
% so a=1,b=2,D1=3,c=4,...,l=14,j=15,k=16.
% The elimination ordering they propose on p370 is [14 15 16 11 12 1 4 5 10 8 13 9 7 6 3 2];

if 0
  total_order = topological_sort(partial_order);
  order = total_order(end:-1:1); % no attempt to find an optimal constrained ordering!
  return;
end

% The following implementation is due to Ilya Shpitser and seems to give wrong
% results on cg1

n = length(G);
MG = G; % copy the original graph
uneliminated = ones(1,n);
order = zeros(1,n);

for i=1:n
  roots = [];
  k = 1;
  for j=1:n
    if sum(partial_order(j,:)) == 0
      roots(k) = j;
      k = k + 1;
    end
  end
  U = find(uneliminated);
  valid = myintersect(U, roots);
  % Choose the best node from the set of valid candidates
  score1 = zeros(1,length(valid));
  score2 = zeros(1,length(valid));
  for j=1:length(valid)
    k = valid(j);
    ns = myintersect(neighbors(G, k), U);
    l = length(ns);
    M = MG(ns,ns);
    score1(j) = l^2 - sum(M(:)); % num. added edges
    score2(j) = prod(node_sizes([k ns])); % weight of clique
  end
  j1s = find(score1==min(score1));
  j = j1s(argmin(score2(j1s)));
  k = valid(j);
  uneliminated(k) = 0;
  order(i) = k;
  ns = myintersect(neighbors(G, k), U);
  if ~isempty(ns)
    G(ns,ns) = 1;
    G = setdiag(G,0);
  end
  partial_order(:,k) = 0;
end


function [dataout] = mmse_det(ch_and_rx)
% mmse_det  User-defined function for MMSE detector.
% Input: 'ch_and_rx'
%    - first elements are MIMO parameters (STBC, Nss, etc.)
%    - next 16*NST elements are channel estimates.
%    - next (NSTS+1)*NST elements are reference training symbols.
%    - next (NSTS+1)*NST elements are received training symbols.
%    - remaining x*NST elements are received data symbols.
% Output: 'detout'
%    - x*NST elements are MMSE detector outputs.
%    - x*NST elements are Viterbi metric weights.
global PER_snr snr_idx ch_type;

% Initialize vectors
Clk = ch_and_rx(1);
STBC = ch_and_rx(2);
Nss  = ch_and_rx(3);
Nsts = Nss + STBC;
Nltf = Nsts;
if (Nsts==3) % use 4 TRN symbols for NSTS=3
   Nltf = 4;
end

Nrx  = ch_and_rx(4);
ch_est  = ch_and_rx(4+(1:16*56));
ref_trn = ch_and_rx(4+16*56+(1:4*56*(Nltf)));
rx_trn  = ch_and_rx(4+16*56+4*56*(Nltf)+(1:4*56*(Nltf)));
rx_data = ch_and_rx((5+16*56+2*4*56*(Nltf)):end);

%%%% Form channel matrix
idx = 0;
H = zeros(4,4,56);
for c=1:4
    for r=1:4
        H(r,c,:) = ch_est(idx+(1:56));
        idx=idx+56;
    end
end
H = H(1:Nrx,1:Nsts,:);  %% channel matrix is Nrx x Nsts

%%%% Train data symbols...
idx = 0;
nsymtrn = (length(ref_trn)/4/56);
trnRef = zeros(Nrx,56,nsymtrn);
for n=1:nsymtrn
    for m=1:Nsts
        trnRef(m,:,n) = ref_trn(idx+(m-1)*56+(1:56));
        trnRx(m,:,n)  =  rx_trn(idx+(m-1)*56+(1:56));
    end
    idx=idx+4*56;
end

%%%% Rx data symbols...
idx = 0;
nsymdata = (length(rx_data)/4/56);
rdata = zeros(4,56,nsymdata);
for n=1:nsymdata
    for m=1:Nrx
        rdata(m,:,n) = rx_data(idx+(m-1)*56+(1:56));
    end
    idx=idx+4*56;
end

%%%%
%%%% STBC or MMSE detection...
%%%%
if (STBC)
    %%%% Form space-time decoding matrices
    for k=1:56
        if (STBC==1 && Nss==1)
            % Heff matrix (assumes channel stationary for 2 symbols)
            Heff(:,:,k) = [      H(1,1,k)      -H(1,2,k); ...
                            conj(H(1,2,k)) conj(H(1,1,k)) ];
        elseif (STBC==1 && Nss==2)
            % Heff matrix (assumes channel stationary for 2 symbols)
            Heff(:,:,k) = [      H(1,1,k)      -H(1,2,k)       H(1,3,k)              0;  ...
                            conj(H(1,2,k)) conj(H(1,1,k))             0  conj(H(1,3,k)); ...
                                 H(2,1,k)      -H(2,2,k)       H(2,3,k)              0;  ...
                            conj(H(2,2,k)) conj(H(2,1,k))             0  conj(H(2,3,k)) ];
        elseif (STBC==1 && Nss==3)
            % Heff matrix (assumes channel stationary for 2 symbols)
            Heff(:,:,k) = [      H(1,1,k)      -H(1,2,k)       H(1,3,k)              0        H(1,4,k)              0;  ...
                            conj(H(1,2,k)) conj(H(1,1,k))             0  conj(H(1,3,k))              0  conj(H(1,4,k)); ...
                                 H(2,1,k)      -H(2,2,k)       H(2,3,k)              0        H(2,4,k)              0;  ...
                            conj(H(2,2,k)) conj(H(2,1,k))             0  conj(H(2,3,k))              0  conj(H(2,4,k)); ...
                                 H(3,1,k)      -H(3,2,k)       H(3,3,k)              0        H(3,4,k)              0;  ...
                            conj(H(3,2,k)) conj(H(3,1,k))             0  conj(H(3,3,k))              0  conj(H(3,4,k)) ];
        elseif (STBC==2 && Nss==2)
            % Heff matrix (assumes channel stationary for 2 symbols)
            Heff(:,:,k) = [      H(1,1,k)      -H(1,2,k)       H(1,3,k)      -H(1,4,k);  ...
                            conj(H(1,2,k)) conj(H(1,1,k)) conj(H(1,4,k)) conj(H(1,3,k)); ...
                                 H(2,1,k)      -H(2,2,k)       H(2,3,k)      -H(2,4,k);  ...
                            conj(H(2,2,k)) conj(H(2,1,k)) conj(H(2,4,k)) conj(H(2,3,k)) ];
        end
    end

    %%%% Compute noise power...
    N_o = 0;
    for k=1:56
        He = Heff(:,:,k);
        N_o = N_o + (1/56)*sum(sum(He.*conj(He)))/Nrx;     % Signal power
    end
    snr_val = PER_snr(snr_idx);
    N_o = N_o * 10^(-snr_val/10);     % Adjust by SNR

    %%%% Compute space-time detector coeff's
    C = zeros(Nss*2,Nss*2,56);
    for k=1:56
        %%%% MMSE coeff's
        He = Heff(:,:,k);
        C(:,:,k) = inv(He'*He + N_o*eye(Nss*2))*He';
    end

    %%%% Compute space-time decoding outputs, Viterbi metric weights
    idx = 0;
    detout = zeros(4*56*nsymdata,1);
    metout = zeros(4*56*nsymdata,1);
    for n=1:2:nsymdata
        for k=1:56
            if (STBC==1 && Nss==1)
                % MMSE detector outputs
                detset = C(:,:,k) * [ rdata(1,k,n); ...
                                      conj(rdata(1,k,n+1)) ];
                detout(k+idx+0*56) = detset(1);         % first symbol in time, ss1
                detout(k+idx+4*56) = conj(detset(2));   % second symbol in time, ss1
                % Viterbi metric weights
                metset = eps+abs(1-diag(C(:,:,k)*Heff(:,:,k)));
                metout(k+idx+0*56) = 1/metset(1);
                metout(k+idx+4*56) = 1/metset(2);
            elseif (STBC==1 && Nss==2)
                % MMSE detector outputs
                detset = C(:,:,k) * [ rdata(1,k,n); ...
                                      conj(rdata(1,k,n+1)); ...
                                      rdata(2,k,n); ...
                                      conj(rdata(2,k,n+1)) ];
                detout(k+idx+0*56) = detset(1);         % first symbol in time, ss1
                detout(k+idx+4*56) = conj(detset(2));   % second symbol in time, ss1
                detout(k+idx+1*56) = detset(3);         % first symbol in time, ss2
                detout(k+idx+5*56) = conj(detset(4));   % second symbol in time, ss2
                % Viterbi metric weights
                metset = eps+abs(1-diag(C(:,:,k)*Heff(:,:,k)));
                metout(k+idx+0*56) = 1/metset(1);
                metout(k+idx+4*56) = 1/metset(2);
                metout(k+idx+1*56) = 1/metset(3);
                metout(k+idx+5*56) = 1/metset(4);
            elseif (STBC==1 && Nss==3)
                % MMSE detector outputs
                detset = C(:,:,k) * [ rdata(1,k,n); ...
                                      conj(rdata(1,k,n+1)); ...
                                      rdata(2,k,n); ...
                                      conj(rdata(2,k,n+1)); ...
                                      rdata(3,k,n); ...
                                      conj(rdata(3,k,n+1)) ];
                detout(k+idx+0*56) = detset(1);         % first symbol in time, ss1
                detout(k+idx+4*56) = conj(detset(2));   % second symbol in time, ss1
                detout(k+idx+1*56) = detset(3);         % first symbol in time, ss2
                detout(k+idx+5*56) = conj(detset(4));   % second symbol in time, ss2
                detout(k+idx+2*56) = detset(5);         % first symbol in time, ss3
                detout(k+idx+6*56) = conj(detset(6));   % second symbol in time, ss3
                % Viterbi metric weights
                metset = eps+abs(1-diag(C(:,:,k)*Heff(:,:,k)));
                metout(k+idx+0*56) = 1/metset(1);
                metout(k+idx+4*56) = 1/metset(2);
                metout(k+idx+1*56) = 1/metset(3);
                metout(k+idx+5*56) = 1/metset(4);
                metout(k+idx+2*56) = 1/metset(5);
                metout(k+idx+6*56) = 1/metset(6);
            elseif (STBC==2 && Nss==2)
                % MMSE detector outputs
                detset = C(:,:,k) * [ rdata(1,k,n); ...
                                      conj(rdata(1,k,n+1)); ...
                                      rdata(2,k,n); ...
                                      conj(rdata(2,k,n+1)) ];
                detout(k+idx+0*56) = detset(1);         % first symbol in time, ss1
                detout(k+idx+4*56) = conj(detset(2));   % second symbol in time, ss1
                detout(k+idx+1*56) = detset(3);         % first symbol in time, ss2
                detout(k+idx+5*56) = conj(detset(4));   % second symbol in time, ss2
                % Viterbi metric weights
                metset = eps+abs(1-diag(C(:,:,k)*Heff(:,:,k)));
                metout(k+idx+0*56) = 1/metset(1);
                metout(k+idx+4*56) = 1/metset(2);
                metout(k+idx+1*56) = 1/metset(3);
                metout(k+idx+5*56) = 1/metset(4);
            end
        end
        idx=idx+8*56;
    end

else

    %%%% Compute noise power...
    N_o = 0;
    for k=1:56
        He = H(:,:,k);
        N_o = N_o + (1/56)*sum(sum(He.*conj(He)))/Nrx;     % Signal power
    end
    snr_val = PER_snr(snr_idx);
    N_o = N_o * 10^(-snr_val/10);     % Adjust by SNR

    %%%%% MMSE linear detector (no STBC)
    C = zeros(Nsts,Nrx,56);
    for k=1:56
        %%%% MMSE coeff's
        He = H(:,:,k);
        C(:,:,k) = inv(He'*He + N_o*eye(Nrx)) * He';
    end

    %%%% MMSE detector outputs, Viterbi metric weights
    idx = 0;
    detset = zeros(4,1);
    detout = zeros(4*56*nsymdata,1);
    metset = eps+zeros(4,1);
    metout = zeros(4*56*nsymdata,1);
    for n=1:nsymdata
        for k=1:56
            % MMSE detector outputs
            detset(1:Nsts) = C(:,:,k)*rdata(1:Nrx,k,n);
            detout(k+idx+0*56) = detset(1);
            detout(k+idx+1*56) = detset(2);
            detout(k+idx+2*56) = detset(3);
            detout(k+idx+3*56) = detset(4);
            % Viterbi metric weights
            metset(1:Nsts) = eps+abs(1-diag(C(:,:,k)*H(:,:,k)));
            metout(k+idx+0*56) = 1/metset(1);
            metout(k+idx+1*56) = 1/metset(2);
            metout(k+idx+2*56) = 1/metset(3);
            metout(k+idx+3*56) = 1/metset(4);
        end
        idx=idx+4*56;
    end

end

% Adjust metric scaling range (0 - least confident, 7 - most confident)
% (based on use of 4-bit data, soft decisions, for Viterbi module)
max_metout = max(metout(find(metout<1e14)));
log_metout = log(metout + 1e-6);
log_max_metout = log(max_metout + 1e-6);
metout = floor(7.99 * log_metout/log_max_metout);
metout = (metout >= 7).*7 + (metout < 7).*metout;

% MMSE module output
dataout = [detout; metout];


function pde = Helmholtzdata3
%% HELMHOTLZDATA4 trigonometric  data for PML Helmholtz equation
% -laplace u -k^2 u = f;
% The Intresting domain is [0,1]^2; The computing domain including the PML
%domain is [-0.1,1.1]^2;
% boundary condition
% u =0 on the PML boundary;
% Created by Jie Zhou.
% The data come from
% Advances in Iterative Methods and Preconditioners for the Helmholtz Equation
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

pde = struct('f',@f,'k2',@k2,'g_D',@g_D,'d',@d);

    % load data (right hand side function)
    function rhs =  f(p)

      x = p(:,1); y = p(:,2);
     k0 = size(p,1);
    rhs = zeros(k0,1);
     ii = find(abs(x-0.5)<=1.0e-2&abs(y-0.5)<=1.0e-2);
    rhs(ii) = 10000;
    end

    function PMLp = d(p)
    global omegal k  c
    c = 340;
    omegal = k*340;
          i = sqrt(-1);
          x = p(:,1); y = p(:,2);
      size0 = size(x,1);
     sigma1 = ones(size0,1);
     sigma2 = ones(size0,1);
    index = find((x<0)|(x>1)|(y>1)|(y<0));
    sigma1(index) = x(index);
    sigma2(index) = y(index);

    s1 = 1 + sigma1/((i)*omegal);
    s2 = 1 + sigma2/((i)*omegal);

    PMLp = [s2./s1 s1./s2];
    end
    function s =  g_D(p)
    s = zeros(size(p,1),1);
    end
    function wavenumber = k2(p)
    global k omegal  c
    omegal = k*340;
          i = sqrt(-1);
          x = p(:,1); y = p(:,2);
      size0 = size(x,1);
     sigma1 = ones(size0,1);
     sigma2 = ones(size0,1);
    index = find((x<0)|(x>1)|(y>1)|(y<0));
    sigma1(index) = x(index);
    sigma2(index) = y(index);

    s1 = 1 + sigma1/((i)*omegal);
    s2 = 1 + sigma2/((i)*omegal);
    wavenumber = k.^2.*s1.*s2;
    end

end


function h = max2(f, g, dims)
%MAX2   Maximum value of a CHEBFUN3 in two directions.
%   MAX2(F) returns a 1D CHEBFUN representing the maximum of the CHEBFUN3
%   object F along the y and z directions, i.e,
%                                          MAX2(F) = @(z) max(F( :, :, z)).
%
%   MAX2(F, [], dims) returns a CHEBFUN representing the maximum of F along
%   the dimensions DIMS, where DIMS = [1, 2] means along the x and y
%   directions, etc.
%
%   WARNING: This function is not always accurate to the expected precision.
%
%   For the global maximum use MAX3.
%
% See also CHEBFUN3/MAX and CHEBFUN3/MAX3.

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

% Empty check:
if ( isempty(f) )
    error('CHEBFUN:CHEBFUN3:max2:input', 'CHEBFUN3 is empty');
end

% Default to max2 of one chebfun3:
if ( nargin < 2 )
    g = [];
end

% Default to maximum along the x and y directions:
if ( nargin < 3 )
    dims = [1, 2];
end

% Do not allow max(F, G):
if ( nargin > 1 && ~isempty(g) )
    error('CHEBFUN:CHEBFUN3:max2:twoCHEBFUN3Inputs', ...
        'Unable to maximize two CHEBFUN3 objects.');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% We have no idea how to achieve this in an efficient way. This
% is an attempt to return a result, but typically it won't be accurate to
% more than 4-5 digits.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

dom = f.domain;
n = 129;
if ( all(dims == [1, 2]) || all(dims == [2, 1]) )
    vals = sample(f, n, n, n);
    temp = chebfun3.unfold(vals, [3]);
    h = chebfun(max(temp, [], 2), dom(5:6), 'splitting', 'on');
    h = simplify(h);
elseif ( all(dims == [1, 3]) || all(dims == [3, 1]) )
    vals = sample(f, n, n, n);
    temp = chebfun3.unfold(vals, [2]);
    h = chebfun(max(temp, [], 2), dom(3:4), 'splitting', 'on');
    h = simplify(h);
elseif ( all(dims == [2, 3]) || all(dims == [3, 2]) )
    vals = sample(f, n, n, n);
    temp = chebfun3.unfold(vals, [1]);
    h = chebfun(max(temp, [], 2), dom(1:2), 'splitting', 'on');
    h = simplify(h);
elseif ( dims == 0 )
    error('CHEBFUN:CHEBFUN3:max2:dims', ...
        'Dimension arguments must be two positive integer scalars within indexing range.')
else
   % return the CHEBFUN3. This is analogous to that MAX() command in
   % MATLAB.
   h = f;
end

end


function [bor,F0,F1] = spm_BMS_bor(L,posterior,priors,C)
% Compute Bayes Omnibus Risk
% FORMAT [bor,F0,F1] = spm_BMS_bor(L,posterior,priors,C)
%
% L         Log model evidence table (models x  subjects)
% posterior .a model counts, .r model-subject probs
% priors    .a model counts
% C         if this field is specified then BOR under family prior
%           is computed, otherwise BOR under model prior is computed.
%           C(k,f) = 1 if model k belongs to family f (0 otherwise)
%
% REFERENCES:
%
% Rigoux, L, Stephan, KE, Friston, KJ and Daunizeau, J. (2014)
% Bayesian model selection for group studies - Revisited.
% NeuroImage 84:971-85. doi: 10.1016/j.neuroimage.2013.08.065
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_BMS_bor.m 6444 2015-05-21 11:15:48Z guillaume $

if nargin < 4
    options.families = 0;
    % Evidence of null (equal model freqs)
    F0 = FE_null(L,options);
else
    options.families = 1;
    options.C = C;
    % Evidence of null (equal model freqs) under family prior
    [tmp,F0] = FE_null(L,options);
end

% Evidence of alternative
F1 = FE(L,posterior,priors);

% Implied by Eq 5 (see also p39) in Rigoux et al.
% See also, last equation in Appendix 2
bor = 1/(1+exp(F1-F0));

function [F,ELJ,Sqf,Sqm] = FE(L,posterior,priors)
% derives the free energy for the current approximate posterior
% This routine has been copied from the VBA_groupBMC function
% of the VBA toolbox http://code.google.com/p/mbb-vb-toolbox/
% and was written by Lionel Rigoux and J. Daunizeau
%
% See equation A.20 in Rigoux et al. (should be F1 on LHS)

[K,n] = size(L);
a0 = sum(posterior.a);
Elogr = psi(posterior.a) - psi(sum(posterior.a));
Sqf = sum(gammaln(posterior.a)) - gammaln(a0) - sum((posterior.a-1).*Elogr);
Sqm = 0;
for i=1:n
    Sqm = Sqm - sum(posterior.r(:,i).*log(posterior.r(:,i)+eps));
end
ELJ = gammaln(sum(priors.a)) - sum(gammaln(priors.a)) + sum((priors.a-1).*Elogr);
for i=1:n
    for k=1:K
        ELJ = ELJ + posterior.r(k,i).*(Elogr(k)+L(k,i));
    end
end
F = ELJ + Sqf + Sqm;

function [F0m,F0f] = FE_null (L,options)
% Free energy of the 'null' (H0: equal frequencies)
%
% F0m       Evidence for null (ie. equal probs) over models
% F0f       Evidence for null (ie. equal probs) over families
%
% This routine derives from the VBA_groupBMC function
% of the VBA toolbox http://code.google.com/p/mbb-vb-toolbox/
% written by Lionel Rigoux and J. Daunizeau
%
% See Equation A.17 in Rigoux et al.

[K,n] = size(L);
if options.families
    f0 = options.C*sum(options.C,1)'.^-1/size(options.C,2);
    F0f = 0;
else
    F0f = [];
end
F0m = 0;
for i=1:n
    tmp = L(:,i) - max(L(:,i));
    g = exp(tmp)./sum(exp(tmp));
    for k=1:K
        F0m = F0m + g(k).*(L(k,i)-log(K)-log(g(k)+eps));
        if options.families
            F0f = F0f + g(k).*(L(k,i)-log(g(k)+eps)+log(f0(k)));
        end
    end
end


function varargout=wt(d,varargin)
%% Continous Wavelet Transform
% Creates a figure of wavelet power in units of
% normalized variance.
%
% USAGE: [wave,period,scale,coi,sig95]=wt(d[,params])
%
% d: a time series
% wave: the wavelet transform of d
% period: a vector of "Fourier" periods associated with wave
% scale: a vector of wavelet scales associated with wave
% coi: the cone of influence
%
% Settings: Pad: pad the time series with zeros?
% .         Dj: Octaves per scale (default: '1/12')
% .         S0: Minimum scale
% .         J1: Total number of scales
% .         Mother: Mother wavelet (default 'morlet')
% .         MaxScale: An easier way of specifying J1
% .         MakeFigure: Make a figure or simply return the output.
% .         BlackandWhite: Create black and white figures
% .         AR1: the ar1 coefficient of the series
% .              (default='auto' using a naive ar1 estimator. See ar1nv.m)
%
% Settings can also be specified using abbreviations. e.g. ms=MaxScale.
% For detailed help on some parameters type help wavelet.
%
%
% Example:
%      wt([0:200;sin(0:200)],'dj',1/20,'bw','maxscale',32)
%
% (C) Aslak Grinsted 2002-2014
%
% http://www.glaciology.net/wavelet-coherence

% -------------------------------------------------------------------------
%The MIT License (MIT)
%
%Copyright (c) 2014 Aslak Grinsted
%
%Permission is hereby granted, free of charge, to any person obtaining a copy
%of this software and associated documentation files (the "Software"), to deal
%in the Software without restriction, including without limitation the rights
%to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
%copies of the Software, and to permit persons to whom the Software is
%furnished to do so, subject to the following conditions:
%
%The above copyright notice and this permission notice shall be included in
%all copies or substantial portions of the Software.
%
%THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
%IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
%FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
%AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
%LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
%OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
%THE SOFTWARE.
%---------------------------------------------------------------------------

% ------validate and reformat timeseries.
[d,dt]=formatts(d);

n=size(d,1);
sigma2=var(d(:,2));

%----------default arguments for the wavelet transform-----------
Args=struct('Pad',1,...      % pad the time series with zeroes (recommended)
    'Dj',1/12, ...    % this will do 12 sub-octaves per octave
    'S0',2*dt,...    % this says start at a scale of 2 years
    'J1',[],...
    'Mother','Morlet', ...
    'MaxScale',[],...   %a more simple way to specify J1
    'MakeFigure',(nargout==0),...
    'AR1','auto');
Args=parseArgs(varargin,Args,{'BlackandWhite'});
if isempty(Args.J1)
    if isempty(Args.MaxScale)
        Args.MaxScale=(n*.17)*2*dt; %automaxscale
    end
    Args.J1=round(log2(Args.MaxScale/Args.S0)/Args.Dj);
end

if strcmpi(Args.AR1,'auto')
    Args.AR1=ar1nv(d(:,2));

    if any(isnan(Args.AR1))
        error('Automatic AR1 estimation failed. Specify it manually (use arcov or arburg).')
    end
end

%----------------::::::::---------- Analyze: ---------:::::::::::::-----------------

[wave,period,scale,coi] = wavelet(d(:,2),dt,Args.Pad,Args.Dj,Args.S0,Args.J1,Args.Mother);

t=d(:,1);
power = (abs(wave)).^2 ;        % compute wavelet power spectrum
signif = wave_signif(1.0,dt,scale,0,Args.AR1,-1,-1,Args.Mother);
sig95 = (signif')*(ones(1,n));  % expand signif --> (J+1)x(N) array
sig95 = power ./ (sigma2*sig95);
Yticks = 2.^(fix(log2(min(period))):fix(log2(max(period))));

if Args.MakeFigure
    H=imagesc(t,log2(period),log2(abs(power/sigma2)));%#ok,log2(levels));  %*** or use 'contourfill'
    %logpow=log2(abs(power/sigma2));
    %[c,H]=contourf(t,log2(period),logpow,[min(logpow(:)):.25:max(logpow(:))]);
    %set(H,'linestyle','none')

    clim=get(gca,'clim'); %center color limits around log2(1)=0
    clim=[-1 1]*max(clim(2),3);
    set(gca,'clim',clim)

    HCB=colorbar;
    set(HCB,'ytick',-7:7);
    barylbls=rats(2.^(get(HCB,'ytick')'));
    barylbls([1 end],:)=' ';
    barylbls(:,all(barylbls==' ',1))=[];
    set(HCB,'yticklabel',barylbls);

    set(gca,'YLim',log2([min(period),max(period)]), ...
        'YDir','reverse', ...
        'YTick',log2(Yticks(:)), ...
        'YTickLabel',num2str(Yticks'), ...
        'layer','top')
    %xlabel('Time')
    ylabel('Period')
    hold on

    [c,h] = contour(t,log2(period),sig95,[1 1],'k'); %#ok
    set(h,'linewidth',2)
    %plot(t,log2(coi),'k','linewidth',3)
    tt=[t([1 1])-dt*.5;t;t([end end])+dt*.5];
    hcoi=fill(tt,log2([period([end 1]) coi period([1 end])]),'w');
    set(hcoi,'alphadatamapping','direct','facealpha',.5)

    hold off
    set(gca,'box','on','layer','top');
end
varargout={wave,period,scale,coi,sig95};
varargout=varargout(1:nargout);


function [Y, optinf] = bpdn(D, S, lambda, opt)

% bpdn -- Basis Pursuit DeNoising
%
%         argmin_x (1/2)||D*x - s||_2^2 + lambda*||x||_1
%
%         The solution of the BPDN problem (see chen-1998-atomic) is
%         computed using the ADMM approach (see boyd-2010-distributed).
%
% Usage:
%       [Y, optinf] = bpdn(D, S, lambda, opt)
%
% Input:
%       D           Dictionary matrix
%       S           Signal vector (or matrix)
%       lambda      Regularization parameter
%       opt         Options/algorithm parameters structure (see below)
%
% Output:
%       Y           Dictionary coefficient vector (or matrix)
%       optinf      Details of optimisation
%
%
% Options structure fields:
%   Verbose           Flag determining whether iteration status is displayed.
%                     Fields are iteration number, functional value,
%                     data fidelity term, l1 regularisation term, and
%                     primal and dual residuals (see Sec. 3.3 of
%                     boyd-2010-distributed). The value of rho is also
%                     displayed if options request that it is automatically
%                     adjusted.
%   MaxMainIter       Maximum main iterations
%   AbsStopTol        Absolute convergence tolerance (see Sec. 3.3.1 of
%                     boyd-2010-distributed)
%   RelStopTol        Relative convergence tolerance (see Sec. 3.3.1 of
%                     boyd-2010-distributed)
%   L1Weight          Weighting array for coefficients in l1 norm of X
%   Y0                Initial value for Y
%   U0                Initial value for U
%   rho               ADMM penalty parameter
%   AutoRho           Flag determining whether rho is automatically updated
%                     (see Sec. 3.4.1 of boyd-2010-distributed)
%   AutoRhoPeriod     Iteration period on which rho is updated
%   RhoRsdlRatio      Primal/dual residual ratio in rho update test
%   RhoScaling        Multiplier applied to rho when updated
%   AutoRhoScaling    Flag determining whether RhoScaling value is
%                     adaptively determined (see wohlberg-2015-adaptive). If
%                     enabled, RhoScaling specifies a maximum allowed
%                     multiplier instead of a fixed multiplier.
%   RhoRsdlTarget     Residual ratio targeted by auto rho update policy.
%   StdResiduals      Flag determining whether standard residual definitions
%                     (see Sec 3.3 of boyd-2010-distributed) are used instead
%                     of normalised residuals (see wohlberg-2015-adaptive)
%   RelaxParam        Relaxation parameter (see Sec. 3.4.3 of
%                     boyd-2010-distributed)
%   NonNegCoef        Flag indicating whether solution should be forced to
%                     be non-negative
%   AuxVarObj         Flag determining whether objective function is computed
%                     using the auxiliary (split) variable
%
%
% Author: Brendt Wohlberg <brendt@lanl.gov>  Modified: 2015-07-23
%
% This file is part of the SPORCO library. Details of the copyright
% and user license can be found in the 'License' file distributed with
% the library.

if nargin < 4,
  opt = [];
end
checkopt(opt, defaultopts([]));
opt = defaultopts(opt);

% Default lambda is 1/10 times the lambda value beyond which the
% solution is a zero vector
if nargin < 3 | isempty(lambda),
  lambda = 0.1*max(vec(abs(D'*S)));
end

% Set up status display for verbose operation
hstr = 'Itn   Fnc       DFid      l1        r         s      ';
sfms = '%4d %9.2e %9.2e %9.2e %9.2e %9.2e';
nsep = 54;
if opt.AutoRho,
  hstr = [hstr '   rho   '];
  sfms = [sfms ' %9.2e'];
  nsep = nsep + 10;
end
if opt.Verbose && opt.MaxMainIter > 0,
  disp(hstr);
  disp(char('-' * ones(1,nsep)));
end

% Start timer
tstart = tic;

% Set up algorithm parameters and initialise variables
rho = opt.rho;
if isempty(rho), rho = 50*lambda+1; end;
[Nr, Nc] = size(D);
Nm = size(S,2);
Nx = Nc*Nm;
DTS = D'*S;
[luL, luU] = factorise(D, rho);
optinf = struct('itstat', [], 'opt', opt);
r = Inf;
s = Inf;
epri = 0;
edua = 0;

% Initialise main working variables
X = [];
if isempty(opt.Y0),
  Y = zeros(Nc,Nm);
else
  Y = opt.Y0;
end
Yprv = Y;
if isempty(opt.U0),
  if isempty(opt.Y0),
    U = zeros(Nc,Nm);
  else
    U = (lambda/rho)*sign(Y);
  end
else
  U = opt.U0;
end

% Main loop
k = 1;
while k <= opt.MaxMainIter && (r > epri | s > edua),

  % Solve X subproblem
  X = linsolve(D, rho, luL, luU, DTS + rho*(Y - U));

  % See pg. 21 of boyd-2010-distributed
  if opt.RelaxParam == 1,
    Xr = X;
  else
    Xr = opt.RelaxParam*X + (1-opt.RelaxParam)*Y;
  end

  % Solve Y subproblem
  Y = shrink(Xr + U, (lambda/rho)*opt.L1Weight);
  if opt.NonNegCoef,
    Y(Y < 0) = 0;
  end

  % Update dual variable
  U = U + Xr - Y;

  % Objective function and convergence measures
  if opt.AuxVarObj,
    Jdf = sum(vec(abs(D*Y - S).^2))/2;
    Jl1 = sum(abs(vec(bsxfun(@times, opt.L1Weight, Y))));
  else
    Jdf = sum(vec(abs(D*X - S).^2))/2;
    Jl1 = sum(abs(vec(bsxfun(@times, opt.L1Weight, X))));
  end
  Jfn = Jdf + lambda*Jl1;

  nX = norm(X(:)); nY = norm(Y(:)); nU = norm(U(:));
  if opt.StdResiduals,
    % See pp. 19-20 of boyd-2010-distributed
    r = norm(vec(X - Y));
    s = norm(vec(rho*(Yprv - Y)));
    epri = sqrt(Nx)*opt.AbsStopTol+max(nX,nY)*opt.RelStopTol;
    edua = sqrt(Nx)*opt.AbsStopTol+rho*nU*opt.RelStopTol;
  else
    % See wohlberg-2015-adaptive
    r = norm(vec(X - Y))/max(nX,nY);
    s = norm(vec(Yprv - Y))/nU;
    epri = sqrt(Nx)*opt.AbsStopTol/max(nX,nY)+opt.RelStopTol;
    edua = sqrt(Nx)*opt.AbsStopTol/(rho*nU)+opt.RelStopTol;
  end

  % Record and display iteration details
  tk = toc(tstart);
  optinf.itstat = [optinf.itstat; [k Jfn Jdf Jl1 r s epri edua rho tk]];
  if opt.Verbose,
    if opt.AutoRho,
      disp(sprintf(sfms, k, Jfn, Jdf, Jl1, r, s, rho));
    else
      disp(sprintf(sfms, k, Jfn, Jdf, Jl1, r, s));
    end
  end

  % See wohlberg-2015-adaptive and pp. 20-21 of boyd-2010-distributed
  if opt.AutoRho,
    if k ~= 1 && mod(k, opt.AutoRhoPeriod) == 0,
      if opt.AutoRhoScaling,
        rhomlt = sqrt(r/(s*opt.RhoRsdlTarget));
        if rhomlt < 1, rhomlt = 1/rhomlt; end
        if rhomlt > opt.RhoScaling, rhomlt = opt.RhoScaling; end
      else
        rhomlt = opt.RhoScaling;
      end
      rsf = 1;
      if r > opt.RhoRsdlTarget*opt.RhoRsdlRatio*s, rsf = rhomlt; end
      if s > (opt.RhoRsdlRatio/opt.RhoRsdlTarget)*r, rsf = 1/rhomlt; end
      rho = rsf*rho;
      U = U/rsf;
      if rsf ~= 1,
        [luL, luU] = factorise(D, rho);
      end
    end
  end

  Yprv = Y;
  k = k + 1;

end

% Record run time and working variables
optinf.runtime = toc(tstart);
optinf.X = X;
optinf.Y = Y;
optinf.U = U;
optinf.lambda = lambda;
optinf.rho = rho;

% End status display for verbose operation
if opt.Verbose && opt.MaxMainIter > 0,
  disp(char('-' * ones(1,nsep)));
end

return

function u = vec(v)

  u = v(:);

return

function u = shrink(v, lambda)

  if isscalar(lambda),
    u = sign(v).*max(0, abs(v) - lambda);
  else
    u = sign(v).*max(0, bsxfun(@minus, abs(v), lambda));
  end

return

function [L,U] = factorise(A, c)

  [N,M] = size(A);
  % If N < M it is cheaper to factorise A*A' + cI and then use the
  % matrix inversion lemma to compute the inverse of A'*A + cI
  if N >= M,
    [L,U] = lu(A'*A + c*eye(M,M));
  else
    [L,U] = lu(A*A' + c*eye(N,N));
  end

return

function x = linsolve(A, c, L, U, b)

  [N,M] = size(A);
  if N >= M,
    x = U \ (L \ b);
  else
    x = (b - A'*(U \ (L \ (A*b))))/c;
  end

return

function opt = defaultopts(opt)

  if ~isfield(opt,'Verbose'),
    opt.Verbose = 0;
  end
  if ~isfield(opt,'MaxMainIter'),
    opt.MaxMainIter = 1000;
  end
  if ~isfield(opt,'AbsStopTol'),
    opt.AbsStopTol = 0;
  end
  if ~isfield(opt,'RelStopTol'),
    opt.RelStopTol = 1e-4;
  end
  if ~isfield(opt,'L1Weight'),
    opt.L1Weight = 1;
  end
  if ~isfield(opt,'Y0'),
    opt.Y0 = [];
  end
  if ~isfield(opt,'U0'),
    opt.U0 = [];
  end
  if ~isfield(opt,'rho'),
    opt.rho = [];
  end
  if ~isfield(opt,'AutoRho'),
    opt.AutoRho = 1;
  end
  if ~isfield(opt,'AutoRhoPeriod'),
    opt.AutoRhoPeriod = 10;
  end
  if ~isfield(opt,'RhoRsdlRatio'),
    opt.RhoRsdlRatio = 1.2;
  end
  if ~isfield(opt,'RhoScaling'),
    opt.RhoScaling = 100;
  end
  if ~isfield(opt,'AutoRhoScaling'),
    opt.AutoRhoScaling = 1;
  end
  if ~isfield(opt,'RhoRsdlTarget'),
    opt.RhoRsdlTarget = 1;
  end
  if ~isfield(opt,'StdResiduals'),
    opt.StdResiduals = 0;
  end
  if ~isfield(opt,'RelaxParam'),
    opt.RelaxParam = 1.8;
  end
  if ~isfield(opt,'NonNegCoef'),
    opt.NonNegCoef = 0;
  end
  if ~isfield(opt,'AuxVarObj'),
    opt.AuxVarObj = 1;
  end

return


function lambda = pei_eigenvalues ( alpha, n )

%*****************************************************************************80
%
%% PEI_EIGENVALUES returns the eigenvalues of the PEI matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the scalar that defines the Pei matrix.  A
%    typical value of ALPHA is 1.0.
%
%    Input, integer N, the order of A.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  lambda(1:n-1,1) = alpha;
  lambda(n,1) = alpha + n;

  return
end


function x = r8bb_sl ( n1, n2, ml, mu, a_lu, pivot, b )

%*****************************************************************************80
%
%% R8BB_SL solves a R8BB system factored by R8BB_FA.
%
%  Discussion:
%
%    Note that in C++ and FORTRAN, we can look at A as an abstract
%    vector, but then look at parts of A as storing a two dimensional
%    array.  MATLAB assigns an inherent dimensionality to a data object,
%    and gets very unhappy when you try to manipulate the data yourself.
%    This means that the MATLAB implementation of this routine requires
%    the use of temporary 2D arrays.
%
%    The R8BB storage format is for a border banded matrix.  Such a
%    matrix has the logical form:
%
%      A1 | A2
%      ---+---
%      A3 | A4
%
%    with A1 a (usually large) N1 by N1 banded matrix, while A2, A3 and A4
%    are dense rectangular matrices of orders N1 by N2, N2 by N1, and N2 by N2,
%    respectively.
%
%    A should be defined as a vector.  The user must then store
%    the entries of the four blocks of the matrix into the vector A.
%    Each block is stored by columns.
%
%    A1, the banded portion of the matrix, is stored in
%    the first (2*ML+MU+1)*N1 entries of A, using standard LINPACK
%    general band format.  The reason for the factor of 2 in front of
%    ML is to allocate space that may be required if pivoting occurs.
%
%    The following formulas should be used to determine how to store
%    the entry corresponding to row I and column J in the original matrix:
%
%    Entries of A1:
%
%      1 <= I <= N1, 1 <= J <= N1, (J-I) <= MU and (I-J) <= ML.
%
%      Store the I, J entry into location
%      (I-J+ML+MU+1)+(J-1)*(2*ML+MU+1).
%
%    Entries of A2:
%
%      1 <= I <= N1, N1+1 <= J <= N1+N2.
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+(J-N1-1)*N1+I.
%
%    Entries of A3:
%
%      N1+1 <= I <= N1+N2, 1 <= J <= N1.
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%
%    Entries of A4:
%
%      N1+1 <= I <= N1+N2, N1+1 <= J <= N1+N2
%
%      Store the I, J entry into location
%      (2*ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%      (same formula used for A3).
%
%    The linear system A * x = b is decomposable into the block system:
%
%      ( A1 A2 ) * (X1) = (B1)
%      ( A3 A4 )   (X2)   (B2)
%
%    All the arguments except B are input quantities only, which are
%    not changed by the routine.  They should have exactly the same values
%    they had on exit from R8BB_FA.
%
%    If more than one right hand side is to be solved, with the same matrix,
%    R8BB_SL should be called repeatedly.  However, R8BB_FA only needs to be
%    called once to create the factorization.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N1, N2, the order of the banded and dense blocks.
%    N1 and N2 must be nonnegative, and at least one must be positive.
%
%    Input, integer ML, MU, the lower and upper bandwidths.
%    ML and MU must be nonnegative and no greater than N1-1.
%
%    Input, real A_LU( (2*ML+MU+1)*N1 + 2*N1*N2 + N2*N2), the factor information
%    computed by R8BB_FA.
%
%    Input, integer PIVOT(N1+N2), the pivoting information from R8BB_FA.
%
%    Input, real B(N1+N2), the right hand side of the linear system.
%
%    Output, real X(N1+N2), the solution.
%
  nband = (2*ml+mu+1)*n1;
%
%  Set B1 := inverse(A1) * B1.
%  Copy the banded matrix out of A_LU and into A1_LU.
%
  if ( 0 < n1 )

    a1_lu(1:2*ml+mu+1,1:n1) = r8vec_to_r8gb ( n1, n1, ml, mu, a_lu(1:nband) );

    job = 0;

    x(1:n1) = r8gb_sl ( n1, ml, mu, a1_lu, pivot, b, job );

  end
%
%  Modify the right hand side of the second linear subsystem.
%  Set B2 := B2 - A3*B1.
%
  for i = 1 : n2
    for j = 1 : n1
      ij = nband + n1*n2 + (j-1)*n2 + i;
      b(n1+i) = b(n1+i) - a_lu(ij) * x(j);
    end
  end
%
%  Set B2 := inverse(A4) * B2.
%  Copy the dense matrix out of A_LU and into A4_LU.
%
  if ( 0 < n2 )

    a4_lu(1:n2,1:n2) = r8vec_to_r8ge ( ...
      n2, n2, a_lu(nband+2*n1*n2+1:nband+2*n1*n2+n2*n2) );

    job = 0;
    x(n1+1:n1+n2) = r8ge_sl ( n2, a4_lu, pivot(n1+1:n1+n2), b(n1+1:n1+n2), job );

  end
%
%  Modify the first subsolution.
%  Set B1 := B1 + A2*B2.
%
  for i = 1 : n1
    for j = 1 : n2
      ij = nband + (j-1)*n1 + i;
      x(i) = x(i) + a_lu(ij) * x(n1+j);
    end
  end

  return
end


function [ehat,v_e,etahat,v_eta] = VBA_getNoise(posterior,out)
% returns the Laplace approximation to the innovations posterior density
% function [ehat,v_e,etahat,v_eta] = VBA_getNoise(posterior)
% This may be useful if one is interested in recovering, e.g., the state
% noise that enters and perturbs the system. In particular, posterior
% covariances are attached to state noise, which means that statistical
% inference can be performed in the usual way...
% IN:
%   - posterior/out: output structures of VBA_NLStateSpaceModel.m
% OUT:
%   - ehat/etahat: the 1st-order moment of the posterior density on the
%   measurement (resp. state) noise
%   - v_e/v_eta: the second-order moment of the posterior density on the
%   measurement (resp. state) noise

dim = out.dim;
options = out.options;
u = out.u;
y = out.y;

ehat = zeros(dim.p,dim.n_t);
v_e = cell(1,dim.n_t);
etahat = zeros(dim.n,dim.n_t);
v_eta = cell(1,dim.n_t);

% initial condition
if dim.n > 0
    [fx,dfdx,dfdp] = VBA_evalFun('f',posterior.muX0,posterior.muTheta,u(:,1),options,dim,1);
    etahat(:,1) = posterior.muX(:,1) - fx;
    if isinf(posterior.a_alpha) && isequal(posterior.b_alpha,0)
        v_eta{1} = zeros(dim.n,dim.n);
    else
        v_eta{1} = posterior.SigmaX.current{1} ...
            + dfdx'*posterior.SigmaX0*dfdx;
        if dim.n_theta > 0
            v_eta{1} = v_eta{1} + dfdp'*posterior.SigmaTheta*dfdp;
        end
    end
end
[gx,dgdx,dgdp] = VBA_evalFun('g',posterior.muX(:,1),posterior.muPhi,u(:,1),options,dim,1);
ehat(:,1) = y(:,1) - gx;
v_e{1} = zeros(dim.p,dim.p);
if dim.n > 0
    v_e{1} = dgdx'*posterior.SigmaX.current{1}*dgdx;
end
if dim.n_phi > 0
    v_e{1} = v_e{1} + dgdp'*posterior.SigmaPhi*dgdp;
end

% loop over time samples
for t = 2:dim.n_t
    if dim.n > 0
        [fx,dfdx,dfdp] = VBA_evalFun('f',posterior.muX(:,t-1),posterior.muTheta,u(:,t),options,dim,t);
        etahat(:,t) = posterior.muX(:,t) - fx;
        if isinf(posterior.a_alpha) && isequal(posterior.b_alpha,0)
            v_eta{t} = zeros(dim.n,dim.n);
        else
            P = [-dfdx',eye(dim.n)];
            jointCov = ...
                [ posterior.SigmaX.current{t}  posterior.SigmaX.inter{t-1}'
                posterior.SigmaX.inter{t-1}  posterior.SigmaX.current{t-1} ];
            v_eta{t} = P*jointCov*P';
            if dim.n_theta > 0
                v_eta{t} = v_eta{t} + dfdp'*posterior.SigmaTheta*dfdp;
            end
        end
    end
    [gx,dgdx,dgdp] = VBA_evalFun('g',posterior.muX(:,t),posterior.muPhi,u(:,t),options,dim,t);
    ehat(:,t) = y(:,t) - gx;
    v_e{t} = zeros(dim.p,dim.p);
    if dim.n > 0
        v_e{t} = dgdx'*posterior.SigmaX.current{t}*dgdx;
    end
    if dim.n_phi > 0
        v_e{t} = v_e{t} + dgdp'*posterior.SigmaPhi*dgdp;
    end
end


function [R,current_eps]=DT_USTM(snrdB,T,L,Mt,Mr,epsilon,prec,filename)
%
% Function to compute the DT lower bound for a Rayleigh block-fading
% channel with no CSI at transmitter and receiver. The bound assumes that
% USTM is chosen as input distribution
% snrdB: SNR in dB
% Mt: number of transmit antennas
% Mr: number of receive antennas (Mt<=Mr);
% T: size of coherence interval
% L: number of independent coherence intervals; L*T is the blocklength
% epsilon: maximal block error probability
% prec: it controls the number of samples for the Monte Carlo simulation; Note nsamples=2^prec; One should have nsamples>> 100 x 1/epsilon
% filename: data file where the samples of the information density are saved for possible future refinements.
%
% The outputs of the program are
% R: R^*(T*L,current_eps,snrdB)
% current_eps: actual upper bound on the maximal error probability
%
%-------------------------------------------------------------------
%                       SET-UP PARAMETERS
%-------------------------------------------------------------------

SAVE=0; % if this flag is active the samples of the information density are saved for possibe further refinemes
MAT=0; % save with .mat extension

K = 2^prec; % number of monte carlo simulations (it should be at least 100 x 1/epsilon)
rho = 10.^(snrdB/10); % SNR in linear scale

%-------------------------------------------------------------------
%                       MONTE CARLO SIMULATION
%-------------------------------------------------------------------
I = zeros(K,1); %allocate for the montecarlo runs
%-------------------------------------------------------------------
%                       CONSTANTS
%-------------------------------------------------------------------
rho_tilde = T*rho/Mt;
D= [sqrt(1+rho_tilde)*eye(Mt), zeros(Mt,T-Mt);
    zeros(T-Mt,Mt), eye(T-Mt)]; % D matrix (covariance matrix of equivalent noise)

lambda=1+rho_tilde;
lambda1=1/lambda;
lambda2=rho_tilde*lambda1;
c2 = logComplexGammaRatio(Mt,Mr,T); %gamma constant
c1 = Mt*(T-Mt)*log(lambda2); % SNR constant
const = c1+c2;

noise_norm=sqrt(.5);

%-------------------------------------------------------------------
%                       MONTE CARLO
%-------------------------------------------------------------------

for k = 1:K %do K montecarlo runs
        i_L = 0;
        Z = (randn(T,Mr,L)+1i*randn(T,Mr,L))*noise_norm;
        for l = 1:L %Create each realization
            Sigma = svd(D*Z(:,:,l)).^2;
            if (Mt==1)
                M = createMalt(Mt,Mr,T,Sigma,lambda2); %create  matrix
                logdetM=log(det(M))+Sigma(1)*lambda2-(T-Mr)*log(Sigma(1));
                partial_sum=sum(Sigma)-logdetM;
            elseif Mr >= Mt
                M = createM_rx_larger(Mt,Mr,T,Sigma,lambda2);
                logdetM=logdet(M);
                logdetSigma = (T-Mr)*sum(log(Sigma));% compute logdet(Sigma^(T-M))
                partial_sum=lambda1*sum(Sigma) - logdetM +logdetSigma;
            else
                logdetM = createM_tx_larger(Mt, Mr, T, Sigma, lambda2); %already in log domain
                logdetSigma = (T-Mr)*sum(log(Sigma));% compute logdet(Sigma^(T-M))
                const2 = sum(gammaln(T-((Mt+1):T)+1)) - sum(gammaln(1:(T-Mr)));
                partial_sum = sum(Sigma) -logdetM + logdetSigma - const2;
            end

            vanderTerm = det(vander(Sigma)); %get the determinant of the vandermode matrix
            TraceZ=real(trace(Z(:,:,l)'*Z(:,:,l)));

            i_temp = const- TraceZ  +partial_sum + log(vanderTerm);  %Information density for time l (i(x_l, y_l)
            i_L = i_L + i_temp; %add it to the total i_L
        end

        I(k) =  i_L; %put all computations on a pile to compute the average later

end

if (SAVE==1)
  if (MAT==1)
    save(filename,'I')
  else
    save(filename,'I','-ascii','-append')
  end
end

%---------------------------------------
%   START SEARCHING FOR THE RATE
%---------------------------------------

I=sort(I);

Kcurrent=length(I); % redefine K to account for append

current_prec=floor(log2(Kcurrent)); % actual precision

K=2^(current_prec); % round off K to avoid search errors

step=K/2;
index=step;

onevec=ones(K,1);

while(step>1),

   th=I(index);

   current_eps=sum(exp(-max(0,I-th)))/K;

   step=step/2;

   if current_eps> epsilon,

       index=index-step;

   else

       index=index+step;

   end

end

current_eps=sum(exp(-max(0,I-I(index))))/K;

if (I(index)>500),

  R=I(index)/(L*T*log(2));

else

  R=log2(exp(I(index))+1)/(L*T); % factor 2 removed from DT bound to account for max error probability

end

end

function val = logComplexGammaRatio(Mt,Mr,T)
    k=T-Mt+1:1:T;
    val = -sum(gammaln(k));
    r=1:1:Mt;
    val=val+sum(gammaln(r));
end

function M = createMalt(Mt,Mr,T,Sigma,lambda)
M = nan(Mr,Mr); %(l is the row, k is the column)
for l = 1:Mr
    for k=1:Mt
        M(l,k)= exp((Mt-k)*log(Sigma(l)) + log(gammainc(lambda*Sigma(l),T+k-Mt-Mr)) + lambda*(Sigma(l)-Sigma(1)) + (T-Mr)*log(Sigma(1)/Sigma(l)));
    end

    for k=Mt+1:1:Mr
        M(l,k)= Sigma(l)^(Mr-k);
    end
end
end

function M = createM_rx_larger(Mt,Mr,T,Sigma,lambda)
M = nan(Mr,Mr); %(l is the row, k is the column)
P=Mr;

for l = 1:Mr
    for k=1:Mt
        M(l,k)=(Sigma(l)^(Mt-k))*gammainc(lambda*Sigma(l),T+k-Mt-P);
    end

    for k=Mt+1:Mr
        M(l,k)=exp((T-k)*log(Sigma(l)) -Sigma(l)*lambda); % case Mt<Mr
    end
end

end

function C = createM_tx_larger(M,N,T,Sigma,lambda)
inf_flag = 0;
A = zeros(max(M,N), max(M,N));

for i = 1:M
    for j = 1:M
        if j <= N
            if Sigma(j) == 0 && M-i == 0
                A(i,j) = exp(lambda*Sigma(j) + log(gammainc(Sigma(j)*lambda, T-N-M+i-1)));
            else
                if (T-N-M+i-1) > 0
                    A(i,j) = exp((M-i)*log(Sigma(j))  + lambda*Sigma(j) + log(gammainc(Sigma(j)*lambda, T-N-M+i-1)));
                else
                    A(i,j) = exp((M-i)*log(Sigma(j))  + lambda*Sigma(j));
                end
            end
            if isinf(A(i,j)) == 1
                inf_flag = 1;
                break;
            end
            %A(i,j) = exp((M-i)*log(lambda(j))  + gammaln(T-M) +  log(gammainc(lambda(j)*p, T+i-2*M)));
        else
            A(i,j) = exp((T-j-(M-i))*log(lambda) + sum(log(T-j-(0:(M-i-1)))));
            %A(i,j) = exp((T-j-(M-i))*log(p)  + sum(log(T-j-(0:(M-i-1)))));
        end
    end
    if inf_flag == 1
        break;
    end
end
if inf_flag == 1
    for i = 1:M
        for j = 1:M
            if j <= N
                if (T-N-M+i-1) > 0
                    A(i,j) = exp((M-i)*log(Sigma(j))  +  log(gammainc(Sigma(j)*lambda, (T-N-M+i-1))));
                else
                    A(i,j) = exp((M-i)*log(Sigma(j)));
                end
            else
                A(i,j) = exp((T-j-(M-i))*log(lambda)  + sum(log(T-j-(0:(M-i-1)))));
            end
        end
    end
end

if inf_flag == 0
    C = logdet(A);
else
    C = logdet(A) + lambda*sum(Sigma);
end
end

function v = logdet(A, op)

assert(isfloat(A) && ndims(A) == 2 && size(A,1) == size(A,2), ...
    'logdet:invalidarg', ...
    'A should be a square matrix of double or single class.');

if nargin < 2
    use_chol = 0;
else
    assert(strcmpi(op, 'chol'), ...
        'logdet:invalidarg', ...
        'The second argument can only be a string ''chol'' if it is specified.');
    use_chol = 1;
end

%% computation

if use_chol
    v = 2 * sum(log(diag(chol(A))));
else
    [L, U, P] = lu(A);
    du = diag(U);
    c = det(P) * prod(sign(du));
    v = log(c) + sum(log(abs(du)));
end
end


function [ n_data, x, fx ] = sqrt_values ( n_data )

%*****************************************************************************80
%
%% SQRT_VALUES returns some values of the square root function.
%
%  Discussion:
%
%    SQRT(X) = positive real number Y such that Y * Y = X.
%
%    In Mathematica, the function can be evaluated by:
%
%      Sqrt[x]
%
%  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, real X, the argument of the function.
%
%    Output real FX, the value of the function.
%
  n_max = 14;

  fx_vec = [ ...
     0.0000000000000000E+00, ...
     0.9000000040950000E-04, ...
     0.3000000000000000E+00, ...
     0.3162277660168379E+00, ...
     0.6324555320336759E+00, ...
     0.1000000000000000E+01, ...
     0.1414213562373095E+01, ...
     0.1732050807568877E+01, ...
     0.1772453850905516E+01, ...
     0.4358898943540674E+01, ...
     0.5385164807134504E+01, ...
     0.8426149773176359E+01, ...
     0.9848857801796105E+01, ...
     0.1111111106055556E+05 ];

  x_vec = [ ...
     0.0000000000000000E+00, ...
     0.8100000073710001E-08, ...
     0.9000000000000000E-01, ...
     0.1000000000000000E+00, ...
     0.4000000000000000E+00, ...
     0.1000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3141592653589793E+01, ...
     0.1900000000000000E+02, ...
     0.2900000000000000E+02, ...
     0.7100000000000000E+02, ...
     0.9700000000000000E+02, ...
     0.1234567890000000E+09 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function tetrahedron_arbq_rule_test02 ( degree, n, header )

%*****************************************************************************80
%
%% TETRAHEDRON_ARBQ_RULE_TEST02 gets a rule and writes it to a file.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    08 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer DEGREE, the desired total polynomial degree exactness
%    of the quadrature rule.  0 <= DEGREE <= 15.
%
%    Input, integer N, the number of nodes to be used by the rule.
%
%    Input, string HEADER, an identifier for the filenames.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TETRAHEDRON_ARBQ_RULE_TEST02\n' );
  fprintf ( 1, '  Get a quadrature rule for the tetrahedron.\n' );
  fprintf ( 1, '  Then write it to a file.\n' );
  fprintf ( 1, '  Polynomial exactness degree DEGREE = %d\n', degree );
%
%  Retrieve a symmetric quadrature rule.
%
  [ x, w ] = tetrahedron_arbq ( degree, n );
%
%  Write the points and weights to a file.
%
  rule_filename = strcat ( header, '.txt' );

  rule_unit = fopen ( rule_filename, 'wt' );
  for i = 1 : n
    fprintf ( rule_unit, '%g  %g  %g  %g\n', x(1,i), x(2,i), x(3,i), w(i) );
  end
  fclose ( rule_unit );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Quadrature rule written to file "%s"\n', rule_filename );

  return
end


function rf=v_lpcaa2rf(aa)
%V_LPCAA2RF LPC: Convert vocal tract areas to reflection coefficients RF=(AA)

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_lpcaa2rf.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p2]=size(aa);
rf = (aa(:,2:p2)-aa(:,1:p2-1))./(aa(:,2:p2)+aa(:,1:p2-1));


function [Q, R] = gson(X)
% Gram-Schmidt orthonormalization which produces the same result as [Q,R]=qr(X,0)
% Written by Mo Chen (sth4nth@gmail.com).
[d,n] = size(X);
m = min(d,n);
R = zeros(m,n);
Q = zeros(d,0);
for i = 1:m
    R(1:i-1,i) = Q'*X(:,i);
    v = X(:,i)-Q*R(1:i-1,i);
    R(i,i) = norm(v);
    Q(:,i) = v/R(i,i);
end
R(:,m+1:n) = Q'*X(:,m+1:n);


function lik = lik_binomial(varargin)
%LIK_BINOMIAL  Create a Binomial likelihood structure
%
%  Description
%    LIK = LIK_BINOMIAL creates Binomial likelihood structure.
%
%    The likelihood is defined as follows:
%                  __ n
%      p(y|f, z) = || i=1 [ p_i^(y_i)*(1-p_i)^(z_i-y_i)) *
%                           gamma(z_i+1)/(gamma(y_i+1)*gamma(z_i-y_i+1))]
%    where p_i = exp(f_i)/ (1+exp(f_i)) is the succes probability,
%    which is a function of the latent variable f_i and z is a
%    vector of numbers of trials.
%
%    When using Binomial likelihood you need to give the vector z
%    as an extra parameter to each function that requires y also.
%    For example, you should call gpla_e as follows
%      gpla_e(w, gp, x, y, 'z', z)
%
%  See also
%    GP_SET, LIK_*
%

% Copyright (c) 2009-2010 Jaakko Riihim�ki & Jarno Vanhatalo
% Copyright (c) 2010-2011 Aki Vehtari

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

  ip=inputParser;
  ip.FunctionName = 'LIK_BINOMIAL';
  ip.addOptional('lik', [], @isstruct);
  ip.parse(varargin{:});
  lik=ip.Results.lik;

  if isempty(lik)
    init=true;
    lik.type = 'Binomial';
  else
    if ~isfield(lik,'type') || ~isequal(lik.type,'Binomial')
      error('First argument does not seem to be a valid likelihood function structure')
    end
    init=false;
  end

  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_binomial_pak;
    lik.fh.unpak = @lik_binomial_unpak;
    lik.fh.ll = @lik_binomial_ll;
    lik.fh.llg = @lik_binomial_llg;
    lik.fh.llg2 = @lik_binomial_llg2;
    lik.fh.llg3 = @lik_binomial_llg3;
    lik.fh.tiltedMoments = @lik_binomial_tiltedMoments;
    lik.fh.predy = @lik_binomial_predy;
    lik.fh.predprcty = @lik_binomial_predprcty;
    lik.fh.invlink = @lik_binomial_invlink;
    lik.fh.recappend = @lik_binomial_recappend;
  end

end

function [w,s] = lik_binomial_pak(lik)
%LIK_BINOMIAL_PAK  Combine likelihood parameters into one vector.
%
%  Description
%    W = LIK_BINOMIAL_PAK(LIK) takes a likelihood structure LIK
%    and returns an empty verctor W. If Binomial likelihood had
%    parameters this would combine them into a single row vector
%    W (see e.g. likelih_negbin). This is a mandatory subfunction
%    used for example in energy and gradient computations.
%
%  See also
%    LIK_NEGBIN_UNPAK, GP_PAK

  w = []; s = {};
end

function [lik, w] = lik_binomial_unpak(lik, w)
%LIK_BINOMIAL_UNPAK  Extract likelihood parameters from the vector.
%
%  Description
%    W = LIK_BINOMIAL_UNPAK(W, LIK) Doesn't do anything.
%
%    If Binomial likelihood had parameters this would extracts
%    them parameters from the vector W to the LIK structure.
%    This is a mandatory subfunction used for example in energy
%    and gradient computations.
%
%  See also
%    LIK_BINOMIAL_PAK, GP_UNPAK

  lik=lik;
  w=w;

end

function ll = lik_binomial_ll(lik, y, f, z)
%LIK_BINOMIAL_LL  Log likelihood
%
%  Description
%    LL = LIK_BINOMIAL_LL(LIK, Y, F, Z) takes a likelihood
%    structure LIK, succes counts Y, numbers of trials Z, and
%    latent values F. Returns the log likelihood, log p(y|f,z).
%    This subfunction is needed when using Laplace approximation
%    or MCMC for inference with non-Gaussian likelihoods. This
%    subfunction is also used in information criteria (DIC, WAIC)
%    computations.
%
%  See also
%    LIK_BINOMIAL_LLG, LIK_BINOMIAL_LLG3, LIK_BINOMIAL_LLG2, GPLA_E

  if isempty(z)
    error(['lik_binomial -> lik_binomial_ll: missing z!'...
           'Binomial likelihood needs the expected number of   '...
           'occurrences as an extra input z. See, for         '...
           'example, lik_binomial and gpla_e.             ']);
  end

  expf = exp(f);
  p = expf ./ (1+expf);
  N = z;
  ll =  sum(gammaln(N+1)-gammaln(y+1)-gammaln(N-y+1)+y.*log(p)+(N-y).*log(1-p));
end

function llg = lik_binomial_llg(lik, y, f, param, z)
%LIK_BINOMIAL_LLG    Gradient of the log likelihood
%
%  Description
%    LLG = LIK_BINOMIAL_LLG(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, succes counts Y, numbers of trials Z and
%    latent values F. Returns the gradient of the log likelihood
%    with respect to PARAM. At the moment PARAM can be 'param' or
%    'latent'. This subfunction is needed when using Laplace
%    approximation or MCMC for inference with non-Gaussian
%    likelihoods.
%
%  See also
%    LIK_BINOMIAL_LL, LIK_BINOMIAL_LLG2, LIK_BINOMIAL_LLG3, GPLA_E

  if isempty(z)
    error(['lik_binomial -> lik_binomial_llg: missing z!'...
           'Binomial likelihood needs the expected number of   '...
           'occurrences as an extra input z. See, for         '...
           'example, lik_binomial and gpla_e.             ']);
  end

  switch param
    case 'latent'
      expf = exp(f);
      N = z;

      llg = y./(1+expf) - (N-y).*expf./(1+expf);
  end
end

function llg2 = lik_binomial_llg2(lik, y, f, param, z)
%LIK_BINOMIAL_LLG2  Second gradients of the log likelihood
%
%  Description
%    LLG2 = LIK_BINOMIAL_LLG2(LIK, Y, F, PARAM) takes a
%    likelihood structure LIK, succes counts Y, numbers of trials
%    Z, and latent values F. Returns the Hessian of the log
%    likelihood with respect to PARAM. At the moment PARAM can be
%    only 'latent'. G2 is a vector with diagonal elements of the
%    Hessian matrix (off diagonals are zero). This subfunction
%    is needed when using Laplace approximation or EP for inference
%    with non-Gaussian likelihoods.
%
%  See also
%    LIK_BINOMIAL_LL, LIK_BINOMIAL_LLG, LIK_BINOMIAL_LLG3, GPLA_E

  if isempty(z)
    error(['lik_binomial -> lik_binomial_llg2: missing z!'...
           'Binomial likelihood needs the expected number of    '...
           'occurrences as an extra input z. See, for          '...
           'example, lik_binomial and gpla_e.              ']);
  end

  switch param
    case 'latent'
      expf = exp(f);
      N = z;

      llg2 = -N.*expf./(1+expf).^2;
  end
end

function llg3 = lik_binomial_llg3(lik, y, f, param, z)
%LIK_BINOMIAL_LLG3  Third gradients of the log likelihood
%
%  Description
%    LLG3 = LIK_BINOMIAL_LLG3(LIK, Y, F, PARAM) takes a
%    likelihood structure LIK, succes counts Y, numbers of trials
%    Z and latent values F and returns the third gradients of the
%    log likelihood with respect to PARAM. At the moment PARAM
%    can be only 'latent'. G3 is a vector with third gradients.
%    This subfunction is needed when using Laplace appoximation
%    for inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_BINOMIAL_LL, LIK_BINOMIAL_LLG, LIK_BINOMIAL_LLG2, GPLA_E, GPLA_G

  if isempty(z)
    error(['lik_binomial -> lik_binomial_llg3: missing z!'...
           'Binomial likelihood needs the expected number of    '...
           'occurrences as an extra input z. See, for          '...
           'example, lik_binomial and gpla_e.              ']);
  end

  switch param
    case 'latent'
      expf = exp(f);
      N = z;
      llg3 = N.*(expf.*(expf-1))./(1+expf).^3;
  end
end

function [logM_0, m_1, sigm2hati1] = lik_binomial_tiltedMoments(lik, y, i1, sigm2_i, myy_i, z)
%LIK_BINOMIAL_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_BINOMIAL_TILTEDMOMENTS(LIK, Y, I, S2,
%    MYY, Z) takes a likelihood structure LIK, succes counts Y,
%    numbers of trials Z, index I and cavity variance S2 and mean
%    MYY. Returns the zeroth moment M_0, mean M_1 and variance
%    M_2 of the posterior marginal (see Rasmussen and Williams
%    (2006): Gaussian processes for Machine Learning, page 55).
%    This subfunction is needed when using EP for inference with
%    non-Gaussian likelihoods.
%
%  See also
%    GPEP_E

%  if isempty(z)
%    error(['lik_binomial -> lik_binomial_tiltedMoments: missing z!'...
%           'Binomial likelihood needs the expected number of               '...
%           'occurrences as an extra input z. See, for                     '...
%           'example, lik_binomial and gpla_e.                         ']);
%  end

  yy = y(i1);
  N = z(i1);
  logM_0=zeros(size(yy));
  m_1=zeros(size(yy));
  sigm2hati1=zeros(size(yy));

  for i=1:length(i1)
    % Create function handle for the function to be integrated
    % (likelihood * cavity) and useful integration limits
    [tf,minf,maxf]=init_binomial_norm(yy(i),myy_i(i),sigm2_i(i),N(i));

    % Integrate with quadrature
    RTOL = 1.e-6;
    ATOL = 1.e-10;
    [m_0, m_1(i), m_2] = quad_moments(tf,minf, maxf, RTOL, ATOL);
    sigm2hati1(i) = m_2 - m_1(i).^2;

    % If the second central moment is less than cavity variance
    % integrate more precisely. Theoretically for log-concave
    % likelihood should be sigm2hati1 < sigm2_i.
    if sigm2hati1(i) >= sigm2_i(i)
      ATOL = ATOL.^2;
      RTOL = RTOL.^2;
      [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
      sigm2hati1 = m_2 - m_1(i).^2;
      %    if sigm2hati1 >= sigm2_i
      %      error('lik_binomial_tilted_moments: sigm2hati1 >= sigm2_i');
      %    end
    end
    logM_0(i) = log(m_0);
  end
end

function [lpy, Ey, Vary] = lik_binomial_predy(lik, Ef, Varf, yt, zt)
%LIK_BINOMIAL_PREDY  Returns the predictive mean, variance and density of y
%
%  Description
%    [LPY] = LIK_BINOMIAL_PREDY(LIK, EF, VARF YT, ZT)
%    Returns logarithm of the predictive density PY of YT, that is
%        p(yt | y, zt) = \int p(yt | f, zt) p(f|y) df.
%    This requires also the succes counts YT, numbers of trials ZT.
%    This subfunction is needed when computing posterior predictive
%    distributions for future observations.
%
%    [LPY, EY, VARY] = LIK_BINOMIAL_PREDY(LIK, EF, VARF) takes a
%    likelihood structure LIK, posterior mean EF and posterior
%    Variance VARF of the latent variable and returns the
%    posterior predictive mean EY and variance VARY of the
%    observations related to the latent variables. This subfunction
%    is needed when computing posterior predictive distributions for
%    future observations.
%
%
%  See also
%    GPEP_PRED, GPLA_PRED, GPMC_PRED

  if isempty(zt)
    error(['lik_binomial -> lik_binomial_predy: missing z!'...
           'Binomial likelihood needs the expected number of       '...
           'occurrences as an extra input z. See, for             '...
           'example, lik_binomial and gpla_e.                 ']);
  end

  if nargout > 1
    nt=length(Ef);
    Ey=zeros(nt,1);
    EVary = zeros(nt,1);
    VarEy = zeros(nt,1);
    for i1=1:nt
      ci = sqrt(Varf(i1));
      F  = @(x)zt(i1)./(1+exp(-x)).*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
      Ey(i1) = quadgk(F,Ef(i1)-6*ci,Ef(i1)+6*ci);

      F2  = @(x)zt(i1)./(1+exp(-x)).*(1-1./(1+exp(-x))).*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
      EVary(i1) = quadgk(F2,Ef(i1)-6*ci,Ef(i1)+6*ci);

      F3  = @(x)(zt(i1)./(1+exp(-x))).^2.*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
      VarEy(i1) = quadgk(F3,Ef(i1)-6*ci,Ef(i1)+6*ci) - Ey(i1).^2;
    end
    Vary = EVary+VarEy;
  end

  nt=length(yt);
  lpy=zeros(nt,1);
  for i1=1:nt
    ci = sqrt(Varf(i1));
    F  = @(x)exp(gammaln(zt(i1)+1)-gammaln(yt(i1)+1)-gammaln(zt(i1)-yt(i1)+1) + yt(i1).*log(1./(1+exp(-x))) + (zt(i1)-yt(i1)).*log(1-(1./(1+exp(-x))))).*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
    lpy(i1) = log(quadgk(F,Ef(i1)-6*ci,Ef(i1)+6*ci));
  end

end

function prctys = lik_binomial_predprcty(lik, Ef, Varf, zt, prcty)
%LIK_BINOMIAL_PREDPRCTY  Returns the percentiled of predictive density of y
%
%  Description
%    PRCTY = LIK_BINOMIAL_PREDPRCTY(LIK, EF, VARF YT, ZT)
%    Returns percentiles of the predictive density PY of YT, that is
%    This requires also the succes counts YT, numbers of trials ZT. This
%    subfunction is needed when using function gp_predprcty.
%
%  See also
%    GP_PREDPCTY

  if isempty(zt)
    error(['lik_binomial -> lik_binomial_predprcty: missing z!'...
           'Binomial likelihood needs the expected number of       '...
           'occurrences as an extra input z. See, for             '...
           'example, lik_binomial and gpla_e.                 ']);
  end

  opt=optimset('TolX',.5,'Display','off');
  nt=size(Ef,1);
  prctys = zeros(nt,numel(prcty));
  prcty=prcty/100;
  for i1=1:nt
    ci = sqrt(Varf(i1));
    for i2=1:numel(prcty)
      a=floor(fminbnd(@(a) (quadgk(@(f) binocdf(a,zt(i1),logitinv(f)).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)-prcty(i2)).^2,binoinv(prcty(i2),zt(i1),logitinv(Ef(i1)-1.96*ci)),binoinv(prcty(i2),zt(i1),logitinv(Ef(i1)+1.96*ci)),opt));
      if quadgk(@(f) binocdf(a,zt(i1),logitinv(f)).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)<prcty(i2)
        a=a+1;
      end
      prctys(i1,i2)=a;
    end
  end
end

function [df,minf,maxf] = init_binomial_norm(yy,myy_i,sigm2_i,N)
%INIT_LOGIT_NORM
%
%  Description
%    Return function handle to a function evaluating Binomial *
%    Gaussian which is used for evaluating (likelihood * cavity)
%    or (likelihood * posterior) Return also useful limits for
%    integration. This is private function for lik_binomial. This
%    subfunction is needed by subfunctions tiltedMoments and predy.
%
% See also
%   LIK_BINOMIAL_TILTEDMOMENTS, LIK_BINOMIAL_PREDY

% avoid repetitive evaluation of constant part
  ldconst = gammaln(N+1)-gammaln(yy+1)-gammaln(N-yy+1) - log(sigm2_i)/2 - log(2*pi)/2;
%   ldconst = log(factorial(N)/(factorial(yy)*factorial(N-yy))-log(sigm2_i)/2 -log(2*pi)/2;

 % Create function handle for the function to be integrated
  df = @binomial_norm;
 % use log to avoid underflow, and derivates for faster search
  ld = @log_binomial_norm;
  ldg = @log_binomial_norm_g;
  ldg2 = @log_binomial_norm_g2;

  % Set the limits for integration
  % Binomial likelihood is log-concave so the binomial_norm
  % function is unimodal, which makes things easier
  if yy==0 || yy==N
    % with yy==0 or yy==N the mode of the likelihood is not defined
    % use the mode of the Gaussian (cavity or posterior) as a first guess
    modef = myy_i;
  else
    % use precision weighted mean of the Gaussian approximation of the
    % binomial likelihood and Gaussian
    mean_app = log(yy./(N-yy));
    ld0=1/(1+exp(-mean_app));
    ld1=(1-ld0)*ld0;
    ld2=ld0-3*ld0^2+2*ld0^3;
    var_app=inv(-( yy*(ld2*ld0-ld1^2)/ld0^2 + (N-yy)*(ld2*(ld0-1)-ld1^2)/(ld0-1)^2 ));

    modef = (myy_i/sigm2_i + mean_app/var_app)/(1/sigm2_i + 1/var_app);
%     sigm_app = sqrt((1/sigm2_i + 1/var_app)^-1);
  end
  % find the mode of the integrand using Newton iterations
  % few iterations is enough, since the first guess in the right direction
  niter=3;       % number of Newton iterations
  mindelta=1e-6; % tolerance in stopping Newton iterations
  for ni=1:niter
      g = ldg(modef);
      h = ldg2(modef);
      delta=-g/h;
      modef=modef+delta;
      if abs(delta)<mindelta
          break
      end
  end
  % integrand limits based on Gaussian approximation at mode
  modes=sqrt(-1/h);
  minf=modef-4*modes;
  maxf=modef+4*modes;
  modeld=ld(modef);
  iter=0;
  % check that density at end points is low enough
  lddiff=12; % min difference in log-density between mode and end-points
  minld=ld(minf);
  step=1;
  while minld>(modeld-lddiff)
    minf=minf-step*modes;
    minld=ld(minf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_negbin -> init_negbin_norm: ' ...
             'integration interval minimun not found ' ...
             'even after looking hard!'])
    end
  end
  maxld=ld(maxf);
  step=1;
  while maxld>(modeld-lddiff)
    maxf=maxf+step*modes;
    maxld=ld(maxf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_negbin -> init_negbin_norm: ' ...
             'integration interval maximun not found ' ...
             'even after looking hard!'])
    end
  end

  function integrand = binomial_norm(f)
  % Logit * Gaussian
    integrand = exp(ldconst + yy*log(1./(1.+exp(-f)))+(N-yy)*log(1-1./(1.+exp(-f)))...
                   - 0.5 * (f-myy_i).^2./sigm2_i);
%     integrand = exp(ldconst ...
%                     +yy*log(x)+(N-yy)*log(1-x) ...
%                     -0.5*(f-myy_i).^2./sigm2_i);
    integrand(isnan(integrand))=0;
  end

  function log_int = log_binomial_norm(f)
  % log(Binomial * Gaussian)
  % log_binomial_norm is used to avoid underflow when searching
  % integration interval

    log_int = ldconst + yy*log(1./(1.+exp(-f)))+(N-yy)*log(1-1./(1.+exp(-f)))...
                   - 0.5 * (f-myy_i).^2./sigm2_i;
%     log_int = ldconst ...
%               -log(1+exp(-yy.*f)) ...
%               -0.5*(f-myy_i).^2./sigm2_i;
  end

  function g = log_binomial_norm_g(f)
  % d/df log(Binomial * Gaussian)
  % derivative of log_logit_norm
    g = -(f-myy_i)./sigm2_i - exp(-f).*(N-yy)./((1+exp(-f)).^2.*(1-1./(1+exp(-f)))) ...
        + exp(-f).*yy./(1+exp(-f));
%     g = yy./(exp(f*yy)+1)...
%         + (myy_i - f)./sigm2_i;
  end

  function g2 = log_binomial_norm_g2(f)
  % d^2/df^2 log(Binomial * Gaussian)
  % second derivate of log_logit_norm
    g2 = - (1+exp(2.*f)+exp(f).*(2+N*sigm2_i)./((1+exp(f))^2*sigm2_i));
%     a=exp(f*yy);
%     g2 = -a*(yy./(a+1)).^2 ...
%          -1/sigm2_i;
  end

end

function p = lik_binomial_invlink(lik, f, z)
%LIK_BINOMIAL_INVLINK  Returns values of inverse link function
%
%  Description
%    P = LIK_BINOMIAL_INVLINK(LIK, F) takes a likelihood structure LIK and
%    latent values F and returns the values of inverse link function P.
%    This subfunction is needed when using gp_predprctmu.
%
%     See also
%     LIK_BINOMIAL_LL, LIK_BINOMIAL_PREDY

  p = logitinv(f);
end

function reclik = lik_binomial_recappend(reclik, ri, lik)
%RECAPPEND  Append the parameters to the record
%
%  Description
%    RECLIK = GPCF_BINOMIAL_RECAPPEND(RECLIK, RI, LIK) takes a
%    likelihood record structure RECLIK, record index RI and
%    likelihood structure LIK with the current MCMC samples of
%    the parameters. Returns RECLIK which contains all the old
%    samples and the current samples from LIK. This subfunction
%    is needed when using MCMC sampling (gp_mc).
%
%  See also
%    GP_MC

  if nargin == 2
    reclik.type = 'Binomial';

    % Set the function handles
    reclik.fh.pak = @lik_binomial_pak;
    reclik.fh.unpak = @lik_binomial_unpak;
    reclik.fh.ll = @lik_binomial_ll;
    reclik.fh.llg = @lik_binomial_llg;
    reclik.fh.llg2 = @lik_binomial_llg2;
    reclik.fh.llg3 = @lik_binomial_llg3;
    reclik.fh.tiltedMoments = @lik_binomial_tiltedMoments;
    reclik.fh.invlink = @lik_binomial_invlink;
    reclik.fh.predprcty = @lik_binomial_predprcty;
    reclik.fh.predy = @lik_binomial_predy;
    reclik.fh.recappend = @likelih_binomial_recappend;
    return
  end

end


function a = sin(a)
%SIN          Gradient sine sin(a)
%

% written  10/16/98     S.M. Rump
% modified 10/14/00     S.M. Rump  use Tony's trick
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/08/08     S.M. Rump  improved sparse multiplication: not using intval data type
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  % use full(a.x(:)): cures Matlab V6.0 bug
  % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
  % ax is full anyway
  ax = cos(full(a.x(:)));
  a.x = sin(a.x);
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    if isa(a.x,'intval')
      adx = times(ax(ia),sa,0);
      if adx.complex
        a.dx = intval( sparse(ia,ja,adx.mid,sizeax,N) , sparse(ia,ja,adx.rad,sizeax,N) , 'midrad' );
      else
        a.dx = intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
      end
    else
      a.dx = sparse(ia,ja,ax(ia).*sa,sizeax,N);
    end
  else
    a.dx = a.dx .* ax(:,ones(1,N));
  end

  if rndold
    setround(rndold)
  end


function C = encode(p,R)
%
% Compute vector of transmit costs for each stage of the KD-tree
%

N = getNpts(p);  bw = getBW(p,1);
if (size(p.bandwidth,2) > 2*N)
  error('Encoding of variable bandwidths not yet supported...');
end;
if (any( abs(getWeights(p) - 1/N) > 2*eps ))
  error('Encoding of variable weights not yet supported...');
end;
C = zeros(1,2*N); C(1) = 2*getDim(p)*R;		% Root node...

for i=1:N-1,            % calc costs of splitting each node; 1/2 cost to each side
 if (~isLeaf(i,N))
  mu0 = p.means(:,i);				% get means of parent, children
  mu1 = p.means(:, double(p.leftch(i))+1);
  mu2 = p.means(:, double(p.rightch(i))+1);
  sig0= sqrt( p.bandwidth(:,i) );		% and bw's of parent, children
  sig1= sqrt( p.bandwidth(:,double(p.leftch(i))+1) );
  sig2= sqrt( p.bandwidth(:,double(p.rightch(i))+1) );
  sig0UB = sqrt(max( sig0.^2 - bw.^2, 2^-R));	% don't round down too far...

  [tmp,splitDim] = max( sig0UB );		% which dimension are we splitting on...
  %[tmp,splitDim2] = max( abs(mu0-mu1) + abs(mu0-mu2) );
  %if (splitDim ~= splitDim2) warning('Disagreement?'); end;

  % COMPUTE COST OF SENDING MEANS
  muMax = max(mu1,mu2); sigDiff = sig0.^2 - (mu1.^2 + mu2.^2 - mu0.^2);
  for j=splitDim,
    costMu = gauss( muMax(j), mu0(j), sig0UB(j).^2 ,1,R );
  end;
  for j = [1:splitDim-1,splitDim+1:getDim(p)];
    costMu = costMu + gauss2( mu1(j), mu0(j), 1*sig0UB(j).^2 ,1,R );
  end;

  % COMPUTE COST OF SENDING VARIANCES
  if (isLeaf(p.leftch(i),N) && isLeaf(p.rightch(i),N)), costSig = 0; costMu = 0;
  elseif (isLeaf(p.leftch(i),N) || isLeaf(p.rightch(i),N)), costSig = 0;
  else
    for j=splitDim,
      costSig = gauss2( sig1(j).^2 , sig0(j).^2/2, sig0(j).^2/4, 1, R);
    end;
    for j = [1:splitDim-1,splitDim+1:getDim(p)]
      costSig = costSig + gauss2( sig1(j).^2 , sig0(j).^2, sig0(j).^2/2, 1, R);
    end;
  end;

  %[costMu,costSig]
  C(double(p.leftch(i))+1) = .5 * (C(i) + costMu + costSig);
  C(double(p.rightch(i))+1) = .5 * (C(i) + costMu + costSig);
 end;
end;

function v = gauss(x,mu,sig2,sigOut2,R)		% SIMPLE 1-SIDED GAUSSIAN
  v = R-log2(  2*1/sqrt(2*pi*sig2) .* exp(-(x-mu).^2 ./(2*sig2)) );

%function v = gauss(x,mu,sig2,sigOut2,R)	% 1-SIDED W/ OUTLIER PROCESS
%  v = R-log2(  2*1/sqrt(2*pi*sig2) .* exp(-(x-mu).^2 ./(2*sig2)) ); + ...
%               1/sqrt(2*pi*sig2) .* exp(-(x-mu).^2 ./(2*sigOut2)) );

function v = gauss2(x,mu,sig2,sigOut2,R)	% SIMPLE 2-SIDED GAUSSIAN
  v = R-log2(  1/sqrt(2*pi*sig2) .* exp(-(x-mu).^2 ./(2*sig2)) );

%function v = gauss2(x,mu,sig2,sigOut2,R)	% 2-SIDED W/ OUTLIER PROCESS
%  v = R-log2(  1/sqrt(2*pi*sig2) .* exp(-(x-mu).^2 ./(2*sig2))  + ...
%               1/sqrt(2*pi*sig2) .* exp(-(x-mu).^2 ./(2*sigOut2)) );

function b = isLeaf(ind,N)
  ind = double(ind);
  if (ind <= 0 || ind > 2*N) b = 0;
  elseif (ind <= N-1) b = 0;
  else b = 1;
  end;