%This file will generate the library we prepared for the MMK example.
% Last Updated: 2019/04/22
% Coded By: K

function [Data,Sym_Struct]=SINDyLib(X,dX,u,Highest_Poly_Order,Highest_Trig_Order,Highest_U_Order,Highest_dPoly_Order)
%% First get the size of the X matrix, determin the data length and the number of variables we have.
[Data_Length,Variable_Number]=size(X);
[~,Variable_Number_dX]=size(dX);
[~,Variable_Number_u]=size(u);
%Also create the symbolic variable
Symbol=sym('z',[Variable_Number,1]);
Symbol_dX=sym('dz',[Variable_Number,1]);
Symbol_u=sym('u',[Variable_Number_u,1]);

%Now according the Highest Polynomial Order entered, we will calculate the data matrix.
Data=[];
Index=1;

%% First calculate the polynomial term
%Order zero:
Index=1;
Data(:,Index)=ones(Data_Length,1);
Sym_Struct{1,Index}=1;

%Order One:
if Highest_Poly_Order>=1
    for i=1:Variable_Number
        Index=Index+1;
        Data(:,Index)=X(:,i);
        Sym_Struct{1,Index}=Symbol(i,1);
    end
end

%Order Two:
if Highest_Poly_Order>=2
    for i=1:Variable_Number
        for j=i:Variable_Number
            Index=Index+1;
            Data(:,Index)=X(:,i).*X(:,j);
            Sym_Struct{1,Index}=Symbol(i,1)*Symbol(j,1);
        end
    end
end

%Order Three:
if Highest_Poly_Order>=3
    for i=1:Variable_Number
        for j=i:Variable_Number
            for k=j:Variable_Number
                Index=Index+1;
                Data(:,Index)=X(:,i).*X(:,j).*X(:,k);
                Sym_Struct{1,Index}=Symbol(i,1)*Symbol(j,1)*Symbol(k,1);
            end
        end
    end
end

%Order Four:
if Highest_Poly_Order>=4
    for i=1:Variable_Number
        for j=i:Variable_Number
            for k=j:Variable_Number
                for pi=k:Variable_Number
                    Index=Index+1;
                    Data(:,Index)=X(:,i).*X(:,j).*X(:,k).*X(:,pi);
                    Sym_Struct{1,Index}=Symbol(i,1)*Symbol(j,1)*Symbol(k,1)*Symbol(pi,1);
                end
            end
        end
    end
end

%% Then add the Trigonometric Function in the output data:

%Order One:
if Highest_Trig_Order>=1
    for i=1:Variable_Number
        Index=Index+1;
        Data(:,Index)=sin(X(:,i));
        Sym_Struct{1,Index}=sin(Symbol(i,1));
    end
    for i=1:Variable_Number
        Index=Index+1;
        Data(:,Index)=cos(X(:,i));
        Sym_Struct{1,Index}=cos(Symbol(i,1));
    end
end

%Order Two:
if Highest_Trig_Order>=2
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)+X(:,j));
            Sym_Struct{1,Index}=sin(Symbol(i,1)+Symbol(j,1));
        end
    end
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)+X(:,j));
            Sym_Struct{1,Index}=cos(Symbol(i,1)+Symbol(j,1));
        end
    end
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-X(:,j));
            Sym_Struct{1,Index}=sin(Symbol(i,1)-Symbol(j,1));
        end
    end
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-X(:,j));
            Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1));
        end
    end
    %
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-2*X(:,j));
            Sym_Struct{1,Index}=sin(Symbol(i,1)-2*Symbol(j,1));
        end
    end
    %
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-2*X(:,j));
            Sym_Struct{1,Index}=cos(Symbol(i,1)-2*Symbol(j,1));
        end
    end
end

%Order Three:
if Highest_Trig_Order>=3
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-X(:,j)).^2;
            Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1))^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-X(:,j)).^2;
            Sym_Struct{1,Index}=sin(Symbol(i,1)-Symbol(j,1))^2;
        end
    end
end

%Order Four
if Highest_Trig_Order>=4
    %
    for i=1:Variable_Number
        for j=i:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(2*X(:,i)-2*X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=sin(2*Symbol(i,1)-2*Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=i:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(2*X(:,i)-2*X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=cos(2*Symbol(i,1)-2*Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=sin(Symbol(i,1)-Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        j=2;
        Index=Index+1;
        Data(:,Index)=cos(X(:,i)-X(:,j)).*sin(X(:,i));
        Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1))*sin(Symbol(i,1));
    end
end

pin=Index;

%% From here, we add the dX*Theta elements in our data.
if Highest_dPoly_Order>=1
    for j=1:Variable_Number_dX
    for k=1:pin
        Index=Index+1;
        Data(:,Index)=dX(:,j).*Data(:,k);
        Sym_Struct{1,Index}=Symbol_dX(j,1)*(Sym_Struct{1,k});
    end
    end
end

%% Frome here, we add the u*Theta elements in our data
if Highest_U_Order>=1
    for j=1:Variable_Number_u
        for k=1:pin
            Index=Index+1;
            Data(:,Index)=u(:,j).*Data(:,k);
            Sym_Struct{1,Index}=Symbol_u(j,1)*(Sym_Struct{1,k});
        end
    end
end


%%*****************************************************************
%% min sum_k bk*yk
%% s.t. sum yk*Hk  <= 0
%%      y1 = 1
%% Hk = -hankel(ek) if   1 <= k <= n
%%    = -hankel(0,e(k-n+1)) if n+1 <= k <=2*n-1
%%
%% [blk,At,C,b] = sdphankel(n);
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

   function [blk,At,C,b] = sdphankel(n);

   randn('seed',0);
   tmp = randn(n,n);
   tmp = tmp+tmp';
   X{1} = tmp + norm(tmp,'fro')*speye(n,n);
%%
   for k = 1:n
      ek = zeros(n,1); ek(k) = -1;
      AA{k} = sparse(hankel(ek));
   end
   zz = zeros(n,1);
   for k = n+1:2*n-1
      ek = zeros(n,1); ek(k-n+1) = -1;
      AA{k} = sparse(hankel(zz,ek));
   end
   blk{1,1} = 's'; blk{1,2} = n;
   At = svec(blk,AA,1);
   C{1} = spconvert([n n 0]);
   b = AXfun(blk,At,[],X);
%%
   blk{2,1} = 'u'; blk{2,2} = 1;
   ee = zeros(1,2*n-1); ee(1) = 1;
   At{2,1} = ee;
   C{2,1} = 1;
%%***************************************************


% toy test
g1 = [ 1     2     3     1     2     3 ;
       4     5     6     5     6     4 ;
       3     3     3     1     1     5 ];
n = 6;
e1 = csaAssign(n,g1)

% big random test
n = 1000;
dg = (rand(n,n) > 0.5);
m = sum(dg(:));
i = find(dg==1)' - 1;
g2 = [ 1 + floor(i/n) ;
       1 + mod(i,n) + n ;
       1 + floor(rand(1,m)*1000) ];
tic;
e2 = csaAssign(2*n,g2);
toc;
if sum(e2(1,:)) ~= n*(n+1)/2, error('bug'); end
if sum(e2(2,:)) ~= n*(n+1)/2 + n*n, error('bug'); end
if sum(sum(e2(1:2,:))) ~= 2*n*(2*n+1)/2, error('bug'); end
disp('[n m cost] = ');
[n m sum(e2(3,:))]


function x = english_word_length_cdf_inv ( cdf )

%*****************************************************************************80
%
%% ENGLISH_WORD_LENGTH_CDF_INV inverts the English Word Length CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 August 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Henry Kucera, Winthrop Francis,
%    Computational Analysis of Present-Day American English,
%    Brown University Press, 1967.
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%    0.0 <= CDF <= 1.0.
%
%    Output, integer X, the corresponding word length for which
%    CDF(X-1) < CDF <= CDF(X)
%
  word_length_max = 27;

  pdf_vec = [ ...
    0.03160, ...
    0.16975, ...
    0.21192, ...
    0.15678, ...
    0.10852, ...
    0.08524, ...
    0.07724, ...
    0.05623, ...
    0.04032, ...
    0.02766, ...
    0.01582, ...
    0.00917, ...
    0.00483, ...
    0.00262, ...
    0.00099, ...
    0.00050, ...
    0.00027, ...
    0.00022, ...
    0.00011, ...
    0.00006, ...
    0.00005, ...
    0.00002, ...
    0.00001, ...
    0.00001, ...
    0.00001, ...
    0.00001, ...
    0.00001 ];
  pdf_sum = 0.99997;

  if ( cdf < 0.0 || 1.0 < cdf )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'ENGLISH_WORD_LENGTH_CDF_INV - Fatal error!\n' );
    fprintf ( 1, '  CDF < 0 or 1 < CDF.\n' );
    error ( 'ENGLISH_WORD_LENGTH_CDF_INV - Fatal error!' );
  end

  cum = 0.0;

  for j = 1 : word_length_max

    cum = cum + pdf_vec(j);

    if ( cdf <= cum / pdf_sum )
      x = j;
      return
    end

  end

  x = word_length_max;

  return
end


function [x, w] = trigpts(n)
%TRIGPTS   Equispaced points in [-1, 1).
%   TRIGPTS(N) returns N equispaced points in [-1, 1).
%
%   [X, W] = TRIGPTS(N) returns also a row vector of the weights for
%   the trapezoidal rule.
%
% See also CHEBPTS, LEGPTS, JACPTS, LAGPTS, and HERMPTS.

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

% Special case (no points).
if ( n <= 0 )
    x = [];
    w = [];
    return

end

x = linspace(-1, 1, n+1).';
x(end) = [];

% Quadrature weights:
if ( nargout > 1 )
    w = trigtech.quadwts(n);
end

end


function imOut=compareImagesBySide(imIn,showlines,shownumbers,frac)
% Compares images by putting parts of the imags side by side.
% imOut=compareImagesBySide(imIn,showlines,shownumbers,frac)
% imIn - stack of images to be compared
% showlines - if true (default) then line is drawn to separate individual sections.
% shownumbers - if true (default) then number to individual section is
% displayed.
% frac - vector specifying fractions of the individual sections (default: frac=size(Im(1))/numberOfImages

if ~exist('showlines','var')
    showlines = 1;
end

if ~exist('shownumbers','var')
    shownumbers = 1;
end

sizeIm=size(imIn);
numberOfImages=sizeIm(3);
if ~exist('frac','var')
    frac=repmat(floor(sizeIm(1)/numberOfImages),1,size(imIn,3));
end

imOut = imIn(0:frac(1)-1,:,0);
for ii=1:numberOfImages-1
    startInd=frac(ii);
    endInd=sum(frac(1:ii+1))-1;
    imOut = cat(1,imOut,imIn(startInd:endInd,:,ii));
end

colorvalue = 'w';

if showlines
dipshow(double(imOut))
for ii=1:numberOfImages-1
    startInd=(ii)*frac(ii)-1;
    line([startInd,startInd],[0,sizeIm(2)],'color',colorvalue,'linewidth',1)
end
end

if shownumbers
for ii=1:numberOfImages
    startInd=(ii)*frac(ii)-1-0.5*frac(ii);
    text(startInd,sizeIm(2)/10,num2str(ii),'color',colorvalue,'fontsize',20)
end
end


function [f] = fact(n)
%FACT  Vectorized Factorial function
%
%usage: f = fact(n)
%
%tested under version 5.3.1
%
%     This function computes the factorial of
%     the elements of N.
%     N can be any size but must contain
%     Real, Non-Negative, Integers.
%
%     This routine is much more robust than
%     the built in FACTORIAL function.
%
%see also: Gamma, Prod, Factorial, Binomial

%Paul Godfrey
%pgodfrey@conexant.com
%8-23-00

[row,col]=size(n);
n=n(:);

f=NaN*n;

pp=find(imag(n)==0 & real(n)>=0 & round(n)==n);
%find integer values
nn=n(pp);
ff=zeros(length(pp),1);

s=1;
p=[];
if ~isempty(nn)
   p=find(nn==0);
end
if ~isempty(p)
   ff(p)=s;
end

%upper limit here depends upon realmax
if ~isempty(nn)
for k=1:170
    s=s*k;
%empty=scalar warning
    p=find(nn==k);
    if ~isempty(p)
        ff(p)=s;
    end
end
end

p=find(nn>170);
if ~isempty(p)
    ff(p)=Inf;
end

f(pp)=ff;
f=reshape(f,row,col);

return


% Test script for solving the 2D advection
Globals2D;

% Generate simple mesh
filename = 'Maxwell025.neu';

[Nv, VX, VY, K, EToV] = MeshReaderGambit2D(filename);
Nfaces = 3;
[EToE,EToF] = tiConnect2D(EToV);
BCType = Wall*(EToE==((1:K)'*ones(1,Nfaces)));

% Build mesh
Norder = ceil(10*rand(K,1));

% Set up arbitrary order elements mesh
[pinfo] = BuildPNonCon2D(Norder, K, VX, VY, EToV, BCType);

% Set initial conditions
mmode = 1; nmode = 1;

x = []; y = [];
for N1=1:max(Norder)
  pinf = pinfo(N1);
  x(pinf.ids) = pinf.x;
  y(pinf.ids) = pinf.y;
end
x = x'; y = y';
Ez = sin(mmode*pi*x).*sin(nmode*pi*y);
Hx = zeros(size(x)); Hy = zeros(size(x));

% Solve Problem for exactly one period
FinalTime = 1;
[Hx,Hy,Ez,time] = MaxwellPNonCon2D(pinfo, Hx,Hy,Ez,FinalTime);


function eAltitude = altitudeefficiency(h,M,throttle,assumptions)
% Change in gas turbine core thermal efficiency as function of altitude.
%
%   relativeEfficiency = altitudeefficiency(h,M,throttle,assumptions)
%
%   h is in meters.
%
%   See also CALCULATEPSFC.

%% Interpolation method
%{
h0 = [0
    3474.72
    6797.04
    9083.04
    11277.6
    14630.4
    17068.8];

e0 = [0.846
    0.93
    0.978
    0.995
    1
    0.992
    0.966];

eAltitude = interp1(h0,e0,h,'linear');
%}

%% Quadratic curve fit from interpolation data
if nargin < 4
    assumptions.jnk = nan;
end
if ~isfield(assumptions,'efficiencyAtSeaLevel')
    assumptions.efficiencyAtSeaLevel = .846;
end
if ~isfield(assumptions,'hMaxEfficiency')
    assumptions.hMaxEfficiency = 11277.6; % meters (37000 ft)
end

k = (1-assumptions.efficiencyAtSeaLevel)./assumptions.hMaxEfficiency.^2;
eAltitude = 1-k.*(h-assumptions.hMaxEfficiency).^2;

end


function sin_test ( )

%*****************************************************************************80
%
%% SIN_TEST tests R4_SIN and R8_SIN.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'SIN_TEST:\n' );
  fprintf ( 1, '  Test SIN_VALUES, R4_SIN, R8_SIN.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             X         SIN(X)\n' );
  fprintf ( 1, '                    R4_SIN(X)         Diff\n' );
  fprintf ( 1, '                    R8_SIN(X)         Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx1 ] = sin_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_sin ( single ( x ) );
    fx3 = r8_sin ( x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14.4f  %14.6g\n', x, fx1 );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  rmpath ( '../test_values' );

  return
end


function [max_err, mean_err] = tperror(lpv, S, U, domain, n)
%TPERROR Calculate the error of the discretized TP model at random points
%	[max_err, mean_err] = TPERROR(lpv, S, U, domain, n)
%
%	lpv      - LPV model
%	S        - core tensor of the TP model
%	U        - weight function data of the TP model
%	domain   - parameter domain of the LPV model
%	n        - number of random test points
%
%	max_err  - maximum L2 error over the n test points
%	mean_err - mean L2 error over the n test points

% TODO: use queryw instead of queryw1

% n random points in the given parameter space
P = size(domain,1);
x = zeros(n, P);
for i = 1:P
	x(:,i) = rand(n,1) * (domain(i,2) - domain(i,1)) + domain(i,1);
end

err = zeros(n, 1);
for i = 1:n
	% S from original model
	S_lpv = querylpv(lpv, x(i,:));

	% S from tp model
	W = queryw1(U, domain, x(i,:));
	S_tp = squeeze(tprod(S, W));

	% L2 error
	err(i) = norm(S_lpv - S_tp, 2);
	if err(i) > 100
		disp('Huge error')
		disp(x(i,:))
		S_lpv
		S_tp
	end
end

max_err = max(err);
mean_err = mean(err);


% ALS Completion
% as described in
%
%   Michael Steinlechner, Riemannian optimization for high-dimensional tensor completion,
%   Technical report, March 2015, revised December 2015.
%   To appear in SIAM J. Sci. Comput.
%

%   TTeMPS Toolbox.
%   Michael Steinlechner, 2013-2016
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt
function [X,cost,test,stats] = completion_als( A_Omega, Omega, A_Gamma, Gamma, X, opts )

    if ~isfield( opts, 'maxiter');  opts.maxiter = 100;     end
    if ~isfield( opts, 'tol');      opts.tol = 1e-6;        end
    if ~isfield( opts, 'reltol');   opts.reltol = 1e-6;     end

	n = X.size;
	r = X.rank;
    d = X.order;

	cost = zeros(2*opts.maxiter,1);
	test = zeros(2*opts.maxiter,1);

    norm_A_Omega = norm( A_Omega );
    norm_A_Gamma = norm( A_Gamma );

    X = orthogonalize( X, 1 );

    t = tic;
    stats.time = [0];
    stats.conv = false;

	for i = 1:opts.maxiter

        % ===================
        % FORWARD SWEEP:
        % ===================
        fprintf(1,'Currently optimizing core: ')
        for mu = 1:d-1
            fprintf(1,'%i ', mu)
            X.U{mu} = solve_least_squares( A_Omega, Omega, X, mu );
            X = orth_at( X, mu, 'left' );
        end
		cost(2*i-1) = sqrt(2*func(A_Omega, X, Omega)) / norm_A_Omega;

        if cost(2*i-1) < opts.tol
            disp(sprintf('CONVERGED AFTER %i HALF-SWEEPS. Rel. residual smaller than %0.3g', ...
                          2*i-1, opts.tol))
            stats.conv = true;
            cost = cost(1:2*i-1,1);
            stats.time = [stats.time stats.time(end)+toc(t)];
            test(2*i-1) = sqrt(2*func(A_Gamma, X, Gamma)) / norm_A_Gamma;
            test = test(1:2*i-1,1);
            break
        end

        if i > 1
            reltol = abs(cost(2*i-1) - cost(2*i-2)) / cost(2*i-1);
            if reltol < opts.reltol
                disp(sprintf('No more progress in gradient change, but not converged after %i half-sweeps. ABORTING!. \nRelative change is smaller than %0.3g', ...
                              i, opts.reltol))
                stats.conv = false;
                cost = cost(1:2*i-1,1);
                stats.time = [stats.time stats.time(end)+toc(t)];
                test(2*i-1) = sqrt(2*func(A_Gamma, X, Gamma)) / norm_A_Gamma;
                test = test(1:2*i-1,1);
                break
            end
        end

        stats.time = [stats.time stats.time(end)+toc(t)];
        test(2*i-1) = sqrt(2*func(A_Gamma, X, Gamma)) / norm_A_Gamma;
        t = tic;

        fprintf(1,'\nFinished forward sweep.\n    Cost: %e\n    Test: %e\n', cost(2*i-1), test(2*i-1) );
        % ===================
        % BACKWARD SWEEP:
        % ===================
        fprintf(1,'Currently optimizing core: ')
        for mu = d:-1:2
            fprintf(1,'%i ', mu)
            X.U{mu} = solve_least_squares( A_Omega, Omega, X, mu );
            X = orth_at( X, mu, 'right' );
        end

		cost(2*i) = sqrt(2*func(A_Omega, X, Omega)) / norm_A_Omega;

        if cost(2*i) < opts.tol
            disp(sprintf('CONVERGED AFTER %i HALF-SWEEPS. Rel. residual smaller than %0.3g', ...
                          2*i, opts.tol))
            stats.conv = true;
            cost = cost(1:2*i,1);
            stats.time = [stats.time stats.time(end)+toc(t)];
            test(2*i) = sqrt(2*func(A_Gamma, X, Gamma)) / norm_A_Gamma;
            test = test(1:2*i,1);
            break
        end

        if i > 1
            reltol = abs(cost(2*i) - cost(2*i-1)) / cost(2*i);
            if reltol < opts.reltol
                disp(sprintf('No more progress in gradient change, but not converged after %i half-sweeps. ABORTING!. \nRelative change is smaller than %0.3g', ...
                              2*i, opts.reltol))
                stats.conv = false;
                cost = cost(1:2*i,1);
                stats.time = [stats.time stats.time(end)+toc(t)];
                test(2*i) = sqrt(2*func(A_Gamma, X, Gamma)) / norm_A_Gamma;
                test = test(1:2*i,1);
                break
            end
        end

        stats.time = [stats.time stats.time(end)+toc(t)];
        test(2*i) = sqrt(2*func(A_Gamma, X, Gamma)) / norm_A_Gamma;
        t = tic;
        fprintf(1,'\nFinished backward sweep.\n    Cost: %e\n    Test: %e\n', cost(2*i), test(2*i) );

        disp('_______________________________________________________________')
    end

    % This is to match original shape of stats.time, since we artificially start w/ [0]
    % for consistency in how we count time
    stats.time = stats.time(2:end);

end

function res = func(A_Omega, X, Omega)
	res = 0.5*norm( A_Omega - X(Omega) )^2;
end

function res = solve_least_squares( A_Omega, Omega, X, mu )

    n = X.size;
    d = X.order;
    r = X.rank;

    [jmu,idx] = sort(Omega(:,mu),'ascend');
    Omega = Omega(idx,:);
    A_Omega = A_Omega(idx);

    C = cell(1,d);
    for i=1:d
        C{i} = permute( X.U{i}, [1 3 2]);
    end
    res = zeros( size(C{mu}) );

    %B = zeros(size(Omega,1), r(mu)*r(mu+1));

    %imu = 1;
    %for sample = 1:size(Omega,1)

    %    L = 1;
    %    for i = 1:mu-1
    %        L = L * C{i}(:,:,Omega(sample,i));
    %    end

    %    R = 1;
    %    for i = d:-1:mu+1
    %        R = C{i}(:,:,Omega(sample,i)) * R;
    %    end
    %
    %    %B(sample,:) = kron(R',L);
    %    B(sample,:) = reshape( L'*R', 1, r(mu)*r(mu+1) );
    %end

    B = als_solve_mex( n, r, C, Omega', mu)';

    for i = 1:X.size(mu)
        idx = find(jmu == i);

        if isempty(idx)
            error('No samples for this slice!')
        end
        res(:,:,i) = reshape(B(idx,:)\A_Omega(idx), r(mu), r(mu+1));
    end

    res = permute( res, [1 3 2] );

end


function C = exp(X,p)
%EXP          Long exponential function
%
%  C = exp(X,p)
%
%for long number X. Input parameter p is optional; if specified,
%approximate accuracy of C approximately p decimals, otherwise that of X.
%Input X must be less than beta.
%

% written  12/30/98     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% 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

  INTLAB_LONG_BETA = getappdata(0,'INTLAB_LONG_BETA');
  INTLAB_LONG_LOGBETA = getappdata(0,'INTLAB_LONG_LOGBETA');

  % convert decimal precision in beta-digits
  if nargin==2
    longprecision(p);
    p = ceil( p/log10(INTLAB_LONG_BETA) );
  else
    p = size(X.mantissa,2) + 1;
  end

  if any( X.exponent>1 )
    error('exponent too big for long exponential')
  end

  index = ( X.exponent>0 );
  if any( index )
    large = 1;
    E = zeros(size(X.sign));
    while any(index)
      E(index) = E(index) + 1;
      X(index) = X(index)/2;
      index = ( X.exponent>0 );
    end
  else
    large = 0;
  end

  C = 1 + X;
  T = X;
  i = 1;
  while 1
    i = i+1;
    T = T * X / i;
    if all( T.exponent<-p )
      if isequal( longinit('ErrorTerm',0) , 'WithErrorTerm' )
        C.error = errorupdate( 1 , C.error , 0 , 1 , 1 , -p );
      end
      break
    end
    C = C + T;
  end

  if large
    index = ( E~=0 );
    while any(index)
      C(index) = C(index)*C(index);
      E(index) = E(index) - 1;
      index = ( E~=0 );
    end
  end

  if rndold
    setround(rndold)
  end


function redoxsteady
% Steady state redox zones
%    using MATLAB ode
%
%   $Ekkehard Holzbecher  $Date: 2006/03/31 $
%--------------------------------------------------------------------------
L = 100;                        % length [m]
v = 1;                          % velocity [m/s]
D = 0.2;                        % diffusivity [m*m/s]
lambda = 0.01;                  % organic carbon degradation parameter [1/m]
k1 = [0.1; 1; 0.9];             % 1. Michaelis-Menten parameter
k2 = [0.035; 1; 1];             % 2. Michaelis-Menten parameter [kg/m*m*m]
k3 = [3.5e-3; 1];               % inhibition coefficient [kg/m*m*m]
corg = 1;                       % organic carbon concentration at interface
cin = [4; 3; 0.001];            % interface concentrations [kg/m*m*m]
N = 100;                        % number of nodes

%----------------------execution-------------------------------------------

x = linspace(0,L,N);
solinit = bvpinit (x,[cin; zeros(3,1)]);
sol = bvp4c(@redox,@bcs,solinit,odeset,D,v,lambda,k1,k2,k3,corg,cin);

%---------------------- graphical output ----------------------------------

plot (x,corg*exp(-lambda*x),sol.x,sol.y(1:3,:));
legend ('C_{org}','O_2','NO_2','Mn'); grid;

%----------------------functions------------------------------
function dydx = redox(x,y,D,v,lambda,k1,k2,k3,corg,cin)

c0 = corg*exp(-lambda*x);
monod = k1.*(y(1:3)>0).*y(1:3)./(k2+y(1:3));
monod(3) = k1(3);
inhib = k3./(k3+y(1:2));
dydx = zeros (6,1);
dydx(1) =  y(4);
dydx(4) =  (v*y(4)+c0*monod(1))/D;
dydx(2) =  y(5);
dydx(5) =  (v*y(5)+c0*monod(2)*inhib(1))/D;
dydx(3) =  y(6);
dydx(6) =  (v*y(6)-c0*monod(3)*inhib(1)*inhib(2))/D;

function res = bcs (ya,yb,D,v,lambda,k1,k2,k3,corg,cin)
res = [ya(1:3)-cin; yb(4:6)-zeros(3,1)];


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

% Vertices
vtx      = zeros(N,3);
vtx(:,1) = (-0.5*L:L/(N-1):0.5*L);

% Elements
elt = [(1:N-1) ; (2:N)]';

% Build mesh
mesh = msh(vtx,elt);
end


% RES = histoMatch(MTX, N, X)
%
% Modify elements of MTX so that normalized histogram matches that
% specified by vectors X and N, where N contains the histogram counts
% and X the histogram bin positions (see histo).

% Eero Simoncelli, 7/96.

function res = histoMatch(mtx, N, X)

if ( exist('histo') == 3 )
  [oN, oX] = histo(mtx(:), size(X(:),1));
else
  [oN, oX] = hist(mtx(:), size(X(:),1));
end

oStep = oX(2) - oX(1);
oC = [0, cumsum(oN)]/sum(oN);
oX = [oX(1)-oStep/2, oX+oStep/2];

N = N(:)';
X = X(:)';
N = N + 1e-10;   %% HACK: no empty bins ensures nC strictly monotonic

nStep = X(2) - X(1);
nC = [0, cumsum(N)]/sum(N);
nX = [X(1)-nStep/2, X+nStep/2];

nnX = interp1(nC, nX, oC, 'linear');

if ( exist('pointOp') == 3 )
  res = pointOp(mtx, nnX, oX(1), oStep);
else
  res = reshape(interp1(oX, nnX, mtx(:)),size(mtx,1),size(mtx,2));
end


% nufft_table_test.m

%
% 3D test
%
if 1
	if ~isvar('s.t'), disp 'precompute structure'
%		ktype = 'minmax:kb';
		ktype = 'kaiser';
		Jd = [3 4 3];
		Nd = [10 8 9];
		Ld = [2^8 2^7 2^7];
		Kd = 2 * Nd;
%		n_shift = zeros(size(Nd)); disp 'easy: 0 shift'
		n_shift = [3 7 2]; % stress it

		gam = 2*pi ./ Kd;
		k1 = linspace(-2*Kd(1), 2*Kd(1), 21)';
		k2 = linspace(-2*Kd(2), 2*Kd(2), 18)';
		k3 = linspace(-2*Kd(3), 2*Kd(3), 23)';
		[kk1 kk2 kk3] = ndgrid(k1, k2, k3);
		om = [gam(1)*kk1(:) gam(2)*kk2(:) gam(3)*kk3(:)];

		tic
		s.p = nufft_init(om, Nd, Jd, Kd, n_shift, ktype);
		printf('pre time init %g', toc)

		tic
		s.t = nufft_init(om, Nd, Jd, Kd, n_shift, 'table', Ld, ktype);
		printf('tab time init %g', toc)
	end

	% 3D forward direction
	if 1, disp '3D forward'
%		x = [1:Nd(1)]'*triang(Nd(2))';
		rand('state', 0);
		x = rand(Nd);
		x = zeros(Nd);
		x(4, 2, 3) = 1;

		tic
		Y.d = dtft(x, om, n_shift); % exact
		printf('dtft3 time %g', toc)

		tic
		Y.p = nufft(x, s.p);
		printf('pre time nufft %g', toc)
		printf('pre max%%diff = %g', max_percent_diff(Y.d, Y.p))

		tic
		Y.t = nufft(x, s.t);
		printf('tab time nufft %g', toc)
		printf('tab max%%diff = %g', max_percent_diff(Y.d, Y.t))
		printf('tab vs pre max%%diff = %g', max_percent_diff(Y.p, Y.t))

		if 1
			clf
			im(231, k1, k2, abs(Y.p - Y.d), 'pre'), cbar
			im(232, k1, k2, abs(Y.t - Y.d), 'tab'), cbar
			im(233, k1, k2, abs(Y.t - Y.p), 'tab-pre'), cbar
		end
	end

	% 3D adjoint
	if 1, disp '3D adjoint'
		X = [1:s.t.M]' + 1i * ones(s.t.M,1);

		tic
		c.d = dtft_adj(X, om, Nd, n_shift); % exact
		printf('dtft time adj %g', toc)
		tic
		c.p = nufft_adj(X, s.p);
		printf('pre time nufft adj %g', toc)
		tic
		c.t = nufft_adj(X, s.t);
		printf('tab time nufft adj %g', toc)

		printf('pre max%%diff = %g', max_percent_diff(c.d, c.p))
		printf('tab max%%diff = %g', max_percent_diff(c.d, c.t))
		printf('tab vs pre max%%diff = %g', max_percent_diff(c.p, c.t))

		if 1
			im(234, abs(c.p - c.d), 'pre adj err'), cbar
			im(235, abs(c.t - c.d), 'tab adj err'), cbar
			im(236, abs(c.t - c.p), 'tab-pre adj'), cbar
		end
	end
return
end

%
% 2D test
%
if 1
	if ~isvar('s.t'), disp 'precompute structure'
%		ktype = 'minmax:kb';
		ktype = 'kaiser';
		Jd = [4 5];
		Nd = [20 16];
		Ld = [2^12 2^13];
		Kd = 2 * Nd;
%		n_shift = zeros(size(Nd));
		n_shift = [3 7]; % stress it

		gam = 2*pi ./ Kd;
		k1 = linspace(-2*Kd(1), 2*Kd(1), 51)';
		k2 = linspace(-2*Kd(2), 2*Kd(2), 81)';
		[kk1 kk2] = ndgrid(k1, k2);
		om = [gam(1)*kk1(:) gam(2)*kk2(:)];

		tic
		s.p = nufft_init(om, Nd, Jd, Kd, n_shift, ktype);
		printf('pre time init %g', toc)

		tic
		s.t = nufft_init(om, Nd, Jd, Kd, n_shift, 'table', Ld, ktype);
		printf('tab time init %g', toc)
	end

	% forward direction
	if 1, disp 'test forward'
		x = [1:Nd(1)]'*triang(Nd(2))';

		tic
		Y.d = dtft2(x, om, n_shift); % exact
		printf('dtft2 time %g', toc)

		tic
		Y.p = nufft(x, s.p);
		printf('pre time nufft %g', toc)
		printf('pre max%%diff = %g', max_percent_diff(Y.d, Y.p))

		tic
		Y.t = nufft(x, s.t);
		printf('tab time nufft %g', toc)
		printf('tab max%%diff = %g', max_percent_diff(Y.d, Y.t))
		printf('tab vs pre max%%diff = %g', max_percent_diff(Y.p, Y.t))

		clf
		im(231, k1, k2, abs(Y.p - Y.d), 'pre'), cbar
		im(232, k1, k2, abs(Y.t - Y.d), 'tab'), cbar
		im(233, k1, k2, abs(Y.t - Y.p), 'tab-pre'), cbar
	end

	% 2D adjoint
	if 1, disp '2D adjoint'
		X = [1:s.t.M]' + 1i * ones(s.t.M,1);

		tic
		c.d = dtft2_adj(X, om, Nd(1), Nd(2), n_shift); % exact
		printf('dtft2 time adj %g', toc)
		tic
		c.p = nufft_adj(X, s.p);
		printf('pre time nufft adj %g', toc)
		tic
		c.t = nufft_adj(X, s.t);
		printf('tab time nufft adj %g', toc)

		printf('pre max%%diff = %g', max_percent_diff(c.d, c.p))
		printf('tab max%%diff = %g', max_percent_diff(c.d, c.t))
		printf('tab vs pre max%%diff = %g', max_percent_diff(c.p, c.t))

		im(234, abs(c.p - c.d), 'pre adj err'), cbar
		im(235, abs(c.t - c.d), 'tab adj err'), cbar
		im(236, abs(c.t - c.p), 'tab-pre adj'), cbar
	end
return
end

%
% 1D test
%
if ~isvar('s.t')
%	ktype = 'minmax:kb';
	ktype = 'kaiser';
	Jd = 4;
	Nd = 32;
	Ld = 2^14; % table over-sampling
	Kd = 2 * Nd;
	n_shift = zeros(size(Nd));
	% n_shift = Nd/2;

	gam = 2*pi ./ Kd;
	kv = 2*Kd;
	kv = linspace(-kv, kv, 1001)';
	om = gam * kv;

	tic
	s.p = nufft_init(om, Nd, Jd, Kd, n_shift, ktype);
	printf('pre time init %g', toc)

	tic
	s.t = nufft_init(om, Nd, Jd, Kd, n_shift, 'table', Ld, ktype);
	printf('tab time init %g', toc)
end

if 0
	ramp = [1:Nd]';

	Y.dr = dtft(ramp, om, n_shift);
	Y.de = dtft(eye(Nd), om, n_shift);
	printf('dtft max%%diff = %g', max_percent_diff(Y.dr,Y.de*ramp))

	Y.pr = nufft(ramp, s.p);
	Y.pe = nufft(eye(Nd), s.p);
	printf('pre max%%diff = %g', max_percent_diff(Y.pr,Y.pe*ramp))

	printf('max%%diff = %g', max_percent_diff(Y.dr,Y.pr))
	printf('max%%diff = %g', max_percent_diff(Y.de,Y.pe))

	e = Y.pr - Y.dr;
	clf, plot(kv, abs(e), 'c-')
%	clf, plot(kv, sum(abs(e),2), 'c-')
return
end

% forward direction
if 1
	x = [1:Nd]';
	% x = unitv(Nd, ii);

	Y.d = dtft(x, om, n_shift); % exact
	tic
	Y.p = nufft(x, s.p);
	printf('pre time nufft %g', toc)
	printf('pre max%%diff = %g', max_percent_diff(Y.d,Y.p))

	tic
	Y.t = nufft(x, s.t);
	printf('tab time nufft %g', toc)
	printf('tab max%%diff = %g', max_percent_diff(Y.d, Y.t))

	clf
	subplot(211)
	plot(kv, abs(Y.p - Y.d), 'c-'), title 'pre'
	subplot(212)
	plot(kv, abs(Y.t - Y.d), 'y-'), title 'tab'
prompt
end

% adjoint
if 1
	X = [1:s.t.M]' + 1i * ones(size(om));

	c.d = dtft2_adj(X, [om 0*om], Nd(1), 1, [n_shift 0]); % exact
	tic
	c.p = nufft_adj(X, s.p);
	printf('pre time nufft adj %g', toc)
	tic
	c.t = nufft_adj(X, s.t);
	printf('tab time nufft adj %g', toc)

	printf('pre nufft adj vs dtft adj max%%diff = %g', max_percent_diff(c.d,c.p))
	printf('tab nufft adj vs dtft adj max%%diff = %g', max_percent_diff(c.d,c.t))

	clf
	nn = 0:Nd(1)-1;
	subplot(211)
	plot(nn, abs(c.p - c.d) / mean(abs(c.d)), 'c-'), title 'pre'
	subplot(212)
	plot(nn, abs(c.t - c.d) / mean(abs(c.d)), 'y-'), title 'tab'
end


%% data association for every kalman filter (every object from previous frame)
function [k, ocn, szn] = asc(k, ocn, szn)
for i             = 1 : size(k, 2)                      % for every kalman filter
[k(i).s, idx]     = assoc(k(i).s, ocn);                 % apply data association
k(i).sz           = szn(:, idx);
szn(:, idx)       = [];
ocn(:, idx)       = [];                                 % eliminated checked objects from the new frame and go for
end                                                     % associating remained objects to the next kalman filter
end
%% data association: 1. no object, 2. one object, 3. multiple objects
function [s, idx] = assoc(s, ocn)
can    = ocn(1 : 2, :);                                 % candidates
pe     = [s.x(1); s.x(3)];                              % previous estimate (to do: replace with last prediction!)
gate   = 4;                                             % gating: 7
idx    = ((sum((can - repmat(pe, 1, size(can, 2)))...   % indexes of objects inside the gate
         .^2)) .^0.5) < gate;
if     sum(idx) == 0                                    % 1. no object
s.z    = pe;                                            % previous estimate/prediction!
elseif sum(idx) == 1                                    % 2. one object
s.z    = can(:, idx);                                   % associated object
elseif sum(idx) > 1                                     % 3. multiple objects (take nearest object)
idx    = ((sum((can - repmat(pe, 1, size(can, 2)))...   % index of the nearest object
         .^2)) .^0.5) == min((sum((can - repmat(pe...
         , 1, size(can, 2))) .^2)) .^0.5);
s.z    = can(:, idx);                                   % associated object
end
end


function C=coef(f)
% Coefficient matrix of constraint and linear objective functions

syms X1 X2 X3 real

C(4)=-double(subs(f,{X1,X2,X3},{0,0,0}));
C(1)=double(subs(f,{X1,X2,X3},{1,0,0}))+C(4);
C(2)=double(subs(f,{X1,X2,X3},{0,1,0}))+C(4);
C(3)=double(subs(f,{X1,X2,X3},{0,0,1}))+C(4);

github-repos/MATLAB/Sable-mcbench-benchmarks/mcbench-benchmarks-ba13b2f0296ef49491b95e3f984c7c41fccdb6d8/12401-optimize-truss-by-fsd-and-slp/Optimize Truss by FSD and SLP/coef.m


%% generate new location for the K users
close all
clear all

K=4;
%%
Pt=zeros(K,2);
%%
Lroom=200;
Wroom=30;
k1=[1,0];
k2=[0,1];
R=10;
%%
for k0=1:K
    r=rand(1,1)*R;
    theta=rand(1,1)*2*pi;
    px=r*cos(theta);
    py=r*sin(theta);
    pt=[Lroom,Wroom]+px*k1+py*k2;
    Pt(k0,:)=pt;
end
%%
save('user_location.mat','K','Pt','Lroom','Wroom','R');
figure
plot(Pt(:,1),Pt(:,2),'ro');
xlim([Lroom-R,Lroom+R]);ylim([Wroom-R,Wroom+R]);
hold on
theta=linspace(0,1,100).*2.*pi;
hold on
plot(Lroom+R*cos(theta),Wroom+R*sin(theta),'r.')


function QQ = augmentQ(tau_vec,r)
%augmentQ constructs an augmented cost matrix
% nth order poly
n = 2*r+1;
K = length(tau_vec);

QQ = zeros((n+1)*K);
for i = 1:K
    QQ((n+1)*(i-1)+1:(n+1)*i,(n+1)*(i-1)+1:(n+1)*i) = costMat(tau_vec(i),r);
end


function x = sample(p, n)
% SAMPLE    Sample from categorical distribution.
% Returns a row vector of integers, sampled according to the probability
% distribution p.
% Uses the stick-breaking algorithm.
% Much faster algorithms are also possible.

if nargin < 2
  n = 1;
end

cdf = cumsum(p(:));
for i = 1:n
  x(i) = sum(cdf < rand) + 1;
end


function triangulation_test12 ( )

%*****************************************************************************80
%
%% TEST12 tests TRIANGULATION_ORDER3_ADJ_COUNT, TRIANGULATION_ORDER3_ADJ_SET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 August 2006
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST12\n' );
  fprintf ( 1, '  For an order3 triangulation:\n' );
  fprintf ( 1, '  TRIANGULATION_ORDER3_ADJ_COUNT counts adjacencies\n' );
  fprintf ( 1, '  TRIANGULATION_ORDER3_ADJ_SET sets adjacencies.\n' );
%
%  Get the sizes.
%
  [ node_num, triangle_num, hole_num ] = ...
    triangulation_order3_example1_size ( );
%
%  Get the data.
%
  [ node_xy, triangle_node, triangle_neighbor ] = ...
    triangulation_order3_example1 ( node_num, triangle_num );
%
%  Get the count of the adjacencies.
%
  [ adj_num, adj_col ] = triangulation_order3_adj_count ( node_num, ...
    triangle_num, triangle_node, triangle_neighbor );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of adjacency entries is %d\n', adj_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Adjacency pointers:\n' );
  fprintf ( 1, '\n' );
  for node = 1 : node_num
    fprintf ( 1, '  %8d  %8d  %8d\n', node, adj_col(node), adj_col(node+1)-1 );
  end
%
%  Get the adjacencies.
%
  adj = triangulation_order3_adj_set ( node_num, ...
    triangle_num, triangle_node, triangle_neighbor, adj_num, adj_col );
%
%  Print the adjacencies.
%
  for node = 1 : node_num

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Nodes adjacent to node %d\n', node );
    fprintf ( 1, '\n' );

    for k = adj_col(node) : adj_col(node+1)-1
      fprintf ( 1, '  %d\n', adj(k) );
    end

  end

  return
end


% test for subdivision curves

options.h = [1 4 6 4 1];

rep = 'results/subdivision-curve/';
if not(exist(rep))
    mkdir(rep);
end

%%%%%%% subdivision function %%%%%%%%
name = 'rand';
name = 'dirac';
name = 'curve';
name = 'square';

switch name
    case 'rand'
        n0 = 9;
        f0 = rescale(rand(1,n0), .05,.95);
    case 'dirac'
        f0 = [0 0 0 1 0 0];
    case 'curve'
        f0 = []; b = 1;
        while b==1
            clf;
            if size(f0,2)>1
                plot(f0(1,:), f0(2,:), '.-');
            end
            axis([0 1 0 1]); box on;
            [x,y,b] = ginput(1);
            f0(:,end+1) = [x;y];
        end
    case 'square'
        f0 = [0 0 1 1; 0 1 1 0];
        f0 = rescale(f0,.05,.95);
end
n0 = size(f0,2);
x0 = linspace(0,1,n0+1);

Jmax = 5; ms = 20; lw = 1.5;
for j=1:Jmax
    f = perform_curve_subdivision(f0, j, options);
    x = linspace(0,1,size(f,2)+1);
    clf;
    hold on;
    if size(f0,1)>1
        h = plot([f(1,:) f(1,1)], [f(2,:) f(2,1)], 'k.-');
    else
        h = plot(x, [f f(1)], 'k.-');
    end
    set(h, 'MarkerSize', ms);
    set(h, 'LineWidth', lw);
    if size(f0,1)>1
        h = plot([f0(1,:) f0(1,1)],[f0(2,:) f0(2,1)], 'r.--');
    else
        h = plot(x0,[f0 f0(1)], 'r.--');
    end
    set(h, 'LineWidth', lw);
    hold off;
    axis([0 1 0 1]); box on;
    saveas(gcf, [rep 'subdivision-func-' name '-' num2str(j) '.png'], 'png');
end


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

%*****************************************************************************80
%
%% BESSEL_Y1_VALUES returns some values of the Y1 Bessel function.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      BesselY[1,x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 August 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 = 16;

  fx_vec = [ ...
     -0.6458951094702027E+01, ...
     -0.7812128213002887E+00, ...
     -0.1070324315409375E+00, ...
      0.3246744247918000E+00, ...
      0.3979257105571000E+00, ...
      0.1478631433912268E+00, ...
     -0.1750103443003983E+00, ...
     -0.3026672370241849E+00, ...
     -0.1580604617312475E+00, ...
      0.1043145751967159E+00, ...
      0.2490154242069539E+00, ...
      0.1637055374149429E+00, ...
     -0.5709921826089652E-01, ...
     -0.2100814084206935E+00, ...
     -0.1666448418561723E+00, ...
      0.2107362803687351E-01 ];

  x_vec = [ ...
      0.1E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      3.0E+00, ...
      4.0E+00, ...
      5.0E+00, ...
      6.0E+00, ...
      7.0E+00, ...
      8.0E+00, ...
      9.0E+00, ...
     10.0E+00, ...
     11.0E+00, ...
     12.0E+00, ...
     13.0E+00, ...
     14.0E+00, ...
     15.0E+00  ];

  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


% test for optimization of the conditionning of the CS matrix

n = 100; m = 300;

[M,eOrth,eAngle,eNorm] = perform_cs_matrix_optimization(randn(n,m), options);

clf;
subplot(2,1,1);
plot(log(eOrth(3:end))); axis tight;
title('Orth');
subplot(2,1,2);
plot(log(eNorm(3:end))); axis tight;
title('Norm');

return;

%%% OLD CODE %%%

c = 0.15*2;
c = Inf;

niter = 20;
eOrth = [];
eNorm = [];
eAngle = [];
for i=1:niter
    % orthogonality constraint
    eOrth(end+1) = norm( M*M' - eye(n)*m/n );
    M = orth(M')';
    M = M(1:n,:) * sqrt(m/n);
    % norm constraint
    d = sqrt( sum( M.^2, 1 ) );
    eNorm(end+1) = sum( abs(d-1) );
    M = M./repmat( d, [n 1] );
    % angle constraint
    if 1
    G = M'*M;
    eAngle(end+1) = norm( G-eye(m) );
    G1 = min(max(G,-c),c);
    G1 = G1-diag(diag(G1))+diag(ones(1,m)); % remet ? 1 la diagonale
    [U,S,V] = svd(G1);
    M = U*diag(sqrt(diag(S)));
    M = U(:,1:n)';
    end
end

clf;
subplot(2,1,1);
plot(log(eNorm(3:end)));
subplot(2,1,2);
plot(log(eOrth(3:end)));


function linpack_d_test18 ( )

%*****************************************************************************80
%
%% TEST18 tests DPOFA and DPOSL.
%
%  Discussion:
%
%    DPOFA factors a positive definite symmetric matrix,
%    and DPOSL can solve a factored linear system.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 20;
  lda = n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST18\n' );
  fprintf ( 1, '  For a positive definite symmetric matrix,\n' );
  fprintf ( 1, '  DPOFA computes the LU factors.\n' );
  fprintf ( 1, '  DPOSL solves a factored linear system.\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Set the matrix A.
%
  a(1:n,1:n) = 0.0;

  for i = 1 : n
    a(i,i) = 2.0;
    if ( 1 < i )
      a(i,i-1) = -1.0;
    end
    if ( i < n )
      a(i,i+1) = -1.0;
    end
  end
%
%  Set the right hand side.
%
  x = zeros ( n, 1 );

  for i = 1 : n
    x(i) = i;
  end

  b(1:n) = a(1:n,1:n) * x(1:n);
%
%  Factor the matrix.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Factor the matrix.\n' );

  [ a, info ] = dpofa ( a, lda, n );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Error, DPOFA returns INFO = %d\n', info );
    return
  end
%
%  Solve the linear system.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the linear system.\n' );

  b = dposl ( a, lda, n, b );
%
%  Print the result.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The first and last five entries of the solution:\n' );
  fprintf ( 1, '  (Should be 1,2,3,4,5,...,n-1,n.)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    if ( i <= 5 | n-5 < i )
      fprintf ( 1, '  %6d  %14f\n', i, b(i) );
    end
    if ( i == 5 )
      fprintf ( 1, '  ......  ..............\n' );
    end
  end

  return
end


function test_approx_test06 ( )

%*****************************************************************************80
%
%% TEST_APPROX_TEST06 uses cubic spline interpolation on all problems.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 August 2011
%
%  Author:
%
%    John Burkardt
%
  ibcbeg = 0;
  ibcend = 0;
  ybcbeg = 0.0;
  ybcend = 0.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST_APPROX_TEST06\n' );
  fprintf ( 1, '  Cubic spline interpolation.\n' );

  prob_num = p00_prob_num ( );

  for prob = 1 : prob_num

    title = p00_title ( prob );

    data_num = p00_data_num ( prob );

    [ xdata, ydata ] = p00_dat ( prob, data_num );

    a = xdata(1);
    b = xdata(data_num);
%
%  Set up the interpolation function.
%
    ypp = spline_cubic_set ( data_num, xdata, ydata, ibcbeg, ybcbeg, ...
      ibcend, ybcend );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Problem %d\n', prob );
    fprintf ( 1, '  %s\n', title );
    fprintf ( 1, '\n' );
    fprintf ( 1, '    X   Y\n' );
    fprintf ( 1, '\n' );
%
%  Evaluate the interpolation function.
%
    for i = 1 : data_num - 1

      jmax = 3;

      if ( i == data_num - 1 )
        jhi = jmax;
      else
        jhi = jmax - 1;
      end

      for j = 1 : jhi

        xval = ( ( jmax - j     ) * xdata(i)     ...
               + (        j - 1 ) * xdata(i+1) ) ...
               / ( jmax     - 1 );

        [ yval, ypval, yppval ] = spline_cubic_val ( data_num, xdata, ydata, ...
          ypp, xval );

        if ( j == 1 || j == 3 )
          mark = '*';
        else
          mark = ' ';
        end

        fprintf ( 1, '  %c  %14g  %14g\n', mark, xval, yval );

      end

    end

  end

  return
end


function predicted=cosmo_classify_nn(samples_train, targets_train, samples_test, unused)
% nearest neighbor classifier
%
% predicted=cosmo_classify_nn(samples_train, targets_train, samples_test[, opt])
%
% Inputs:
%   samples_train      PxR training data for P samples and R features
%   targets_train      Px1 training data classes
%   samples_test       QxR test data
%   opt                (currently ignored)
%
% Output:
%   predicted          Qx1 predicted data classes for samples_test
%
% Example:
%     ds=cosmo_synthetic_dataset('ntargets',5,'nchunks',15);
%     test_chunk=1;
%     te=cosmo_slice(ds,ds.sa.chunks==test_chunk);
%     tr=cosmo_slice(ds,ds.sa.chunks~=test_chunk);
%     pred=cosmo_classify_nn(tr.samples,tr.sa.targets,te.samples,struct);
%     % show targets and predicted labels (100% accuracy)
%     disp([te.sa.targets pred])
%     %||      1     1
%     %||      2     2
%     %||      3     3
%     %||      4     4
%     %||      5     5
%
% See also: cosmo_crossvalidate, cosmo_crossvalidation_measure
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

    [ntrain, nfeatures]=size(samples_train);
    [ntest, nfeatures_]=size(samples_test);
    ntrain_=numel(targets_train);

    if nfeatures~=nfeatures_ || ntrain_~=ntrain, error('illegal input size'); end

    % allocate space for output
    predicted=zeros(ntest,1);

    for k=1:ntest
        % for each sample in the test set:
        %
        % - compute its squared euclidian distance to each sample in
        %   the train set, and store this in a vector
        %   squared_distances (which must have size ntrain x 1).
        %   For two vectors a=[a_1, a_2, ..., a_N] and b=[b_1, b_2, ..., b_N],
        %   the squared euclidean distance between a and b is:
        %       (a_1 - b_1)^2 + (a_2 - b_2)^2 + ... + (a_N - b_N)^2
        %
        % - assign the class label of the sample in the training set that has
        %   the smallest squared distance.
        %
        % >@@>
        % compute difference to each sample in the training set
        delta=bsxfun(@minus, samples_train, samples_test(k,:));

        % compute distance (sqrt is unnecessary because monotonic)
        squared_distances=sum(delta.^2,2);

        % the following code is equivalent to (but slower than) the code above:
        %   squared_distances=zeros(ntrain,1);
        %   for j=1:ntrain
        %       elementwise_delta=samples_train(j,:)-samples_test(j,:);
        %       squared_elementwise_delta=elementwise_delta.^2;
        %       squared_distance=sum(squared_elementwise_delta);
        %       squared_distances(j)=squared_distance;
        %   end

        [unused, i]=min(squared_distances);
        predicted(k)=targets_train(i);
        % <@@<
    end


%MULTIPLY  Calculates the per-element scaled product of two arrays
%
%     dst = cv.multiply(src1, src2)
%     dst = cv.multiply(src1, src2, 'OptionName',optionValue, ...)
%
% ## Input
% * __src1__ first input array.
% * __src2__ second input array of the same size and type as `src1`.
%
% ## Output
% * __dst__ output array of the same size and type as `src1`.
%
% ## Options
% * __Scale__ optional scalar factor. default 1
% * __DType__ optional depth of the output array: `uint8`, `int16`, `double`,
%   etc. default -1
%
% The function cv.multiply calculates the per-element product of two arrays:
%
%     dst(I) = saturate(scale*src1(I) * src2(I))
%
% Note: Saturation is not applied when the output array has the depth `int32`.
% You may even get result of an incorrect sign in the case of overflow.
%
% See also: cv.add, cv.subtract, cv.divide, cv.addWeighted, cv.accumulate,
%  cv.accumulateProduct, cv.accumulateSquare, immultiply, times, mtimes
%


n = 10;
N = 2^n;
K = 2^(n - 1);
R = K/N;
design_snr = 2.5;%dB
%You;d better to learn more about the relationship between RM code and
%polar code. Then you can construct good polar code.
sigma = 1/sqrt(2 * R) * 10^(-design_snr/20);
max_runs = 1e5;
llr_layer_vec = get_llr_layer(N);
bit_layer_vec = get_bit_layer(N);
lambda_offset = 2.^(0 : n);
ber = zeros(N, 1);
for i_run = 1 : max_runs
    if mod(i_run, max_runs/100) == 1
        disp(['Type I monte carlo code construction running = ' num2str(i_run/max_runs*100) '%']);
    end
    dummy_info = rand(N, 1) > 0.5;
    noise = randn(N, 1);
    x = my_polar_encode(dummy_info, lambda_offset, llr_layer_vec);
    bpsk = 1 - 2 * x;
    y = bpsk + sigma * noise;
    llr = 2 * y / sigma^2;
    ber_tmp = mc_typeI_SC_decoder(llr, lambda_offset, llr_layer_vec, bit_layer_vec, dummy_info);
    ber = ber + ber_tmp;
end
[~, channel_ordered] = sort(ber);
info_bits = sort(channel_ordered(1 : K));
disp('Type I MC CC done.')
disp('Variable "info_bits" is what you want')


function th_img = mmse7(inp,inp_map,s,n)
[row col]=size(inp);
th_img=zeros(row,col);
th_img(1:(row/2^n),1:(col/2^n))=inp(1:(row/2^n),1:(col/2^n));
for i=n:-1:1
    a=inp((row/2^i)+1:(row/2^(i-1)),(col/2^i)+1:(col/2^(i-1))); %diagonal coefficients
    b=inp_map((row/2^i)+1:(row/2^(i-1)),(col/2^i)+1:(col/2^(i-1)));
    th_img((row/2^i)+1:(row/2^(i-1)),(col/2^i)+1:(col/2^(i-1)))=th(a,b,s);
    a=inp(1:(row/2^(i)),(col/2^i)+1:(col/2^(i-1)));   %horizontal coefficients
    b=inp(1:(row/2^(i)),(col/2^i)+1:(col/2^(i-1)));
    th_img(1:(row/2^(i)),(col/2^i)+1:(col/2^(i-1)))=th(a,b,s);
    a=inp((row/2^i)+1:(row/2^(i-1)),1:(col/2^(i)));     %vertical coefficients
    b=inp((row/2^i)+1:(row/2^(i-1)),1:(col/2^(i)));
    th_img((row/2^i)+1:(row/2^(i-1)),1:(col/2^(i)))=th(a,b,s);
end
end
function y=th(inp,inp1,s)
[row col]=size(inp);
lambda=1/var(inp1(:));
test1=zeros(row+6,col+6);
test2=zeros(size(test1));
test1(4:end-3,4:end-3)=inp;
[row col]=size(test1);
for i=4:row-3
    for j=4:col-3
        b=test1(i-3:i+3,j-3:j+3);
%         sigsq=max(0,((1/numel(b))*sum(sum(b.^2))-s));
%         lambda=1/sqrt(sigsq);
        A=numel(b)*(-1+sqrt(1+(8*lambda/numel(b)^2)*sum(sum(b.^2))));
        sig_cap_sq=max(0,(A/(4*lambda)-s));
        %test2(i,j)=(sqrt(2)*s)./sqrt(sigsq);
        test2(i,j)=(sig_cap_sq.*test1(i,j))./(sig_cap_sq+s);
    end
end
y=test2(4:row-3,4:col-3);
% % c=(abs(inp)-test2(4:row-3,4:col-3));
% % c(c<0)=0;
% % y=sign(inp).*c;
% [row col]=size(inp);
% y(1:row/2^n,1:col/2^n)=inp(1:row/2^n,1:col/2^n);
end


function tests = test_kmeans_pp
  tests = functiontests(localfunctions);
end

function test_1(testCase)
    X = [1 2 7 8 20 21
         1 2 7 8 20 21];
    % Capture current random number generator state
    st = rng;
    % Go to default state
    rng('default');
    % Perform testing
    [seeds, labels] = spx.cluster.kmeans.pp_initialize(X, 3);
    verifyEqual(testCase, seeds, [
        20     8     1
        20     8     1]);
    verifyEqual(testCase, labels, [3     3     2     2     1     1]);
    % restore previous state
    rng(st);
end


function h = p10_fh ( p, varargin )

%*****************************************************************************80
%
%% P10_FH returns a mesh size function for problem 10.
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Modified:
%
%    06 February 2006
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, real P(NP,ND), the point coordinates.
%
%    Input, VARARGIN, room for extra arguments.
%
%    Output, real H(NP,1), the mesh size function.
%
  np = size ( p, 1 );
  h = ones ( np, 1 );

  return
end


function mesh = genMesh3D(domain, nx, ny, nz)

%% Usage: mesh structure of a uniform cubic partition
%
% INPUTS:
% domain --- cubic domain = [xmin, xmax, ymin, ymax, zmin, zmax].
% nx --- the number of uniform partition in x direction.
% ny --- the number of uniform partition in y direction.
% nz --- the number of uniform partition in z direction.
%
% option.meshinfo --- basic (default): generate only p and t.
%                     all: enriched mesh information
%
% OUTPUTS:
% mesh --- a struct data contains mesh information.
%
% Last Modified: 08/07/2020 by Xu Zhang
%
%                A8-------------------A7        The Cube is divided into
%                /|                   /|        six congruent tetrahedrons
%               / |                  / |
%              /  |                 /  |
%             /   |                /   |        (1) A1-A2-A3-A7
%            /    |               /    |        (2) A1-A6-A2-A7
%          A5-----+-------------A6     |        (3) A1-A5-A6-A7
%           |     |             |      |        (4) A1-A8-A5-A7
%           |     |             |      |        (5) A1-A4-A8-A7
%           |     |             |      |        (6) A1-A3-A4-A7
%           |     A4------------+------A3
%           |     /             |      /
%           |    /              |     /
%           |   /               |    /
%           |  /                |   /
%           | /                 |  /
%           |/                  | /
%          A1-------------------A2
%
%% 1. Generate basic mesh info: p t
[p,T] = genMesh3DRectPT(domain, nx, ny, nz);
c1 = [1,2,3,7];
c2 = [1,6,2,7];
c3 = [1,5,6,7];
c4 = [1,8,5,7];
c5 = [1,4,8,7];
c6 = [1,3,4,7];
t = [T(:,c1); T(:,c2); T(:,c3); T(:,c4); T(:,c5); T(:,c6)];
mesh = struct('p',p,'t',t,'T',T);


% [INPUT]
% data = A float t-by-n matrix (-Inf,Inf) representing the model input.
% tail = A string representing the target tail:
%   - 'L' for lower tail;
%   - 'U' for upper tail.
% bw = An integer [21,252] representing the dimension of each rolling window (optional, default=252).
% f = A float [0.05,0.20] representing the percentage of observations to be included in tails (optional, default=0.10).
% pt = A float [0,1) representing the initial penantly term for underrepresented samples with respect to the bandwidth (optional, default=0.5).
%
% [OUTPUT]
% chi = A float n-by-n-by-t matrix [0,1] representing the Chi coefficients.
% chi_bar = A float n-by-n-by-t matrix [-1,1] representing the Chi Bar coefficients.
%
% [NOTES]
% The bandwidth is automatically expanded as much as possible following an optimality criteria.

function [chi,chi_bar] = asymptotic_tail_dependence(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('data',@(x)validateattributes(x,{'double'},{'real' '2d' 'nonempty'}));
        ip.addRequired('tail',@(x)any(validatestring(x,{'L' 'U'})));
        ip.addOptional('bw',252,@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 21 '<=' 252 'scalar'}));
        ip.addOptional('f',0.10,@(x)validateattributes(x,{'double'},{'real' 'finite' '>=' 0.05 '<=' 0.2 'scalar'}));
        ip.addOptional('pt',0.5,@(x)validateattributes(x,{'double'},{'real' 'finite' '>=' 0 '<' 1 'scalar'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    [data,bw] = validate_input(ipr.data,ipr.bw);
    tail = ipr.tail;
    f = ipr.f;
    pt = ipr.pt;

    nargoutchk(1,2);

    [chi,chi_bar] = asymptotic_tail_dependence_internal(data,bw,tail,f,pt);

end

function [chi,chi_bar] = asymptotic_tail_dependence_internal(data,bw,tail,f,pt)

    up = isempty(getCurrentTask());

    [t,n] = size(data);

    c = nchoosek(1:n,2);
    c_len = size(c,1);

    dc = cell(c_len,2);

    for i = 1:c_len
        c_i = c(i,:);
        dc(i,:) = {c_i data(:,c_i)};
    end

    pt = [linspace(pt,1,bw).'; ones(t-bw,1)];

    dc_results = cell(c_len,2);

    if (up)
        parfor k = 1:c_len
            windows = extract_rolling_windows(dc{k,2},bw);

            chi_k = zeros(t,1);
            chibar_k = zeros(t,1);

            for w = 1:t
                dc_kw = windows{w};

                [chi_kw,chibar_kw] = calculate_coefficients(dc_kw,tail,f);
                chi_k(w) = chi_kw;
                chibar_k(w) = chibar_kw;
            end

            dc_results(k,:) = {(chi_k .* pt) (chibar_k .* pt)};
        end
    else
        for k = 1:c_len
            windows = extract_rolling_windows(dc{k,2},bw);

            chi_k = zeros(t,1);
            chibar_k = zeros(t,1);

            for w = 1:t
                dc_kw = windows{w};

                [chi_kw,chibar_kw] = calculate_coefficients(dc_kw,tail,f);
                chi_k(w) = chi_kw;
                chibar_k(w) = chibar_kw;
            end

            dc_results(k,:) = {(chi_k .* pt) (chibar_k .* pt)};
        end
    end

    en = eye(n);
    chi = repmat(en,1,1,t);
    chi_bar = repmat(en,1,1,t);

    for k = 1:c_len
        dc_k = dc{k,1};
        i = dc_k(1);
        j = dc_k(2);

        [chi_k,chibar_k] = deal(dc_results{k,:});
        chi(i,j,:) = chi_k;
        chi(j,i,:) = chi_k;
        chi_bar(i,j,:) = chibar_k;
        chi_bar(j,i,:) = chibar_k;
    end

    for i = 1:t
        nan_indices = isnan(data(i,:));

        if (any(nan_indices))
            chi(nan_indices,:) = NaN;
            chi(:,nan_indices) = NaN;

            chi_bar(nan_indices,:) = NaN;
            chi_bar(:,nan_indices) = NaN;
        end
    end

end

function [chi,chi_bar] = calculate_coefficients(data,tail,f)

    if (any(any(isnan(data),1)))
        chi = NaN;
        chi_bar = NaN;
        return;
    end

    t = size(data,1);
    t1 = t + 1;

    nu = max(round(t * f),1);
    nu1 = nu + 1;

    if (strcmp(tail,'L'))
        u1 = 1 - (tiedrank(data(:,1)) ./ t1);
        u2 = 1 - (tiedrank(data(:,2)) ./ t1);
    else
        u1 = tiedrank(data(:,1)) ./ t1;
        u2 = tiedrank(data(:,2)) ./ t1;
    end

    zs = -1 ./ log(u1);
    zt = -1 ./ log(u2);
    z =  sort(min(zs,zt),'descend');

    if (nu == 1)
        eta = 0;
    else
        eta = sum(log(z(1:nu) ./ z(nu1)));
    end

    chi_bar = ((2 / nu1) * eta) - 1;
    sigma = (chi_bar + 1)^2 / nu1;
    ci = chi_bar + (norminv(0.975) * sigma^0.5);

    if (ci >= 1)
        chi = z(nu1) * (nu1 / t);
    else
       chi = 0;
    end

end

function [data,bw] = validate_input(data,bw)

    [t,n] = size(data);

    if (n < 2)
        error('The value of ''data'' is invalid. Expected input to be a matrix with at least 2 columns.');
    end

    nan_counts = sum(isnan(data),1);
    nan_threshold = round(t * 0.70,0);

    if (any(nan_counts > nan_threshold))
        error(['The value of ''data'' is invalid. Expected input to contain no more than 70% of NaN values (' num2str(nan_threshold) ') for each time series.']);
    end

    for i = 2:ceil(t / bw)
        bw_i = bw * i;

        if ((bw_i / t) >= 0.3)
            bw = bw_i;
            break;
        end
    end

end


function [H,p,ci,stats] = ttest2_printout(sc1,sc2,varargin)
% :Usage:
% ::
%
%     [H,p,ci,stats] = ttest2_printout(sc1,sc2,[doplot],[covts])
%
% one or two sample t-test printout and plot
% covariates are not done yet!
%
% :Inputs:
%
%   **sc1:**
%        data from first group
%
%   **sc2:**
%        data from second group (if missing or empty, performs one-sample
%        t-test)
%
% ..
%    tor wager, last updated Sept 2007 (cosmetic update)
% ..

if length(varargin) > 2
    % covariates of no interest
    X = varargin{2};
    % NOT DONE YET.
end

if nargin == 1 || isempty(sc2)
    [H,p,ci,stats] = ttest(sc1,0,.05,'both');
    fprintf(1,'u1 = %3.2f, t(%3.1f) = %3.2f, p = %3.4f\n',nanmean(sc1),stats.df,stats.tstat,p);
else
    [H,p,ci,stats] = ttest2(sc1,sc2,.05,'both','unequal');
    fprintf(1,'u1 = %3.2f, u2 = %3.2f, udiff = %3.2f, t(%3.1f) = %3.2f, p = %3.4f\n',nanmean(sc1),nanmean(sc2),nanmean(sc1) - nanmean(sc2),stats.df,stats.tstat, p);
end

means = [nanmean(sc1) nanmean(sc2)];
stats.means = means;

if length(varargin) > 0 && varargin{1}

    hh = bar(means);
    set(hh,'FaceColor',[.7 .7 .7]);

    se = stats.sd ./ sqrt(length(sc1)+length(sc2));

    se = se';
    tor_bar_steplot(means,se,{'k'});

    set(gca,'XTick',[1 2],'XTickLabel',{'High' 'Low'});

end

return


function test_failed=test_frames
%TEST_FRAMES  Test the frames methods

test_failed=0;

disp(' ===============  TEST_FRAMES ================');
global LTFAT_TEST_TYPE;

tolchooser.double=1e-9;
tolchooser.single=2e-4;
tolerance = tolchooser.(LTFAT_TEST_TYPE);

% Iterative algorithms need a bigger tolerance
tolchooseriter.double=1e-1;
tolchooseriter.single=1e-1;
toleranceiter = tolchooseriter.(LTFAT_TEST_TYPE);

Fr=cell(1,32);

L=200;
Fr{1}  = frame('dgt','gauss',10,20);
Fr{2}  = frame('dgtreal','gauss',10,20);
Fr{3}  = frame('dwilt','gauss',20);
Fr{4}  = frame('wmdct','gauss',20);
Fr{5}  = frame('gen',tester_crand(200,300),20);

Fr{6}  = frametight(frame('dgt','gauss',10,20));
Fr{7}  = frametight(frame('dgtreal','gauss',10,20));
Fr{8}  = frametight(frame('dwilt','gauss',20));
Fr{9}  = frametight(frame('wmdct','gauss',20));
Fr{10} = frametight(frame('gen',tester_crand(200,300),20));

Fr{11} = frame('dft');
Fr{12} = frame('dcti');
Fr{13} = frame('dctii');
Fr{14} = frame('dctiii');
Fr{15} = frame('dctiv');
Fr{16} = frame('dsti');
Fr{17} = frame('dstii');
Fr{18} = frame('dstiii');
Fr{19} = frame('dstiv');

% Repeat generation of the filters until they have a nice condition number
condnum = 1e10;
while condnum > 1e3
   gfilt={tester_rand(30,1),...
          tester_rand(20,1),...
          tester_rand(15,1),...
          tester_rand(10,1)};

   gfilt=cellfun(@(gEl) cast(gEl,'double'),gfilt,'UniformOutput',0);

   % These two frames might be badly conditioned,
   Fr{20} = frame('ufilterbank',    gfilt,3,4);
   Fr{21} = frame('ufilterbankreal',gfilt,3,4);

   condnum = framebounds(Fr{20},128);
end

Fr{22} = frame('dgt','gauss',4,6,'lt',[1 2]);
Fr{23} = frame('identity');
Fr{24} = frame('fusion',[1 1],Fr{1},Fr{1});
Fr{25} = frametight(frame('dgt','hamming',10,20));
Fr{26} = frametight(frame('wmdct','hamming',20));

g={randn(30,1),randn(50,1),randn(70,1),randn(90,1)};
a=[20,40,60,80];
M=[30,50,70,100];

Fr{27} = frametight(frame('nsdgt',g,a,M));
Fr{28} = frametight(frame('unsdgt',g,a,100));
Fr{29} = frametight(frame('nsdgtreal',g,a,M));
Fr{30} = frametight(frame('unsdgtreal',g,a,100));

Fr{31} = frametight(frame('dftreal'));

Fr{32} = frame('fwt','ana:spline2:2',5);
Fr{33} = frame('wfbt',{'syn:spline2:2',5});
%Fr{34} = frame('wpfbt',{'db4',5});
%Fr{35} = frame('wpfbt',{'db4',5});
%Fr{36} = frame('wpfbt',{'db4',5});

Fr{37} = frame('ufwt','db4',4);
Fr{38} = frame('ufwt','db4',4,'scale');
Fr{39} = frame('ufwt','db4',4,'noscale');

Fr{40} = frame('uwfbt',{'db4',4});
Fr{41} = frame('uwfbt',{'db4',4},'scale');
Fr{42} = frame('uwfbt',{'db4',4},'noscale');

Fr{43} = frame('uwpfbt',{'db4',4});
Fr{44} = frame('uwpfbt',{'db4',4},'scale');
Fr{45} = frame('uwpfbt',{'db4',4},'noscale');

%Fr{36} = frame('uwfbt',{'db4',5});
%Fr{37} = frame('uwpfbt',{'db4',5});

% The tensor frame implementation is currenly broken
%Fr{33} = frame('tensor',Fr{11});

%Fr{31} = frame('filterbank',     gfilt,[4 3 2 2],4);
%Fr{32} = frame('filterbankreal', gfilt,[4 3 2 2],4);

Fr{60} = frame('erbletfb',44100,L,'real','regsampling');
Fr{61} = frame('erbletfb',44100,L,'complex','regsampling');

Fr{62} = frame('erbletfb',44100,L,'real','fractional');
Fr{63} = frame('erbletfb',44100,L,'complex','fractional');

Fr{64} = frametight(Fr{60});
Fr{65} = frametight(Fr{62});

Fr{66} = frame('cqtfb',44100,200,20000,20,L,'real','regsampling');
Fr{67} = frame('cqtfb',44100,200,20000,20,L,'complex','regsampling');

Fr{68} = frame('cqtfb',44100,200,20000,20,L,'real','fractional');
Fr{69} = frame('cqtfb',44100,200,20000,20,L,'complex','fractional');

Fr{70} = frametight(Fr{66});
Fr{71} = frametight(Fr{67});

for cmpx = {'real','complex'}

    if strcmp(cmpx{1},'real')
        f=tester_rand(L,1);
    else
        f=tester_crand(L,1);
    end

for ii=1:numel(Fr)

  F=Fr{ii};

  % To avoid holes in Fr
  if isempty(F)
    continue;
  end;

  % Do not test real-only frames with complex arrays
  if strcmp(cmpx{1},'complex') && F.realinput
      continue;
  end

  Fd=framedual(F);

  c=frana(F,f);
  r=frsyn(Fd,c);
  res=norm(r(1:L)-f);

  lendiff = size(c,1) - frameclength(F,L);
  [test_failed,fail]=ltfatdiditfail(lendiff ,test_failed);
  s=sprintf(['FRAMES CLENGTH        frameno:%3i %s %0.5g %s'],ii,F.type,lendiff,fail);
  disp(s);

  [test_failed,fail]=ltfatdiditfail(res,test_failed,tolerance);
  s=sprintf(['FRAMES DUAL REC       frameno:%3i %s %0.5g %s'],ii,F.type,res,fail);
  disp(s);

  % Checking equality F == framedual(framedual(F))
  Fdd = framedual(framedual(F));
  cdd = frana(Fdd,f);
  res_dd = norm(cdd-c);

  [test_failed,fail]=ltfatdiditfail(res_dd,test_failed,tolerance);
  s=sprintf(['FRAMES DUAL DUAL      frameno:%3i %s %0.5g %s'],ii,F.type,res_dd,fail);
  disp(s);

  F2=frameaccel(F,L);
  F2d=frameaccel(Fd,L);

  c=frana(F2,f);
  r=frsyn(F2d,c);
  res=norm(r(1:L)-f);

  [test_failed,fail]=ltfatdiditfail(res,test_failed,tolerance);
  s=sprintf(['FRAMES ACCEL DUAL REC frameno:%3i %s %0.5g %s'],ii,F.type,res,fail);
  disp(s);

  % Checking equality F == framedual(framedual(F)) after acceleration
  F2dd = framedual(framedual(F2));
  c2dd = frana(Fdd,f);
  res2_dd = norm(c2dd-c);

  [test_failed,fail]=ltfatdiditfail(res2_dd,test_failed,tolerance);
  s=sprintf(['FRAMES ACCEL DUAL DUAL      frameno:%3i %s %0.5g %s'],ii,F.type,res2_dd,fail);
  disp(s);

  % Test that framebounds are able to run, not actual resting is done on
  % the values.
  [A,B]=framebounds(F,L);

  %% Test iterative analysis and synthesis
  r=frsyniter(F,c);

  res=norm(r(1:L)-f);
  [test_failed,fail]=ltfatdiditfail(res,test_failed,toleranceiter);
  s=sprintf(['FRSYNITER             frameno:%3i %s %0.5g %s'],ii,F.type,res,fail);
  disp(s);

  c2=franaiter(Fd,f);
  res=norm(c2-c);
  [test_failed,fail]=ltfatdiditfail(res,test_failed,toleranceiter);
  s=sprintf(['FRANAITER             frameno:%3i %s %0.5g %s'],ii,F.type,res,fail);
  disp(s);

  %% Test matrix representations
  if (~F.realinput)
    LL=framelength(F,L);
    G=frsynmatrix(F,LL);
    res=norm(c-G'*postpad(f,LL));

    [test_failed,fail]=ltfatdiditfail(res,test_failed,tolerance);
    s=sprintf(['FRAMES ANA MATRIX     frameno:%3i %s %0.5g %s'],ii,F.type,res,fail);
    disp(s);

    % We create a different set of coefficients here.
    % The old code used c and failed the test for some filterbank frames
    ctmp = tester_crand(numel(c),1);
    res=norm(frsyn(F,ctmp)-G*ctmp);
    [test_failed,fail]=ltfatdiditfail(res,test_failed,tolerance);
    s=sprintf(['FRAMES SYN MATRIX     frameno:%3i %s %0.5g %s'],ii,F.type,res,fail);
    disp(s);

  end;

  %% Test the frame multipliers: test framemul, framemuladj and
  %% iframemul
  if F.realinput
      m=1+0.01*tester_rand(size(c,1),1);
  else
      m=1+1i+0.01*tester_crand(size(c,1),1);
  end;
  ff=framemul(f,F,Fd,m);
  fr=iframemul(ff,F,Fd,m);
  res=norm(f-fr(1:L))/norm(f);
  [test_failed,fail]=ltfatdiditfail(res,test_failed,tolerance);
  s=sprintf('IFRAMEMUL             frameno:%3i %s %0.5g %s',ii, ...
            F.type,res,fail);
  disp(s);

end;
end


function [C, Q] = procCosp(DATA, WINDOW_TYPE, WINDOW_SIZE, OVERLAP, FREQUENCY_RANGE, PHASE_CORRECTION)
% [C, Q] = procCosp(DATA, WINDOW_TYPE, WINDOW_SIZE, OVERLAP, FREQUENCY_RANGE)
%
% This function computes cospectra and quadrature spectra on multivariate
% data using Welch's method. For a better estimation, each tappering window
% can be phase corrected according to its length and its position in data.
%
% Inputs:
% - DATA            --> matrix with the data (M samples by N channels matrix)
% - WINDOW_TYPE     --> (optionnal) string with the type of the window
% - WINDOW_SIZE     --> (optionnal) size of the window in number of samples
% - OVERLAP         --> (optionnal) percentage of the overlapping window (from 0 to 0.99)
% - FREQUENCY_RANGE --> (optionnal) range of interesting frequencies to keep,
%                                   format must be [sample_rate min_frec max_frec]
% - PHASE_CORRECTION--> (optionnal) correction of phase shift intrinsic to Welch's windows.
%                                   (1: correction, 0: no correction)
%
% Outputs:
% - C        --> cospectral matrix [N x N x f]
% - Q        --> quadrature matrix [N x N x f]
%
% History
% First version: Jonas Chatel-Goldman @ GIPSA-Lab, 01/12/2011

% default UI display
if(nargin < 9)  UIdisplay  = 0;  end
% default correction
if(nargin < 6)  PHASE_CORRECTION = 0; end
% default frequency range
if(nargin < 5)  FREQUENCY_RANGE = []; end
% default overlapping
if(nargin < 4)  OVERLAP = .75;  end
% default windows size
if(nargin < 3)  WINDOW_SIZE = 128;  end
% default windows type
if(nargin < 2)  WINDOW_TYPE = 'hanning';  end

% Adjust the window to the next pow of 2 (for the FFT)
WINDOW_SIZE = 2 ^ nextpow2(WINDOW_SIZE);
% Calculate the number of frequencies
number_freqs = (WINDOW_SIZE / 2); % +1;

s_data = size(DATA);

% UI DISPLAY
if(UIdisplay)  h= waitbar(0,'Processing cospectrum...');  end

%% Create the window from the inputs and preallocates memory
switch lower(WINDOW_TYPE)
    case 'hamming'
        win = hamming(WINDOW_SIZE);
    case 'hann'
        win = hann(WINDOW_SIZE);
    case 'hanning'
        win = hanning(WINDOW_SIZE);
    case 'blackman'
        win = blackman(WINDOW_SIZE);
    case 'barthannwin'
        win = barthannwin(WINDOW_SIZE);
    case 'blackmanharris'
        win = blackmanharris(WINDOW_SIZE);
    case 'bohmanwin'
        win = bohmanwin(WINDOW_SIZE);
    case 'chebwin'
        win = chebwin(WINDOW_SIZE);
    case 'gausswin'
        win = gausswin(WINDOW_SIZE);
    case 'kaiser'
        win = kaiser(WINDOW_SIZE);
    case 'nuttallwin'
        win = nuttallwin(WINDOW_SIZE);
    case 'parzenwin'
        win = parzenwin(WINDOW_SIZE);
    case 'tukeywin'
        win = tukeywin(WINDOW_SIZE);
    case 'rectangular'
        win = ones(WINDOW_SIZE,1);
    case 'flattopwin'
        win = flattopwin(WINDOW_SIZE);
    case 'welch'
         %win = welchwin(WINDOW_SIZE, 0);
        Nd2=(WINDOW_SIZE-1)/2;
        for i=1 : WINDOW_SIZE
            win(i)=1-((i-Nd2)/Nd2)^2;
        end
        win = win';
    otherwise
        disp('WARNING - Unknown window type!');
        disp('Switching to default window type : Hamming Window');
        win = hamming(WINDOW_SIZE);
end

if s_data(1) < WINDOW_SIZE
    error('Cospectra computation: data segment too short for this window length, consider changing frequency resolution');
end

% Calculate the number of windows because of the overlapping
if(OVERLAP == 0)
    number_windows = floor(s_data(1) / WINDOW_SIZE);
else
    nbFullWin = floor(s_data(1)/WINDOW_SIZE);
    number_windows = 1 + (nbFullWin-1)/(1-OVERLAP) + floor((s_data(1)-((nbFullWin)*WINDOW_SIZE))/((1-OVERLAP)*WINDOW_SIZE));
end

% pre-allocation of memory
S = zeros(s_data(2), s_data(2), number_freqs);

%% Loop on all frequencies
for window_ix = 1 : number_windows

    % UI display
    if(UIdisplay)  waitbar(window_ix/number_windows,h); end

    % time markers to select the data
    t1 = floor((window_ix-1) * (1-OVERLAP) * WINDOW_SIZE) +1;   % marker of the beginning of the time window
    t2 = t1 + WINDOW_SIZE -1;                           % marker of the end of the time window

    % select current window and apodize it
    cdata = DATA(t1:t2, :) .* (win*ones(1,s_data(2)));

    % FFT calculation
    fdata = fft(cdata,WINDOW_SIZE) ./ number_windows; % complex data
    if(PHASE_CORRECTION == 1)
        fdata = fdata.*(exp(-sqrt(-1)*t1*( 0:(WINDOW_SIZE-1) )'/WINDOW_SIZE*2*pi)*ones(1,s_data(2)));
    end

    % Complexe cospectrum averaging over windows
    for f = 1 : number_freqs
        S(:,:,f) = S(:,:,f) + (fdata(f,:)' * fdata(f,:));
    end
end

C = real(S);
Q = imag(S);

% Adjust Frequency range to specified range (in case it is a parameter)
if ~isempty(FREQUENCY_RANGE)
    if  FREQUENCY_RANGE(3) <= (FREQUENCY_RANGE(1) / 2)
        Faxis = [0:(1/number_freqs):1-(1/number_freqs)];
        FindexMin = find(Faxis >= FREQUENCY_RANGE(2)/(FREQUENCY_RANGE(1)/2), 1);
        FindexMax = find(Faxis >= FREQUENCY_RANGE(3)/(FREQUENCY_RANGE(1)/2), 1);
        C = C(:, :, FindexMin:FindexMax);
        Q = Q(:, :, FindexMin:FindexMax);

    else
        % Assertion on frequency, to comply with Nyquist Theorem
        error('Frequency range exceeds data sampling rate in consideration of Shannon''s limitation.')
    end
end

% UI display
if(UIdisplay) close(h); end


function [ax, data] = swissRollScatter(Y, shade);

% SWISSROLLSCATTER 3-D scatter plot with colors.
% FORMAT
% DESC produces a 3-D scatter plot with colors of the type peple use for  the 'swiss roll data'.
% ARG Y : scatter points.
% ARG shade : color indicator for each data point so that they may be given different
% colours.
% RETURN ax : the axes handle where the scatter plot was placed.
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006, 2008, 2011
%
% SEEALSO : fgplvmVisualise, lvmTwoDPlot, lvmScatterPlot

% MLTOOLS

  shade = shade - min(shade)+eps;
  shade = shade/max(shade);
  shade = ceil(shade*64);

  ax = gca;
  jt = colormap('jet');
  plot3(Y(:, 1), Y(:, 2), Y(:, 3), '.');

  xLim = get(ax, 'xlim');
  yLim = get(ax, 'ylim');
  zLim = get(ax, 'zlim');
  %cla
  set(ax, 'xLim', xLim);
  set(ax, 'yLim', yLim);
  set(ax, 'zLim', zLim);
  hold on
  for i = 1:size(Y, 1)
    data(i) = plot3(Y(i, 1), Y(i, 2), Y(i, 3), '.');
    set(data(i), 'color', jt(shade(i), :), 'markersize', 10);
  end

  set(ax, 'fontname', 'arial');
  set(ax, 'fontsize', 20);
end


function [x, mu, sigma] = zscore(x)
    mu=mean(x);
    sigma=max(std(x),eps);
	x=bsxfun(@minus,x,mu);
	x=bsxfun(@rdivide,x,sigma);
end


clear all;

f = @(p)[p-p^2+3 2*p]';

% sampling intervals for each parameter
domain = [-3 3];
% grid size: number of grid points for each parameter
gridsize = 19;

% sampling
D = sampling_vec(f, domain, gridsize);

% hosvd
[S U] = hosvd(D, [1 0]);
U = U{1};

[W V] = genhull(U,'box');
[Wc3 Vc3] = genhull(U);

close all
set(0, 'DefaultFigureWindowStyle', 'docked')
plothull(U);
plothull(W);
plothull(Wc3);

figure
hold on
plot(U(:,1), U(:,2))
plot(V(:,1), V(:,2), 'ko')
plot(Vc3(:,1), Vc3(:,2), 'ro')
plot(Vc3(:,1), Vc3(:,2), 'r-')
hold off


function [model] = rvr_train(X, y, options)
% RVM_TRAIN Trains an RVM Model using SB1 Tipping Toolbox
%
%   input ----------------------------------------------------------------
%
%       o X        : (N x D), N  input data points of D dimensionality.
%
%       o y        : (N x 1), N  output data points
%
%       o options     : struct
%
%
%   output ----------------------------------------------------------------
%
%       o model       : struct.
%
%
%% %    RVM OPTIONS
%       ALPHA   Scalar initial value for hyperparameters
%       BETA    Initial value for inverse noise variance (in regression)
%               Set this negative to fix the value, rather than estimate
%       KERNEL  Kernel type: see SB1_KERNELFUNCTION for options
%       LEN     Kernel length scale
%       USEBIAS Set to non-zero to utilise a "bias" offset
%       MAXITS  Maximum iterations to run for.

%% Parse RVM Options

% Transform Data to Columns
X = X(:);
y = y(:);

% Parsing Parameter for RVR
N	= length(X);
useBias = options.useBias;
kernel_	= options.kernel_;
width   = options.width;
maxIts	= options.maxIts;
monIts	= round(maxIts/20);

% Set verbosity of output (0 to 4)
setEnvironment('Diagnostic','verbosity',3);
% Set file ID to write to (1 = stdout)
setEnvironment('Diagnostic','fid',1);

%% Set up initial hyperparameters - precise settings should not be critical
%
initAlpha	= (1/N)^2;
epsilon		= std(y) * 10/100; % Initial guess of 10% noise-to-signal
initBeta	= 1/epsilon^2;

%% Train RVR Model

% "Train" a sparse Bayes kernel-based model (relevance vector machine)
[weights, used, bias, marginal, alpha, beta, gamma] = ...
    SB1_RVM(X,y,initAlpha,initBeta,kernel_,width,useBias,maxIts,monIts);

%% Model Parameters
%       WEIGHTS Parameter values of estimated model (sparse)
%       USED    Index vector of "relevant" kernels (data points)
%       BIAS    Value of bias or offset parameter
%       ML      Log marginal likelihood of model
%       ALPHA   Estimated hyperparameter values (sparse)
%       BETA    Estimated inverse noise variance for regression
%       GAMMA   "Well-determinedness" factors for relevant kernels

model.weights  = weights;
model.kernel_  = kernel_;
model.width    = width;
model.RVs_idx  = used;
model.bias     = bias;
model.marginal = marginal;
model.alpha    = alpha;
model.beta     = beta;
model.gamma    = gamma;
model.RVs      = X(used,:);

end


function ll = composite_likelihood(S,data,indices)
% Computes the negative of the composite normal loglikelihood (bivariate) for a K-dimensional array
%
% USAGE:
%   [LL] = composite_likelihood(S,DATA,INDICES)
%
% INPUTS:
%   S       - K by K covariance matrix
%   DATA    - Either a K by K matrix (e.g. outer-produces) or a 1 by K vector (e.g. returns)
%   INDICES - Q by 2 array of indices to use when computing the composite likleihood
%
% OUTPUTS:
%   LL      - Composite likelihood
%
% COMMENTS:
%  This is a helper function for various multivariate GARCH models. A MEX file version of this file
%  is available which provides a large speed-up.
%
%  See also DCC, SCALAR_VEC_VECH, BEKK, RARCH

q = size(indices,1);
[m,n] = size(data);
likConst = 3.67575413281869;
ll = 0;
if m==n
    for k=1:q
        i = indices(k,1);
        j = indices(k,2);
        s11 = S(i,i);
        s12 = S(i,j);
        s22 = S(j,j);
        det = s11*s22-s12*s12;
        x11 = data(i,i);
        x12 = data(i,j);
        x22 = data(j,j);
        ll = ll + 0.5*(likConst + log(det) + (s22*x11 - 2*s12*x12 + s11*x22)/det)/q;
    end
else
    for k=1:q
        i = indices(k,1);
        j = indices(k,2);
        s11 = S(i,i);
        s12 = S(i,j);
        s22 = S(j,j);
        det = s11*s22-s12*s12;
        x11 = data(i)*data(i);
        x12 = data(i)*data(j);
        x22 = data(j)*data(j);
        ll = ll + 0.5*(likConst + log(det) + (s22*x11 - 2*s12*x12 + s11*x22)/det)/q;
    end
end


function [fx,dfdx,dfdp] = f_gen(Xt,Theta,ut,inF)

% Generic evolution function (up to quadratic terms)

% [fx,dfdx,dfdp] = f_gen(Xt,Theta,ut,inF)

deltat = inF.deltat;
n = size(Xt,1);
nc = factorial(n)./(factorial(2)*factorial(n-2));
A = reshape(Theta(1:n^2),n,n);
B = reshape(Theta(n^2+1:n*(2*n+nc)),n,n+nc);

xij = zeros(n+nc,1);
ind = cell(n,1);
ind2 = zeros(n,1);
k = 0;
for i=1:n
    for j=1:n
        if j >= i
            k = k+1;
            xij(k) = Xt(i).*Xt(j);
            if i == j
                ind2(i) = k;
            else
                ind{i} = [ind{i},k];
                ind{j} = [ind{j},k];
            end
        end
    end
end
dbxdx = zeros(n,n);
for i=1:n
    for j=1:n
        dbxdx(i,j) = 2*B(i,ind2(j)).*Xt(j) ...
            + B(i,ind{j})*Xt(setdiff(1:n,j));
    end
end

f = A*Xt + B*xij;
fx = Xt + deltat.*f;
dfdx = eye(n) + deltat*(A + dbxdx)';

dfdp = zeros(n,n*(2*n+nc));
dfdp(:,1:n^2) = kron(Xt',eye(n));
dfdp(:,n^2+1:n*(2*n+nc)) = kron(xij',eye(n));
dfdp = deltat*dfdp';


% Some Utility Functions used in the Library
%
% Description
% Including hertz to angular coversion,
% frame_by_frame_calculation used for computer conputation,
% and inner() to calculate inner product of two matrix.
%
% Methods
% angular = hertz_to_angular(hertz, samp_rate)
% hertz = angular_to_hertz(angle, samp_rate)
% coeffs = frame_by_frame_calculation(computer, signal, chunk_size)
% y = inner(a,b)
%
%
% Copyright (c) 2018 Department of Computer Science,
%                    University of Toronto, Canada,
%                    Vector Institute, Canada
%
% License
% This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Yingxue Wang <yingxue@cs.toronto.edu>
%  Sean Robertson <sdrobert@cs.toronto.edu>
%

classdef Util < handle

    methods (Static)
        function angular = hertz_to_angular(hertz, samp_rate)
            % Convert cycles/sec to radians/sec
            %
            % Input
            % samp_rate : [Hz] sampling rate in Hz
            %
            % Output
            % hertz : [radian] sampling rate in radian
            %
            % Example
            % >> samp_rate = 16
            % >> Angular = hertz_to_angular(samp_rate);
            %
            angular = hertz * 2 * pi / samp_rate;
        end

        function hertz = angular_to_hertz(angle, samp_rate)
            % Convert radians/sec to cycles/sec
            %
            % Input
            % samp_rate : [radian] sampling rate in radian
            %
            % Output
            % hertz : [Hz] sampling rate in Hz
            %
            % Example
            % >> samp_rate = 16
            % >> Hertz = angular_to_hertz(samp_rate);
            %
            hertz = angle * samp_rate / (2 * pi);
        end

        function coeffs = frame_by_frame_calculation(computer, signal, chunk_size)
            % Compute feature representation of entire signal iteratively
            %
            % Description
            % This function constructs a feature matrix of a signal through
            % successive calls to `computer.compute_chunk`. Its return value
            % should be identical to that of calling
            % `computer.compute_full(signal)`, but is possibly much slower.
            % `computer.compute_full` should be favoured.
            %
            % Inputs
            % computer   : Computer Object
            % signal     : array-like,  A 1D float array of the entire signal
            % chunk_size : int
            %              Length of the signal buffer to process at a given time
            %
            % Outputs
            % coeffs :  A 2D float array of shape ``(num_frames, num_coeffs)``.
            % ``num_frames`` is nonnegative (possibly 0). Contains some number
            % of feature vectors, ordered in time over axis 0.
            %
            % Example
            % >> bank = GaborFilterBank(MelScaling());
            % >> frame_length_ms = 25;
            % >> frame_shift_ms = 10;
            % >> frame_style = 'causal';
            % >> include_energy = true;
            % >> pad_to_nearest_power_of_two = true;
            % >> use_log = true;
            % >> use_power = true;
            % >> computer = ShortTimeFourierTransformFrameComputer(...
            %                                         bank, ...
            %                                         frame_length_ms, ...
            %                                         frame_shift_ms, ...
            %                                         frame_style, ...
            %                                         include_energy, ...
            %                                         pad_to_nearest_power_of_two, ...
            %                                         use_log, ...
            %                                         use_power);
            % feats_framewise = Util.frame_by_frame_calculation(computer, buff);
            %

            if nargin == 2
                chunk_size = 2 ^ 10;
            end
            if (computer.started == true)
                error('Already started computing frames');
            end
            coeffs = [];

            while length(signal) > 0
                coeff = computer.compute_chunk(signal(1:min(chunk_size, length(signal))));
                coeffs = [coeffs; ...
                            coeff];
                if chunk_size <= length(signal)
                    signal = signal(chunk_size+1:end);
                else
                    signal = [];
                end
            end

            chunk_finalize = computer.finalize();

            coeffs = [coeffs; chunk_finalize];
            coeffs = horzcat(coeffs);
        end

        function res = inner(a,b)
            % Compute the inner product of two vectors a and b.
            %
            % Inputs
            % a : vector
            % b : vector
            %   If a and b are nonscalar, their last dimensions must match.
            %
            % Outputs:
            % res : The value of the inner product of a and b.
            %       res.shape = a.shape[:-1] + b.shape[:-1]
            %
            % Example
            % >> y = inner(a,b) or inner(a,b)
            %

            c = 0;
            n = length(a);
            for k=1:n
                c=c+a(k)*b(k);
            end
            res = c;
        end

    end
end


% Two-input NAND gate sizing (GP)
% Boyd, Kim, Patil, and Horowitz, "Digital circuit optimization
% via geometric programming"
% Written for CVX by Almir Mutapcic 02/08/06
% (a figure is generated)
%
% This is an example taken directly from the paper:
%
%   Digital circuit optimization via geometrical programming
%   by Boyd, Kim, Patil, and Horowitz
%   Operations Research 53(6): 899-932, 2005.
%
% Solves the problem of choosing device widths w_i for the given
% NAND2 gate in order to achive minimum Elmore delay for different
% gate transitions, subject to limits on the device widths,
% gate area, power, and so on. The problem is a GP:
%
%   minimize   D = max( D_1, ..., D_k )  for k transitions
%       s.t.   w_min <= w <= w_max
%              A <= Amax, etc.
%
% where variables are widths w.
%
% This code is specific to the NAND2 gate shown in figure 19
% (page 926) of the paper. All the constraints and the objective
% are hard-coded for this particular circuit.

%********************************************************************
% problem data and hard-coded GP specs (evaluate all transitions)
%********************************************************************
N = 4;       % number of devices
Cload = 12;  % load capacitance
Vdd = 1.5;   % voltage

% device specs
NMOS = struct('R',0.4831, 'Cdb',0.6, 'Csb',0.6, 'Cgb',1, 'Cgs',1);
PMOS = struct('R',2*0.4831, 'Cdb',0.6, 'Csb',0.6, 'Cgb',1, 'Cgs',1);

% maximum area and power specification
wmin = 1;

% varying parameters for the tradeoff curve
Npoints = 25;
Amax = linspace(5,45,Npoints);
Dopt = [];

disp('Generating the optimal tradeoff curve...')
for k = 1:Npoints
    fprintf(1,'  Amax = %5.2f:', Amax(k));
    cvx_begin gp quiet
        % device width variables
        variable w(N)

        % device specs
        device(1:2) = PMOS; device(3:4) = NMOS;

        for num = 1:N
            device(num).R   = device(num).R/w(num); %#ok
            device(num).Cdb = device(num).Cdb*w(num); %#ok
            device(num).Csb = device(num).Csb*w(num); %#ok
            device(num).Cgb = device(num).Cgb*w(num); %#ok
            device(num).Cgs = device(num).Cgs*w(num); %#ok
        end

        % capacitances
        C1 = sum([device(1:3).Cdb]) + Cload;
        C2 = device(3).Csb + device(4).Cdb;

        % input capacitances
        Cin_A = sum([ device([2 3]).Cgb ]) + sum([ device([2 3]).Cgs ]);
        Cin_B = sum([ device([1 4]).Cgb ]) + sum([ device([1 4]).Cgs ]);

        % resistances
        R = [device.R]';

        % area definition
        area = sum(w);

        % delays and dissipated energies for all six possible transitions
        % transition 1 is A: 1->1, B: 1->0, Z: 0->1
        D1 = R(1)*(C1 + C2);
        E1 = (C1 + C2)*Vdd^2/2;
        % transition 2 is A: 1->0, B: 1->1, Z: 0->1
        D2 = R(2)*C1;
        E2 = C1*Vdd^2/2;
        % transition 3 is A: 1->0, B: 1->0, Z: 0->1
        % D3 = C1*R(1)*R(2)/(R(1) + R(2)); % not a posynomial
        E3 = C1*Vdd^2/2;
        % transition 4 is A: 1->1, B: 0->1, Z: 1->0
        D4 = C1*R(3) + R(4)*(C1 + C2);
        E4 = (C1 + C2)*Vdd^2/2;
        % transition 5 is A: 0->1, B: 1->1, Z: 1->0
        D5 = C1*(R(3) + R(4));
        E5 = (C1 + C2)*Vdd^2/2;
        % transition 6 is A: 0->1, B: 0->1, Z: 1->0
        D6 = C1*R(3) + R(4)*(C1 + C2);
        E6 = (C1 + C2)*Vdd^2/2;

        % objective is the worst-case delay
        minimize( max( [D1 D2 D4] ) )
        subject to
            area <= Amax(k); %#ok
            w >= wmin; %#ok
    cvx_end
    % display and store computed values
    fprintf(1,' delay = %3.2f\n',cvx_optval);
    Dopt = [Dopt cvx_optval]; %#ok
end

% plot the tradeoff curve
plot(Dopt,Amax);
xlabel('Dmin'); ylabel('Amax');
disp('Optimal tradeoff curve plotted.')


% THRESHOLD_1D_WAVELET: Thresholded 1D wavelet transform.
% Usage
%    [x_phi, x_psi, meta_phi, meta_psi] = threshold_wavelet_1d(x, ...
%       filters, options)
% Input
%    x: The signal to be transformed.
%    filters: The filters of the wavelet transform.
%    options: Various options for the transform. options.oversampling controls
%       the oversampling factor when subsampling. options.threshold specifies
%       the threshold used.
% Output
%    x_phi: x filtered by lowpass filter phi
%    x_psi: cell array of x filtered by wavelets psi
%    meta_phi, meta_psi: meta information on x_phi and x_psi, respectively
% See also  WAVELET_1D

function [x_phi, x_psi, meta_phi, meta_psi] = threshold_wavelet_1d(x, ...
    filters, options)

if nargin < 3
    options = struct();
end

options = fill_struct(options, 'threshold', 1e-4);

[x_phi, x_psi, meta_phi, meta_psi] = wavelet_1d(x, filters, options);

for p1 = find(~cellfun(@isempty, x_psi))
    res = meta_psi.resolution(p1);

    x_psi{p1 ...
        } = T_theta(x_psi{p1}, options.threshold*2^(res/2));
end
end

function y = T_theta(x, theta)
y=sign(x).*max(0,abs(x)-theta);

end


%% Analyzing Neural Time Series Data
% Matlab code for Chapter 2
% Mike X Cohen
%
% This code accompanies the book, titled "Analyzing Neural Time Series Data"
% (MIT Press). Using the code without following the book may lead to confusion,
% incorrect data analyses, and misinterpretations of results.
% Mike X Cohen assumes no responsibility for inappropriate or incorrect use of this code.

%% Figure 2.1

load sampleEEGdata.mat

nTrials = 6; % modifiable

figure
data = zeros(nTrials,EEG.pnts);

% wavelet... more on this in Chapters 13 and 14
wavetime   = -1:1/EEG.srate:1;
n_conv     = length(wavetime)+EEG.pnts-1;
waveletfft = fft(exp(2*1i*pi*10.*wavetime) .* exp(-wavetime.^2./(2*(5/(2*pi*10))^2))/10,n_conv);
data10hz   = zeros(nTrials,EEG.pnts);

for triali=1:nTrials

    % create single trial "ERP"
    data(triali,:) = .5*sin(2*pi*6.*EEG.times/1000 + 2*pi*triali/nTrials-pi) + randn(1,EEG.pnts)/6;
    % add non-phase-locked stimulus potential (note distributed phases in sine wave)
    data(triali,260:360) = data(triali,260:360) + sin(2*pi*10.*EEG.times(260:360)/1000 + 2*pi*triali/nTrials-pi) + randn(1,101)/5;

    % plot data from this trial
    subplot(nTrials,3,(triali-1)*3+1)
    plot(EEG.times,data(triali,:))
    set(gca,'xlim',[-250 850],'ylim',[-2.2 2.2])

    % plot ERP from trial 1 to current
    subplot(nTrials,3,(triali-1)*3+2)
    plot(EEG.times,mean(data(1:triali,:),1))
    set(gca,'xlim',[-250 850],'ylim',[-2.2 2.2])

    % convolve with 10 Hz wavelet (more on convolution in a few chapters...)
    convolution_result_fft = ifft(waveletfft.*fft(data(triali,:),n_conv)) * sqrt(5/(2*pi*10));
    convolution_result_fft = convolution_result_fft(floor(length(wavetime)/2)+1:end-floor(length(wavetime)/2));
    data10hz(triali,:) = abs(convolution_result_fft).^2;

    % plot 10 Hz power
    subplot(nTrials,3,(triali-1)*3+3)
    plot(EEG.times,mean(data10hz(1:triali,:),1))
    set(gca,'xlim',[-250 850],'ylim',[-.1 .8])
end

%% Figure 2.2
% (This code involves performing convolution with a complex Morlet wavelet,
% which you will learn about in Chapters 10-13.)

srate=1000;

time=(0:1/srate:10);

DCoffset=-.5;

% create multi-frequency signal
a = sin(2*pi*10.*time);             % part 1 of signal (high frequency)
b = .1*sin(2*pi*.3*time)+DCoffset;  % part 2 of signal (low frequency)

data = a.*b; % combined signal
data = data + (2*sin(2*pi*3*time) .* sin(2*pi*.07*time)*.1+DCoffset);

% morlet wavelet convolution (more on this in later chapters)
num_frex = 40;
min_freq =  2;
max_freq = 20;

Ldata  = length(data);
Ltapr  = length(data);
Lconv1 = Ldata+Ltapr-1;
Lconv  = pow2(nextpow2(Lconv1));

frex=logspace(log10(min_freq),log10(max_freq),num_frex);

% initialize
tf=zeros(num_frex,length(data));
datspctra = fft(data,Lconv);

s=4./(2*pi.*frex);
t=-((length(data)-1)/2)/srate:1/srate:((length(data)-2)/2)/srate+1/srate;

for fi=1:length(frex)

    wavelet=exp(2*1i*pi*frex(fi).*t).*exp(-t.^2./(2*s(fi)^2));

    m = ifft(datspctra.*fft(wavelet,Lconv),Lconv);
    m = m(1:Lconv1);
    m = m(floor((Ltapr-1)/2):end-1-ceil((Ltapr-1)/2));

    tf(fi,:) = abs(m).^2;
end

figure

subplot(221)
plot(a)
set(gca,'xlim',[1 8]*1000,'ylim',[-1 1],'xtick',0:1000:10000,'xticklabel',0:10);
title('10 Hz signal, DC=0')

subplot(222)
plot(b)
set(gca,'xlim',[1 8]*1000,'ylim',[-1 1],'xtick',0:1000:10000,'xticklabel',0:10);
title([ '.3 Hz signal, DC=' num2str(DCoffset) ])

subplot(223)
plot(data)
set(gca,'xlim',[1 8]*1000,'ylim',[-1 1],'xtick',0:1000:10000,'xticklabel',0:10);
title('Time-domain signal')

subplot(224)
imagesc(1:length(data),[],tf);
set(gca,'xlim',[1 8]*1000,'ydir','normal','ytick',1:8:num_frex,'yticklabel',round(frex(1:8:end)),'xtick',0:1000:10000,'xticklabel',0:10)
title('Time-frequency representation')

%% Figure 2.3

chan2plot = 'pz'; % you can pick any electrode (type {EEG.chanlocs.labels} for all electrodes)

% compute ERP (time-domain trial average from selected electrode)
erp = squeeze(mean(EEG.data(strcmpi(chan2plot,{EEG.chanlocs.labels}),:,:),3));

% low-pass filter data (uses signal processing toolbox; you'll learn about
% how filtering works in chapter 14. If you don't have the signal processing
% toolbox, just comment out the next eight lines; they are not necessary.)
nyquist       = EEG.srate/2;
filter_cutoff = 40;  % Hz
trans_width   = 0.1; % transition width, in fraction of 1

ffrequencies  = [ 0 filter_cutoff filter_cutoff*(1+trans_width) nyquist ]/nyquist;
idealresponse = [ 1 1 0 0 ];
filterweights = firls(100,ffrequencies,idealresponse);
filtered_erp  = filtfilt(filterweights,1,double(erp));

figure
% plot ERP
plot(EEG.times,filtered_erp,'k.-')

% now down-sample and plot
times2plot = dsearchn(EEG.times',(-200:40:1000)');
hold on
plot(EEG.times(1:5:end),filtered_erp(1:5:end),'mo-')

set(gca,'xlim',[-200 1000],'ydir','r')
xlabel('Time (ms)'), ylabel('Voltage (\muV)')
title([ 'ERP from electrode ' chan2plot ])
legend({'256 Hz';'50 Hz'})

%%


% test omp vs bp

n = 100;
m = 200;
s = 20;
p = 100; % nbr tests

err_bp = [];
err_omp = [];

D = randn(n,m);
D = D./repmat( sqrt(sum(D.^2)), [n 1] );

X0 = randn(m,p);
t = sort(abs(X0));
t = t(end-s,:);
t = repmat(t, [m 1]);
X0 = X0 .* (abs(X0)>t);
Y = D*X0;

options.use_mex = 1;
for k=1:2*s
    progressbar(k,2*s);
    options.nbr_max_atoms = k;
    options.sparse_coder = 'mp';
    X = perform_omp(D,Y,options);
    err_bp(end+1) = norm(Y-D*X,'fro');

    options.sparse_coder = 'omp';
    X = perform_omp(D,Y,options);
    err_omp(end+1) = norm(Y-D*X,'fro');
end

clf;
plot(1:2*s, err_omp, 1:2*s, err_bp);
axis tight;
legend('OMP', 'BP');