function [logbf,t] = spm_bms_ttest (y,prior)
% Log Bayes Factor against null for one sample t-test
% FORMAT [logbf,t] = spm_bms_ttest (y,prior)
%
% y         [N x 1] data vector
% prior     'jzs' (default), 'unit'
%
% logbf     Log Bayes Factor
% t         t-statistic
%
% Default prior is 'jzs'. The different priors are described in
% [1] Rouder et al. (2009) Psych Bull Rev, 16(2),225-237.
%
% These tests consider the quantity d = mean / standard deviation.
% The unit information prior, 'unit', uses a zero mean unit variance
% Gaussian prior over d (the prior variance of d, sig_d^2, is unity).
%
% The (Jeffreys-Zellner-Snow) JZS prior, 'jzs', uses a Cauchy over d
% and an inverse chi^2 over sig_d^2.
%_______________________________________________________________________
% Copyright (C) 2014 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_bms_ttest.m 6038 2014-06-04 15:22:42Z will $

try
    pr=prior;
catch
    pr='jzs';
end

N=length(y);
m=mean(y);
s=std(y);
sem=s/sqrt(N);
t=m/sem;
v=N-1;

switch pr,
    case 'unit',
        rs=1;
    case 'jzs',
        logbf = jzs_logbf(t,v,N);
        return
    otherwise
        disp('Unknown prior in bayes_ttest');
        return
end

% For Unit Information prior, 'unit':
%
% The minimum value of logbf obtains when t=0
% This minimum value is logbf=-0.5*log(1+N*r)
% For r=1 logbf=-3 can be gotten with N=405 samples.

% See note 5 at end of [1]
t2=t^2;
nr=1+N*rs^2;
log_num=-0.5*N*log(1+t2/v);
log_denom1=-0.5*log(nr);
log_denom2=-0.5*N*log(1+t2/(nr*v));

logbf=log_num-log_denom1-log_denom2;
logbf=-logbf;

%-------------------------------------------------------
function [logbf] = jzs_logbf(t,v,N)
% JZS Log Bayes factor
%
% See equation (1) in
% Rouder et al. (2009) Psych Bull Rev, 16(2),225-237.

t2=t^2;
num=(1+t2/v)^(-0.5*(v+1));

gmax=1000;
denom=quad(@(g)jzs_integrand(g,t2,N,v),0,gmax);

logbf=log(num/denom);
logbf=-logbf;

%--------------------------------------------------------
function [j] = jzs_integrand(g,t2,N,v)

ng=1+N*g;
t1=ng.^(-0.5);
t2=(1+t2./(ng*v)).^(0.5*(v+1));
t3=(2*pi)^(-0.5)*(g.^(-1.5)).*exp(-1./(2*g));
j=t1.*t2.*t3;


function [R,S]=rga(G)
% RGA   Relative Gain Array
%
% R=rga(G) returns the Relative Gain Array, R based on the gain matrix, G.
%
% For square matrix, G, R is the normal RGA, defined as follows:
%   R = inv(G') .* G
% If G is nonsquare, R is the so called General RGA:
%   R = pinv(G') .* G
%
% The RGA is a very useful tool for control structure design. For a square
% system (the number of inputs equals to the number of output), the RGA can
% be used to find input-output pairs (one input to control one output).
% There are many publications on how to pair input and output using the
% RGA. The basic rule is:
% 1. Avoid input and output pairs which have negative relative gains.
% 2. Avoid input and output pairs which have large relative gains.
% 3. Select input and output pairs which have the relative gain close to 1.
%
% Interestingly, if the RGA is repeatedly calculated based on R, i.e.
%
% R = rga(R);
%
% Eventualy, the RGA will converge to a selection (0,1)-matrix. With the
% second output,
%
% [R,S] = rga(G) produces a selection matrix, S based on the repeated RGA.
%
% where S is (0,1) matrix with the same dimension as G. Each row and each
% column of S have only one element of 1, but all others are 0. The element
% of 1 indicates which input (column) should be used to pair which output
% (row).
% Note: The selection matrix, S, is only a recommendation. It may not
% satisfy the rules stated above and may not be appropriate for some
% systems.
%
% For nonsquare systems, the general RGA can also be used to select inputs
% if the number of inputs is larger than the number of outputs, or to
% choose outputs, if the system has more outputs than inputs. The second
% output in this case gives the input (output) effectiveness, E.
%
% [R,E] = rga(G)
%
% Where E is a column vector corresponding to the input (output). Inputs
% (outputs) with largest input (output) effectiveness are recommented to be
% selected for the control system.
%
% By Yi Cao at Cranfield University on 7th March 2008
%
% References:
%
% 1. Bristol, E.H., On a new measure of interactions for multivariable process
% control. IEEE Trans. Automatic Control, AC-11:133-134, 1966.
% 2. Cao, Y and Rossiter, D, An input pre-screening technique for control
% structure selection, Computers and Chemical Engineering, 21(6), pp.
% 563-569, 1997.

% Examples:
%
% Example 1: A random 5 x 5 matrix
%{
G = randn(5);
[R,S] = rga(G)
%}
% Example 2: A random 5 x 10 matrix for input selection
%{
G = randn(5,10);
[R,IE] = rga(G)
%}
% Example 3: A random 10 x 5 matrix for output selection
%{
G = randn(10,5);
[R,OE] = rga(G)
%}
%

% Check input and output
error(nargchk(1,1,nargin));
error(nargoutchk(0,2,nargout));

% The RGA
R=pinv(G.').*G;
if nargout<2
    return
end

[m,n]=size(G);
% The square case
if n==m
    S=R;
    while norm(round(S)-S)>1e-9
        S(S<0)=0;
        S = inv(S.').*S;
    end
    return
end

% The nonsquare case
S = sqrt(sum(R))';
if m>n
    S = sqrt(sum(R,2));
end


%% LSHAPESTOKES Poiseuille flow in a L-shaped domain
%
% Stokes equations on the square [-1,1]^2.
%
% Reference: page 237 in Finite Elements and Fast Iterative Solvers with
% Applications in Incompressible Fluid Dynamics. by Howard C. Elman, David
% J. Silvester, and Andrew J. Wathen.
%
% See also squareStokes, StokesP2P1
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

close all; clear all;
%% Parameters
maxIt = 4; N = zeros(maxIt,1);
erru = zeros(maxIt,1); errp = zeros(maxIt,1);

%% Generate an initial mesh
[node,elem] = squaremesh([-1 1 -1 1], 0.25);
bdFlag = setboundary(node,elem,'Dirichlet');

%% PDE and options
pde = Stokesdata1;

%% Finite Element Method
for k = 1:maxIt
    % solve the equation
    [u,p,edge,A] = StokesP2P1(node,elem,pde,bdFlag);
    N(k) = length(u)+length(p);
    if N(k) < 2e3 % show solution for small size
        figure(1);  showresult(node,elem,p);
    end
    % compute error
    uI = pde.exactu([node; (node(edge(:,1),:)+node(edge(:,2),:))/2]);
    erru(k) = sqrt((u-uI(:))'*A*(u-uI(:)));
    errp(k) = getL2error(node,elem,pde.exactp,p);
    % refine mesh
    [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
end

%% Plot convergence rates
figure(2); clf;
showrate2(N,erru,2,'-*','||Du_I-Du_h||',N,errp,2,'k-+','|| p - p_h||');


function p=legendrep(m,x)
%   function which construct Legendre polynomial Pm(x)
%   where M is the degree of polynomial and X is the variable.
%   Result - P is the char string that should be evaluated EVAL(P)
%   Example:
%   P=legendrep(2,'cos(theta)') will produce
%   P='(3*cos(theta).^2 -1)/2' which then can be evaluated as
%   theta=0.3; P=legendrep(2,'cos(theta)'); Lp=eval(P); produce
%   Lp = 0.8690
%   For Matlab R14 the following example can be used:
%   x=-5:.1:5; p=legendrep(5,'x .*cos(x)'); xp = eval(p);
%   figure; plot(x, xp, 'r.-'); grid

%% References:
%   Gradshteyn, Ryzhik "Table of Integrals Series and Products", 6th ed., p.973
%
%__________________________________________________
% 	Sergei Koptenko, Resonant Medical Inc., Toronto
%	sergei.koptenko@resonantmedical.com
%______________March/30/2004_______________________
%%
switch m
case 0
    p='1';     return
case 1
    p=x;     return
case 2
    p=['(3*' x '.^2 -1)/2'];     return
case 3
    p=['(5*' x '.^3 - 3 *' x ')/2'];     return
case 4
    p=['(35 *' x '.^4 - 30 * ' x '.^2 + 3)/8'];     return
case 5
    p=['( 63 * ' x '.^5 - 70 * ' x '.^3 + 15 *' x ' )/8'];     return
case 6
    p=['(231 *' x '.^6 - 315 * ' x '.^4 + 105 * ' x '.^2 -5)/16'];     return
 case 7
   p=['(429 * ' x '.^7 - 693 * ' x '.^5 +315 * ' x '.^3 -35 *' x ' )/16'];     return
case 8
    p=['(6435 *' x '.^8 -12012 *' x '.^6 + 6930 * ' x '.^4 -1260 * ' x '.^2 +35)/128'];     return
case 9
    p=['(12155 * ' x '.^9 -25740 * ' x '.^7 +18018 * ' x '.^5 -4620 * ' x '.^3 +315 *' x ' )/128'];
    return
case 10
       p=['(46189 *' x '.^10 -109395 *' x '.^8 +90090 *' x '.^6 -30030 * ' x '.^4 +3465 * ' x '.^2 -63)/256'];
    return
case 11
   p=['(88179 * ' x '.^11 -230945 * ' x '.^9 +218790 * ' x '.^7 -90090 * ' x '.^5 +15015 * ' x '.^3 -693 *' x ' )/256'];
    return
case 12
    p=['(676039 *' x '.^12 -1939938 *' x '.^10 +2078505 *' x '.^8 -1021020 *' x '.^6 +225225 * ' x '.^4 -18018 * ' x '.^2 +231)/1024'];
    return

otherwise
    iii=m-10;    %shift counter
   pp=strvcat(legendrep(11,x),legendrep(12,x)); % get last two members
    for ii=3:1:iii,   % Begin construct from 13th member
        p_ii=[num2str(1/(ii)) ' * (' num2str(2*ii-1) ' * ' x '.*(' ...
        deblank(pp(ii-1,:)) ')-' num2str(ii-1) '.*(' deblank(pp(ii-2,:)) '))'];
        pp=strvcat(pp, p_ii);
    end
p=deblank(pp(iii,:)); % remove traiing blanks
   return
end

return


function [A,b] = parallax(n)
%PARALLAX Stellar parallax problem with 28 fixed, real observations.
%
% [A,b] = parallax(n)
%
% Stellar parallax problem with 28 fixed, real observations.
%
% The underlying problem is a Fredholm integral equation of the
% first kind with kernel
%    K(s,t) = (1/sigma*sqrt(2*pi))*exp(-0.5*((s-t)/sigma)^2) ,
% and it is discretized by means of a Galerkin method with n
% orthonormal basis functions.  The right-hand side consists of
% a measured distribution function of stellar parallaxes, and its
% length is fixed, m = 26.  The exact solution, which represents
% the true distribution of stellar parallaxes, in not known.

% Reference: W. M. Smart, "Stellar Dynamics", Cambridge University Press,
% 1938; p. 30.

% Discretized by Galerkin method with orthonormal box functions;
% 2-D integration is done by means of the computational molecule:
%       1   4   1
%       4  16   4
%       1   4   1

% Per Christian Hansen, IMM, 09/16/92.

% Initialization.
a = 0; b = 0.1; m = 26; sigma = 0.014234;
hs = 0.130/m; hx = (b-a)/n; hsh = hs/2; hxh = hx/2;
ss = (-0.03 + [0:m-1]'*hs)*ones(1,n);
xx = ones(m,1)*(a + [0:n-1]*hx);

% Set up the matrix.
A =     16*exp(-0.5*((ss+hsh - xx-hxh)/sigma).^2);
A = A + 4*(exp(-0.5*((ss+hsh - xx    )/sigma).^2) + ...
           exp(-0.5*((ss+hsh - xx-hx )/sigma).^2) + ...
           exp(-0.5*((ss     - xx-hxh)/sigma).^2) + ...
           exp(-0.5*((ss+hs  - xx-hxh)/sigma).^2));
A = A +   (exp(-0.5*((ss     - xx    )/sigma).^2) + ...
           exp(-0.5*((ss+hs  - xx    )/sigma).^2) + ...
           exp(-0.5*((ss     - xx-hx )/sigma).^2) + ...
           exp(-0.5*((ss+hs  - xx-hx )/sigma).^2));
A = sqrt(hs*hx)/(36*sigma*sqrt(2*pi))*A;

% Set up the normalized right-hand side.
b = [3;7;7;17;27;39;46;51;56;50;43;45;43;32;33;29;...
     21;12;17;13;15;12;6;6;5;5]/(sqrt(hs)*640);


function [Y, spectrum] = sllle_wg(G, d)
%SLLLE_WG Solves the Locally Linear Embedding from weight graph
%
% $ Syntax $
%   - Y = sllle_wg(G, d)
%   - [Y, spectrum] = sllle_wg(G, d)
%
% $ Arguments $
%   - G:        The weights graph
%   - d:        The dimension of the embedding
%   - Y:        The sample coordinates in the embedded space
%   - spectrum: The spectrum of the embeded space dimensions
%
% $ Description $
%   - Y = sllle_wg(G, d) solves the locally linear embedding from
%     a given weight graph. G should be a graph of n nodes, where
%     the edge value from i-th node to j-th node, means the weights
%     on the i-th sample in constructing the j-th sample, (or the
%     construction of i-th sample to the j-th sample).
%     Y is solved by taking the eigenvectors of (I - W)(I - W)^T,
%     corresponding to the (d+1) smallest eigenvalues, and discarding
%     the smallest one.  In our output, the dimension is sorted in the
%     ascending order of eigenvalues.
%
%   - [Y, spectrum] = sllle_wg(G, d) also returns the spectrum of the
%     corresponding dimensions, a column vector of the corresponding
%     eigenvalues.
%
% $ Remarks $
%   - The dimension d should be strictly less than n.
%
% $ History $
%   - Created by Dahua Lin, on Sep 11st, 2006
%

%% parse and verify input

if nargin < 2
    raise_lackinput('sllle_wg', 2);
end

gi = slgraphinfo(G, {'square'});
if ~gi.valued
    error('sltoolbox:invalidarg', 'The graph G should be a valued graph');
end
if isnumeric(G)
    W = G;
else
    W = sladjmat(G, 'sparse', true);
end

n = gi.n;
if d >= n
    error('sltoolbox:invalidarg', 'd should be strictly less than n');
end

%% compute

if issparse(W)
    M = speye(n) - W;
else
    M = eye(n) - W;
end
clear W;
M = M * M';

[spectrum, Y] = slsymeig(M, d+1, 'ascend');
clear M;
spectrum = spectrum(2:d+1);
Y = Y(:, 2:d+1);
Y = Y';


function [ img_ihs] = rgb2ihs( img_rgb )
r=img_rgb(:,:,1);
g=img_rgb(:,:,2);
b=img_rgb(:,:,3);

% rgbתihs
I=1/sqrt(3).*r+1/sqrt(3).*g+1/sqrt(3).*b;
v1=1/sqrt(6).*r+1/sqrt(6).*g-2/sqrt(6).*b;
v2=1/sqrt(2).*r-1/sqrt(2).*g;
img_ihs=cat(3,I,v1,v2);
end


function cnt = countcover(sz,blocksize,stepsize)
%COUNTCOVER Covering of signal samples by blocks
%  CNT = COUNTCOVER(SZ,BLOCKSIZE,STEPSIZE) assumes a p-dimensional signal
%  of size SZ=[N1 N2 ... Np] covered by (possibly overlapping) blocks of
%  size BLOCKSIZE=[M1 M2 ... Mp]. The blocks start at position (1,1,..,1)
%  and are shifted between them by steps of size STEPSIZE=[S1 S2 ... Sp].
%  COUNTCOVER returns a matrix the same size as the signal, containing in
%  each entry the number of blocks covering that sample.
%
%  See also IM2COLSTEP, COL2IMSTEP, IM2COL.

%  Ron Rubinstein
%  Computer Science Department
%  Technion, Haifa 32000 Israel
%  ronrubin@cs
%
%  August 2008

cnt = ones(sz);
for k = 1:length(sz)

  % this code is modified from function NDGRID, so it computes one
  % output argument of NDGRID at a time (to conserve memory)
  ids = (1:sz(k))';
  s = sz; s(k) = [];
  ids = reshape(ids(:,ones(1,prod(s))),[length(ids) s]);
  ids = permute(ids,[2:k 1 k+1:length(sz)]);

  cnt = cnt .* max( min(floor((ids-1)/stepsize(k)),floor((sz(k)-blocksize(k))/stepsize(k))) - ...
                    max(ceil((ids-blocksize(k))/stepsize(k)),0) + 1 , 0 );
end


function [x1,x2,nS]=bracket(S,x0,d,problem,stepsize)
%   Bracket the minimum (or maximum) of the objective function
%   in the search direction.
%
%   [x1,x2,nS]=bracket(S,x0,d,problem,stepsize)
%
%   S: objective function
%   x0: initial point
%   d: search direction vector
%   problem: (-1): minimum (default), (1): maximum
%   stepsize: initial stepsize (default = 0.01*norm(d))
%   [x1,x2]: unsorted lower and upper limits
%   nS: number of objective function evaluations

%   Copyright (c) 2001 by LASIM-DEQUI-UFRGS
%   $Revision: 1.0 $  $Date: 2001/07/04 21:45:10 $
%   Argimiro R. Secchi (arge@enq.ufrgs.br)

 if nargin < 3,
   error('bracket requires three input arguments');
 end
 if nargin < 4,
   problem=-1;
 end
 if nargin < 5,
   stepsize=0.01*norm(d);
 end

 d=d(:);
 x0=x0(:);
 j=0; nS=1;
 y0=feval(S,x0)*problem;

 while j < 2,
  x=x0+stepsize*d;
  y=feval(S,x)*problem;
  nS=nS+1;

  if y0 >= y,
    stepsize=-stepsize;
    j=j+1;
  else
    while y0 < y,
      stepsize=2*stepsize;
      y0=y;
      x=x+stepsize*d;
      y=feval(S,x)*problem;
      nS=nS+1;
    end
    j=1;
    break;
  end
 end

 x2=x;
 x1=x0+stepsize*(j-1)*d;


clear; clc;
% P2.4
x = [1,-2,4,6,-5,8,10];
n = [-4:2];

% P2.4a
[x11,n11] = sigshift(x,n,-2);
[x12,n12] = sigshift(x,n,4);
[x1,n1] = sigadd(3*x11,n11,x12,n12);
[x1,n1] = sigadd(x1,n1,-2*x,n);
[xe,xo,m] = evenodd(x1,n1);
figure(1);
subplot(2,2,1); stem(n1,x1,'ko'); title('Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(2,2,2); stem(m,xe,'ko'); title('Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(2,2,4); stem(m,xo,'ko'); title('Odd Part')
xlabel('n'); ylabel('xo(n)');

% P2.4b
[x21,n21] = sigshift(x,n,-5);
[x22,n22] = sigshift(x,n,-4);
[x2,n2] = sigadd(5*x21,n21,4*x22,n22);
[x2,n2] = sigadd(x2,n2,3*x,n);
[xe,xo,m] = evenodd(x2,n2);
figure(2);
subplot(2,2,1); stem(n2,x2,'ko'); title('Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(2,2,2); stem(m,xe,'ko'); title('Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(2,2,4); stem(m,xo,'ko'); title('Odd Part')
xlabel('n'); ylabel('xo(n)');

% P2.4c
[x31,n31] = sigshift(x,n,-4);
[x32,n32] = sigshift(x,n,1);
[x33,n33] = sigmult(x31,n31,x32,n32);
[x31,n31] = sigfold(x,n);    [x31,n31] = sigshift(x31,n31,2);
[x32,n32] = sigmult(x31,n31,x,n);
[x3,n3] = sigadd(x33,n33,x32,n32);
[xe,xo,m] = evenodd(x3,n3);
figure(3);
subplot(2,2,1); stem(n3,x3,'ko'); title('Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(2,2,2); stem(m,xe,'ko'); title('Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(2,2,4); stem(m,xo,'ko'); title('Odd Part')
xlabel('n'); ylabel('xo(n)');

% P2.4d
[x41,n41] = sigshift(x,n,-2);
x42 = cos(0.1*pi*n);
[x42,n42] = sigmult(x42,n,x41,n41);
x43 = 2*exp(0.5*n);
[x41,n41] = sigmult(x43,n,x,n);
[x4,n4] = sigadd(x42,n42,x41,n41);
[xe,xo,m] = evenodd(x4,n4);
figure(4);
subplot(2,2,1); stem(n4,x4,'ko'); title('Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(2,2,2); stem(m,xe,'ko'); title('Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(2,2,4); stem(m,xo,'ko'); title('Odd Part')
xlabel('n'); ylabel('xo(n)');

% P2.4e
x5 = zeros(size(n));
for k = 1:5,
    [x51,n51] = sigshift(x,n,k);
    x52 = n.*x51;
    x5 = sigadd(x5,n51,x52,n51);
end
[xe,xo,m] = evenodd(x5,n51);
figure(5);
subplot(2,2,1); stem(n51,x5,'ko'); title('Original Sequence')
xlabel('n'); ylabel('x(n)');
subplot(2,2,2); stem(m,xe,'ko'); title('Even Part')
xlabel('n'); ylabel('xe(n)');
subplot(2,2,4); stem(m,xo,'ko'); title('Odd Part')
xlabel('n'); ylabel('xo(n)');


function x = inv_digamma(y,niter)
% INV_DIGAMMA    Inverse of the digamma function.
%
% inv_digamma(y) returns x such that digamma(x) = y.

% a different algorithm is provided by Paul Fackler:
% http://www.american.edu/academic.depts/cas/econ/gaussres/pdf/loggamma.src

% Newton iteration to solve digamma(x)-y = 0
x = exp(y)+1/2;
i = find(y <= -2.22);
x(i) = -1./(y(i) - digamma(1));

% never need more than 5 iterations
if nargin < 2
  niter = 5;
end
for iter = 1:niter
  x = x - (digamma(x)-y)./trigamma(x);
end
return

% test
y = -3:0.01:0.1;
x = digamma(inv_digamma(y));
max(abs(x-y))
max(abs(x-y)./inv_digamma(y))


function [rVectECI, vVectECI] = getInertialVectFromFixedFrameVect(ut, rVectECEF, bodyInfo, vVectECEF)
%getInertialVectFromFixedFrameVect Summary of this function goes here
%   Detailed explanation goes here
    numElems = length(ut);

    spinAngle = getBodySpinAngle(bodyInfo, ut);

    cSA = reshape(cos(spinAngle),1,1,numElems);
    sSA = reshape(sin(spinAngle),1,1,numElems);
    zero = zeros(1,1,numElems);
    one = zero + 1;

%     R = [cos(spinAngle) -sin(spinAngle) 0;
%          sin(spinAngle) cos(spinAngle) 0;
%          0 0 1];

    R = [cSA, -sSA, zero;
         sSA, cSA, zero;
         zero, zero, one];

    rVectECEF = reshape(rVectECEF,3,1,numElems);
%     rVectECI = R * rVectECEF;
    rVectECI = mtimesx(R,rVectECEF);
    rVectECI = reshape(rVectECI,3,numElems);

    rotRateRadSec = 2*pi/bodyInfo.rotperiod;
    omegaRI = repmat([0;0;rotRateRadSec],1,1,numElems);
    vVectECEF = reshape(vVectECEF,3,1,numElems);
%     vVectECI = R*(vVectECEF + cross(omegaRI, rVectECEF));
    vVectECI = mtimesx(R,(vVectECEF + cross(omegaRI, rVectECEF)));
    vVectECI = reshape(vVectECI,3,numElems);
end


function [LIFT] = Lift2D()

% function [LIFT] = Lift2D()
% Purpose  : Compute surface to volume lift term for DG formulation

Globals2D;
Emat = zeros(Np, Nfaces*Nfp);

% face 1
faceR = r(Fmask(:,1));
V1D = Vandermonde1D(N, faceR);
massEdge1 = inv(V1D*V1D');
Emat(Fmask(:,1),1:Nfp) = massEdge1;

% face 2
faceR = r(Fmask(:,2));
V1D = Vandermonde1D(N, faceR);
massEdge2 = inv(V1D*V1D');
Emat(Fmask(:,2),Nfp+1:2*Nfp) = massEdge2;

% face 3
faceS = s(Fmask(:,3));
V1D = Vandermonde1D(N, faceS);
massEdge3 = inv(V1D*V1D');
Emat(Fmask(:,3),2*Nfp+1:3*Nfp) = massEdge3;

% inv(mass matrix)*\I_n (L_i,L_j)_{edge_n}
LIFT = V*(V'*Emat);
return


function [ num_int, pint ] = halfspace_imp_triangle_int_3d ( a, b, c, d, t )

%*****************************************************************************80
%
%% HALFSPACE_IMP_TRIANGLE_INT_3D: intersection ( implicit halfspace, triangle ) in 3D.
%
%  Discussion:
%
%    The implicit form of a half-space in 3D may be described as the set
%    of points (X,Y,Z) on or "above" an implicit plane:
%
%      0 <= A * X + B * Y + C * Z + D
%
%    The triangle is specified by listing its three vertices.
%
%    The intersection may be described by the number of vertices of the
%    triangle that are included in the halfspace, and by the location of
%    points between vertices that separate a side of the triangle into
%    an included part and an unincluded part.
%
%    0 vertices, 0 separators    (no intersection)
%    1 vertex, 0 separators      (point intersection)
%    2 vertices, 0 separators    (line intersection)
%    3 vertices, 0 separators    (triangle intersection)
%
%    1 vertex, 2 separators,     (intersection is a triangle)
%    2 vertices, 2 separators,   (intersection is a quadrilateral).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, D, the parameters that define the
%    implicit plane, which in turn define the implicit halfspace.
%
%    Input, real T(3,3), the vertices of the triangle.
%
%    Output, integer NUM_INT, the number of intersection points returned,
%    which will always be between 0 and 4.
%
%    Output, real PINT(3,4), the coordinates of the NUM_INT
%    intersection points.  The points will lie in sequence on the triangle.
%    Some points will be vertices, and some may be separators.
%

%
%  Compute the signed distances between the vertices and the plane.
%
  dist1 = a * t(1,1) + b * t(2,1) + c * t(3,1) + d;
  dist2 = a * t(1,2) + b * t(2,2) + c * t(3,2) + d;
  dist3 = a * t(1,3) + b * t(2,2) + c * t(3,3) + d;
%
%  Now we can find the intersections.
%
  [ num_int, pint ] = halfspace_triangle_int_3d ( t, dist1, dist2, dist3 );

  return
end


% ======================================================== %
% Files of the Matlab programs included in the book:       %
% Xin-She Yang, Nature-Inspired Metaheuristic Algorithms,  %
% Second Edition, Luniver Press, (2010).   www.luniver.com %
% ======================================================== %

% -------------------------------------------------------- %
% Firefly Algorithm for constrained optimization using     %
% for the design of a spring (benchmark)                   %
% by Xin-She Yang (Cambridge University) Copyright @2009   %
% -------------------------------------------------------- %

function fa_ndim
% parameters [n N_iteration alpha betamin gamma]
para=[20 500 0.5 0.2 1];

help fa_ndim.m

% Simple bounds/limits for d-dimensional problems
d=15;
Lb=zeros(1,d);
Ub=2*ones(1,d);

% Initial random guess
u0=Lb+(Ub-Lb).*rand(1,d);

[u,fval,NumEval]=ffa_mincon(@cost,u0,Lb,Ub,para);

% Display results
bestsolution=u
bestojb=fval
total_number_of_function_evaluations=NumEval

%%% Put your own cost/objective function here --------%%%
%% Cost or Objective function
 function z=cost(x)
% Exact solutions should be (1,1,...,1)
z=sum((x-1).^2);

%%% End of the part to be modified -------------------%%%

%%% --------------------------------------------------%%%
%%% Do not modify the following codes unless you want %%%
%%% to improve its performance etc                    %%%
% -------------------------------------------------------
% ===Start of the Firefly Algorithm Implementation ======
%         Lb = lower bounds/limits
%         Ub = upper bounds/limits
%   para == optional (to control the Firefly algorithm)
% Outputs: nbest   = the best solution found so far
%          fbest   = the best objective value
%      NumEval = number of evaluations: n*MaxGeneration
% Optional:
% The alpha can be reduced (as to reduce the randomness)
% ---------------------------------------------------------

% Start FA
function [nbest,fbest,NumEval]...
           =ffa_mincon(fhandle,u0, Lb, Ub, para)
% Check input parameters (otherwise set as default values)
if nargin<5, para=[20 500 0.25 0.20 1]; end
if nargin<4, Ub=[]; end
if nargin<3, Lb=[]; end
if nargin<2,
disp('Usuage: FA_mincon(@cost,u0,Lb,Ub,para)');
end

% n=number of fireflies
% MaxGeneration=number of pseudo time steps
% ------------------------------------------------
% alpha=0.25;      % Randomness 0--1 (highly random)
% betamn=0.20;     % minimum value of beta
% gamma=1;         % Absorption coefficient
% ------------------------------------------------
n=para(1);  MaxGeneration=para(2);
alpha=para(3); betamin=para(4); gamma=para(5);

% Total number of function evaluations
NumEval=n*MaxGeneration;

% Check if the upper bound & lower bound are the same size
if length(Lb) ~=length(Ub),
    disp('Simple bounds/limits are improper!');
    return
end

% Calcualte dimension
d=length(u0);

% Initial values of an array
zn=ones(n,1)*10^100;
% ------------------------------------------------
% generating the initial locations of n fireflies
[ns,Lightn]=init_ffa(n,d,Lb,Ub,u0);

% Iterations or pseudo time marching
for k=1:MaxGeneration,     %%%%% start iterations

% This line of reducing alpha is optional
 alpha=alpha_new(alpha,MaxGeneration);

% Evaluate new solutions (for all n fireflies)
for i=1:n,
   zn(i)=fhandle(ns(i,:));
   Lightn(i)=zn(i);
end

% Ranking fireflies by their light intensity/objectives
[Lightn,Index]=sort(zn);
ns_tmp=ns;
for i=1:n,
 ns(i,:)=ns_tmp(Index(i),:);
end

%% Find the current best
nso=ns; Lighto=Lightn;
nbest=ns(1,:); Lightbest=Lightn(1);

% For output only
fbest=Lightbest;

% Move all fireflies to the better locations
[ns]=ffa_move(n,d,ns,Lightn,nso,Lighto,nbest,...
      Lightbest,alpha,betamin,gamma,Lb,Ub);

end   %%%%% end of iterations

% -------------------------------------------------------
% ----- All the subfunctions are listed here ------------
% The initial locations of n fireflies
function [ns,Lightn]=init_ffa(n,d,Lb,Ub,u0)
  % if there are bounds/limits,
if length(Lb)>0,
   for i=1:n,
   ns(i,:)=Lb+(Ub-Lb).*rand(1,d);
   end
else
   % generate solutions around the random guess
   for i=1:n,
   ns(i,:)=u0+randn(1,d);
   end
end

% initial value before function evaluations
Lightn=ones(n,1)*10^100;

% Move all fireflies toward brighter ones
function [ns]=ffa_move(n,d,ns,Lightn,nso,Lighto,...
             nbest,Lightbest,alpha,betamin,gamma,Lb,Ub)
% Scaling of the system
scale=abs(Ub-Lb);

% Updating fireflies
for i=1:n,
% The attractiveness parameter beta=exp(-gamma*r)
   for j=1:n,
      r=sqrt(sum((ns(i,:)-ns(j,:)).^2));
      % Update moves
if Lightn(i)>Lighto(j), % Brighter and more attractive
   beta0=1; beta=(beta0-betamin)*exp(-gamma*r.^2)+betamin;
   tmpf=alpha.*(rand(1,d)-0.5).*scale;
   ns(i,:)=ns(i,:).*(1-beta)+nso(j,:).*beta+tmpf;
      end
   end % end for j

end % end for i

% Check if the updated solutions/locations are within limits
[ns]=findlimits(n,ns,Lb,Ub);

% This function is optional, as it is not in the original FA
% The idea to reduce randomness is to increase the convergence,
% however, if you reduce randomness too quickly, then premature
% convergence can occur. So use with care.
function alpha=alpha_new(alpha,NGen)
% alpha_n=alpha_0(1-delta)^NGen=10^(-4);
% alpha_0=0.9
delta=1-(10^(-4)/0.9)^(1/NGen);
alpha=(1-delta)*alpha;

% Make sure the fireflies are within the bounds/limits
function [ns]=findlimits(n,ns,Lb,Ub)
for i=1:n,
     % Apply the lower bound
  ns_tmp=ns(i,:);
  I=ns_tmp<Lb;
  ns_tmp(I)=Lb(I);

  % Apply the upper bounds
  J=ns_tmp>Ub;
  ns_tmp(J)=Ub(J);
  % Update this new move
  ns(i,:)=ns_tmp;
end

%% ==== End of Firefly Algorithm implementation ======


% Fig. 8.19   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%

clear all;
close all;

numGs=1;
denGs=[1 0 0];  % s^2

numDs=.81*[1 .2];
denDs=[1 2];

numC=conv(numGs,numDs);
denC=conv(denGs,denDs);

[numCL,denCL]=feedback(numC,denC,1,1);
damp(denCL)

tf=30;
t=0:.2:tf;
y=step(numCL,denCL,t);

axis([0 30 0 1.5])
plot(t,y),grid
hold on

% discrete designs

T=1;
[numGz,denGz]=c2dm(numGs,denGs,T,'zoh');

numDz1=[.389 -.319];
denDz1=[1 -.135];

numDz2=.374*[1 -.85];
denDz2=[1 0];

numz1=conv(numGz,numDz1);
denz1=conv(denGz,denDz1);

numz2=conv(numGz,numDz2);
denz2=conv(denGz,denDz2);

[numCLz1,denCLz1]=feedback(numz1,denz1,1,1);
[numCLz2,denCLz2]=feedback(numz2,denz2,1,1);

ddamp(denCLz1,T)
ddamp(denCLz2,T)

N=tf/T+1;
td=0:1:tf;
yd1=dstep(numCLz1,denCLz1,N);
yd2=dstep(numCLz2,denCLz2,N);
plot(td,yd1,'-',td,yd1,'*')
plot(td,yd2,'-',td,yd2,'o')
text(10,.55,'----------- continuous design')
text(10,.75,'*----*----* discrete equivalent design')
text(10,.65,'o----o----o discrete design')
title('Fig. 8.19  Step responses of the various design methods')
xlabel('Time (sec)')
ylabel('Plant output')
hold off


%% OCR of hand-written digits using HoG and SVM
%
% In this tutorial, we will build an SVM classifer to recognize
% hand-written digits (0 to 9), using Histogram of Oriented Gradients (HOG)
% as feature vectors.
%
% For additional resources, see this
% <https://www.youtube.com/watch?v=0Zib1YEE4LU HoG video lecture>.
%
% Sources:
%
% * <https://docs.opencv.org/3.2.0/dd/d3b/tutorial_py_svm_opencv.html>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/digits.py>
% * <https://www.mathworks.com/help/vision/examples/digit-classification-using-hog-features.html>
%

function svm_hog_ocr_digits_demo()
    %% Data
    % Load MNIST handwritten digits (5000 images)
    % (params in this demo are hardcoded assuming 20x20 images)
    [imgs, labels, img] = load_mnist_digits();
    assert(numel(imgs)==5000 && isequal(size(imgs{1}),[20 20]));

    %%
    % Show a portion of the big image
    imshow(img(1:350,1:450))
    %imtool(img)

    %%
    % Show a sample of an image before and after deskew is applied
    figure
    subplot(121), imshow(imgs{1}), title('original')
    subplot(122), imshow(deskew(imgs{1})), title('deskewed')

    %% HoG
    % choose an HoG implementation to use: OpenCV, MATLAB, or a simple custom one
    HOG_ALG = 'opencv';
    HOG_ALG = validatestring(HOG_ALG, {'custom', 'matlab', 'opencv'});

    %%
    % HoG features are stored in a matrix, one row per image
    switch HOG_ALG
        case 'custom'
            nbins = 16;          % number of bins to quantize gradient angles
            nfeats = 4 * nbins;  % feature length
        case 'matlab'
            opts = {'CellSize',[10 10], 'BlockSize',[2 2], ...
                'NumBins',9, 'UseSignedOrientation',true};
            nfeats = numel(extractHOGFeatures(imgs{1}, opts{:}));
        case 'opencv'
            % create HOG object
            opts = {'WinSize',[20 20], 'BlockSize',[10 10], ...
                'BlockStride',[5 5], 'CellSize',[10 10], ...
                'NBins',9, 'SignedGradient',true};
            %opts = [opts, 'GammaCorrection',false];
            hog = cv.HOGDescriptor(opts{:});
            nfeats = numel(hog.compute(imgs{1}));
            display(hog)
    end
    descs = zeros(numel(imgs), nfeats, 'single');

    %%
    % Process images and compute HOG descriptors for each
    tic
    disp('Extracting...')
    for i=1:numel(imgs)
        % deskew image
        img = deskew(imgs{i});
        %img = cv.threshold(img, 'Otsu');

        % compute HOG descriptors flattened as a vector
        switch HOG_ALG
            case 'custom'
                descs(i,:) = my_hog(img, nbins);
            case 'matlab'
                descs(i,:) = extractHOGFeatures(img, opts{:});
            case 'opencv'
                d = hog.compute(img).'; % returns a matrix, one row per window
                descs(i,:) = d(:);
        end
    end
    toc

    %% Classification
    % Create multi-class linear SVM classifier
    svm = cv.SVM();
    svm.Type = 'C_SVC';
    svm.KernelType = 'Linear';
    svm.C = 0.5;  % or do a trainAuto grid search to find optimal C
    display(svm)

    %%
    % split data samples and corresponding labels
    labels = int32(labels(:));
    N = 2500;  % fix(numel(labels)/2)
    whos descs labels
    if ~mexopencv.isOctave() && mexopencv.require('stats')
        % train/test split
        tabulate(labels(1:N))
        tabulate(labels(N+1:end))
    end

    %%
    % use half as training data
    tic
    disp('Training...')
    svm.train(descs(1:N,:), labels(1:N));
    toc

    %%
    % the other half for testing
    tic
    disp('Testing...')
    yhat = svm.predict(descs(N+1:end,:));
    toc
    %TODO: https://savannah.gnu.org/bugs/?50716

    %%
    % performance on test set
    acc = nnz(yhat == labels(N+1:end));
    confmat = accumarray([labels(N+1:end), yhat]+1, 1);
    display(confmat)
    fprintf('Accuracy = %f%%\n', (acc/N)*100);

    %%
    % Show some of the wrong predictions
    idx = (yhat ~= labels(N+1:end));
    fprintf('Number of wrong predictions = %d / %d\n', nnz(idx), N);
    figure
    idx = find(idx, 3*3, 'first');
    for i=1:numel(idx)
        ii = idx(i);
        subplot(3,3,i), imshow(imgs{ii+N})
        title(sprintf('%d \\rightarrow %d', labels(ii+N), yhat(ii)))
    end
end

%% Helper functions

function [imgs, labels, img] = load_mnist_digits()
    % Load MNIST handwritten digits, one big image
    fname = fullfile(mexopencv.root(), 'test', 'digits.png');
    if exist(fname, 'file') ~= 2
        disp('Downloading image...')
        url = 'https://cdn.rawgit.com/opencv/opencv/3.2.0/samples/data/digits.png';
        urlwrite(url, fname);
    end
    img = cv.imread(fname, 'Grayscale',true);

    % split it into 5000 small images (500 from each 0:9 digits),
    % each image is 20x20 pixels
    [h,w] = size(img);
    imgs = mat2cell(img, ones(1,50)*h/50, ones(1,100)*w/100);
    imgs = reshape(imgs', 500, [])';  % cell array of size 10x500
    labels = repmat((0:9)', 1, 500);  % each row is one digit
end

function img = deskew(img)
    % deskew image using its second order moments
    sz = 20;
    m = cv.moments(img);
    if abs(m.mu02) > 0.2  % 0.01
        skew = m.mu11 / m.mu02;
        M = [1 skew -0.5*sz*skew; 0 1 0];
        img = cv.warpAffine(img, M, 'DSize',[sz sz], 'WarpInverse',true);
    end
end

function histo = my_hog(img, nbins)
    % this is a simplified implementation of HoG descriptors
    % (fixed 10x10 cells, 1 block, no overlapping, no block normalization)

    % number of orientation histogram bins
    if nargin < 2, nbins = 16; end

    % compute image gradient (its magnitude and direction)
    gx = cv.Sobel(img, 'DDepth','single', 'XOrder',1, 'YOrder',0);
    gy = cv.Sobel(img, 'DDepth','single', 'XOrder',0, 'YOrder',1);
    mag = hypot(gx, gy);
    ang = atan2(gy, gx) + pi;  % directions in [0,2*pi] (signed gradient)

    % quantize directions into 16 bins
    bins = fix((nbins-1) * ang/(2*pi)) + 1; % bin indices in [1,16] inclusive

    % divide 20x20 image into 4 10x10 cells, and in each,
    % accumulate gradient magnitudes inside each direction bin
    % (binned orientations each weighted by its magnitude contribution)
    bb = mat2cell(bins, [10 10], [10 10]);
    mm = mat2cell(mag, [10 10], [10 10]);
    histo = cell(size(bb));
    for i=1:numel(bb)
        H = accumarray(bb{i}(:), mm{i}(:), [nbins 1]);
        if false && any(H)
            % transform histogram to space with Hellinger metric
            H = sqrt(H ./ sum(H));
            H = H ./ norm(H);
        end
        histo{i} = H;
    end

    % stack features from cells into one tall vector of length 4*16
    histo = cat(1, histo{:});
end


function a = laguerre ( n )

%*****************************************************************************80
%
%% LAGUERRE returns the LAGUERRE matrix.
%
%  Example:
%
%    N = 8
%
%      1      .     .      .    .    .    .    .
%      1     -1     .      .    .    .    .    .
%      2     -4     1      .    .    .    .    .   /    2
%      6    -18     9     -1    .    .    .    .   /    6
%     24    -96    72    -16    1    .    .    .   /   24
%    120   -600   600   -200   25   -1    .    .   /  120
%    720  -4320  5400  -2400  450  -36    1    .   /  720
%   5040 -35280 52920 -29400 7350 -882   49   -1   / 5040
%
%  Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    A is lower triangular.
%
%    The columns of A alternate in sign.
%
%    A(I,1) = 1
%    A(I,I) = (-1)^(I-1) / (I-1)!.
%
%    LAMBDA(I) = (-1)^(I-1) / (I-1)!.
%
%    det ( A ) = product ( 1 <= I <= N ) (-1)^(I-1) / (I-1)!
%
%    The I-th row contains the coefficients of the Laguerre
%    polynomial of degree I-1.
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  if ( n <= 0 )
    a = [];
    return
  end

  a(1,1) = 1.0;

  if ( n == 1 )
    return
  end

  a(2,1) = 1.0;
  a(2,2) = -1.0;

  if ( n == 2 )
    return
  end

  for i = 3 : n
    for j = 1 : n
      if ( j == 1 )
        a(i,j) = (  ( 2 * i - 3 ) * a(i-1,j) ...
                 +  (   - i + 2 ) * a(i-2,j) ) ...
                 /  (     i - 1 );
      else
        a(i,j) = (  ( 2 * i - 3 ) * a(i-1,j) ...
                 -  (         1 ) * a(i-1,j-1) ...
                 +  (   - i + 2 ) * a(i-2,j) ) ...
                 /  (     i - 1 );
      end
    end
  end

  return
end


% function thetaOpt = StochasticGradientDescent (gradFunc, theta0, maxiter)
% runs gradient descent until convergence, returning the optimal parameters thetaOpt.
%
% Inputs:
% gradFunc      function handle to a function [cost, grad] = gradFunc(theta, i)
%               that computes the LR cost / objective function and the gradient
%               of the negative log likelihood of the ith data instance, given
%               parameters theta.
% theta0        initial value of theta to start gradient descent from.
% maxIter       number of iterations to run SGD for.
%
% Output:
% thetaOpt      optimal value of theta.
%
%
% Note - function handles may be new to some of you. Briefly, function handles
% are a way of passing a function to another function as an argument. The
% syntax for calling a function handle is exactly the same as for calling any
% other function.
%
% For more information, refer to the official documentation:
% Matlab - http://www.mathworks.com/help/techdoc/matlab_prog/f2-38133.html
% Octave - http://www.gnu.org/software/octave/doc/interpreter/Function-Handles.html
%
% Copyright (C) Daphne Koller, Stanford Univerity, 2012

function thetaOpt = StochasticGradientDescent (gradFunc, theta0, maxIter)

    % The grader will accept all answers that are near enough
    % to the optimal value, so don't worry about being off by one
    % iteration etc.

    thetaOpt = zeros(size(theta0));

    %%%%%%%%%%%%%%
    %%% Student code
    for i = 1:maxIter
	    [cost, grad] = gradFunc(theta0,i);
	    theta0 = theta0 - (0.1)*(grad)/(1+sqrt(i));
    end
    thetaOpt = theta0;
    %%%%%%%%%%%

end


function z = mutInfo(x, y)
% Compute mutual information I(x,y) of two discrete variables x and y.
% Input:
%   x, y: two integer vector of the same length
% Output:
%   z: mutual information z=I(x,y)
% Written by Mo Chen (sth4nth@gmail.com).
assert(numel(x) == numel(y));
n = numel(x);
x = reshape(x,1,n);
y = reshape(y,1,n);

l = min(min(x),min(y));
x = x-l+1;
y = y-l+1;
k = max(max(x),max(y));

idx = 1:n;
Mx = sparse(idx,x,1,n,k,n);
My = sparse(idx,y,1,n,k,n);
Pxy = nonzeros(Mx'*My/n); %joint distribution of x and y
Hxy = -dot(Pxy,log2(Pxy));

Px = nonzeros(mean(Mx,1));
Py = nonzeros(mean(My,1));

% entropy of Py and Px
Hx = -dot(Px,log2(Px));
Hy = -dot(Py,log2(Py));
% mutual information
z = Hx+Hy-Hxy;
z = max(0,z);


clear; clc;

%% DFT filter bank
N = 8;          % number of subbands
K = 16;         % overlapping factor
L = 2*K*N;      % length of analysis filters

hopt = fir1(L-1,1/N);       % prototype filter
[H,F] = make_bank_DFT(hopt,N);
H = sqrt(N)*H;                    % analysis section
F = sqrt(N)*F;                    % synthesis section

%% signals in the DFT filter bank
xn = sin(2*pi*0.01*(0:1000));
ITER = length(xn);
yn = zeros(ITER,1);
x = zeros(length(H),1);
b = zeros(length(F),1);

%% analysis and synthesis filter banks implementation
for n = 1:ITER
    x = [xn(n); x(1:end-1)];
    % block transformation is performed once for every N input samples
    if mod(n,N) == 0
        x_D = H.'*x;
        % =====================
        % insert subband processing here
        % =====================
        b = F*x_D + b;
    end
    yn(n) = real(b(1));
    b = [b(2:end); 0];
end

%% plot input and output signal
figure;
plot(0:length(xn)-1, xn); hold on;
plot(0:length(yn)-1, yn, 'r:');
xlabel('Time index, n'); ylabel('Amplitude');

%{
 %% plot frequency response
DFTpoint = 4096;
[Hz,w] = FreqResp(H, DFTpoint);
Hz = Hz.';
figure;
subplot(3,1,[1 2]); hold on;
for k = 1:N
    if mod(k,2) == 0
        plot(w/pi,20*log10(abs(Hz(k,:))));
    else
        plot(w/pi,20*log10(abs(Hz(k,:))),':');
    end
end
title('frequency response');
%}

github-repos/MATLAB/CharlesThaCat-acoustic-interference-cancellation/acoustic-interference-cancellation-edb394499ea6f9c96445a3e9613bd64a854c289e/Subband processing/Book/After reading Chapter 2 -- Design Analysis & Synthesis Filter bank/DFTFilterBank.m


%%                   runCoordinateDescent.m

% This test file implements the Coordinate Descent solver. The code builds
% a synthetic formulation of the Phase Retrieval problem, b = |Ax| and
% computes an estimate to x. The code finally plots a convergence curve
% and also makes a scatter plot of the true vs recovered solution.

% PAPER TITLE:
%              Coordinate Descent Algorithms for Phase Retrieval

% ARXIV LINK:
%              https://arxiv.org/abs/1706.03474

% 1) Each test script starts out by defining the length of the unknown
% signal, n and the number of measurements, m. These mesurements can be
% made complex by setting the isComplex flag to be true.
%
% 2) We then build the test problem by invoking the function
% 'buildTestProblem' which generates random gaussian measurements according
% to the user's choices in step(1). The function returns the measurement
% matrix 'A', the true signal 'xt' and the measurements 'b0'.
%
% 3) We set the options for the PR solver. For example, the maximum
% number of iterations, the tolerance value, the algorithm and initializer
% of choice. These options are controlled by setting the corresponding
% entries in the 'opts' struct.  Please see the user guide for a complete
% list of options.
%
% 4) We solve the phase retrieval problem by running the following line
% of code:
%   >>  [x, outs, opts] = solvePhaseRetrieval(A, A', b0, n, opts)
% This solves the problem using the algorithm and initialization scheme
% specified by the user in the struct 'opts'.
%
% 5) Determine the optimal phase rotation so that the recovered solution
% matches the true solution as well as possible.
%
% 6) Report the relative reconstruction error. Plot residuals (a measure
% of error) against the number of iterations and plot the real part of the
% recovered signal against the real part of the original signal.

% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017
%% -----------------------------START----------------------------------

clc
clear
close all

n = 100;          % Dimension of unknown vector
m = 8 * n;        % Number of measurements
isComplex = true; % If the signal and measurements are complex

%%  Build a random test problem
fprintf('Building test problem...\n');
[A, xt, b0] = buildTestProblem(m, n, isComplex);

% Options
opts = struct;
opts.initMethod = 'Truncatedspectral';
% opts.initMethod = 'optimalspectral';
opts.algorithm = 'CoordinateDescent';
opts.isComplex = isComplex;
opts.maxIters = 500000;
opts.tol = 1e-6;
opts.verbose = 2;

%% Try to recover x
fprintf('Running algorithm...\n');
[x, outs, opts] = solvePhaseRetrieval(A, A', b0, n, opts);

%% Determine the optimal phase rotation so that the recovered solution
%  matches the true solution as well as possible.
alpha = (x'*xt)/(x'*x);
x = alpha * x;

%% Determine the relative reconstruction error.  If the true signal was
%  recovered, the error should be very small - on the order of the numerical
%  accuracy of the solver.
reconError = norm(xt-x)/norm(xt);
fprintf('relative recon error = %d\n', reconError);

% Plot a graph of error(definition depends on if opts.xt is provided) versus
% the number of iterations.
plotErrorConvergence(outs, opts)

% Plot a graph of the recovered signal x against the true signal xt.
plotRecoveredVSOriginal(x,xt);


function pass = test_intops(pref)
% Test integral operators
% Toby Driscoll 28 May 2009
% Nick Hale 6 Jan 2012

% NOTE: Taken from V4 test chebop_intops.

if ( nargin == 0 )
    pref = cheboppref();
end
tol = 1e-10;

% From http://en.wikipedia.org/wiki/Integro-differential_equation
d = [0 5];
x = chebfun('x', d);
N = chebop(@(u) diff(u) + 2*u + 5*cumsum(u), d);
N.bc = @(x,u) u(0);
u = mldivide(N, 1, pref);
u_exact = 0.5*exp(-x).*sin(2*x);
err(1) = norm(u - u_exact);

% Fredholm
d = [0, 1];
x = chebfun(@(x) x, d);
K = @(x,y) sin(2*pi*(x-y));
A = chebop(@(x,u) u + fred(K,u), d);
u = x.*exp(x);
f = A*u;
err(2) = norm(u - mldivide(A, f, pref));

% Volterra
d = [0, pi];
x = chebfun(@(x) x, d);
K = @(x,y) x.*y;
A = chebop(@(x,u) u - volt(K,u), d);
f = x.^2.*cos(x) + (1-x).*sin(x);
u = mldivide(A, f, pref);
err(3) = norm( u - sin(x) );
err(4) = norm( A*u - f );

pass = err < tol;

%%
% From #2122
K = @(x,t) exp(t.*(x-t));
N = chebop(@(x,y) diff(y) + y - x.*(1+2*x).*volt(K, y) - 1 - 2*x);
N.lbc = 1;
y = N\0;
pass(5) = norm(N.op(y)) + abs(y(0)-1);

%%
% From #2179
K = @(x,y) exp(-(x-y));
A = chebop(@(x,u) diff(u) + fred(K,u));
A.lbc = 0;
u = A\1;
pass(6) = norm(A*u - 1, inf) < tol;

%%
% From https://groups.google.com/forum/#!topic/chebfun-users/0dpsggp1RyA .
% Also, see #2210.

L = chebop(@(u) diff(u)+sum(u), [0 1]);
L.lbc = 1;
u = L\0;
pass(7) = norm(u - (1- 2*chebfun('x',[0 1])/3), inf) < tol;

%%
% From #2185
A = chebop(@(x,u) diff(u,2) + sin(2*pi*x)*u + sum(u).*u);
A.bc = 'periodic';
u = A\1;
pass(8) = norm(A*u-1) < tol;

f = chebfun(@(x) sin(5*pi*x));
K = @(x,y) cos(-pi*(x-y));
A = chebop(@(x,u) 1/25*diff(u,2) + fred(K, u));
A.bc = 'periodic';
u = A\f;
pass(9) = norm(A*u-f) < tol;

%%
% From #2360.
K = @(x,y) 1+0*x;
N = chebop(@(x,u) - u + exp(x) + 1/3*x*(1-exp(3*x)) + x*volt(K, u^3), [0, 1]);
u = N\0;
pass(10) = norm(u-chebfun(@exp, [0 1])) < tol;

end


function K = covMaterniso(d, hyp, x, z, i)

% Matern covariance function with nu = d/2 and isotropic distance measure. For
% d=1 the function is also known as the exponential covariance function or the
% Ornstein-Uhlenbeck covariance in 1d. The covariance function is:
%
%   k(x^p,x^q) = sf^2 * f( sqrt(d)*r ) * exp(-sqrt(d)*r)
%
% with f(t)=1 for d=1, f(t)=1+t for d=3 and f(t)=1+t+t²/3 for d=5.
% Here r is the distance sqrt((x^p-x^q)'*inv(P)*(x^p-x^q)), P is ell times
% the unit matrix and sf2 is the signal variance. The hyperparameters are:
%
% hyp = [ log(ell)
%         log(sf) ]
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2010-09-10.
%
% See also COVFUNCTIONS.M.

if nargin<3, K = '2'; return; end                  % report number of parameters
if nargin<4, z = []; end                                   % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

ell = exp(hyp(1));
sf2 = exp(2*hyp(2));
if all(d~=[1,3,5]), error('only 1, 3 and 5 allowed for d'), end         % degree

switch d
  case 1, f = @(t) 1;               df = @(t) 1;            % df(t) = f(t)-f'(t)
  case 3, f = @(t) 1 + t;           df = @(t) t;
  case 5, f = @(t) 1 + t.*(1+t/3);  df = @(t) t.*(1+t)/3;
end
          m = @(t,f) f(t).*exp(-t); dm = @(t,f) df(t).*exp(-t).*t;

% precompute distances
if dg                                                               % vector kxx
  K = zeros(size(x,1),1);
else
  if xeqz                                                 % symmetric matrix Kxx
    K = sq_dist(sqrt(d)/ell*x');
  else                                                   % cross covariances Kxz
    K = sq_dist(sqrt(d)/ell*x',sqrt(d)/ell*z');
  end
end

if nargin<5                                                        % covariances
  K = sf2*m(sqrt(K),f);
else                                                               % derivatives
  if i==1
    K = sf2*dm(sqrt(K),f);
  elseif i==2
    K = 2*sf2*m(sqrt(K),f);
  else
    error('Unknown hyperparameter')
  end
end


function p = mvnormpdfln(x, m, S, V)
% MVNORMPDFLN    log of multivariate normal density.
%   See MVNORMPDF for argument description.

log2pi = 1.83787706640935;
[d, n] = size(x);
if nargin == 1
  dx = x;
elseif isempty(m)
  dx = x;
else
  % m specified
  sz = size(m);
  if sz(1) ~= d
    error('rows(m) ~= rows(x)')
  end
  nm = sz(2);
  if nm == 1
    dx = x - repmat(m,1,n);
  elseif n == 1
    dx = repmat(x,1,nm) - m;
  elseif nm == n
    dx = x - m;
  else
    error('incompatible number of columns in x and m')
  end
end
if nargin < 3
  % unit variance
  p = -0.5*(d*log2pi + col_sum(dx.*dx));
  return
end
have_inv = 0;
if nargin == 3
  % standard deviation given
  if d == 1
    dx = dx./S;
    p = (-log(S) -0.5*log2pi) - 0.5*(dx.*dx);
    return;
  end
  if S(2,1) ~= 0
    error('S is not upper triangular')
  end
  if any(size(S) ~= [d d])
    error('S is not the right size')
  end
else
  if ischar(V)
    if strcmp(V,'inv')
      % inverse stddev given
      iS = S;
      have_inv = 1;
    else
      error('unknown directive')
    end
  elseif ischar(S)
    if strcmp(S,'inv')
      % inverse variance given
      if d == 1
	iS = sqrt(V);
      else
	iS = chol(V);
      end
      have_inv = 1;
    else
      error('unknown directive')
    end
  else
    % variance given
    if d == 1
      S = sqrt(V);
    else
      S = chol(V);
    end
  end
end
if have_inv
  if d == 1
    dx = iS .* dx;
    logdetiS = log(iS);
  else
    dx = iS*dx;
    logdetiS = sum(log(diag(iS)));
  end
else
  if d == 1
    dx = dx./S;
    logdetiS = -log(S);
  else
    dx = solve_tril(S',dx);
    %dx = S'\dx;
    logdetiS = -sum(log(diag(S)));
  end
end
p = (logdetiS -0.5*d*log2pi) -0.5*col_sum(dx.*dx);


% Local Regression and Likelihood, Figure 6.1.
%
% Derivative (local slope) estimation, for the Old Faithful Geyser Data.
% The 'deriv' argument specifies derivative estimation,
%   'deriv',1        First-order derivative.
%   'deriv',[1 1]    Second-order derivative.
%   'deriv',2        For bivariate fits, partial deriv. wrt second variable.
%   'deriv',[1 2]    Mixed second-order derivative.
%
% Density estimation is done on the log-scale. That is, the estimate
% is of g(x) = log(f(x)), where f(x) is the density.
%
% The relation between derivatives is therefore
%     f'(x) = f(x)g'(x) = g'(x)exp(g(x)).
% To estimate f'(x), we must estimate g(x) and g'(x) (fit1 and fit2 below),
% evaluate on a grid of points (p1 and p2), and apply the back-transformation.
%
% Disclaimer: I don't consider derivative estimation from noisy data
% to be a well-defined problem. Use at your own risk.
%
% Author: Catherine Loader
%
% NEED: m argument passed to lfmarg().

load geyser;
fit1 = locfit(geyser,'alpha',[0.1 0.6],'ll',1,'ur',6);
fit2 = locfit(geyser,'alpha',[0.1 0.6],'ll',1,'ur',6,'deriv',1);
z = lfmarg(fit1);
p1 = predict(fit1,z);
p2 = predict(fit2,z);
figure('Name','fig6_1: slope estimation: Old faithful data' );
plot(z{1},p2.*exp(p1));
xlabel('Eruption Duration (Minutes)');
ylabel('Density Derivative');


function pyramid_exactness ( quad_filename, degree_max )

%*****************************************************************************80
%
%% MAIN is the main program for PYRAMID_EXACTNESS.
%
%  Discussion:
%
%    This program investigates the polynomial exactness of a quadrature
%    rule for the pyramid.
%
%    The integration region is:
%
%      - ( 1 - Z ) <= X <= 1 - Z
%      - ( 1 - Z ) <= Y <= 1 - Z
%                0 <= Z <= 1.
%
%    When Z is zero, the integration region is a square lying in the (X,Y)
%    plane, centered at (0,0,0) with "radius" 1.  As Z increases to 1, the
%    radius of the square diminishes, and when Z reaches 1, the square has
%    contracted to the single point (0,0,1).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 July 2009
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PYRAMID_EXACTNESS\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Investigate the polynomial exactness of\n' );
  fprintf ( 1, '  a quadrature rule for the pyramid.\n' );
%
%  Get the quadrature file root name:
%
  if ( 1 <= nargin )

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'PYRAMID_EXACTNESS:\n' );

    quad_filename = input ( '  Enter the "root" name of the quadrature files.' );

  end
%
%  Create the names of:
%    the quadrature X file;
%    the quadrature W file;
%
  quad_x_filename = strcat ( quad_filename, '_x.txt' );
  quad_w_filename = strcat ( quad_filename, '_w.txt' );
%
%  The second command line argument is the maximum degree.
%
  if ( 2 <= nargin )

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'PYRAMID_EXACTNESS:\n' );

    degree_max = input ( '  Please enter the maximum total degree to check.' );

  end
%
%  Summarize the input.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PYRAMID_EXACTNESS: User input:\n' );
  fprintf ( 1, '  Quadrature rule X file = "%s".\n', quad_x_filename );
  fprintf ( 1, '  Quadrature rule W file = "%s".\n', quad_w_filename );
  fprintf ( 1, '  Maximum total degree to check = %d', degree_max );
%
%  Read the X file.
%
  [ dim_num, order ] = r8mat_header_read ( quad_x_filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension = %d\n', dim_num );
  fprintf ( 1, '  Number of points  = %d\n', order );

  if ( dim_num ~= 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PYRAMID_EXACTNESS - Fatal error!\n' );
    fprintf ( 1, '  The quadrature abscissas must be 3 dimensional.\n' );
    error ( 'PYRAMID_EXACTNESS - Fatal error!' );
  end

  x = r8mat_data_read ( quad_x_filename, dim_num, order );
%
%  Read the W file.
%
  [ dim_num2, order2 ] = r8mat_header_read ( quad_w_filename );

  if ( dim_num2 ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PYRAMID_EXACTNESS - Fatal error!\n' );
    fprintf ( 1, '  The quadrature weight file should have exactly\n');
    fprintf ( 1, '  one value on each line.\n' );
    error ( 'PYRAMID_EXACTNESS - Fatal error!' );
  end

  if ( order2 ~= order )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PYRAMID_EXACTNESS - Fatal error!\n' );
    fprintf ( 1, '  The quadrature weight file should have exactly\n' );
    fprintf ( 1, '  the same number of lines as the abscissa file.\n' );
    error ( 'PYRAMID_EXACTNESS - Fatal error!' );
  end

  w = r8mat_data_read ( quad_w_filename, 1, order );
%
%  Explore the monomials.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '      Error    Degree  Exponents\n' );
  fprintf ( 1, '\n' );

  for degree = 0 : degree_max

    expon = [];
    more = 0;
    h = 0;
    t = 0;

    while ( 1 )

      [ expon, more, h, t ] = comp_next ( degree, dim_num, expon, more, h, t );

      v = monomial_value ( dim_num, order, expon, x );

      quad = pyra_unit_volume ( ) * w(1:order) * v(1:order)';

      exact = pyra_unit_monomial ( expon );

      quad_error = abs ( quad - exact );

      fprintf ( 1, '  %24.16f   %2d  ', quad_error, degree );
      for dim = 1 : dim_num
        fprintf ( 1, '%3d', expon(dim) );
      end
      fprintf ( 1, '\n' );

      if ( ~more )
        break
      end

    end

    fprintf ( 1, '\n' );

  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PYRAMID_EXACTNESS:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function [ a, more, h, t ] = comp_next ( n, k, a, more, h, t )

%*****************************************************************************80
%
%% COMP_NEXT computes the compositions of the integer N into K parts.
%
%  Discussion:
%
%    A composition of the integer N into K parts is an ordered sequence
%    of K nonnegative integers which sum to N.  The compositions (1,2,1)
%    and (1,1,2) are considered to be distinct.
%
%    The routine computes one composition on each call until there are no more.
%    For instance, one composition of 6 into 3 parts is
%    3+2+1, another would be 6+0+0.
%
%    On the first call to this routine, set MORE = FALSE.  The routine
%    will compute the first element in the sequence of compositions, and
%    return it, as well as setting MORE = TRUE.  If more compositions
%    are desired, call again, and again.  Each time, the routine will
%    return with a new composition.
%
%    However, when the LAST composition in the sequence is computed
%    and returned, the routine will reset MORE to FALSE, signaling that
%    the end of the sequence has been reached.
%
%    This routine originally used a SAVE statement to maintain the
%    variables H and T.  I have decided that it is safer
%    to pass these variables as arguments, even though the user should
%    never alter them.  This allows this routine to safely shuffle
%    between several ongoing calculations.
%
%    There are 28 compositions of 6 into three parts.  This routine will
%    produce those compositions in the following order:
%
%     I         A
%     -     ---------
%     1     6   0   0
%     2     5   1   0
%     3     4   2   0
%     4     3   3   0
%     5     2   4   0
%     6     1   5   0
%     7     0   6   0
%     8     5   0   1
%     9     4   1   1
%    10     3   2   1
%    11     2   3   1
%    12     1   4   1
%    13     0   5   1
%    14     4   0   2
%    15     3   1   2
%    16     2   2   2
%    17     1   3   2
%    18     0   4   2
%    19     3   0   3
%    20     2   1   3
%    21     1   2   3
%    22     0   3   3
%    23     2   0   4
%    24     1   1   4
%    25     0   2   4
%    26     1   0   5
%    27     0   1   5
%    28     0   0   6
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2008
%
%  Author:
%
%    FORTRAN77 original version by Albert Nijenhuis and Herbert Wilf
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Albert Nijenhuis, Herbert Wilf,
%    Combinatorial Algorithms for Computers and Calculators,
%    Second Edition,
%    Academic Press, 1978,
%    ISBN: 0-12-519260-6,
%    LC: QA164.N54.
%
%  Parameters:
%
%    Input, integer N, the integer whose compositions are desired.
%
%    Input, integer K, the number of parts in the composition.
%
%    Input, integer A(K), the previous composition.  On the first call,
%    with MORE = FALSE, set A = [].  Thereafter, A should be the
%    value of A output from the previous call.
%
%    Input, logical MORE.  The input value of MORE on the first
%    call should be FALSE, which tells the program to initialize.
%    On subsequent calls, MORE should be TRUE, or simply the
%    output value of MORE from the previous call.
%
%    Input, integer H, T, two internal parameters needed for the
%    computation.  The user may need to initialize these before the
%    very first call, but these initial values are not important.
%    The user should not alter these parameters once the computation
%    begins.
%
%    Output, integer A(K), the next composition.
%
%    Output, logical MORE, will be TRUE unless the composition
%    that is being returned is the final one in the sequence.
%
%    Output, integer H, T, the updated values of the two internal
%    parameters.
%
  if ( ~more )

    t = n;
    h = 0;
    a(1) = n;
    a(2:k) = 0;

  else

    if ( 1 < t )
      h = 0;
    end

    h = h + 1;
    t = a(h);
    a(h) = 0;
    a(1) = t - 1;
    a(h+1) = a(h+1) + 1;

  end

  more = ( a(k) ~= n );

  return
end
function column_num = file_column_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_COLUMN_COUNT counts the columns in the first line of a file.
%
%  Discussion:
%
%    The file is assumed to be a simple text file.
%
%    Most lines of the file are presumed to consist of COLUMN_NUM words,
%    separated by spaces.  There may also be some blank lines, and some
%    comment lines, which have a "#" in column 1.
%
%    The routine tries to find the first non-comment non-blank line and
%    counts the number of words in that line.
%
%    If all lines are blanks or comments, it goes back and tries to analyze
%    a comment line.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the file.
%
%    Output, integer COLUMN_NUM, the number of columns in the file.
%
  FALSE = 0;
  TRUE = 1;
%
%  Open the file.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_COLUMN_COUNT - Error!' );
  end
%
%  Read one line, but skip blank lines and comment lines.
%  Use FGETL so we drop the newline character!
%
  got_one = FALSE;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( s_len_trim ( line ) == 0 )

    elseif ( line(1) == '#' )

    else
      got_one = TRUE;
      break;
    end

  end

  fclose ( input_unit );

  if ( got_one == FALSE )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Warning!\n' );
    fprintf ( 1, '  The file does not seem to contain any data.\n' );
    column_num = -1;
    return;
  end

  column_num = s_word_count ( line );

  return
end
function row_num = file_row_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_ROW_COUNT counts the number of row records in a file.
%
%  Discussion:
%
%    Each input line is a "RECORD".
%
%    The records are divided into three groups:
%
%    * BLANK LINES (nothing but blanks)
%    * COMMENT LINES (begin with a '#')
%    * DATA RECORDS (anything else)
%
%    The value returned by the function is the number of data records.
%
%    By the way, if the MATLAB routine FGETS is used, instead of
%    FGETL, then the variable LINE will include line termination
%    characters, which means that a blank line would not actually
%    have zero characters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the input file.
%
%    Output, integer ROW_NUM, the number of rows found.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_ROW_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_ROW_COUNT - Error!' );
  end

  blank_num = 0;
  comment_num = 0;
  row_num = 0;

  record_num = 0;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    record_num = record_num + 1;
    record_length = s_len_trim ( line );

    if ( record_length <= 0 )
      blank_num = blank_num + 1;
    elseif ( line(1) == '#' )
      comment_num = comment_num + 1;
    else
      row_num = row_num + 1;
    end

  end

  fclose ( input_unit );

  return
end
function value = monomial_value ( dim_num, point_num, expon, x )

%*****************************************************************************80
%
%% MONOMIAL_VALUE evaluates a monomial.
%
%  Discussion:
%
%    This routine evaluates a monomial of the form
%
%      product ( 1 <= dim <= dim_num ) x(dim)^expon(dim)
%
%    where the exponents are nonnegative integers.  Note that
%    if the combination 0^0 is encountered, it should be treated
%    as 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer POINT_NUM, the number of points at which the
%    monomial is to be evaluated.
%
%    Input, integer EXPON(DIM_NUM), the exponents.
%
%    Input, real X(DIM_NUM,POINT_NUM), the point coordinates.
%
%    Output, real VALUE(POINT_NUM), the value of the monomial.
%
  value(1:point_num) = 1.0;

  for dim = 1 : dim_num
    if ( 0 ~= expon(dim) )
      value(1:point_num) = value(1:point_num) .* x(dim,1:point_num).^expon(dim);
    end
  end

  return
end
function value = pyra_unit_monomial ( expon )

%*****************************************************************************80
%
%% PYRA_UNIT_MONOMIAL: monomial integral in a unit pyramid.
%
%  Discussion:
%
%    This routine returns the integral of
%
%      product ( 1 <= I <= 3 ) X(I)^EXPON(I)
%
%    over the unit pyramid.
%
%    The unit pyramid is defined as:
%
%    - ( 1 - Z ) <= X <= 1 - Z
%    - ( 1 - Z ) <= Y <= 1 - Z
%              0 <= Z <= 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971,
%    ISBN: 0130438936,
%    LC: QA311.S85.
%
%  Parameters:
%
%    Input, integer EXPON(3), the exponents.
%
%    Output, real VALUE, the integral of the monomial.
%
  value = 0.0;

  if ( mod ( expon(1), 2 ) == 0 && mod ( expon(2), 2 ) == 0 )

    i_hi = 2 + expon(1) + expon(2);

    for i = 0 : i_hi
      value = value + r8_mop ( i ) * r8_choose ( i_hi, i ) / ( i + expon(3) + 1 );
    end

    value = value * 2.0 / ( expon(1) + 1 ) * 2.0 / ( expon(2) + 1 );

  end

  return
end
function value = pyra_unit_volume ( )

%*****************************************************************************80
%
%% PYRA_UNIT_VOLUME_3D returns the volume of a unit pyramid.
%
%  Discussion:
%
%    A pyramid with square base can be regarded as the upper half of a
%    3D octahedron.
%
%    The integration region:
%
%      - ( 1 - Z ) <= X <= 1 - Z
%      - ( 1 - Z ) <= Y <= 1 - Z
%                0 <= Z <= 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VALUE, the volume of the pyramid.
%
  value = 4.0 / 3.0;

  return
end
function value = r8_choose ( n, k )

%*****************************************************************************80
%
%% R8_CHOOSE computes the binomial coefficient C(N,K).
%
%  Discussion:
%
%    The value is calculated in such a way as to avoid overflow and
%    roundoff.  The calculation is done in integer arithmetic.
%
%    The formula used is:
%
%      C(N,K) = N! / ( K! * (N-K)! )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    ML Wolfson, HV Wright,
%    Algorithm 160:
%    Combinatorial of M Things Taken N at a Time,
%    Communications of the ACM,
%    Volume 6, Number 4, April 1963, page 161.
%
%  Parameters:
%
%    Input, integer N, K, are the values of N and K.
%
%    Output, real VALUE, the number of combinations of N
%    things taken K at a time.
%
  mn = min ( k, n - k );

  if ( mn < 0 )
    value = 0;
  elseif ( mn == 0 )
    value = 1;
  else
    mx = max ( k, n - k );
    value = mx + 1;
    for i = 2 : mn
      value = ( value * ( mx + i ) ) / i;
    end
  end

  return
end
function value = r8_mop ( i )

%*****************************************************************************80
%
%% R8_MOP returns the I-th power of -1 as an R8 value.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, the power of -1.
%
%    Output, real R8_MOP, the I-th power of -1.
%
  if ( mod ( i, 2 ) == 0 )
    value = + 1.0;
  else
    value = - 1.0;
  end

  return
end
function table = r8mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% R8MAT_DATA_READ reads data from an R8MAT file.
%
%  Discussion:
%
%    An R8MAT is an array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns of data.
%
%    Output, real TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %f' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the file.\n' );
    error ( 'R8MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = r8mat_header_read ( input_filename )

%*****************************************************************************80
%
%% R8MAT_HEADER_READ reads the header from an R8MAT file.
%
%  Discussion:
%
%    An R8MAT is an array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function len = s_len_trim ( s )

%*****************************************************************************80
%
%% S_LEN_TRIM returns the length of a character string to the last nonblank.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be measured.
%
%    Output, integer LEN, the length of the string up to the last nonblank.
%
  len = length ( s );

  while ( 0 < len )
    if ( s(len) ~= ' ' )
      return
    end
    len = len - 1;
  end

  return
end
function word_num = s_word_count ( s )

%*****************************************************************************80
%
%% S_WORD_COUNT counts the number of "words" in a string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be examined.
%
%    Output, integer WORD_NUM, the number of "words" in the string.
%    Words are presumed to be separated by one or more blanks.
%
  FALSE = 0;
  TRUE = 1;

  word_num = 0;
  s_length = length ( s );

  if ( s_length <= 0 )
    return;
  end

  blank = TRUE;

  for i = 1 : s_length

    if ( s(i) == ' ' )
      blank = TRUE;
    elseif ( blank == TRUE )
      word_num = word_num + 1;
      blank = FALSE;
    end

  end

  return
end
function timestamp ( )

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

  return
end


nj = 10000;
ms = 210;
mt = 210;

xj = sort((rand(nj,1)*2-1)*pi);
yj = sort((rand(nj,1)*2-1)*pi);
cj = randn(nj,1)+1i*randn(nj,1);

eps=1e-12;

iflag = +1;

tic
fk = dirft2d1(nj,xj,yj,cj,iflag,ms,mt);
toc

tic
fk1 = nufft2d1(nj,xj,yj,cj,iflag,eps,ms,mt);
toc

abs_error=norm(fk-fk1,2)
rel_error=norm(fk-fk1,2)/norm(fk,2)

iflag = -1;

tic
fk = dirft2d1(nj,xj,yj,cj,iflag,ms,mt);
toc

tic
fk1 = nufft2d1(nj,xj,yj,cj,iflag,eps,ms,mt);
toc

abs_error=norm(fk-fk1,2)
rel_error=norm(fk-fk1,2)/norm(fk,2)


function y = gaussian_response(rect_size, sigma)
%GAUSSIAN_RESPONSE create the (fixed) target response of the correlation filter response
    [i1, i2] = circ_grid(rect_size(1), rect_size(2));
    y = exp(-(i1.^2 + i2.^2) / (2 * sigma^2));
end


function loglik = RicianLogLik(meas, signals, sig)
% Computes the log likelihood of the measurements given the model signals
% for the Rician noise model.
%
% loglik = RicianLogLik(meas, signals, sig) returns the likelihood of
% measuring meas given the signals and the noise standard deviation sig.
%
% meas are the measurements
%
% signals are computed from a model
%
% sig is the standard deviation of the Gaussian distributions underlying
% the Rician distribution.
%
% author: Daniel C Alexander (d.alexander@ucl.ac.uk)
%

sumsqsc = (signals.^2 + meas.^2)./(2*sig.^2);
scp = meas.*signals./(sig.^2);
lb0 = logbesseli0(scp);
logliks = - 2*log(sig) - sumsqsc + log(signals) + lb0;
loglik = sum(logliks);


function N = nees(x,m,P)

% NEES  Normalized Estimation Error Squared.
%   N = NEES(X,M,P) computes the Normalized Estimation Error Squared given
%   true state X and a Gaussian estimate of mean M and covariance P:
%
%       NEES = (X-M)'*inv(P)*(X-M);
%
%   See also MAHALANOBIS.

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

N = (x-m)'/P*(x-m);

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


% This demo compares exponential vs hyperbolic temporal discounting.
% This script simulates an agent making choices between two options, which
% differ in terms of the expected reward and temporal horizons, under both
% an hyperbolic and an exponantial temporal discounting model.
% The choice data are then inverted using these two models. NB: in both
% models, the discount factor and the behavioural temperature are unknown.
% The effect of simulating the data under each model onto the log-Bayes
% factor (comparison between hyperbolic vs exponential models) is assessed
% using Monte-Carlo simulations.

clear all
close all

models = {'hyperbolic','exponential'};

% initialize simulations
ntrials = 128;
g_fname = @g_discounting;
phi = [1;-1];
in.ind.logk = 1; % index of discount factor (phi)
in.ind.logb = 2; % index of behavioural temperature (phi)
in.ind.t = [1;3]; % index of temporal horizon (u)
in.ind.R = [2;4]; % index of expected reward (u)

% Build options and dim structures for model inversion
dim.n_theta = 0;
dim.n_phi = 2;
dim.n = 0;
dim.p = 1;
dim.n_t = ntrials;
options.inG = in;
options.dim = dim;
options.sources = struct('type',1 ,'out', 1); % one binomial observation;
options.DisplayWin = 0;

% Build time series of hidden states and observations
Nmcmc = 16;
nm = length(models);
p = cell(nm,nm,Nmcmc);
o = cell(nm,nm,Nmcmc);
F = zeros(nm,nm,Nmcmc);
for ii=1:Nmcmc
    R = exp(randn(2,ntrials));
    t = exp(randn(2,ntrials));
    u = zeros(4,ntrials);
    u(in.ind.t,:) = t;
    u(in.ind.R,:) = R;
    for i=1:nm
        options.inG.model = models{i};
        [y,x,x0,eta,e] = VBA_simulate (ntrials,[],g_fname,[],phi,u,[],[],options);
        % Call inversion routine
        for j=1:nm
            options.inG.model = models{j};
            [p{i,j,ii},o{i,j,ii}] = VBA_NLStateSpaceModel(y,u,[],g_fname,dim,options);
            F(i,j,ii) = o{i,j,ii}.F;
        end
    end
end

hf = figure('color',[1 1 1]);
ha = axes('parent',hf);
dF = F(:,1,:) - F(:,2,:);
mdF = mean(dF,3);
vdF = var(dF,[],3)./Nmcmc;
plotUncertainTimeSeries(mdF,vdF,[],ha);
set(ha,'xlim',[0,3],'xtick',[1,2],'xticklabels',models)
xlabel(ha,'type of simulated data')
ylabel(ha,['log p(y|',models{1},') - log p(y|',models{2},')'])
box(ha,'off')


function [ error_per_image ] = compute_eye_corner_error( ground_truth_all, detected_points_all )
num_of_images = size(ground_truth_all,3);
num_of_points = size(ground_truth_all,1);

error_per_image = zeros(num_of_images,1);

for i =1:num_of_images
    detected_points      = detected_points_all(:,:,i);
    ground_truth_points  = ground_truth_all(:,:,i);
    interocular_distance = norm(ground_truth_points(37,:)-ground_truth_points(46,:));
    %interocular_distance = norm(mean(ground_truth_points(37:42,:))-mean(ground_truth_points(43:48,:)));
    sum=0;
    for j=1:num_of_points
        sum = sum+norm(detected_points(j,:)-ground_truth_points(j,:));
    end
    error_per_image(i) = sum/(num_of_points*interocular_distance);
end
end


%  Figure 10.11      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
%  fig10_11.m is a script to generate Fig. 10.11
%  the frequency response of the notch network
clf;
nnotch=[1/.81 0  1] ;
dnotch=[1/625 2/25  1];
% define frequency range
w=logspace(-1,1);
w(36)=1;
subplot(211)
w=logspace(-1,1);
w(24)=0.89;
% compute Bode
[magn phn]=bode(nnotch,dnotch,w);
% phn=php-360*ones(phn);
magn1=[magn, ones(size(magn))];
subplot(211); loglog(w,magn1); grid;
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 10.11 Bode plot of a notch filter')
subplot(212); semilogx(w,phn); grid;
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');
%Bode grid
bodegrid


function fd1d_heat_explicit_test03 ( )

%*****************************************************************************80
%
%% FD1D_HEAT_EXPLICIT_TEST03 does a simple test problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FD1D_HEAT_EXPLICIT_TEST03:\n' );
  fprintf ( 1, '  Compute an approximate solution to the time-dependent\n' );
  fprintf ( 1, '  one dimensional heat equation:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    dH/dt - K * d2H/dx2 = f(x,t)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Run a simple test case.\n' );
%
%  Heat coefficient.
%
  k = k_test03 ( );
%
%  X_NUM is the number of equally spaced nodes to use between 0 and 1.
%
  x_num = 21;
  x_min = -5.0;
  x_max = +5.0;
  dx = ( x_max - x_min ) / ( x_num - 1 );
  x = linspace ( x_min, x_max, x_num );
%
%  T_NUM is the number of equally spaced time points between 0 and 10.0.
%
  t_num = 81;
  t_min = 0.0;
  t_max = 4.0;
  dt = ( t_max - t_min ) / ( t_num - 1 );
  t = linspace ( t_min, t_max, t_num );
%
%  Get the CFL coefficient.
%
  cfl = fd1d_heat_explicit_cfl ( k, t_num, t_min, t_max, x_num, x_min, x_max );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of X nodes = %d\n', x_num );
  fprintf ( 1, '  X interval is [%f,%f]\n', x_min, x_max );
  fprintf ( 1, '  X spacing is %f\n', dx );
  fprintf ( 1, '  Number of T values = %d\n', t_num );
  fprintf ( 1, '  T interval is [%f,%f]\n', t_min, t_max );
  fprintf ( 1, '  T spacing is %f\n', dt );
  fprintf ( 1, '  Constant K = %g\n', k );
  fprintf ( 1, '  CFL coefficient = %g\n', cfl );
%
%  Running the code produces an array H of temperatures H(t,x),
%  and vectors x and t.
%

  hmat = zeros ( x_num, t_num );

  for j = 1 : t_num
    if ( j == 1 )
      h = ic_test03 ( x_num, x, t(j) );
      h = bc_test03 ( x_num, x, t(j), h );
    else
      h = fd1d_heat_explicit ( x_num, x, t(j-1), dt, cfl, @rhs_test03, @bc_test03, h );
    end
    hmat(1:x_num,j) = h(1:x_num);
  end
%
%  Plot the data.
%
  figure ( 3 )
  [ tmat, xmat ] = meshgrid ( t, x );
  mesh ( tmat, xmat, hmat );
  title ( 'H(X,T) for TEST03 computed by FD1D\_HEAT\_EXPLICIT' );
  xlabel ( '<-- Time -->' );
  ylabel ( '<-- X -->' );
  zlabel ( '<--  H(X,T) -->' );
%
%  Write the data to files.
%
  r8mat_write ( 'h_test03.txt', x_num, t_num, hmat );
  r8vec_write ( 't_test03.txt', t_num, t );
  r8vec_write ( 'x_test03.txt', x_num, x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  H(X,T) written to "h_test03.txt"\n' );
  fprintf ( 1, '  T values written to "t_test03.txt"\n' );
  fprintf ( 1, '  X values written to "x_test3.txt"\n' );
  return
end


function krn = spm_smoothkern(fwhm,x,t)
% Generate a Gaussian smoothing kernel
% FORMAT krn = spm_smoothkern(fwhm,x,t)
% fwhm - full width at half maximum
% x    - position
% t    - either 0 (nearest neighbour) or 1 (linear).
%        [Default: 1]
%
% krn  - value of kernel at position x
%__________________________________________________________________________
%
% For smoothing images, one should really convolve a Gaussian with a sinc
% function. For smoothing histograms, the kernel should be a Gaussian
% convolved with the histogram basis function used. This function returns
% a Gaussian convolved with a triangular (1st degree B-spline) basis
% function (by default). A Gaussian convolved with a hat function (0th
% degree B-spline) can also be returned.
%
% Note that the convolution kernel returned by this function differ from
% the ones that other packages currently use for Gaussian smoothing -
% particularly when the FWHM is small compared with the voxel dimensions.
% The fact that SPM does it differently from other software does not mean
% that it is wrong.
%__________________________________________________________________________
% Copyright (C) 2005-2011 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_smoothkern.m 7460 2018-10-29 15:55:12Z john $

if nargin<3, t = 1; end

% Variance from FWHM
s = (fwhm/sqrt(8*log(2)))^2+eps;

% The simple way to do it. Not good for small FWHM
% krn = (1/sqrt(2*pi*s))*exp(-(x.^2)/(2*s));

if t==0
    % Gaussian convolved with 0th degree B-spline
    % int(exp(-((x+t))^2/(2*s))/sqrt(2*pi*s),t= -0.5..0.5)
    w1  = 1/sqrt(2*s);
    krn = 0.5*(erf(w1*(x+0.5))-erf(w1*(x-0.5)));
    krn(krn<0) = 0;

elseif t==1
    % Gaussian convolved with 1st degree B-spline
    %  int((1-t)*exp(-((x+t))^2/(2*s))/sqrt(2*pi*s),t= 0..1)
    % +int((t+1)*exp(-((x+t))^2/(2*s))/sqrt(2*pi*s),t=-1..0)
    w1  =  0.5*sqrt(2/s);
    w2  = -0.5/s;
    w3  = sqrt(s/2/pi);
    krn = 0.5*(erf(w1*(x+1)).*(x+1) + erf(w1*(x-1)).*(x-1) - 2*erf(w1*x   ).* x)...
          +w3*(exp(w2*(x+1).^2)     + exp(w2*(x-1).^2)     - 2*exp(w2*x.^2));
    krn(krn<0) = 0;

else
    error('Only defined for nearest neighbour and linear interpolation.');
    % This should probably be based on https://arxiv.org/pdf/1608.05854.pdf:
    %     Martin TB, Prunet S, Drissen L. Optimal fitting of Gaussian-apodized
    %     or under-resolved emission lines in Fourier transform spectra
    %     providing new insights on the velocity structure of NGC 6720. Monthly
    %     Notices of the Royal Astronomical Society. 2016 Sep 14;463(4):4223-38.
    % (thanks for the pointer Guillaume).
end


%  rouwenhorst approximates and AR(1) process with a Markov chain
%
%  ::
%
%    [Thetai,y]=rouwenhorst(mu,rho,sigma)
%    [Thetai,y]=rouwenhorst(mu,rho,sigma,N)
%    [Thetai,y]=rouwenhorst(mu,rho,sigma,N,p)
%    [Thetai,y]=rouwenhorst(mu,rho,sigma,N,p,q)
%
%  Args:
%
%     mu (numeric): unconditonal mean of the ar(1) process
%     rho [numeric): autoregressive coefficient
%     sigma (numeric): standard deviation of the shock
%     N (numeric | {2}): number of states of the markov chain
%     p (numeric | {.5*(1+rho)}): first parameter of the procedure
%     q (numeric | {.5*(1+rho)}): second parameter of the procedure
%
%  Returns:
%     :
%
%     - **Thetai** [numeric] : NxN transition matrix
%     - **y** [numeric] : vector of nodes or states
%
%  Note:
%
%     - The process is assumed to be of the form
%       x_t-mu=rho*(x_{t-1}-mu)+sigma*error_term where error_term ~ N(0,1)
%
%  Reference:
%
%     - Karen A. Kopecky, Richard M. H. Suen (2010): "Finite state
%       Markov-chain approximations to highly persistent processes". Review of
%       Economic Dynamics 13, pp 701-714
%
%


function dist = distance(x1,y1,x2,y2)
%This function calculates the distance between any two cartesian
%coordinates.
%   Copyright 2009-2010 The MathWorks, Inc.
dist=sqrt((x1-x2)^2 + (y1-y2)^2);


% SOSDEMO4 --- Matrix Copositivity
% Section 3.4 of SOSTOOLS User's Manual
%

clear; echo on;
syms x1 x2 x3 x4 x5;
vartable = [x1; x2; x3; x4; x5];

% The matrix under consideration
J = [1 -1  1  1 -1;
    -1  1 -1  1  1;
     1 -1  1 -1  1;
     1  1 -1  1 -1;
    -1  1  1 -1  1];

% =============================================
% First, initialize the sum of squares program

prog = sosprogram(vartable);     % No decision variables.

% =============================================
% Next, define SOSP constraints

% Constraint : r(x)*J(x) - p(x) = 0
J = [x1^2 x2^2 x3^2 x4^2 x5^2]*J*[x1^2; x2^2; x3^2; x4^2; x5^2];
r = x1^2 + x2^2 + x3^2 + x4^2 + x5^2;

prog = sosineq(prog,r*J);

% =============================================
% And call solver
prog = sossolve(prog);

% =============================================
% Program is feasible. The matrix J is copositive.

echo off


% KM_DEMO_KDE Kernel density estimation demo.
%
% Author: Steven Van Vaerenbergh (steven *at* gtas.dicom.unican.es), 2013
%
% This file is part of the Kernel Methods Toolbox for MATLAB.
% https://github.com/steven2358/kmbox

close all
clear

%% PARAMETERS

N1 = 100; % number of data in data set 1
m1 = -1; % mean value
s1 = 0.1; % variance

N2 = 500; % number of data in data set 2
m2 = 2; % mean value
s2 = 0.5; % variance

sigma = 0.1; % bandwidth
nsteps = 100; % number of abscis points in kde

%% PROGRAM
tic

x1 = m1 + sqrt(s1)*randn(N1,1);
x2 = m2 + sqrt(s2)*randn(N2,1);
x = [x1;x2];

[xx,pp] = km_kde(x,sigma,nsteps,1.2);

toc
%% OUTPUT

figure;

[n,h] = hist(x,100);
bar(h,n/sum(n),'b','EdgeColor','b')

hold on
plot(xx,pp,'r','LineWidth',3);

legend('histogram','KDE')


function fx = edcheb ( x, coef, npl )

%*****************************************************************************80
%
%% EDCHEB evaluates the derivative of a Chebyshev series at a point.
%
%  Discussion:
%
%    This routine evaluates the derivative of a Chebyshev series
%    at a point in [-1,+1].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Roger Broucke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Roger Broucke,
%    Algorithm 446:
%    Ten Subroutines for the Manipulation of Chebyshev Series,
%    Communications of the ACM,
%    October 1973, Volume 16, Number 4, pages 254-256.
%
%  Parameters:
%
%    Input, real ( kind = 8 ) X, the evaluation point.
%    -1 <= X <= +1.
%
%    Input, real ( kind = 8 ) COEF(NPL), the Chebyshev series.
%
%    Input, integer ( kind = 4 ) NPL, the number of terms in the
%    Chebyshev series.
%
  xjp2 = 0.0;
  xjpl = 0.0;
  bjp2 = 0.0;
  bjpl = 0.0;
  n = npl - 1;

  for k = 1 : n
    j = npl - k;
    dj = j;
    xj = 2.0 * coef(j+1) * dj + xjp2;
    bj = 2.0 * x * bjpl - bjp2 + xj;
    bf = bjp2;
    bjp2 = bjpl;
    bjpl = bj;
    xjp2 = xjpl;
    xjpl = xj;
  end

  fx = 0.5 * ( bj - bf );

  return
end


function prob = LogisticRegressionTest(data, model)

prob = 1 ./ (1 + exp(- (model.w(1:end-1)' * data.feat) - model.w(end)));
% prob = 1 ./ (1 + exp(- (model.w' * data.feat)));


function pass = test_dst(pref)

if ( nargin == 0 )
    pref = chebfunpref();
end

% TODO: include more extensive tests.

n = 15;
seedRNG(42)
R = rand(n);

tol = 50*eps;

for k = 1:8
    pass(k) = norm(chebfun.idst(chebfun.dst(R,1),1) - R) < tol;
end

N = 15;
x = rand(N, 2);
tol = 100*eps;

% DST-I
DST1 = sin(pi/(N+1)*((0:N-1)+1)'*((0:N-1)+1));
y = DST1 * x;
pass(9) = norm( y - chebfun.dst(x, 1) ) < tol;

% DST-II
DST2 = sin(pi/N*((0:N-1)+1)'*((0:N-1)+1/2));
y = DST2 * x;
pass(10) = norm( y - chebfun.dst(x, 2) ) < tol;

% DST-III
DST3 = sin(pi/N*((0:N-1)+1/2)' * ((0:N-1)+1));
DST3(:, end) = .5*DST3(:, end);
y = DST3 * x;
pass(11) = norm( y - chebfun.dst(x, 3) ) < tol;

% DST-IV
DST4 = sin(pi/N*((0:N-1)+1/2)'*((0:N-1)+1/2));
y = DST4 * x;
pass(12) = norm( y - chebfun.dst(x, 4) ) < 10*tol;

% DST-V
DST5 = sin(pi/(N+1/2)*((0:N-1)+1)'*((0:N-1)+1));
y = DST5 * x;
pass(13) = norm( y - chebfun.dst(x, 5) ) < 10*tol;

% DST-VI
DST6 = sin(pi/(N+1/2)*((0:N-1)+1)'*((0:N-1)+1/2));
y = DST6 * x;
pass(14) = norm( y - chebfun.dst(x, 6) ) < 10*tol;

% DST-VII
DST7 = sin(pi/(N+1/2)*((0:N-1)+1/2)'*((0:N-1)+1));
y = DST7 * x;
pass(15) = norm( y - chebfun.dst(x, 7) ) < 10*tol;

% DST-VIII
DST8 = sin(pi/(N+1/2)*((0:N-1)+1/2)'*((0:N-1)+1/2));
y = DST8 * x;
pass(16) = norm( y - chebfun.dst(x, 8) ) < 10*tol;

% IDST-I
y = DST1 \ x;
pass(17) = norm( y - chebfun.idst(x, 1) ) < tol;

% IDST-II
y = DST2 \ x;
pass(18) = norm( y - chebfun.idst(x, 2) ) < tol;

% IDST-III
y = DST3 \ x;
pass(19) = norm( y - chebfun.idst(x, 3) ) < tol;

% IDST-IV
y = DST4 \ x;
pass(20) = norm( y - chebfun.idst(x, 4) ) < tol;

% IDST-V
y = DST5 \ x;
pass(21) = norm( y - chebfun.idst(x, 5) ) < tol;

% IDST-VI
y = DST6 \ x;
pass(22) = norm( y - chebfun.idst(x, 6) ) < tol;

% IDST-VII
y = DST7 \ x;
pass(23) = norm( y - chebfun.idst(x, 7) ) < tol;

% % IDST-VIII
pass(24) = true;
% y = DST8 \ x;
% pass(24) = norm( y - chebfun.idst(x, 8) ) < tol;

% Check complex inputs
c = rand(10,1) + 1i*rand(10,1);
for j = 1:8
    v1 = chebfun.dst(c, j);
    v2 = chebfun.dst(real(c),j) + 1i*chebfun.dst(imag(c),j);
    pass(24+j) = norm(v1 - v2) < tol;
end
end


 function Distance=DistanceToBorder(Position,Corner_position)

%     Position=FullNeuronBehaviorDataSet.FullNeuronBehaviorMatrix{1,1}(:,2:3);
%     Corner_position=FullNeuronBehaviorDataSet.EBC_results{1,4}.QP;
%%
     for i=1:1:size(Position,1)

         Position_now=[squeeze(Position(i,:)),0];
         V1=[Corner_position(1,:),0];
         V2=[Corner_position(2,:),0];
         V3=[Corner_position(3,:),0];
         V4=[Corner_position(4,:),0];

        a = V1 - V2;
        b = Position_now - V2;
        d1 = norm(cross(a,b)) /norm(a);

        a = V2 - V3;
        b = Position_now - V3;
        d2 = norm(cross(a,b)) /norm(a);

        a = V3 - V4;
        b = Position_now - V4;
        d3 = norm(cross(a,b)) /norm(a);

        a = V4 - V1;
        b = Position_now - V1;
        d4 = norm(cross(a,b)) /norm(a);

        Distance(i)=min([d1,d2,d3,d4]);
    end

 end


function x = error_f_inverse ( y )

%*****************************************************************************80
%
%% ERROR_F_INVERSE inverts the error function ERF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real Y, the value of the error function.
%
%    Output, real X, the value such that ERROR_F(X) = Y.
%
  z = ( y + 1.0 ) / 2.0;

  x = normal_01_cdf_inv ( z );

  x = x / sqrt ( 2.0 );

  return
end


function backtrack_binary_rc_test01 ( )

%*****************************************************************************80
%
%% BACKTRACK_BINARY_RC_TEST01 seeks binary powers that have a given sum.
%
%  Discussion:
%
%    We consider the binary powers 1, 2, 4, ... 2^(n-1).
%
%    We wish to select some of these powers, so that the sum is equal
%    to a given target value.  We are actually simply seeking the binary
%    representation of an integer.
%
%    A partial solution is acceptable if it is less than the target value.
%
%    We list the powers in descending order, so that the bactracking
%    procedure makes the most significant choices first, thus quickly
%    eliminating many unsuitable choices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 December 2013
%
%  Author:
%
%    John Burkardt
%
  n = 8;
  test_num = 3;
  targets = [ 73, 299, -3 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BACKTRACK_BINARY_RC_TEST01\n' );
  fprintf ( 1, '  Use BACKBIN_RC to find the binary expansion of\n' );
  fprintf ( 1, '  an integer between 0 and 255.\n' );
  fprintf ( 1, '  The choices are 0/1 for the 8 digits.\n' );

  for test = 1 : test_num

    target = targets(test);
    fprintf ( 1, '\n' );
    fprintf ( 1, '  TARGET = %d\n', target );
    call_num = 0;
    reject = 0;
    n2 = -1;
    choice = [];

    while ( 1 )

      [ n2, choice ] = backbin_rc ( n, reject, n2, choice );
      call_num = call_num + 1;

      if ( n2 == -1 )
        fprintf ( 1, '  Termination without solution.\n' );
        break
      end
%
%  Evaluate the integer determined by the choices.
%
      factor = 1;
      for i = n : -1 : n2 + 1
        factor = factor * 2;
      end

      result = 0;
      for i = 1 : n2
        result = result * 2 + choice(i);
      end

      result = result * factor;
%
%  If the integer is too big, then we reject it, and
%  all the related integers formed by making additional choices.
%
      reject = ( target < result );
%
%  If we hit the target, then in this case, we can exit because
%  the solution is unique.
%
      if ( result == target )
        break
      end

    end

    fprintf ( 1, '  Number of calls = %d\n', call_num );
    fprintf ( 1, '  Binary search space = %d\n', 2 ^ n );
    fprintf ( 1, '  ' );
    for i = 1 : n
      fprintf ( 1, '%2d', choice(i) );
    end
    fprintf ( 1, '\n' );

  end

  return
end


function legendre_polynomial_test04 ( )

%*****************************************************************************80
%
%% LEGENDRE_POLYNOMIAL_TEST04 tests P_QUADRATURE_RULE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 March 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LEGENDRE_POLYNOMIAL_TEST04:\n' );
  fprintf ( 1, '  P_QUADRATURE_RULE computes the quadrature rule\n' );
  fprintf ( 1, '  associated with P(n,x)\n' );

  n = 5;
  [ x, w ] = p_quadrature_rule ( n );

  r8vec2_print ( n, x, w,  '      X            W' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use the quadrature rule to estimate:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Q = Integral ( -1 <= X < +1 ) X^E dx\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   E       Q_Estimate      Q_Exact\n' );
  fprintf ( 1, '\n' );

  for e = 0 : 2 * n - 1
    if ( e == 0 )
      f = ones ( n, 1 );
    else
      f = x .^ e;
    end
    q = w' * f;
    q_exact = p_integral ( e );
    fprintf ( 1, '  %2d  %14g  %14g\n', e, q, q_exact );
  end

  return
end


function test07()
% function test07()
%
% Computes a robust mean of angles
%

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:

    % Generate the data
    N = 250;
    theta_true = pi*(2*rand(1)-1);
    thetas = zeros(N,1);
    p_true = .25;
    for i = 1 : N
        if rand(1) < p_true
            thetas(i) = theta_true + .3*randn(1);
        else
            thetas(i) = pi*(2*rand(1)-1);
        end
    end
    X = [cos(thetas') ; sin(thetas)'];

    % Pick the manifold
    problem.M = spherefactory(2);

    % Parameters
    p = p_true;
    kappa = 3;
    c2 = besseli(0, 2*kappa);

    % Define the problem cost function
    problem.cost = @cost;
    function [val store] = cost(x, store)

        if ~isfield(store, 'fi')
            store.fi = (p/c2)*exp(2*kappa*X'*x);
        end
        fi = store.fi;

        val = -sum(log(fi + (1-p)));

    end

    % And its gradient
    problem.grad = @grad;
    function [g store] = grad(x, store)

        if ~isfield(store, 'fi')
            store.fi = (p/c2)*exp(2*kappa*X'*x);
        end
        fi = store.fi;

        g = -X*(2*kappa*(fi./(fi+(1-p))));
        g = g - (x'*g)*x;

    end

    % Check differentials consistency.
    % checkgradient(problem);

    % Solve with trust-regions and FD approximation of the Hessian
    warning('off', 'manopt:getHessian:approx');

    % Test many random initial guess
%     best_cost = inf;
%     best_x = [];
%     for i = 1 : 5
%         [x cst] = trustregions(problem);
%         if cst < best_cost
%             best_cost = cst;
%             best_x = x;
%         end
%     end
%     theta_found = angle(best_x(1)+1i*best_x(2));

    % Do a histogram of the data and select the initial guess as the peak
    [freqs, bins] = hist(thetas, 30);
    [~, ind] = max(freqs);
    x0 = [cos(bins(ind)) ; sin(bins(ind))];
    x = trustregions(problem, x0);
    theta_found = angle(x(1)+1i*x(2));

    fprintf('True theta: %g\nHist theta: %g\nFound theta: %g\n', theta_true, angle(x0(1)+1i*x0(2)), theta_found);

    subplot(2,1,1);
    hist(thetas, 30);
    xlim([-pi pi]);

    th = linspace(-pi, pi);
    fth = zeros(size(th));
    for i = 1 : length(th)
        fth(i) = cost([cos(th(i));sin(th(i))], struct());
    end
    subplot(2,1,2);
    plot(th, -fth);
    xlim([-pi pi]);

%     keyboard;

end


function outputarray = linarray(start,spacing,numpoints)
% outputarray = linarray(start,spacing,numpoints)
%
% The function linarray generates an row vector based on a start value, a
% spacing, and a number of points.  For example: linarray(1,.1,5) returns:
% [1.0000    1.1000    1.2000    1.3000    1.4000]
%
% 3/2/11 (c) James F. Mack

outputarray = [start:spacing:start+(numpoints-1)*spacing];