function [ fx, fy ] = f01_f1 ( n, x, y )

%*****************************************************************************80
%
%% F01_F1 returns first derivatives of function 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N,1), Y(N,1), the evalution points.
%
%    Output, real FX(N,1), FY(N,1), the derivative values.
%
  t1(1:n,1) = exp ( - ( ( 9.0 * x(1:n,1) - 2.0 ).^2 ...
                    + ( 9.0 * y(1:n,1) - 2.0 ).^2 ) / 4.0 );
  t2(1:n,1) = exp ( - ( ( 9.0 * x(1:n,1) + 1.0 ).^2 ) / 49.0 ...
                    - ( 9.0 * y(1:n,1) + 1.0 ) / 10.0 );
  t3(1:n,1) = exp ( - ( ( 9.0 * x(1:n,1) - 7.0 ).^2 ...
                    + ( 9.0 * y(1:n,1) - 3.0 ).^2 ) / 4.0 );
  t4(1:n,1) = exp ( -   ( 9.0 * x(1:n,1) - 4.0 ).^2 ...
                    - ( 9.0 * y(1:n,1) - 7.0 ).^2 );

  fx(1:n,1) = ...
    - 3.375           * ( 9.0 * x(1:n,1) - 2.0 ) * t1 ...
    - ( 27.0 / 98.0 ) * ( 9.0 * x(1:n,1) + 1.0 ) * t2 ...
    - 2.25            * ( 9.0 * x(1:n,1) - 7.0 ) * t3 ...
    + 3.6             * ( 9.0 * x(1:n,1) - 4.0 ) * t4;

  fy(1:n,1) = ...
    - 3.375 * ( 9.0 * y(1:n,1) - 2.0 ) * t1 ...
    - 0.675                          * t2 ...
    - 2.25  * ( 9.0 * y(1:n,1) - 3.0 ) * t3 ...
    + 3.6   * ( 9.0 * y(1:n,1) - 7.0 ) * t4;

  return
end


% This is an approximation of random numbers wrt to wrapped Normal Distrib
%
% Any neater, faster, more precise and accurate generator is very welcome.
%
% Copyright (c) 2012 University of Crete - Computer Science Department (UOC-CSD)
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function wgr = wrappednormrnd(mu, sigma, n, pregen)

    if nargin<4; pregen=false; end

    % The CDF is symetric around the mean. Thus, just need zero-mean random values
    % of unitary std and scale them wrt sigma.
    wgr = wrappednormrndunitystd(n, pregen);

    wgr = sigma.*wgr;

    wgr = wrap(wgr + mu);

return

function wgr = wrappednormrndunitystd(n, pregen)

    if nargin<2; pregen=0; end

    if pregen==1
        global wrappednormrnd;

        % If run for the first time, initialize the cumulative distrib fns
        if isempty(wrappednormrnd) || ~isfield(wrappednormrnd, 'icdf')
            fprintf('wrappednormrnd.m: Pre-compute the cumulative distribution function once for all ... ');

            xs = -pi:(2*pi)/1e6:pi;
            xs = xs(1:end-1);
            cdf = wrappednormcdf(xs, 0, 1);
            % Resample the inverse CDF with 1 million uniform steps
            % This also assumes that we don't need a thinner resolution of the distribution
            wrappednormrnd.icdf_ys = (0:1e-4:1);
            wrappednormrnd.icdf = interp1(cdf, xs, wrappednormrnd.icdf_ys, 'linear', 'extrap').';

            fprintf('done.\n');
            if nargin==0; return; end
        end

        % wgr = interp1(wrappednormrnd.icdf_ys, sigma*wrappednormrnd.icdf, rand(n,1), 'linear', 'extrap');
        % The following is a speed-up version of the above line.
        ri = (length(wrappednormrnd.icdf)-1)*rand(n,1)+1;
        rfi = floor(ri);
        rci = ceil(ri);
        wgr = wrappednormrnd.icdf(rfi);
        wgr = wgr + (ri-rfi).*(wrappednormrnd.icdf(rci)-wgr);

        % % Or a faster sampling (implies only 1e4 different random values can be generated).
        % idx = round(length(wrappednormrnd.icdf)*rand(n,1))+1;
        % idx = max(1,min(length(wrappednormrnd.icdf),idx));
        % wgr = wrappednormrnd.icdf(idx);
    else
        % The non-optimized random generator
        xs = -pi:(2*pi)/1e6:pi;
        xs = xs(1:end-1);
        cdf = wrappednormcdf(xs, 0, 1);

        wgr = interp1(cdf, xs, rand(n,1), 'linear', 'extrap');
    end

return


function [ u_i ] = rotate_b2i( u_b, phi, theta, psi )
% ROTATE_B2I - Rotation of an object (vector/matrix) from the body to the
% inertial 3D-space
%
%
%

    cr = cos(phi);
    cp = cos(theta);
    cy = cos(psi);
    sr = sin(phi);
    sp = sin(theta);
    sy = sin(psi);

    % Rotation matrix from inertial frame to body frame

    Rbi = [cp*cy, sr*sp*cy-cr*sy, cr*sp*cy+sr*sy;...
            cp*sy, sr*sp*sy+cr*cy, cr*sp*sy-sr*cy;...
            -sp,   sr*cp,          cr*cp];

    u_i = Rbi*u_b;

end


% Steady state transport solutions for one Pe=0 and various values of Da2
x = [0:0.01:1];
Da2 = 8; mu1 = sqrt(Da2); mu2 = -mu1;
s = mu2*exp(mu2)-mu1*exp(mu1);
c = (mu2*exp(mu2)*exp(mu1*x)-mu1*exp(mu1)*exp(mu2*x))./s
for Da2 = [4 2 1 0.5 0.25 0.125];
    s = sqrt(Da2); mu1 = s; mu2 = -s;
    s = mu2*exp(mu2)-mu1*exp(mu1);
    c = [c;(mu2*exp(mu2)*exp(mu1*x)-mu1*exp(mu1)*exp(mu2*x))./s];
end
plot (x,c);
legend('Da_2=8','Da_2=4','Da_2=2','Da_2=1','Da_2=0.5','Da_2=0.25','Da_2=0.125');
xlabel ('x/L [-]'); ylabel ('c/c_{in} [-]');


function varargout = gallery(name)
%CHEB.GALLERY   Chebfun example functions.
%   F = CHEB.GALLERY(NAME) returns a chebfun or a quasimatrix corresponding to
%   NAME.  See the listing below for available names.
%
%   For example,  plot(cheb.gallery('zigzag'))  plots a degree 10000 polynomial
%   that doesn't look like a polynomial, and  plot(cheb.gallery('gamma'))  shows
%   a chebfun with poles. For details of how each function is constructed, try
%   type +cheb/gallery  or  edit cheb.gallery.
%
%   [F,FA] = CHEB.GALLERY(NAME) also returns the anonymous function FA used to
%   define the function. Some gallery functions are generated by operations
%   beyond the usual Chebfun constructor (e.g. by solving ODEs), so FA in those
%   cases simply evaluates the chebfun.
%
%   CHEB.GALLERY with no input argument returns a random function from the
%   gallery.
%
%   CHEB.GALLERY with no output argument creates a plot of the selected
%   function.
%
%   airy         Airy Ai function on [-40,40]
%   bessel       Bessel function J_0 on [-100,100]
%   bump         C-infinity function with compact support
%   blasius      Blasius function on [0,10]
%   chirp        Sine with exponentially increasing frequency
%   daubechies   Approximation to Daubechies phi_2 wavelet scaling function
%   erf          Error function on [-10,10]
%   fishfillet   Wild oscillations from Extreme Extrema example
%   gamma        Gamma function on [-4,4]
%   gaussian     Gaussian function on [-Inf,Inf]
%   jitter       A piecewise constant function generated by ROUND
%   kahaner      Challenging integrand with four spikes
%   motto        Chebfun motto (Gilbert Strang)
%   random       Polynomial interpolant through random data in Chebyshev points
%   rose         A complex-valued sinusoid
%   runge        Runge function
%   seismograph  Tanh plus growing oscillation
%   Si           Sine integral on [-50,50]
%   sinefun1     As smooth as it looks
%   sinefun2     Not as smooth as it looks
%   spikycomb    25 peaks, each sharper than the last
%   stegosaurus  max(wiggly, x/10)
%   vandercheb   Chebyshev-Vandermonde quasimatrix
%   vandermonde  Vandermonde quasimatrix
%   wiggly       One of the Chebfun team's favorites
%   wild         An iteratively defined function on [-1 1]
%   zigzag       Degree 10000 polynomial that looks piecewise linear
%
%   Gallery functions are subject to change in future releases of Chebfun.
%
% See also CHEB.GALLERYTRIG, CHEB.GALLERY2, CHEB.GALLERY3, CHEB.GALLERYDISK, CHEB.GALLERYSPHERE.

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

% If the user did not supply an input, return a random function from the
% gallery.
if ( nargin == 0 )
    % NOTE [AB, 2014/12/01]: This is not a particularly scalable way of
    % implementing cheb.gallery. A better approach would be that each gallery
    % function lived in a separate m-file, and could return it's name,
    % description, and the required outputs, F and FA. That way, we would get
    % rid of the switch statement below. Furthermore, this would make the
    % testing of gallery easier, as we wouldn't have to update the list of
    % input options there.
    names = {'airy', 'bessel', 'blasius', 'bump', ...
        'chirp', 'daubechies', 'erf', 'fishfillet', 'gamma', 'gaussian', ...
        'jitter', 'kahaner', 'motto', 'random', 'rose', 'runge', ...
        'seismograph', 'Si', 'sinefun1', 'sinefun2', 'spikycomb', ...
        'stegosaurus', 'vandercheb', 'vandermonde', 'wiggly', 'wild', 'zigzag'};
    name = names{randi(length(names))};
end

% If nargout == 0, then the function is plotted. Each function in the gallery
% has its own plotting preferences.
ylims = [];
axispref = {};

% The main switch statement.
switch lower(name)

    % Airy Ai function on [-40,40]:
    case 'airy'
        fa = @airy;
        f = chebfun(fa, [-40 40]);
        ylims = [-.75 .75];

    % Bessel function with parameter 0 on [-100,100]:
    case 'bessel'
        fa = @(x) besselj(0, x);
        f = chebfun(fa, [-100 100]);
        ylims = [-.5 1.1];

    % Blasius function on [0,10]:
    case 'blasius'
        op = @(u) 2*diff(u,3) + u.*diff(u,2);
        bc  = @(x,u) [u(0); feval(diff(u),0); feval(diff(u),10)-1];
        N = chebop(op, [0 10], bc);
        N.init = chebfun([3.55542; 4.25907; 0.43669; -0.21367;
                0.06382; -0.00118; -0.00865; 0.00306], [0 10], 'coeffs');
        f = N\0;
        fa = @(x) f(x);
        ylims = [-.5 9];

    % Bump function:
    case 'bump'
        fa = @(x) (abs(x) < 1).*exp(-1./(1-x.^2));
        f = chebfun(fa, [-2 2]);

    % Sine with exponentially increasing frequency:
    case 'chirp'
        fa = @(x) sin(x.*exp(x));
        f = chebfun(fa, [0 5]);
        ylims = [-2 2];

    % Approx to Daubechies phi_2 wavelet scaling function:
    case 'daubechies'
        f = daubechies(10);
        fa = @(x) f(x);
        ylims = [-.5 1.5];

    % Error function on [-10,10]:
    case 'erf'
        fa = @erf;
        f = chebfun(fa, [-10 10]);

    % Wild oscillations from Extreme Extrema example:
    case 'fishfillet'
        fa = @(x) cos(x).*sin(exp(x));
        f = chebfun(fa, [0 6]);
        ylims = [-1.2 1.2];

    % Gamma function on [-4,4]:
    case 'gamma'
        fa = @gamma;
        f = chebfun(fa, [-4 4], 'blowup', 'on', 'splitting', 'on');

    % Gaussian function on [-Inf,Inf]:
    case 'gaussian'
        fa = @(x) exp(-x.^2/2)/sqrt(2*pi);
        f = chebfun(fa, [-Inf Inf]);
        ylims = [-.05 .45];

    % A piecewise constant function generated by ROUND:
    case 'jitter'
        fa = @(x) round(exp(x)*2.*sin(8*x));
        f = chebfun(fa, 'splitting', 'on');
        ylims = [-5 6];

    % Challenging integrand with four spikes:
    case 'kahaner'
        fa = @(x) sech(10*(x-0.2)).^2 + sech(100*(x-0.4)).^4 + ...
         sech(1000*(x-0.6)).^6 + sech(1000*(x-0.8)).^8;
        f = chebfun(fa, [0 1]);
        ylims = [-.1 1.2];

    % (Scribbled) Chebfun motto by Gilbert Strang:
    case 'motto'
        f = exp(3i*scribble('there is no fun like chebfun'));
        fa = @(x) f(x);
        axispref = {'equal', [-1 1 -1 1]*1.2, 'off'};

    % Polynomial interpolant through random data in Chebyshev points:
    case 'random'
        f = chebfun(rand(100,1));
        fa = @(x) f(x);

    % Rose curve:
    case 'rose'
        m = 5;
        n = 4;
        fa = @(t) cos(m/n*t).*cos(t) + 1i*cos(m/n*t).*sin(t);
        f = chebfun(fa, [0, 8*pi], 'trig');
        axispref = {'equal', [-1 1 -1 1]*1.2};

    % Runge function:
    case 'runge'
        fa = @(x) 1./(1 + x.^2);
        f = chebfun(fa, [-5 5]);
        ylims = [0 1.2];

    % Tanh plus growing oscillation from ATAP, Chapter 5:
    case 'seismograph'
        fa = @(x) tanh(20*sin(12*x)) + .02*exp(3*x).*sin(300*x);
        f = chebfun(fa);

    % Sine integral:
    case 'si'
        f = cumsum(chebfun(@(x) sin(x)./(x), [-50, 50]));
        fa = @(x) f(x);

    % As smooth as it looks:
    case 'sinefun1'
        fa = @(x) (1.75 + sin(50*x));
        f = chebfun(fa);

    % Not as smooth as it looks:
    case 'sinefun2'
        fa = @(x) (1.75 + sin(50*x)).^1.0001;
        f = chebfun(fa);

    % 25 peaks, each sharper than the last from ATAP, Chapter 18:
    case 'spikycomb'
        fa = @(x) exp(x).*sech(4*sin(40*x)).^exp(x);
        f = chebfun(fa);

    % max(wiggly, x/10):
    case 'stegosaurus'
        fa = @(x) max(sin(x)+sin(x.^2), x/10);
        f = chebfun(fa, [0 10], 'splitting', 'on');
        ylims = [-.2 2.2];

    % Chebyshev-Vandermonde quasimatrix:
    case 'vandercheb'
        f = chebpoly(0:5);
        f = simplify(cheb2quasi(f));
        fa = @(x) f(x);
        ylims = [-1.2 1.2];

    % Vandermonde quasimatrix:
    case 'vandermonde'
        fa = @(x) x.^(0:5);
        f = chebfun(fa, 'vectorize');
        f = simplify(cheb2quasi(f));
        ylims = [-1.2 1.2];

    % One of the Chebfun team's favorites:
    case 'wiggly'
        fa = @(x) sin(x) + sin(x.^2);
        f = chebfun(fa, [0 10]);
        ylims = [-2.4 2.4];

    % An example from one of the first Chebfun papers (Trefethen, 2007):
    case 'wild'
        fa = @(x) wild(x); % Defined below.
        f = chebfun(fa, [-1 1]);
        ylims = [5.5 9.5];

    % Degree 10000 polynomial that looks piecewise linear from ATAP appendix:
    case 'zigzag'
        f = cumsum(chebfun(@(t) sign(sin(100*t./(2-t))), 10000));
        fa = @(x) f(x);

    % Raise an error if the input is unknown.
    otherwise
        error('CHEB:GALLERY:unknown:unknownFunction', ...
            'Unknown function.')
end

% Only return something if there is an output argument.
if ( nargout > 0 )
    varargout = {f, fa};
else
    % Otherwise, plot the function.
    plot(f)
    title([name ', length = ' num2str(length(f))])
    if ( ~isempty(ylims) )
        ylim(ylims)
    end
    if ( ~isempty(axispref) )
        axis(axispref{:})
    end
    shg
end

end

function f = daubechies(n)
% nth order polynomial approximation to Daubechies scaling function phi_2
[x, phi] = daub(n);
f = chebfun([phi'; 0], [0 3], 'equi');

    function [x, phi] = daub(n)
        s = sqrt(3);
        if ( n == 0 )
            % Base case of recursion.
            x = 0:2;
            phi = [0, (1+s)/2, (1-s)/2];
        else
            c = [1+s, 3+s, 3-s, 1-s]/4;
            % Recursively call the daub() method.
            [x2, phi2] = daub(n-1);
            pp = [phi2, 0*phi2];
            N = 2^(n-1);
            ii = 1:6*N;
            phi = c(1)*pp(ii);
            for k = 2:4
                ii = ii([(5*N + 1):6*N, 1:5*N]);
                phi = phi + c(k)*pp(ii);
            end
            x = [x2, x2+3]/2;
        end
    end

end

function s = wild(x)
% The 'wild' function from Computing Numerically with Functions, Trefethen 2007.
f = sin(pi*x);
s = f;
for j = 1:15
    f = (3/4)*(1 - 2*f.^4);
    s = s + f;
end
end


% 2D Lattice Boltzmann (BGK) model of a fluid.
%  c4  c3   c2    D2Q9 model. At each timestep, particle densities propagate
%    \  |  /      outwards in the directions indicated in the figure. An
%  c5 -c9 - c1    equivalent 'equilibrium' density is found, and the densities
%    /  |  \      relax towards that state, in a proportion governed by omega.
%  c6  c7   c8
%                  Iain Haslam, April 2006.

omega=1.0; density=1.0; t1=4/9; t2=1/9; t3=1/36; c_squ=1/3; nx=1; ny=101;
F=repmat(density/9,[nx ny 9]); F_EQ=F; msize=nx*ny; qi=[0:msize:msize*7];
bound=zeros(nx,ny); bound(:,[1 ny])=1;%bound([14:19],[14:16])=1;
obs=find(bound);
bounds_to_bounce=[obs+qi(1) obs+qi(2) obs+qi(3) obs+qi(4) ...
	obs+qi(5) obs+qi(6) obs+qi(7) obs+qi(8)];
bounds_bounced=[obs+qi(5) obs+qi(6) obs+qi(7) obs+qi(8) ...
	obs+qi(1) obs+qi(2) obs+qi(3) obs+qi(4)];
avu=1; prevavu=1; ts=0; deltaU=1e-9; numactivenodes=sum(sum(1-bound));

while (ts<100000 & 1e-10<abs((prevavu-avu)/avu)) | ts<100
    ts=ts+1;

    % Propagate
	F(:,:,4)=F([2:nx 1],[ny 1:ny-1],4);    F(:,:,3)=F(:,[ny 1:ny-1],3);
	F(:,:,2)=F([nx 1:nx-1],[ny 1:ny-1],2); F(:,:,5)=F([2:nx 1],:,5);
	F(:,:,1)=F([nx 1:nx-1],:,1);           F(:,:,6)=F([2:nx 1],[2:ny 1],6);
	F(:,:,7)=F(:,[2:ny 1],7);              F(:,:,8)=F([nx 1:nx-1],[2:ny 1],8);
	BOUNCEDBACK=F(bounds_to_bounce); %Densities bouncing back at next timestep

    % Relax; calculate equilibrium state with equivalent speed and density to F
	DENSITY = sum(F,3);
	UX=(sum(F(:,:,[1 2 8]),3)-sum(F(:,:,[4 5 6]),3))./DENSITY;
	UY=(sum(F(:,:,[2 3 4]),3)-sum(F(:,:,[6 7 8]),3))./DENSITY;

	%UX(1,2:ny-1)=UX(1,2:ny-1)+deltaU;
	UX=UX+deltaU;
	UX(obs)=0; UY(obs)=0; DENSITY(obs)=0;
	U_SQU=UX.^2+UY.^2; U_C2=UX+UY; U_C4=-UX+UY; U_C6=-U_C2; U_C8=-U_C4;

	% stationary
	F_EQ(:,:,9)=t1*DENSITY.*(1-U_SQU/(2*c_squ));

	% nearest-neighbours
	F_EQ(:,:,1)=t2*DENSITY.*(1+UX/c_squ+0.5*(UX/c_squ).^2-U_SQU/(2*c_squ));
	F_EQ(:,:,3)=t2*DENSITY.*(1+UY/c_squ+0.5*(UY/c_squ).^2-U_SQU/(2*c_squ));
	F_EQ(:,:,5)=t2*DENSITY.*(1-UX/c_squ+0.5*(UX/c_squ).^2-U_SQU/(2*c_squ));
	F_EQ(:,:,7)=t2*DENSITY.*(1-UY/c_squ+0.5*(UY/c_squ).^2-U_SQU/(2*c_squ));

	% next-nearest neighbours
	F_EQ(:,:,2)=t3*DENSITY.*(1+U_C2/c_squ+0.5*(U_C2/c_squ).^2-U_SQU/(2*c_squ));
	F_EQ(:,:,4)=t3*DENSITY.*(1+U_C4/c_squ+0.5*(U_C4/c_squ).^2-U_SQU/(2*c_squ));
	F_EQ(:,:,6)=t3*DENSITY.*(1+U_C6/c_squ+0.5*(U_C6/c_squ).^2-U_SQU/(2*c_squ));
	F_EQ(:,:,8)=t3*DENSITY.*(1+U_C8/c_squ+0.5*(U_C8/c_squ).^2-U_SQU/(2*c_squ));
	F=omega*F_EQ+(1-omega)*F;
	F(bounds_bounced)=BOUNCEDBACK;
	prevavu=avu;avu=sum(sum(UX))/numactivenodes; ts=ts+1;
end
%The bounceback boundary condition means that the actual boundary lies
%mid-way between the open and closed nodes. The width of the channel is
%therefore ny-2.
L=(ny-2)/2; arrindices=[-(L-0.5):(L-0.5)];     EXACT=-(arrindices.^2-L^2);
tau = 1/omega;

%Calculate the analytical solution's peak velocity
temp_F         = density * deltaU / tau;
temp_viscosity = (tau-0.5)/3;
calcfactor     = temp_F/(2*temp_viscosity);
CALC           = [UX(1,2:ny-1)];
EXACT          = EXACT*calcfactor;
figure;
plot(arrindices,abs(1-(EXACT./CALC)));
maxerror=max(abs(1-(EXACT./CALC)));
title('Relative error of LBM solution across channel width');
fprintf('RESULTS\n')
fprintf('ts=%d, maxerror=%g\n',ts, maxerror);

%Calculate relative proportional error in centre of channel
[maxcalc, maxindex] = max(CALC);
maxerrorcentre = (EXACT(maxindex)-CALC(maxindex))/EXACT(maxindex)
figure;plot(arrindices,EXACT);
hold on;
plot(arrindices,CALC,'o');
title('Comparison of analytical with LBM results');
xlabel('location in channel'); ylabel('speed')


function D=Delta(Time_to_Maturity,Stock_Value,Stock_Volatility,Strike,Riskfree_Rate)

Time_to_Maturity=max(Time_to_Maturity,10^(-9));  % avoid division by zero message

d1 = 1/(Stock_Volatility*sqrt(Time_to_Maturity))*( log(Stock_Value/Strike) + ...
                (Riskfree_Rate* Stock_Volatility^2/2)*Time_to_Maturity  );

D =normcdf(d1,0,1);


function c=ref_dstiii(f)
%REF_DSTIII  Reference Discrete Sine Transform type III
%   Usage:  c=ref_dstiii(f);
%
%   This is the inverse of DSTII

L=size(f,1);
W=size(f,2);

% Create weights.
w=ones(L,1);
w(L)=1/sqrt(2);
w=w*sqrt(2/L);

% Create transform matrix.
F=zeros(L);

for m=0:L-1
  for n=0:L-1
    F(m+1,n+1)=w(n+1)*sin(pi*(n+1)*(m+.5)/L);
  end;
end;

% Compute coefficients.
c=F*f;


function y = tapas_autocorr(x)
% USAGE:
%     Y = tapas_autocorr(X)
%
% INPUT:
%     X - n-by-m matrix of m time series (columns) of length n
%
% OUTPUT:
%     Y - n-by-m matrix with m columns of autocorrelation coefficients for lags n-1
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2016 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Length of time series
n = size(x,1);

% De-mean time series
x = x - ones(size(x))*diag(mean(x));

% Get the autocovariance
f = fft(x);
fsq = f.*conj(f);
y = ifft(fsq)/n;

% Get the autocorrelation (the next line is equivalent to y = y*diag(1./y(1,:));)
y = y*diag(1./var(x,1));

end


function legendre_exactness ( n, x, w, p_max )

%*****************************************************************************80
%
%% LEGENDRE_EXACTNESS investigates exactness of Legendre quadrature.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points in the rule.
%
%    Input, real X(N), the quadrature points.
%
%    Input, real W(N), the quadrature weights.
%
%    Input, integer P_MAX, the maximum exponent.
%    0 <= P_MAX.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Quadrature rule for Legendre integral.\n' );
  fprintf ( 1, '  Rule of order N = %d\n', n );
  fprintf ( 1, '  Degree          Relative Error\n' );
  fprintf ( 1, '\n' );

  for p = 0 : p_max

    s = legendre_integral ( p );

    v(1:n,1) = x(1:n) .^ p;

    q = w' * v;

    if ( s == 0.0 )
      e = abs ( q - s );
    else
      e = abs ( ( q - s ) / s );
    end

    fprintf ( 1, '  %6d  %24.16f\n', p, e );

  end

  return
end


function plterrel(xo,yo,C,s,ltype)
% PLTERREL  Plots an error ellipse on screen.
%   Note 1: x & y represent north & south (opposite of
%   normal MatLab convention).  Note 2: use a square
%   aspect ratio for plot; i.e., use axis('square')
%   command.  Non-vectorized.
% Version: 18 Jan 96
% Useage:  plterrel(xo,yo,C,s,ltype)
% Input:   xo - x (north) origin of ellipse
%          yo - y (east) origin of ellipse
%          C  - covariance matrix (2x2)
%          s  - ellipse scale factor (default=1)
%          ltype - line type for error ellipse
%                  (default=solid,red)
% Output:  Plotted error ellipse centred at (xo,yo)

% Copyright (c) 2011, Michael R. Craymer
% All rights reserved.
% Email: mike@craymer.com

if nargin<3
  error('Too few input arguements');
end
if nargin<4
  s=1;             % Define default scale factor
end
if nargin<5
  ltype='-r';      % Define default color red
end
dt=0.1;            % Angular resolution
t=[(0:dt:2*pi)';0];
[V,D]=eig(C);      % Eigenvalues & vectors
r=sqrt(diag(D));   % Length of axes
x=s*r(1)*cos(t);
y=s*r(2)*sin(t);
xx=xo+[x y]*V(1,:)';
yy=yo+[x y]*V(2,:)';
plot(yy,xx,ltype);  % Switch x (north) & y (east)


function  fa = cmd3(a, om, omt)
% calculate the growth factor using 1/(ah)^3
% t0 is (1 - om)/om with om the relative matter density

%fa = (a ./ (1 + a .^3 * t0)) .^ 1.5;

fa = (a ./ (om + (omt - om) .* a.^3  + (1 - omt) .* a))  .^1.5;


function [intersects, edgeIndices] = intersectLinePolyline(line, poly, varargin)
%INTERSECTLINEPOLYLINE Intersection points between a line and a polyline.
%
%   P = intersectLinePolyline(LINE, POLY)
%   Returns the intersection points of the lines LINE with polyline POLY.
%   LINE is a 1-by-4 row vector containing parametric representation of the
%   line (in the format [x0 y0 dx dy], see the function 'createLine' for
%   details).
%   POLY is a NV-by-2 array containing coordinates of the polyline vertices
%   P is a K-by-2 array containing the coordinates of the K intersection
%   points.
%
%   P = intersectLinePolyline(LINE, POLY, TOL)
%   Specifies the tolerance for geometric tests. Default is 1e-14.
%
%   [P INDS] = intersectLinePolyline(...)
%   Also returns the indices of edges involved in intersections. INDS is a
%   K-by-1 column vector, such that P(i,:) corresponds to intersection of
%   the line with the i-th edge of the polyline. If the intersection occurs
%   at a polyline vertex, the index of only one of the two neighbor edges
%   is returned.
%   Note that due to numerical approximations, the use of function
%   'isPointOnEdge' may give results not consistent with this function.
%
%
%   Examples
%   % compute intersections between a square and an horizontal line
%     poly = [0 0;10 0;10 10;0 10];
%     line = [5 5 1 0];
%     intersectLinePolyline(line, poly)
%     ans =
%           10     5
%     % also return indices of edges
%     [inters inds] = intersectLinePolyline(line, poly)
%     inters =
%           10     5
%     inds =
%           2
%
%   % compute intersections between a square and a diagonal line
%     poly = [0 0;10 0;10 10;0 10];
%     line = [5 5 1 1];
%     intersectLinePolyline(line, poly)
%     ans =
%            0     0
%           10    10
%
%   See also
%   lines2d, polylines2d, intersectLines, intersectLinePolygon
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2003-10-31
% Copyright 2003-2022 INRA - TPV URPOI - BIA IMASTE

% get computation tolerance
tol = 1e-14;
if ~isempty(varargin)
    tol = varargin{1};
end

% create the array of edges
N = size(poly, 1);
edges = [poly(1:N-1, :) poly(2:N, :)];

% compute intersections with supporting lines of polyline edges
supportLines = edgeToLine(edges);
intersects = intersectLines(line, supportLines, tol);

% find edges that are not parallel to the input line
inds = find(isfinite(intersects(:, 1)));

% compute position of intersection points on corresponding lines
pos = linePosition(intersects(inds, :), supportLines(inds, :), 'diag');

% and keep only intersection points located on edges
b = pos > -tol & pos < 1+tol;
inds = inds(b);
intersects = intersects(inds, :);

% remove multiple vertices (can occur for intersections located at polyline
% vertices)
[intersects, I, J] = unique(intersects, 'rows'); %#ok<ASGLU>

if nargout > 1
    % return indices of edges involved in intersection
    % (in case of intersection located at a vertex, only one of the
    % neighbor edges is returned)
    edgeIndices = inds(I);
end


function [N, D] = ratpolyfit(x,y,kn,kd)
% Ratpolyfit Rational Polynomial Fitting
%
%  This programs finds 2 polynomials N(x) and D(x),
%  of user given order kn and kd respectively,
%  such that N(xi)/D(xi) ~= y(xi) in a least squares sense.
%
%usage: [N, D] = ratpolyfit(x, y, kn, kd)
%
%note: kn and kd must be large enough to get a good fit.
%      usually, kn=kd gives good results
%
%note: If you "overfit" the data, then you will usually have pole-zero
%      cancellations and/or poles and zeros with a very large magnitude.
%      If that happens, then reduce the values of kn and/or kd
%
%note: Often, if you have a good fit, you will find that your polynomials
%      have roots where the real function has zeros and poles.
%
%note: Polynomial curve fitting becomes ill conditioned
%      as the range of x increases and as kn and kd increase
%
%note: If you think that your function goes to infinity at some x value
%      then make sure y(xi) is set equal to Inf at that point.
%      The program will compensate for all +/- Inf values
%
%For example, here's a rational polynomial approximation to the Gamma function:
%   x=[-5:1/32:5]'; y=gamma(x);
%   [N, D]=ratpolyfit(x,y,10,10);
%   figure(1); plot(roots(N),'ob'); hold on; plot(roots(D),'xr'); grid on
%   yy=polyval(N,x)./polyval(D,x);
%   figure(2);plot(x,y,'b', x,yy,'dr'); grid on; axis([min(x) max(x) -25 25]);
%
%other demos are in the text listing at the end of this program
%
%tested under version 6.5 (R13)
%
%see also: polyfit, padefit, polyval, vander
%

%Paul Godfrey
%pgodfrey@conexant.com
%May 31, 2006

%default length if none given
if exist('kn','var')
   kn=round(real(kn));
   if kn<0, kn=0; end
else
   kn=5;
end
if exist('kd','var')
   kd=round(real(kd));
   if kd<0, kd=0;end
else
   kd=5;
end

x=x(:);
y=y(:);

% we must remove +/- Inf values from y first
% and insert those as separate poles at the end
p=find(~isfinite(y)); % find Nan 0/0 and Inf a/0 values
% or find abs(y) values > than some really big number

dinf=[];
while length(p)>0
   y=y.*(x-x(p(1))); %adjust remaining y values
   y(p(1))=[]; % remove bad y value, now a NaN
   dinf=[dinf; x(p(1))]; % remember where pole was
   x(p(1))=[]; % now remove that x value too
   if kd>0, kd=kd-1; end; %reduce expected order of den
   p=find(~isfinite(y)); % have all Inf values been removed yet?
end

yy=length(y);
an=ones(yy,kn+1);
for k=kn:-1:1
    an(:,k)=x.*an(:,k+1); % form vandermonde matrix
end

ad=ones(yy,kd+1);
for k=kd:-1:1
    ad(:,k)=x.*ad(:,k+1);
end
for k=1:yy
    ad(k,:)=y(k)*ad(k,:);
end

% A is basically N-y*D
A=[an -ad]; % LS solution is in the null space of A

[u,s,v]=svd(A); % null space is in the cols of V
ND=v(:,end); % use the "most null" vector

N=ND(1:kn+1).';
D=ND(kn+2:end).';

D1=D(1);
if D1==0; D1=1; end
% we have to make the D polynomial monic since thats
% what poly makes, so we have to first adjust N
N=N/D1;
D=D/D1;
% and then add the removed +/- Inf poles back in
D=poly([dinf; roots(D)]);

Dmax=max(abs(D));
if Dmax==0; Dmax=1; end
% normalize max Den value to be +/- 1
N=N/Dmax;
D=D/Dmax;

return

%a demo of this program is
clc
clear all
close all
x=[-5:1/16:5]';

y=gamma(x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(1);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -15 15]);

y=erf(x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(2);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -1.5 1.5]);

y=1./(1+x.*x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(3);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) 0 1]);

y=tan(x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(4);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -5 5]);

y=exp(-x.*x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(5);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) 0 1]);

y=zeta(x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(6);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -2 2]);

y=psi(x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(7);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -5 5]);

x=x-min(x);

y=log(x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(8);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -5 5]);

x=(x-min(x))*pi/2;

y=besselj(0,x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(9);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -0.5 1]);

y=sin(x)./x;
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(10);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -0.5 1]);

return


function [ nin, pi ] = triangle_line_imp_int_2d ( a, b, c, t, nin, pi )

%*****************************************************************************80
%
%% TRIANGLE_LINE_IMP_INT_2D finds where an implicit line intersects a triangle in 2D.
%
%  Discussion:
%
%    An implicit line is the set of points ( X, Y ) satisfying
%
%      A * X + B * Y + C = 0
%
%    where at least one of A and B is not zero.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, determine the equation of the line:
%    A*X + B*Y + C = 0.
%
%    Input, real T(2,3), the triangle vertices.
%
%    Output, integer NIN, the number of points of intersection
%    of the line with the triangle.  NIN may be 0, 1, 2 or 3.
%
%    Output, real PI(2,3), contains the intersection points.
%
  dim_num = 2;

  nin = 0;

  for i = 1 : 3

    j = i4_wrap ( i+1, 1, 3 );
%
%  Get the implicit form of the line through vertices I and I+1.
%
    [ a1, b1, c1 ] = line_exp2imp_2d ( t(1:2,i), t(1:2,j) );
%
%  Seek an intersection with the original line.
%
    [ ival, p ] = lines_imp_int_2d ( a, b, c, a1, b1, c1 );
%
%  If there is an intersection, determine if it lies between the two vertices.
%
    if ( ival == 1 )

      test1 = ( p(1:dim_num)'  - t(1:dim_num,i) )' ...
        * ( t(1:dim_num,j) - t(1:dim_num,i) );
      test2 = ( t(1:dim_num,j) - t(1:dim_num,i) )' ...
        * ( t(1:dim_num,j) - t(1:dim_num,i) );

      if ( 0 <= test1 & test1 <= test2 )
        nin = nin + 1;
        pi(1:dim_num,nin) = p(1:dim_num)';
      end

    end

  end

  return
end


function coeffs = fitPolynomialTransform2d(pts, ptsRef, degree)
%FITPOLYNOMIALTRANSFORM2D Coefficients of polynomial transform between two point sets.
%
%   COEFFS = fitPolynomialTransform2d(PTS, PTSREF, DEGREE)
%
%   Example
%
%   See also
%     polynomialTransform2d, fitAffineTransform2d

% ------
% Author: David Legland
% E-mail: david.legland@nantes.inra.fr
% Created: 2013-11-05, using Matlab 7.9.0.529 (R2009b)
% Copyright 2013-2022 INRA - Cepia Software Platform

%% Extract data

% ensure degree is valid
if nargin < 3
    degree = 3;
end

% polygon coordinates
xi = pts(:,1);
yi = pts(:,2);
nCoords = size(pts, 1);

% check inputs have same size
if size(ptsRef, 1) ~= nCoords
    error('fitPolynomialTransform2d:sizeError', ...
        'input arrays must have same number of points');
end

%% compute coefficient matrix

% number of coefficients of polynomial transform
nCoeffs = prod(degree + [1 2]) / 2;

% initialize matrix
A1 = zeros(nCoords, nCoeffs);

% iterate over degrees
iCoeff = 0;
for iDegree = 0:degree

    % iterate over binomial coefficients of a given degree
    for k = 0:iDegree
        iCoeff = iCoeff + 1;
        A1(:, iCoeff) = ones(nCoords, 1) .* power(xi, iDegree-k) .* power(yi, k);
    end
end

% concatenate matrix for both coordinates
A = kron(A1, [1 0;0 1]);

%% solve linear system that minimizes least squares

% create the vector of expected values
b = ptsRef';
b = b(:);

% solve the system
coeffs = (A \ b)';


function [ nfactor, factor, exponent, nleft ] = i4_factor ( n )

%*****************************************************************************80
%
%% I4_FACTOR factors an integer into prime factors.
%
%  Formula:
%
%    N = NLEFT * Product ( 1 <= I <= NFACTOR ) FACTOR(I)**EXPONENT(I).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 July 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the integer to be factored.  N may be positive,
%    negative, or 0.
%
%    Output, integer NFACTOR, the number of prime factors of N discovered
%    by the routine.
%
%    Output, integer FACTOR(NFACTOR), the prime factors of N.
%
%    Output, integer EXPONENT(NFACTOR).  EXPONENT(I) is the power of
%    the FACTOR(I) in the representation of N.
%
%    Output, integer NLEFT, the factor of N that the routine could not
%    divide out.  If NLEFT is 1, then N has been completely factored.
%    Otherwise, NLEFT represents factors of N involving large primes.
%
  nfactor = 0;

  factor = [];
  exponent = [];

  nleft = n;

  if ( n == 0 )
    return
  end

  if ( abs ( n ) == 1 )
    nfactor = 1;
    factor(1) = 1;
    exponent(1) = 1;
    return;
  end
%
%  Find out how many primes we stored.
%
  maxprime = prime ( -1 );
%
%  Try dividing the remainder by each prime.
%
  for i = 1 : maxprime

    p = prime ( i );

    if ( mod ( abs ( nleft ), p ) == 0 )

      nfactor = nfactor + 1;
      factor(nfactor) = p;
      exponent(nfactor) = 0;

      while ( 1 )

        exponent(nfactor) = exponent(nfactor) + 1;
        nleft = floor ( nleft / p );

        if ( mod ( abs ( nleft ), p ) ~= 0 )
          break;
        end

      end

      if ( abs ( nleft ) == 1 )
        break;
      end

    end

  end

  return
end


% function to implement the Iterated Extended Kalman Filter (IEKF)
% Inputs:
%  OBSn - the observations (with noise)
%  xest - initial state space estimates
% Ouputs:
%  Xp - predicted states
function Xp = f_IEKF(OBSn,xest)

load avar % r1,r2, L, and T

tol = .1; % tolerance for iterations
diff = 1;
count = 0;

F = [1 T 0 0 0 0 0;
    0 1 0 0 0 0 0;
    0 0 1 T 0 0 0;
    0 0 0 1 0 0 0;
    0 0 0 0 1 0 T;
    0 0 0 0 0 1 T;
    0 0 0 0 0 0 1]; % state transition matrix

n = size(OBSn,2); % number of observations

Xp = zeros(7,n);  % make room

Pkp1 = 1e10*eye(7); %xest*xest'; %.1*ones(7,7);

%Pkp1 = xest*xest';
%Pkp1 = F*Pkp1*F';

%Pkp1 = (10*randn(7,1))*(10*randn(7,1)).';
%Pkp1 = F*Pkp1*F';

% for each observation
for i = 1:n;

    % if this is the first iteration the prior predicted estimate is xest
    % if this is not the first run the prior estimate is in Xp
    if i == 1
        xkm1 = xest;
    else
        xkm1 = Xp(:,i-1);
    end

    Pkm1 = Pkp1; % conditional covariance from last iteration

    % iterations are started with the predicted estimate from the last run
    xkn = xkm1;
    while ~(diff < tol || count > 9)

        count = count + 1;

        H = [gradest(@(x)f_h1(x),xkn); gradest(@(x)f_h2(x),xkn)];

        R = (.01*randn(2,1))*(.01*randn(2,1)).';

        Rdiag = diag(R); R = diag(Rdiag);

        K = Pkm1*H'*(H*Pkm1*H'+R)^-1;

        xkn_temp = xkm1 + K*(OBSn(:,i)-f_h(xkn)-H*(xkm1-xkn));

        diff = norm(abs(xkn_temp-xkn));

        fprintf('diff = %g \n',diff)

        xkn = xkn_temp;

    end

    H = [gradest(@(x)f_h1(x),xkn); gradest(@(x)f_h2(x),xkn)];

    Pkk = (eye(7)-K*H)*Pkm1;

    Pkp1 = F*Pkk*F';

    Xp(:,i) = F*xkn;

    clc;

    fprintf('i = %g; Count is %g \n',i,count)

    count = 0;

    diff = 1;

end


function inv = cauchy_inv(x,m,s)
%---------------------------------------------------
% PURPOSE:
% Estimates the Inverse of the Cumulative Distribution Function of the
% Cauchy-Lorentz Distribution for a series of x values, with m location
% parameter and s scale parameter
%---------------------------------------------------
% USAGE:
% inv = cauchy_inv(x,m,s)
% where:
% x = (n x 1) or (1 x n) vector
% m = (n x 1) or (1 x n) vector
% s = (n x 1) or (1 x n) vector
%---------------------------------------------------
% RETURNS:
% cdf = (n x 1) or (1 x n) vector containing the probability for each
% element of x with corresponding location and scale parameters
%---------------------------------------------------
% Author:
% Alexandros Gabrielsen, a.gabrielsen@city.ac.uk
% Date: 06/2010
%---------------------------------------------------

if nargin == 0
    error('Data, Location Parameter, Scale Parameter')
end

inv = m + s.*tan(pi*(x-1/2));

end


function N = R1(rad)
% Suppose right handed coordinate frames A and B have the same x axis,
% and rotate the A frame along the x axis by rad obtains the B frame, then
% a point in B, p_B, can be transformed from its coordinates in A frame by
% p_B = R1(rad) * p_A.
N = [1 0 0;0 cos(rad) sin(rad);0 -sin(rad) cos(rad)];


function posteriori = slposterioritrue(condprops, nums, priori, op)
%SLPOSTERIORITRUE Computes the posteriori that samples belong to true class
%
% $ Syntax $
%   - posteriori = slposterioritrue(condprops, nums, priori)
%   - posteriori = slposterioritrue(condprops, nums, priori, 'log')
%
% $ Arguments $
%   - condprods:      the conditional probabilities of classes: C x n
%   - nums:           the number of samples belong to the classes: 1 x C
%   - priori:         the prior probabilities of classes: 1 x C
%   - posteriori:     the resulting posterior probabilities: 1 x n
%
% $ Description $
%   - posteriori = slposteriori(condprops, nums, priori) Computes the
%     posterior probability that the samples belong to the true class
%     according to the given conditional probabilities of all samples
%     to all classes and the priori of the classes.
%     In the input argument, each column in condprops represent the
%     conditional probabilities of that sample belong to all the
%     C classes. The samples from the same underlying classes should be
%     put together and the ownership is given by nums.
%     The priori can be given by an 1 x C row vector or [], which
%     means that all classes have equal prior.
%
%   - posteriori = slposterioritrue(condprops, nums, priori, 'log') means
%     that the input condprops are given by its logarithm.
%
% $ History $
%   - Created by Dahua Lin, on Sep 16th, 2006
%

%% parse and verify input arguments

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

if ~isnumeric(condprops) || ndims(condprops) ~= 2
    error('sltoolbox:invalidarg', ...
        'The condprops should be a 2D numeric matrix');
end
[C, n] = size(condprops);

if ~isequal(size(nums), [1, C])
    error('sltoolbox:sizmismatch', ...
        'The nums should be an 1 x C row vector');
end
if nargin < 3 || isempty(priori)
    priori = [];
else
    if ~isequal(size(priori), [1, C])
        error('sltoolbox:sizmismatch', ...
            'The priori should be a an 1 x C row vector');
    end
end

if nargin >= 4 && strcmpi(op, 'log')
    is_log = true;
else
    is_log = false;
end

%% compute

if is_log
    if isempty(priori)
        P = sladdvec(condprops, -max(condprops, [], 1), 2);
    else
        P = sladdvec(condprops, log(priori)', 1);
        P = sladdvec(P, -max(condprops, [], 1), 2);
    end
    P = exp(P);
else
    if isempty(priori)
        P = condprops;
    else
        P = slmulvec(condprops, priori', 1);
    end
end

tP = sum(P, 1);
[sp, ep] = slnums2bounds(nums);
posteriori = zeros(1, n);

for k = 1 : C
    sk = sp(k);
    ek = ep(k);
    posteriori(sk:ek) = P(k, sk:ek);
end
posteriori = posteriori ./ tP;


function [center,rad,v1n,v2nb] = circlefit3d(p1,p2,p3)
% circlefit3d: Compute center and radii of circles in 3d which are defined by three points on the circumference
% usage: [center,rad,v1,v2] = circlefit3d(p1,p2,p3)
%
% arguments: (input)
%  p1, p2, p3 - vectors of points (rowwise, size(p1) = [n 3])
%               describing the three corresponding points on the same circle.
%               p1, p2 and p3 must have the same length n.
%
% arguments: (output)
%  center - (nx3) matrix of center points for each triple of points in p1,  p2, p3
%
%  rad    - (nx1) vector of circle radii.
%           if there have been errors, radii is a negative scalar ( = error code)
%
%  v1, v2 - (nx3) perpendicular vectors inside circle plane
%
% Example usage:
%
%  (1)
%      p1 = rand(10,3);
%      p2 = rand(10,3);
%      p3 = rand(10,3);
%      [center, rad] = circlefit3d(p1,p2,p3);
%      % verification, result should be all (nearly) zero
%      result(:,1)=sqrt(sum((p1-center).^2,2))-rad;
%      result(:,2)=sqrt(sum((p2-center).^2,2))-rad;
%      result(:,3)=sqrt(sum((p3-center).^2,2))-rad;
%      if sum(sum(abs(result))) < 1e-12,
%       disp('All circles have been found correctly.');
%      else,
%       disp('There had been errors.');
%      end
%
%
% (2)
%       p1=rand(4,3);p2=rand(4,3);p3=rand(4,3);
%       [center,rad,v1,v2] = circlefit3d(p1,p2,p3);
%       plot3(p1(:,1),p1(:,2),p1(:,3),'bo');hold on;plot3(p2(:,1),p2(:,2),p2(:,3),'bo');plot3(p3(:,1),p3(:,2),p3(:,3),'bo');
%       for i=1:361,
%           a = i/180*pi;
%           x = center(:,1)+sin(a)*rad.*v1(:,1)+cos(a)*rad.*v2(:,1);
%           y = center(:,2)+sin(a)*rad.*v1(:,2)+cos(a)*rad.*v2(:,2);
%           z = center(:,3)+sin(a)*rad.*v1(:,3)+cos(a)*rad.*v2(:,3);
%           plot3(x,y,z,'r.');
%       end
%       axis equal;grid on;rotate3d on;
%
%
% Author: Johannes Korsawe
% E-mail: johannes.korsawe@volkswagen.de
% Release: 1.0
% Release date: 26/01/2012

% Default values
center = [];rad = 0;v1n=[];v2nb=[];

% check inputs
% check number of inputs
if nargin~=3,
    fprintf('??? Error using ==> cirlefit3d\nThree input matrices are needed.\n');rad = -1;return;
end
% check size of inputs
if size(p1,2)~=3 || size(p2,2)~=3 || size(p3,2)~=3,
    fprintf('??? Error using ==> cirlefit3d\nAll input matrices must have three columns.\n');rad = -2;return;
end
n = size(p1,1);
if size(p2,1)~=n || size(p3,1)~=n,
    fprintf('??? Error using ==> cirlefit3d\nAll input matrices must have the same number or rows.\n');rad = -3;return;
end
% more checks are to follow inside calculation

% Start calculation
% v1, v2 describe the vectors from p1 to p2 and p3, resp.
v1 = p2 - p1;v2 = p3 - p1;
% l1, l2 describe the lengths of those vectors
l1 = sqrt((v1(:,1).*v1(:,1)+v1(:,2).*v1(:,2)+v1(:,3).*v1(:,3)));
l2 = sqrt((v2(:,1).*v2(:,1)+v2(:,2).*v2(:,2)+v2(:,3).*v2(:,3)));
if find(l1==0) | find(l2==0), %#ok<OR2>
    fprintf('??? Error using ==> cirlefit3d\nCorresponding input points must not be identical.\n');rad = -4;return;
end
% v1n, v2n describe the normalized vectors v1 and v2
v1n = v1;for i=1:3, v1n(:,i) = v1n(:,i)./l1;end
v2n = v2;for i=1:3, v2n(:,i) = v2n(:,i)./l2;end
% nv describes the normal vector on the plane of the circle
nv = [v1n(:,2).*v2n(:,3) - v1n(:,3).*v2n(:,2) , v1n(:,3).*v2n(:,1) - v1n(:,1).*v2n(:,3) , v1n(:,1).*v2n(:,2) - v1n(:,2).*v2n(:,1)];
if find(sum(abs(nv),2)<1e-5),
    fprintf('??? Warning using ==> cirlefit3d\nSome corresponding input points are nearly collinear.\n');
end
% v2nb: orthogonalization of v2n against v1n
dotp = v2n(:,1).*v1n(:,1) + v2n(:,2).*v1n(:,2) + v2n(:,3).*v1n(:,3);
v2nb = v2n;for i=1:3,v2nb(:,i) = v2nb(:,i) - dotp.*v1n(:,i);end
% normalize v2nb
l2nb = sqrt((v2nb(:,1).*v2nb(:,1)+v2nb(:,2).*v2nb(:,2)+v2nb(:,3).*v2nb(:,3)));
for i=1:3, v2nb(:,i) = v2nb(:,i)./l2nb;end

% remark: the circle plane will now be discretized as follows
%
% origin: p1                    normal vector on plane: nv
% first coordinate vector: v1n  second coordinate vector: v2nb

% calculate 2d coordinates of points in each plane
% p1_2d = zeros(n,2); % set per construction
% p2_2d = zeros(n,2);p2_2d(:,1) = l1; % set per construction
p3_2d = zeros(n,2); % has to be calculated
for i = 1:3,
    p3_2d(:,1) = p3_2d(:,1) + v2(:,i).*v1n(:,i);
    p3_2d(:,2) = p3_2d(:,2) + v2(:,i).*v2nb(:,i);
end

% calculate the fitting circle
% due to the special construction of the 2d system this boils down to solving
% q1 = [0,0], q2 = [a,0], q3 = [b,c] (points on 2d circle)
% crossing perpendicular bisectors, s and t running indices:
% solve [a/2,s] = [b/2 + c*t, c/2 - b*t]
% solution t = (a-b)/(2*c)

a = l1;b = p3_2d(:,1);c = p3_2d(:,2);
t = 0.5*(a-b)./c;
scale1 = b/2 + c.*t;scale2 = c/2 - b.*t;

% centers
center = zeros(n,3);
for i=1:3,
    center(:,i) = p1(:,i) + scale1.*v1n(:,i) + scale2.*v2nb(:,i);
end

% radii
rad = sqrt((center(:,1)-p1(:,1)).^2+(center(:,2)-p1(:,2)).^2+(center(:,3)-p1(:,3)).^2);


function [M, F, b, x, e_conn] =  assemb_co ( param )

%  The FEM equation for the steady state temperature distribution is
%
%    M_2 z^{ss} + F + b = 0

%
%% Initialization
%---------------------------------------------------------------------

  n_nodesx   =  param.nodesx;    % number of nodes in the x direction
  n_nodesy   =  param.nodesy;    % number of nodes in the y direction

  n_gauss    = 7;        % number of points used in Gauss integration
%
%%  Geometry Module
%-----------------------------------------------------------------------
%
%  Generate a mesh of the L \times W rectangle with quadratic elements
%
  n_nodes    = n_nodesx*n_nodesy;
  n_elements = 2*(n_nodesx-1)*(n_nodesy-1)/4;

  x_nodes = linspace(0.0, param.L, n_nodesx);
  y_nodes = linspace(0.0, param.W, n_nodesy);
%
%  Generate node coordinates.
%
  x = zeros(n_nodes, 2);
  for ii=1:n_nodesx
    x_i = x_nodes(ii); k = ii - n_nodesx;
    for jj=1:n_nodesy
      k = k + n_nodesx;
      x(k,:) = [ x_i  y_nodes(jj)];
    end
  end
%
%  Generate element connectivity
%
  e_conn = zeros(n_elements, 6);
  ie     = 0;
%
%  Template for odd numbered triangles
%
  elmt_odd = [ 0 ; 2+2*n_nodesx;   2*n_nodesx;
                    1+n_nodesx ; 1+2*n_nodesx; n_nodesx];
%
%  Template for even numbered triangles
%
  elmt_evn = [ 0 ;      2 ;    2+2*n_nodesx;
                         1;    2+n_nodesx ;    1+n_nodesx];

  for jj=1:2:n_nodesy-1
    for ii=1:2:n_nodesx-1
      k = ii + (jj-1)*n_nodesx;
      ie = ie + 1;
      e_conn(ie,:) = k + elmt_odd;
      ie = ie + 1;
      e_conn(ie,:) = k + elmt_evn;
    end
  end
%
%  Set up arrays for elements with faces on left/right boundary.
%
  Omega_left  =            1:(n_nodesx-1):n_elements;
  Omega_right = (n_nodesx-1):(n_nodesx-1):n_elements;
%
%  With Neumann/Robins bc all nodes are unknown.
%
  n_equations = n_nodes;
%
%  Begin the SPMD block:
%
  spmd
%
%  Set up codistributed structure
%
%  Column pointers and such for codistributed arrays.
%
    Vc  = codistributed.colon(1, n_equations);
    lP  = getLocalPart(Vc);
    lP_1 = lP(1); lP_end = lP(end); % first and last columns of M on this lab

    fid  = fopen(['out' int2str(labindex)], 'w');
    fprintf(fid, 'Lab %d : column range %d  %d \n', labindex, lP_1, lP_end);

    co_dist_Vc = getCodistributor(Vc);
    dPM = co_dist_Vc.Partition;
    col_shft = [0 cumsum(dPM(1:end-1))];
%
%  sparse arrays on each lab
%
    M_lab = sparse(n_equations, dPM(labindex));
    b_lab = sparse(dPM(labindex), 1); F_lab = b_lab;

    [n_elements, nel_dof] = size(e_conn);
    [r, s, w] = twod_gauss(n_gauss);
%
%  Build the finite element matrices - Begin loop over elements
%
    n_el_lab = 0;
    fprintf(fid, 'Lab %d : begin n_el loop \n', labindex);

    for n_el=1:n_elements
%
%  Which nodes are in this element?
%
      nodes_local = e_conn(n_el,:);
%
%  subset of nodes/columns on this lab
%
      lab_nodes_local = extract( nodes_local, lP_1, lP_end);

%     fprintf(fid, '\n \n n_el = %d \n', n_el);
%     fprintf(fid, 'lab_nodes_local has %d rows \n', size(lab_nodes_local, 1));

      if ~isempty(lab_nodes_local) % continue the calculation for this elmnt
        n_el_lab = n_el_lab + 1;
%       fprintf(fid, 'Lab %d : n_el loop location 1 \n', labindex);
%
%  coordinates, shape functions & gradients evaluated at the Gauss pts.
%
        x_local                   = x(nodes_local,:);
        [x_g, w_g, phi, p_x, p_y] = twod_shape(x_local, r, s, w);

%-------------------------------------------------------------------
%% Element contributions to the weak form of the equations
%  First we evaluate the surface integral (2D) terms
%-------------------------------------------------------------------
        M_loc =   twod_bilinear(param.k_x, p_x, p_x, w_g) + ...
                  twod_bilinear(param.k_y, p_y, p_y, w_g);
        src_g  =  source(x_g,  param); % distributed source term
        F_loc  =  twod_f_int( src_g , phi, w_g );
%
%  pre-allocate boundary arrays
%
        b_loc  =   zeros(nel_dof,1);
%       fprintf(fid, 'Lab %d : n_el loop location 2 \n', labindex);

%---------------------------------------------------------------------
%%  Add boundary terms
%---------------------------------------------------------------------
%
%  Add boundary-integral terms for left and right bounary elements
%  Left  boundary n_el = [1:2*(n_nodesx-1):n_elements
%  Right boundary n_el = [2*(n_nodesx-1):2*(n_nodesx-1):n_elements
%
        if any(Omega_left == n_el);
%
%  Here we impose Robin boundary term. The node points on the boundary
%  are nodes_local([3 6 1]) and  are at x_local([3 6 1],:)
%
          [phi_g1, ip, x_g1,  w_g1] = boundary(3, x_local, 5); % n_gauss=5
          alpha_g1 = steps(x_g1(:,2), param.yh, param.ep, param.alpha );
          phi_a    = diag(alpha_g1)*phi_g1;  % mult. each row by alpha(y_g)
          beta_g1  = steps(x_g1(:,2), param.yh, param.ep, param.beta );
%
%  1D integrals are from y = w to y = 0
%
          for ii=1:3
            for jj=1:3
              M_loc(ip(ii),ip(jj)) = M_loc(ip(ii),ip(jj)) + ...
                            oned_f_int( phi_g1(:, jj), phi_a(:,ii), w_g1 );
            end
            b_loc(ip(ii),1) = b_loc(ip(ii),1) + ...
                                  oned_f_int( beta_g1, phi_a(:,ii), w_g1 );
          end

        elseif any(Omega_right == n_el);
%
%  Here we impose boundary term for a specified flux
%  The node points on the boundary are nodes_local([2 3 5]) and
%  are at x_local([2 3 5],:)
%  We need \int f(y) \phi(L, y) dy along the right boundary.
%
          [phi_g1, ip,  ~  ,  w_g1] = boundary(2, x_local, 5); % n_gauss=5;
          for ii=1:3
             b_loc(ip(ii),1) = b_loc(ip(ii),1) + ...
                             oned_f_int( param.flux, phi_g1(:,ii), w_g1 );
          end
        end

%-----------------------------------------------------------------
%% Assemble contributions into the global system matrices
%-----------------------------------------------------------------
%  For M_lab we want all the rows (test functions) but only the
%  columns associated with nodes on this lab
%
        for n_t = 1:nel_dof               % local DOF  - test fcn
          t_glb  = nodes_local(n_t);     % global DOF - test fcn
          for n_u = 1:size(lab_nodes_local, 1)
            n_locj = lab_nodes_local(n_u, 1);  % local DOF in current n_el
            n_glbj = lab_nodes_local(n_u, 2) ...
                         - col_shft(labindex); % global DOF
            M_lab(t_glb,n_glbj) = M_lab(t_glb,n_glbj) ...
                                          +  M_loc(n_t,n_locj);
          end
%
%  Some triangles have nodes on more than one lab;
%  we want only the rows associated with nodes on this lab
%
          if t_glb >= lP_1 && t_glb <= lP_end
            t_loc = t_glb - col_shft(labindex);
            b_lab(t_loc,1)  = b_lab(t_loc,1)  + b_loc(n_t,1);
            F_lab(t_loc,1)  = F_lab(t_loc,1)  + F_loc(n_t,1);
          end

        end % for  n_t

      end % if not empty

    end  % n_el

    fprintf(fid,'Fraction of elements evaluated on this lab  is %8.6f \n',...
                                                      n_el_lab/n_elements);
%
%  Assemble the 'lab' parts in a codistributed format.
%  syntax for version R2009b
%
    codist_matrix = codistributor1d( 2, dPM, [n_equations, n_equations]);
    M = codistributed.build(M_lab, codist_matrix );
    codist_vector = codistributor1d( 1, dPM, [n_equations, 1]);
    b  = codistributed.build(b_lab , codist_vector );
    F  = codistributed.build(F_lab , codist_vector );

    fclose(fid);

  end
%
%  This terminates the SPMD block.
%
  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   [JX, Jq, Jphi]=compute_jacobians_5R(robot, q)
%
%   Compute Jacobians for the for the 5R planar parallel robot
%
%   JX is the 4x2 Jacobian with respect to the cartesian coordinates (x, y)
%   Jq is the 4x2 Jacobian with respect to the active joints (q1, q2)
%   Jphi is the 4x2 Jacobian with respect to the passive joints (phi1, phi2)
%
%   Author: Arturo Gil Aparicio arturo.gil@umh.es
%   Date: 13/09/2013
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

function [JX, Jq, Jphi]=compute_jacobians_5R(robot, q)

% The geometric parameters of this robot can be retrieved from
% the two DOF planar arms that form it
a1=eval(robot.robot1.DH.a);
a2=eval(robot.robot2.DH.a);

%Link lengths
l1=abs(a1(1));
l2=abs(a1(2));
l3=abs(a2(1));
l4=abs(a2(2));
%distance along X that separate both arms
L=robot.L;

%retrieve the active and passive joints from the vector q
q1=q(1);
phi1=q(2);
q2=q(3);
phi2=q(4);

%The loop closure equations for this robot can be written as:
%
% \begin{bmatrix}
% \Gamma_1\\
% \Gamma_2\\
% \Gamma_3\\
% \Gamma_4
% \end{bmatrix}=
% \begin{bmatrix}
% x-l_1\cos(q_1)-l_2\cos(q_1+\varphi_1)\\
% y-l_1\sin(q_1)-l_2\sin(q_1+\varphi_1)\\
% x-l_4\cos(q_2)-l_3\cos(q_2+\varphi_2)-L\\
% y-l_4\sin(q_2)-l_3\sin(q_2+\varphi_2)
% \end{bmatrix}

%In this matrix, JX11 is the partial derivative of Gamma1 with respect to x
%                JX12 is the partial derivative of Gamma1 with respect to y
%                JX21 is the partial derivative of Gamma2 with respect to x
%...
JX=[1   0;
    0   1;
    1   0;
    0   1];

%In this matrix, Jq11 is the partial der. of Gamma1 with respect to q1
%                Jq12 is the partial der. of Gamma1 with respect to q2
%                Jq21 is the partial der. of Gamma2 with respect to q1
%etc...
Jq = [l1*sin(q1)+l2*sin(q1+phi1)              0;
      -l1*cos(q1)-l2*cos(q1+phi1)             0;
      0                           l4*sin(q2)+l3*sin(q2+phi2);
      0                           -l4*cos(q2)-l3*cos(q2+phi2)];

%In this matrix, Jphi11 is the partial der. of Gamma1 with respect to phi1
%                Jphi12 is the partial der. of Gamma1 with respect to phi2
%                Jphi21 is the partial der. of Gamma2 with respect to phi1
%etc...
Jphi = [l2*sin(q1+phi1)              0;
       -l2*cos(q1+phi1)             0;
      0                           l3*sin(q2+phi2);
      0                           -l3*cos(q2+phi2)];


function   s1 = sigma_8(k, k1, psm1, h)
% sigma_8.m calculates  the integrant for the expected rms mass overdensity in a sphere of radius 8 Mpc/h.
% input : k, wavenumber in units h/Mpc and the vectors k1 resp psm1, the mass power spectrum as a
% function of k1
% output : the integrant to obtain sigma_8, the rms mass overdensity sampled with
% a sphere of radius 8/h Mpc.

% D Vangheluwe 15 july 2005

% radius of the sphere in units Mpc/h
r0 = 8/h;
x = k * r0;
% calculate the point in the mass power spectrum at k :
psm = spline(k1, psm1, k);

% calculate the integrant:
s1 = (3*(sin(x) - x .* cos(x)) ./ (x .^3)) .^2 .* psm .* k .^2;

return


function index = tileIndex3d(tile)
% Return the index of a 2-by-2-by-2 3D binary configuration tile.
%
%   INDEX = tileIndex3d(TILE)
%   Compute the index of a tile, given as a 2x2x2 binary image, by adding
%   powers of two mutliplied by values of the tile. The result is comprised
%   between 0 and 255.
%   Iterate on x, y, then z directions, corresponding to directions 2, 1,
%   and 3.
%
%   Example
%   tile = zeros([2 2 2]);
%   tile(1,1:2,1) = 1;
%   tileIndex3d(tile)
%       3
%
%   See also
%     createTile3d, tileIndex
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2009-05-25,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

tile = tile>0;
index = ...
    tile(1,1,1) + 2*tile(1,2,1) + 4*tile(2,1,1) + 8*tile(2,2,1) + ...
    16*tile(1,1,2) + 32*tile(1,2,2) + 64*tile(2,1,2) + 128*tile(2,2,2);


function fx = p03_fun ( n, x )

%*****************************************************************************80
%
%% P03_FUN evaluates the integrand for problem 3.
%
%  Discussion:
%
%    D&R gives "exact" value as 13.628...
%    Mathematica returns        13.440045415012575106...
%    D&R gives Laguerre(16) as   0.44996932...
%
%  Integral:
%
%    exp ( -2 ) Integral ( 2 <= x < +oo ) 1 / ( x^1.01 ) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 December 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N), the evaluation points.
%
%    Output, real FX(N), the function values.
%
  fx(1:n) = exp ( - 2.0 ) * 1.0 ./ x(1:n).^1.01;

  return
end


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

%% setup for figure 25.3

% generate a random signal of 3 seconds
srate = 1000;

randsig1 = randn(1,3*srate); % 3 seconds, with this sampling rate
randsig2 = randn(1,3*srate);

% now filter at 5 Hz
f       = 5; % frequency of wavelet in Hz
time    = -1:1/srate:1; % time for wavelet, from -1 to 1 second in steps of 1/sampling-rate
s       = 6/(2*pi*f); % width of Gaussian
wavelet = exp(2*pi*1i*f.*time) .* exp(-time.^2./(2*s^2));

% FFT parameters
n_wavelet            = length(wavelet);
n_data               = length(randsig1);
n_convolution        = n_wavelet+n_data-1;
half_of_wavelet_size = (length(wavelet)-1)/2;

% FFT of wavelet and EEG data
convolution_result_fft = ifft(fft(wavelet,n_convolution).*fft(randsig1,n_convolution),n_convolution)*sqrt(s)/10;
filtsig1  = real(convolution_result_fft(half_of_wavelet_size+1:end-half_of_wavelet_size));
anglesig1 = angle(convolution_result_fft(half_of_wavelet_size+1:end-half_of_wavelet_size));

convolution_result_fft = ifft(fft(wavelet,n_convolution).*fft(randsig2,n_convolution),n_convolution)*sqrt(s)/10;
filtsig2  = real(convolution_result_fft(half_of_wavelet_size+1:end-half_of_wavelet_size));
anglesig2 = angle(convolution_result_fft(half_of_wavelet_size+1:end-half_of_wavelet_size));

figure
for i=1:2
    subplot(2,1,i)
    eval([ 'plot(randsig' num2str(i) ')' ]) % "eval" can be useful for increasing control over commands
    hold on
    eval([ 'plot(filtsig' num2str(i) ',''r'')' ])
end

%% Figure 25.3

% initialize output correlation matrix
correlations = zeros(5,round(1000/f));

for i=1:round(1000/f)

    % correlation of unfiltered random signal
    temp = corrcoef(randsig1(1:end-i),randsig1(i+1:end));
    correlations(1,i) = temp(1,2);

    % correlation of filtered signal
    temp = corrcoef(filtsig1(1:end-i),filtsig1(i+1:end));
    correlations(2,i) = temp(1,2);

    % phase clustering
    correlations(3,i) = abs(mean(exp(1i*( angle(anglesig1(1:end-i)-anglesig1(i+1:end))))));

    % difference of correlations of filtered signal
    temp = corrcoef(filtsig2(1:end-i),filtsig2(i+1:end));
    correlations(4,i) = temp(1,2) - correlations(2,i);

    % difference of phase clusterings
    correlations(5,i) = abs(mean(exp(1i*( angle(anglesig2(1:end-i)-anglesig2(i+1:end)))))) - correlations(3,i);
end

figure
plot(correlations')
xlabel('Lag (ms)')
ylabel('Connectivity strength')
legend({'unfiltered';'power corr';'ISPC';'corr diffs';'ISPC diffs'})

%% end.


function [suspicious_index lof] = LOF(A, k)
%
% Local Outlier Factor
% Authors: Markus M. Breunig, Hans-Peter Kriegel,
%          Raymond T. Ng, J?rg Sander
% Original paper :
% LOF: Identifying Density-Based Local Outliers
% e-mail : { breunig | kriegel | sander }
%          @dbs.informatik.uni-muenchen.de
%          rng@cs.ubc.ca
% Programmer: Yi-Ren Yeh(yirenyeh@gmail.com)
% modified by: Zi-Wen Gui(evan176@hotmail.com)
%
%
% Inputs
%   A: the data matrix, each row represents an instance
%   k: the number of nearest neighbors, specified as an integer or
%      as a fraction of the total number of data points
%
% Outputs
%   lof: the local outlier factor for each instance
%   suspicious_index: the ranking of instances according to their
%                     suspicious score
%                     For example, suspicious_index(i)=j means the
%                     ith instance is in jth position in the ranking
%

if k < 1
    [numrows ~] = size(A);
    k = round(k*numrows);
end

try
    %Find the nearest neighbors by "KDTree" for each elements
    [k_index, k_dist] = knnsearch(A,A,'k',k+1,'nsmethod','kdtree','IncludeTies',true);
    %Ignore first element(itself) at nearest neighbors
    k_index = cellfun(@(x) x(2:end),k_index,'UniformOutput',false);
    numneigh = cellfun('length',k_index);
    %Get k-distance
    k_dist1 = cell2mat(cellfun(@(x) x(end),k_dist,'UniformOutput',false));
    %Get row length of matrix A
    n = length(A(:,1));
    %Initialize lrd_value vector
    lrd_value = zeros(n,1);
    %Calculate lrd for each elements
    for i = 1:n
        lrd_value(i) = lrd(A, i, k_dist1, k_index, numneigh(i));
    end
    %Initialize lof vector
    lof = zeros(n,1);
    %Calculate LOF
    for i = 1:n
        lof(i) = sum(lrd_value(k_index{i})/lrd_value(i))/numneigh(i);
    end
    %Indices from sorting lof are the suspicious score rankings
    [~,suspicious_index]=sort(lof,'descend');

catch err
    if (strcmp(err.message, 'Invalid parameter name: IncludeTies.'))
        warning('MATLAB:LOF', 'Matlab not newest version? Falling back to old version.')
        [suspicious_index lof] = LOF_old(A, k);
    else
        rethrow(err)
    end

end

%=========================================================================
function lrd_value = lrd(A, index_p, k_dist,k_index, numneighbors)
%Calculate the reachability distance for nearest neighbors
Temp = repmat(A(index_p,:), numneighbors, 1) - A(k_index{index_p}, :);
Temp = sqrt(sum(Temp.^2,2));
reach_dist = max([Temp k_dist(k_index{index_p})],[],2);
%Calculate the local reachability density for each elements
lrd_value = numneighbors/sum(reach_dist);


function y = normpdfg(x,scale,shape,center,max)
% y = normpdfg(x,scale,shape,center,max)
%
% Evalute the generalized gaussian pdf at point(s) x
% the pdf is defined by the given scale (2*variance) and shape and mean
% (center)
%
% Note the variance of the distribution is scale/2
%
% As shape --> Inf we get a (step function) uniform distribution over [center-scale, center+scale]
% As shape --> 0 we get a delta function
% Beta = 1 is the Laplace
% Beta = 2 is gaussian
%
% (C) Tim Mullen, 2011. SCCN/INC UCSD

A = shape./(2.*scale.*gamma(1/shape));
C = max./A;

y = C.*A.*exp(-((abs(x-center)/scale).^shape));


% Minimize transition bandwidth of a linear phase lowpass FIR filter
% "Filter design" lecture notes (EE364) by S. Boyd
% (figures are generated)
%
% Designs a linear phase FIR lowpass filter such that it:
% - minimizes the transition band width (i.e. minimize w_stop)
% - has a constraint on the maximum passband ripple
% - has a constraint on the maximum stopband attenuation
%
% This is a quasiconvex problem and is solved using a bisection.
%
%   minimize   w_stop
%       s.t.   1/delta <= H(w) <= delta     for w in the passband
%              |H(w)| <= atten_level        for w in the stopband
%
% where H is the frequency response function and variable is
% the filter impulse response h (and its order/length).
% Data is delta (max passband ripple) and atten_level (max stopband
% attenuation level).
%
% Written for CVX by Almir Mutapcic 02/02/06

%********************************************************************
% user's filter specifications
%********************************************************************
% starting point for the stopband (needs to be feasible)
wstop = 0.24*pi;        % stopband start freq (in radians)
TOL = 1e-3;             % precision to which we should run bisection

n = 10;                 % filter order (2n+1 is the full order)
wpass = 0.12*pi;        % passband cutoff freq (in radians)
delta = 1;              % max (+/-) passband ripple in dB
atten_level = -30;      % stopband attenuation level in dB

%********************************************************************
% create optimization parameters
%********************************************************************
m = 30*n; % freq samples (rule-of-thumb)
w = linspace(0,pi,m);

%*********************************************************************
% use bisection algorithm to solve the problem
%*********************************************************************

wstop_bot = wpass;
wstop_top  = wstop;

while( wstop_top - wstop_bot > TOL)
  % try to find a feasible design for given specs
  wstop_cur = (wstop_top + wstop_bot)/2;

  % create optimization matrices (matrix of cosines)
  A = [ones(m,1) 2*cos(kron(w',[1:n]))];

  % passband 0 <= w <= w_pass
  ind = find((0 <= w) & (w <= wpass));    % passband
  Ap  = A(ind,:);

  % transition band is not constrained (w_pass <= w <= w_stop)

  % stopband (w_stop <= w) (this is the changing constraint)
  ind = find((wstop_cur <= w) & (w <= pi));   % stopband
  As  = A(ind,:);

  % formulate and solve the feasibility linear-phase lp filter design
  cvx_begin quiet
    variable h_cur(n+1,1);
    % feasibility problem
    % passband bounds
    Ap*h_cur <= 10^(delta/20);
    Ap*h_cur >= 10^(-delta/20);
    % stopband bounds
    abs( As*h_cur ) <= 10^(atten_level/20);
  cvx_end

  % bisection
  if strfind(cvx_status,'Solved') % feasible
    fprintf(1,'Problem is feasible for stopband freq = %3.4f rads\n',wstop_cur);
    wstop_top = wstop_cur;
    % construct the full impulse response
    h = [flipud(h_cur(2:end)); h_cur];
  else % not feasible
    fprintf(1,'Problem is not feasible for stopband freq = %3.4f rads\n',wstop_cur);
    wstop_bot = wstop_cur;
  end
end

wstop = wstop_top;
fprintf(1,['\nOptimum stopband frequency for given specs is %3.4f*pi rads\n' ...
           'and the minimum transition width is %3.4f*pi radians.\n'],...
            wstop/pi, (wstop-wpass)/pi);

%********************************************************************
% plots
%********************************************************************
figure(1)
% FIR impulse response
plot([0:2*n],h','o',[0:2*n],h','b:')
xlabel('t'), ylabel('h(t)')

figure(2)
% frequency response
H = exp(-j*kron(w',[0:2*n]))*h;
% magnitude
subplot(2,1,1)
plot(w,20*log10(abs(H)),...
     [wstop pi],[atten_level atten_level],'r--',...
     [0 wpass],[delta delta],'r--',...
     [0 wpass],[-delta -delta],'r--');
axis([0,pi,-40,10])
xlabel('w'), ylabel('mag H(w) in dB')
% phase
subplot(2,1,2)
plot(w,angle(H))
axis([0,pi,-pi,pi])
xlabel('w'), ylabel('phase H(w)')


function J=vl_imwhiten(I,alpha,cutoff)
% VL_IMWHITEN  Whiten an image
%   J = VL_IMWHITEN(I,ALPHA) approximatively whitens the power spectrum
%   of the natural image I. The algorithm assumes that the modulus of
%   the spectrum decays as 1/f^ALPHA (f is the frequency).
%
%   VL_IMWHITEN(I) uses ALPHA=1 (a typical value for natural images).
%
%   VL_IMWHITEN(I,ALPHA,CUTOFF) also applies a low-pass filter with
%   cutoff frequency equal to CUTOFF x FN, where FN is the Nyquist
%   frequency (half of the sampling frequency).
%
%   See also: VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

if ~exist('alpha','var'),  alpha = 1 ; end
if ~exist('cutoff','var'), cutoff = [] ; end

[M,N]=size(I) ;

% Frequency domain
fn = 0.5 ; % Nyquist freq (=1/2T, T=1)
fx_range=linspace(-fn, fn, N) ;
fy_range=linspace(-fn, fn, M) ;
[fx fy]=meshgrid(fx_range, fy_range) ;

% Whitening filter
rho=sqrt(fx.*fx+fy.*fy);
filt=rho.^alpha ;

% Low-pass filter
if ~isempty(cutoff)
  fcut = cutoff * fn ;
  filt = filt .* exp(-(rho/fcut).^4);
  %filt = filt .* exp( - 0.5 * (rho / fcut) .^ 2);
end

% Apply filter
J = real(ifft2(fft2(I).*fftshift(filt))) ;


function [tuples,gain]=kCard2DAssign(C,k,maximize)
%%KCARD2DASSIGN Solve the two-dimensional k-cardinality assignment problem
%          with a rectangular cost matrix C. The problem being solved can
%          be formulated as minimize (or maximize)
%          \sum_{i=1}^{numRow}\sum_{j=1}^{numCol}C_{i,j}*x_{i,j}
%          subject to
%          \sum_{j=1}^{numCol}x_{i,j}<=1 for all i
%          \sum_{i=1}^{numRow}x_{i,j}<=1 for all j
%          \sum_{i=1}^{numRow}\sum_{j=1}^{numCol}x_{i,j}=k
%          x_{i,j}=0 or 1.
%
%INPUTS: C A numRowXnumCol cost matrix that does not contain any NaNs and
%          where the largest finite element minus the smallest element is a
%          finite quantity (does not overflow) when performing minimization
%          and where the smallest finite element minus the largest
%          element is finite when performing maximization. Forbidden
%          assignments can be given costs of +Inf for minimization and -Inf
%          for maximization.
%        k The integer number of assignments to make.
%          k<=min(numRow,numCol).
% maximize If true, the minimization problem is transformed into a
%          maximization problem. The default if this parameter is omitted
%          or an empty matrix is passed is false.
%
%OUTPUTS: tuples A 2XnumRow set of assignment values. This is ordered
%                [rowIndex;columnIndex]. If no feasible solution exists,
%                then an empty matrix will be returned.
%          gain This is the value of the cost. This is the sum of the
%               values in C corresponding to the tuples.
%
%As noted in [1], the k-cardinality 2D assignment problem can be
%transformed into a standard rectangular 2D assignment problem. This
%function implements that transformation and calls assign2D.
%
%EXAMPLE:
% C=[7,   51,  52,  87;
%    50,  12,   0,  64;
%    27,  77,   0,  18;
%    62,   0,   3,   8];
% k=4;
% [tuples4,gain4]=kCard2DAssign(C,4)
% [tuples3,gain3]=kCard2DAssign(C,3)
% [tuples2,gain2]=kCard2DAssign(C,2)
% [tuples1,gain1]=kCard2DAssign(C,1)
%One will see optimal gains of 25, 7, 0, and 0.
%
%REFERENCES:
%[1] A. Volgenant, "Solving the k-cardinality assignment problem by
%    transformation," European Journal of Operational Research, vol. 157,
%    no. 2, pp. 322-331, 1 Sep. 2004.
%
%June 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(nargin<3||isempty(maximize))
        maximize=false;
    end

    numRow=size(C,1);
    numCol=size(C,2);

    if(k==0)
       tuples=[];
       gain=0;
       return;
    end

    if(k<0||k~=fix(k))
        error('k is invalid.')
    end

    if(k>numRow||k>numCol)
        %The problem is not feasible.
        tuples=[];
        gain=-1;
        return
    end

    COrig=C;

    if(maximize==true)
        CDelta=max(C(:));

        %If C is all negative, do not shift.
        if(CDelta<0)
            CDelta=0;
        end

        C=-C+CDelta;
    else
        CDelta=min(C(:));

        %If C is all positive, do not shift.
        if(CDelta>0)
            CDelta=0;
        end

        C=C-CDelta;
    end

    %The minimum value in C is now 0 or something positive. We shall shift
    %the values to guarantee that 0 is less than every value in C.
    %Find the maximum finite value in C.
    CMax=max(max(C(isfinite(C))));

    %We shall now offset everything by a small fraction of the maximum
    %value. This is so that we guarantee that values will be assigned
    %to the zero padded rows that shall be added to C.
    COffset=CMax*1e-10;
    C=C+COffset;

    %Perform 2D assignment on the augmented matrix. An algorithm that does
    %not explicitely construct that assignment matrix is also possible.
    [~,row4Col]=assign2D([C;zeros(numCol-k,numCol)],false);

    if(isempty(row4Col))
        %If the problem is infeasible.
        tuples=[];
        gain=-1;
        return
    end

    tuples=zeros(2,k);
    curTuple=1;
    gain=0;
    for curCol=1:numCol

        curRow=row4Col(curCol);

        if(curRow<=numRow)
            gain=gain+COrig(curRow,curCol);
            tuples(:,curTuple)=[curRow;curCol];
            curTuple=curTuple+1;

           if(curTuple>k)
               break;
           end
        end
    end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function b = int(a,x,L,U)
% function B = int(A,X,L,U)
%
% DESCRIPTION
%   Element-by-element integration of a polynomial with respect
%   to a single variable.
%
% INPUTS
%   A: polynomial
%   X: Scalar polynomial variable [Optional with default X = A.varname{1}]
%   L: Lower limit of definite integral
%   U: Upper limit of definite integral
%
% OUTPUTS
%   B: polynomial
%
% SYNTAX
%   B = int(A,X)
%     Indefinite integral of the polynomial, A, with respect to X.
%     X should be a polynomial variable or string.   Integration is done
%     element-by-element if A is a matrix.
%   B = int(A,X,L,U)
%     Definite integral of A with respect to X from lower limit L to
%     upper limit U.
%   B = int(A,X,[L U]);
%     Equivalent to B = diff(A,X,L,U)
%
% EXAMPLE
%   pvar x y z;
%   a = 2*x^3 - 2*x*z^2 + 5*y*z;
%   b = int(a,x)
%   diff(b,x)-a
%   c = int(a,[0 1])
%
% See also: diff, jacobian

% 12/6/2010 PJS  Initial Coding

% Process Inputs/ Error Checking
if nargin==1
    x = a.varname{1};
    L = []; U = [];
elseif nargin==2
    if isa(x,'double')
        % B = int(A,[L,U])
        L = x(1);
        U = x(2);
        x = a.varname{1};
    else
        % B = int(A,X)
        L = []; U = [];
    end
elseif nargin==3
    if isa(x,'double')
        % B = int(A,L,U)
        U = L;
        L = x;
        x = a.varname{1};
    else
        % B = int(A,X,[L U])
        U = L(2);
        L = L(1);
    end
elseif nargin~=4
    error(['Invalid syntax for the "int" command. ' ...
        'Type "help int" for more information.'])
end

if ispvar(x) && length(x)==1
    x = x.varname{1};
elseif ~ischar(x)
    error('X must be a single polynomial variable or a string');
end

% Get polynomial info about A
acoef = a.coefficient;
adeg = a.degmat;
avar = a.varname;
sza = a.matdim;
Naterms = size(acoef,1);

% Find variable we are differentiating with respect to.
varnumb = find( strcmp(avar,x) );

% Perform indefinite integral
szb = sza;
if isempty(varnumb)
    % int a(y) dx = a(y)*x
    bcoef = acoef;
    bdeg = [adeg ones(Naterms,1)];
    bvar = [avar; x];
else
    % int a(y)*x^n dx = a(y)*x^(n+1) / (n+1)
    bdeg = adeg;
    nplus1 = bdeg(:,varnumb)+1;
    bdeg(:,varnumb) = nplus1;

    bcoef = acoef;
    bcoef = lrscale( bcoef, 1./nplus1 , []);

    bvar = avar;
end

chkval = 0; % skip validity check
b = polynomial(bcoef,bdeg,bvar,szb,chkval);

% Evaluate definite integral
if ~isempty(L)
    bU = subs(b,{x},U);
    bL = subs(b,{x},L);
    b = bU-bL;
end

% Combine any common terms
b = combine(b);


function ray = createRay(varargin)
%CREATERAY Create a ray (half-line), from various inputs
%
%   RAY = createRay(POINT, ANGLE)
%   POINT is a N*2 array giving starting point of the ray, and ANGLE is the
%   orientation of the ray.
%
%   RAY = createRay(X0, Y0, ANGLE)
%   Specify ray origin with 2 input arguments.
%
%   RAY = createRay(P1, P2)
%   Create a ray starting from point P1 and going in the direction of point
%   P2.
%
%   Ray is represented in a parametric form: [x0 y0 dx dy]
%   x = x0 + t*dx
%   y = y0 + t*dy;
%   for all t>0
%
%   Example
%   origin  = [3 4];
%   theta   = pi/6;
%   ray = createRay(origin, theta);
%   figure(1); clf; hold on;
%   axis([0 10 0 10]);
%   drawRay(ray);
%
%   See also:
%   rays2d, createLine, points2d
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2007-10-18
% Copyright 2007 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

if length(varargin)==2
    p0 = varargin{1};
    arg = varargin{2};
    if size(arg, 2)==1
        % second input is the ray angle
        ray = [p0 cos(arg) sin(arg)];
    else
        % second input is another point
        ray = [p0 arg-p0];
    end

elseif length(varargin)==3
    x = varargin{1};
    y = varargin{2};
    theta = varargin{3};
    ray = [x y cos(theta) sin(theta)];

else
    error('Wrong number of arguments in ''createRay'' ');
end


function a = r8vec_normalize_l1 ( n, a )

%*****************************************************************************80
%
%% R8VEC_NORMALIZE_L1 normalizes an R8VEC to have unit sum.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, real A(N), the vector to be normalized.
%
%    Output, real A(N), the entries of A should have unit sum.  However,
%    if the input vector has zero sum, the routine halts.
%
  a_sum = sum ( a(1:n) );

  if ( a_sum == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_NORMALIZE_L1 - Fatal error!\n' );
    fprintf ( 1, '  The vector entries sum to 0.\n' );
    error ( 'R8VEC_NORMALIZE_L1 - Fatal error!' );
  end

  a(1:n) = a(1:n) / a_sum;

  return
end


function h = r8mat_house_post ( n, a, row, col )

%*****************************************************************************80
%
%% R8MAT_HOUSE_POST computes a Householder post-multiplier matrix.
%
%  Discussion:
%
%    H(ROW,COL) has the property that the ROW-th column of
%    A*H(ROW,COL) is zero from entry COL+1 to the end.
%
%    In the most common case, where a QR factorization is being computed,
%    ROW = COL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrices.
%
%    Input, real A(N,N), the matrix whose Householder matrix
%    is to be computed.
%
%    Input, integer ROW, COL, specify the location of the
%    entry of the matrix A which is to be preserved.  The entries in
%    the same row, but higher column, will be zeroed out if
%    A is postmultiplied by H.
%
%    Output, real H(N,N), the Householder matrix.
%

%
%  Set up the vector V.
%
  a_row(1,1:col-1) = 0.0;
  a_row(1,col:n) = a(row,col:n);

  a_row = a_row';

  v = r8vec_house_column ( n, a_row, col );
%
%  Form the matrix H(V).
%
  h = r8mat_house_form ( n, v );

  return
end


function [label, model, llh] = mixLinReg(X, y, k, lambda)
% Mixture of linear regression
% input:
%   X: d x n data matrix
%   y: 1 x n responding vector
%   k: number of mixture component
%   lambda: regularization parameter
% output:
%   label: 1 x n cluster label
%   model: trained model structure
%   llh: loglikelihood
% Written by Mo Chen (sth4nth@gmail.com).
if nargin < 4
    lambda = 1;
end
n = size(X,2);
X = [X;ones(1,n)]; % adding the bias term
d = size(X,1);
label = ceil(k*rand(1,n));  % random initialization
R = full(sparse(label,1:n,1,k,n,n));
tol = 1e-6;
maxiter = 500;
llh = -inf(1,maxiter);
Lambda = lambda*eye(d);
W = zeros(d,k);
Xy = bsxfun(@times,X,y);
beta = 1;
for iter = 2:maxiter
    % maximization
    nk = sum(R,2);
    alpha = nk/n;
    for j = 1:k
        Xw = bsxfun(@times,X,sqrt(R(j,:)));
        U = chol(Xw*Xw'+Lambda);
        W(:,j) = U\(U'\(Xy*R(j,:)'));  % 3.15 & 3.28
    end
    D = bsxfun(@minus,W'*X,y).^2;
    % expectation
    logRho = (-0.5)*beta*D;
    logRho = bsxfun(@plus,logRho,log(alpha));
    T = logsumexp(logRho,1);
    logR = bsxfun(@minus,logRho,T);
    R = exp(logR);
    llh(iter) = sum(T)/n;
    if abs(llh(iter)-llh(iter-1)) < tol*abs(llh(iter)); break; end
end
llh = llh(2:iter);
model.alpha = alpha; % mixing coefficient
model.beta = beta; % mixture component precision
model.W = W;  % linear model coefficent
[~,label] = max(R,[],1);
model.label = label;


function [ triangle_index, alpha, beta, gamma, edge, step_num ] = ...
  triangulation_search_delaunay ( node_num, node_xy, triangle_order, ...
  triangle_num, triangle_node, triangle_neighbor, p )

%*****************************************************************************80
%
%% TRIANGULATION_SEARCH_DELAUNAY searches a Delaunay triangulation for a point.
%
%  Purpose:
%
%    The algorithm "walks" from one triangle to its neighboring triangle,
%    and so on, until a triangle is found containing point P, or P is found
%    to be outside the convex hull.
%
%    The algorithm computes the barycentric coordinates of the point with
%    respect to the current triangle.  If all three quantities are positive,
%    the point is contained in the triangle.  If the I-th coordinate is
%    negative, then P lies on the far side of edge I, which is opposite
%    from vertex I.  This gives a hint as to where to search next.
%
%    For a Delaunay triangulation, the search is guaranteed to terminate.
%    For other triangulations, a cycle may occur.
%
%    Note the surprising fact that, even for a Delaunay triangulation of
%    a set of points, the nearest point to P need not be one of the
%    vertices of the triangle containing P.
%
%    The code can be called for triangulations of any order, but only
%    the first three nodes in each triangle are considered.  Thus, if
%    higher order triangles are used, and the extra nodes are intended
%    to give the triangle a polygonal shape, these will have no effect,
%    and the results obtained here might be misleading.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2012
%
%  Author:
%
%    Original FORTRAN77 version by Barry Joe.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Barry Joe,
%    GEOMPACK - a software package for the generation of meshes
%    using geometric algorithms,
%    Advances in Engineering Software,
%    Volume 13, pages 325-331, 1991.
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM), the vertices.
%
%    Input, integer TRIANGLE_ORDER, the order of the triangles.
%
%    Input, integer TRIANGLE_NUM, the number of triangles in the triangulation.
%
%    Input, integer TRIANGLE_NODE(TRIANGLE_ORDER,TRIANGLE_NUM),
%    the nodes that make up each triangle.
%
%    Input, integer TRIANGLE_NEIGHBOR(3,TRIANGLE_NUM), the triangle
%    neighbor list.
%
%    Input, real P(2), the coordinates of a point.
%
%    Output, integer TRIANGLE_INDEX, the index of the triangle where the
%    search ended.  If a cycle occurred, then TRIANGLE_INDEX = -1.
%
%    Output, real ALPHA, BETA, GAMMA, the barycentric
%    coordinates of the point relative to triangle TRIANGLE_INDEX.
%
%    Output, integer EDGE, indicates the position of the point P in
%    triangle TRIANGLE_INDEX:
%    0, the interior or boundary of the triangle;
%    -1, outside the convex hull of the triangulation, past edge 1;
%    -2, outside the convex hull of the triangulation, past edge 2;
%    -3, outside the convex hull of the triangulation, past edge 3.
%
%    Output, integer STEP_NUM, the number of steps.
%
  persistent triangle_index_save;

  dim_num = 2;

  step_num = - 1;
  edge = 0;

  if ( length ( triangle_index_save ) == 0 )
    triangle_index_save = -1;
  end

  if ( triangle_index_save < 1 | triangle_num < triangle_index_save )
    triangle_index = floor ( ( triangle_num + 1 ) / 2 );
  else
    triangle_index = triangle_index_save;
  end

  while ( 1 )

    step_num = step_num + 1;

    if ( triangle_num < step_num )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'TRIANGULATION_SEARCH_DELAUNAY - Fatal error!\n' );
      fprintf ( 1, '  The algorithm seems to be cycling.\n' );
      triangle_index = -1;
      alpha = -1.0;
      beta = -1.0;
      gamma = -1.0;
      edge = -1;
      return
    end
%
%  Get the vertices of triangle TRIANGLE_INDEX.
%
    a = triangle_node(1,triangle_index);
    b = triangle_node(2,triangle_index);
    c = triangle_node(3,triangle_index);
%
%  Using vertex C as a base, compute the distances to vertices A and B,
%  and the point P.
%
    dxa = node_xy(1,a) - node_xy(1,c);
    dya = node_xy(2,a) - node_xy(2,c);

    dxb = node_xy(1,b) - node_xy(1,c);
    dyb = node_xy(2,b) - node_xy(2,c);

    dxp = p(1)         - node_xy(1,c);
    dyp = p(2)         - node_xy(2,c);

    det = dxa * dyb - dya * dxb;
%
%  Compute the barycentric coordinates of the point P with respect
%  to this triangle.
%
    alpha = ( dxp * dyb - dyp * dxb ) / det;
    beta =  ( dxa * dyp - dya * dxp ) / det;
    gamma = 1.0 - alpha - beta;
%
%  If the barycentric coordinates are all positive, then the point
%  is inside the triangle and we're done.
%
    if ( 0.0 <= alpha && 0.0 <= beta && 0.0 <= gamma )
      break
    end
%
%  At least one barycentric coordinate is negative.
%
%  If there is a negative barycentric coordinate for which there exists
%  an opposing triangle neighbor closer to the point, move to that triangle.
%
%  (Two coordinates could be negative, in which case we could go for the
%  most negative one, or the most negative one normalized by the actual
%  distance it represents).
%
    if ( alpha < 0.0 && 0 < triangle_neighbor(2,triangle_index) )
      triangle_index = triangle_neighbor(2,triangle_index);
      continue;
    elseif ( beta < 0.0 && 0 < triangle_neighbor(3,triangle_index) )
      triangle_index = triangle_neighbor(3,triangle_index);
      continue;
    elseif ( gamma < 0.0 && 0 < triangle_neighbor(1,triangle_index) )
      triangle_index = triangle_neighbor(1,triangle_index);
      continue;
    end
%
%  All negative barycentric coordinates correspond to vertices opposite
%  sides on the convex hull.
%
%  Note the edge and exit.
%
    if ( alpha < 0.0 )
      edge = -2;
      break
    elseif ( beta < 0.0 )
      edge = -3;
      break
    elseif ( gamma < 0.0 )
      edge = -1;
      break
    end

  end

  triangle_index_save = triangle_index;

  return
end


function [r,spherCent]=osculatingSpher4LatLon(latLon,a,f,is2D)
%%OSCULATINGSPHER4LATLON Given the latitude and longitude of a point on an
%           ellipsoid, determine the radius and center location of an
%           osculating sphere going through that point. The osculating
%           sphere is such that the azimuth equals the longitude and the
%           elevation equals the latitude of the point on the ellipsoid.
%           Additionally, the local tangent planes at the point are the
%           same. The radius of curvature of the sphere at that point
%           should nominally equal the radius of curvature of the ellipsoid
%           at that point. However, since the ellipsoid has two radii of
%           curvature, the geometric mean is used (the mean radius of
%           curvature). On the other hand, if is2D is true, then just the
%           radius of curvature in the meridian will be used, which is what
%           perfectly matches a cut of the ellipsoid through the origin and
%           pole (A 2D ellipse in the x-z plane).
%
%INPUTS: latLon The 2X1 point of the format [latitude;longitude] in radians
%               where the osculating sphere should touch the ellipsoid.
%             a The semi-major axis length of the ellipsoid. If this
%               argument is omitted or an empty matrix is passed, the value
%               in Constants.WGS84SemiMajorAxis is used.
%             f The flattening factor of the ellipsoid. If this argument is
%               omitted or an empty matrix is passed, the value in
%               Constants.WGS84Flattening is used.
%          is2D Indicates whether one just cares about a 2D cut of the
%               ellipsoid. If so, then the radius used will be the radius
%               of curvature in the meridian. The default if omitted or an
%               empty matrix is passed is false.
%
%OUTPUTS: r The radius of the osculating sphere.
% spherCent The location of the center of the osculating sphere with
%           respect to the center of the ellipsoid.
%
%The formulae in Section 3 of [1] are used.
%
%EXAMPLE:
%Here, we consider a 2D slice of an ellipsoid that goes through
%longitude=0, we choose a point. We then find the osculating sphere. The
%ellipse cut of the ellipsoid and the circle cut of the sphere are then
%plotted with the point. We ALSO plot the circle cut obtained with
%is2D=true. Since we are only considering a 2D cut, one will see that that
%circle is actually better aligned with the curvature of the ellipse at
%that point. However, if one were to take a 2D cut in other directions, it
%would appear to be a worse fit.
% a=20;%Semi-major axis.
% f=0.5;%Flattening factor.
% b=a*(1-f);%The semi-minor axis of the ellipsoid.
%
% figure(1)
% clf
% hold on
% %Draw a 2D cut of the ellipsoid.
% A=inv([a^2,0;
%        0,b^2]);
% drawEllipse([0;0],A,1,'linewidth',4)
% latLon=[50;0]*(pi/180);
% oscPt=ellips2Cart([latLon;0],a,f);
% [r,spherCent]=osculatingSpher4LatLon(latLon,a,f,false);
% [r2D,spherCent2D]=osculatingSpher4LatLon(latLon,a,f,true);%is2D=true
% %Draw the both 2D cuts of the sphere:
% A=inv([r^2,0;
%        0,r^2]);
% drawEllipse([spherCent(1);spherCent(3)],A,1,'-.','linewidth',4)
% A=inv([r2D^2,0;
%        0,r2D^2]);
% drawEllipse([spherCent2D(1);spherCent2D(3)],A,1,'--','linewidth',2)
%
% %Note that spherCent(2)==0.
% scatter(oscPt(1),oscPt(3),100,'filled')
% legend('Ellipsoid Cut','Osculating Sphere','Osculating Sphere 2D','location','southwest')
% h1=xlabel('x');
% h2=ylabel('y');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] P. Williams and D. Last, "On Loran-C time-difference to co-ordinate
%    converters," in Proceedings of the 32nd Annual Convention & Technical
%    Symposium of the International Loran Association, Boulder, CO, 3-7
%    Nov. 2003.
%
%August 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(is2D))
    is2D=false;
end

if(nargin<3||isempty(f))
    f=Constants.WGS84Flattening;
end

if(nargin<2||isempty(a))
    a=Constants.WGS84SemiMajorAxis;
end

%The squared eccentricity of the ellipsoid.
e2=f*(2-f);

lat=latLon(1);%Latitude.
lon=latLon(2);

Phi=lat;

denomTerm=sqrt(1-e2*sin(Phi)^2);
%Equation 3.17 in [1]. The radius of curvature in the meridian.
M0=a*(1-e2)/denomTerm^3;

if(is2D)
    r=M0;
else
    %Equation 3.18 in [1]. The radius of curvature in the prime vertical.
    N0=a/denomTerm;

    %Equation 3.16 in [1].
    r=sqrt(M0*N0);
end

xCartEllips=ellips2Cart([latLon(1:2);0],a,f);
xCartSpher=spher2Cart([r;lon;lat]);

spherCent=-(xCartSpher-xCartEllips);

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


% =========================
%  Robust Bayesian allocation in the stock market, as described in
%  Meucci, A., (2005) "Robust Bayesian Allocation"
%
%  Most recent version of article and code available at http://symmys.com/node/102
%  =========================

clear; clc; close all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% inputs
p_m=.1; % robustness parameter location
p_s=.1; % robustness parameter scatter
load SectorsSnP500

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute weekly returns
Ps=P(1:5:end,:);
R=Ps(2:end,:)./Ps(1:end-1,:)-1;
Dates_P=DP(1:5:end);
Dates_R=Dates_P(2:end);
[Ttot,N]=size(R);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% estimation
W=52; % rolling estimation period
NumPortf=10;
Ret_hat=[];
Ret_rB=[];
Dates=[];
for t=W+1:Ttot-1
    Ttot-t+2
    Rets=R(t-W:t,:);

    % sample estimate
    m_hat=mean(Rets)';
    S_hat=cov(Rets);
    [de_hat,ds_hat,w_hat] = EfficientFrontier(NumPortf, S_hat, m_hat);

    % Bayesian prior
    S0=diag(diag(S_hat));
    m0=.5*S0*ones(N,1)/N;
    T=size(Rets,1);
    T0=2*T;
    nu0=2*T;

    % Bayesian posterior parameters
    T1=T+T0;
    m1=1/T1*(m_hat*T+m0*T0);
    nu1=T+nu0;
    S1=1/nu1*( S_hat*T + S0*nu0 + (m_hat-m0)*(m_hat-m0)'/(1/T+1/T0)  );
    [d,d,w1] = EfficientFrontier(NumPortf, S1, m1);

    % robustness parameters
    q_m2=chi2inv(p_m,N);
    g_m=sqrt(q_m2/T1*nu1/(nu1-2));
    q_s2=chi2inv(p_s,N*(N+1)/2);
    PickVol=round(.8*NumPortf);
    v=(ds_hat(PickVol))^2;
    g_s=v/(  nu1/(nu1+N+1)+sqrt( 2*nu1*nu1*q_s2/((nu1+N+1)^3)));

    Target=[];

    wu=w_hat(PickVol,:)';
    Ret_hat=[Ret_hat R(t+1,:)*wu];

    for k=1:NumPortf-1
        NewTarget=-(10^10);
        if wu'*S1*wu <= g_s
            NewTarget = m1'*wu-g_m*sqrt(wu'*S1*wu);
        end
        Target=[Target NewTarget];
    end

    [Best,k]=max(Target);
    wu=w1(k,:)';
    Ret_rB=[Ret_rB R(t+1,:)*wu];

    Dates=[Dates Dates_R(t+1)];
end

NAV_hat=cumprod(1+Ret_hat);
NAV_rB=cumprod(1+Ret_rB);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plots

figure
subplot(2,1,1)
h=plot(Dates,Ret_hat);
xlim([Dates(1) Dates(end)])
YLim=get(gca,'ylim');
Datetick('x','mmmyy','keeplimits','keepticks');
grid on
subplot(2,1,2)
h=plot(Dates,Ret_rB);
set(gca,'ylim',YLim,'xlim',[Dates(1) Dates(end)]);
Datetick('x','mmmyy','keeplimits','keepticks');
grid on

figure
subplot(2,1,1)
h=plot(Dates,NAV_hat);
xlim([Dates(1) Dates(end)])
YLim=get(gca,'ylim');
Datetick('x','mmmyy','keeplimits','keepticks');
grid on
subplot(2,1,2)
h=plot(Dates,NAV_rB);
set(gca,'ylim',YLim,'xlim',[Dates(1) Dates(end)]);
Datetick('x','mmmyy','keeplimits','keepticks');
grid on


function b = r83p_vxm ( n, a, x )

%*****************************************************************************80
%
%% R83P_VXM multiplies a vector by a R83P matrix.
%
%  Discussion:
%
%    The R83P storage format stores a periodic tridiagonal matrix as
%    a 3 by N array, in which each row corresponds to a diagonal, and
%    column locations are preserved.  The matrix value
%    A(1,N) is stored as the array entry A(3,N), and the matrix value
%    A(N,1) is stored as the array entry A(1,1).
%
%  Example:
%
%    Here is how a R83P matrix of order 5 would be stored:
%
%      A51 A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54 A15
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be at least 3.
%
%    Input, real A(3,N), the R83P matrix.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real B(N), the product X * A.
%
  b(1) =   a(1,1)   * x(n)   + a(2,1) * x(1) + a(3,1)   * x(2);

  for i = 2 : n-1
    b(i) = a(1,i)   * x(i-1) + a(2,i) * x(i) + a(3,i)   * x(i+1);
  end

  b(n) =   a(1,n)   * x(n-1) + a(2,n) * x(n) + a(3,n)   * x(1);

  return
end