function [yi, xi] = downsample_scnlab(y, orig_samprate, new_samprate, varargin)
% Uses linear interpolation to resample a vector from one sampling
% rate to another
%
% :Usage:
% ::
%
%     [yi, xi] = downsample_scnlab(y, orig_samprate, new_samprate, [doplot])
%
% :Inputs:
%
%   **orig_samprate:**
%        sampling rate in Hz
%
%   **new_samprate:**
%        desired sampling rate in Hz
%
% ..
%    I prefer this to matlab's downsample.m because linear interpolation is
%    robust and does fewer weird things to the data.
%
%     Tor Wager, June 2009
% ..
%
% :Example:
% ::
%
%    % Downsample a 100 Hz signal to a scanning TR of 2 sec
%    % signal at 100 Hz, sample to low-freq TR of 0.5 hz (2 sec TR)
%    % every 100 / TR = 100/.5 = 200 samples
%
%    [yi, xi] = downsample_scnlab(y, 100, .5)
%

doplot = 0;
if length(varargin) > 0, doplot = varargin{1}; end

downsamplerate = orig_samprate ./ new_samprate;

nobs = length(y);

% observations in resampled output
nobs_out = round(nobs ./ downsamplerate);

x = 1:nobs;

xi = linspace(0, nobs, nobs_out);

yi = interp1(x, y, xi, 'linear', 'extrap');

if doplot

    create_figure('Downsample plot');
    plot(y)
    hold on; plot(xi, yi, 'r')

end

end


function [ph_out,ph_out_low]=wrap_filt(ph,n_win,alpha,n_pad,low_flag)
%WRAP_FILT Goldstein adaptive and lowpass filtering
%   [ph_out]=wrap_filt(ph,n_win,alpha,n_pad)
%
%   Andy Hooper, June 2006
%
%   Change Log:
%   07/2006 AH: Added zero padding
%   02/2012 AH: Set magnitude to original values

if nargin<4 | isempty(n_pad)
    n_pad=round(n_win*0.25);
end

if nargin<5
    low_flag='n';
end

[n_i,n_j]=size(ph);
n_inc=floor(n_win/2);
n_win_i=ceil(n_i/n_inc)-1;
n_win_j=ceil(n_j/n_inc)-1;

ph_out=zeros(size(ph));
if strcmpi(low_flag,'y')
    ph_out_low=ph_out;
else
    ph_out_low=[];
end
x=[1:n_win/2];
[X,Y]=meshgrid(x,x);
X=X+Y;
wind_func=[X,fliplr(X)];
wind_func=[wind_func;flipud(wind_func)];

ph(isnan(ph))=0;
B=gausswin(7)*gausswin(7)';
ph_bit=zeros(n_win+n_pad);

L=ifftshift(gausswin(n_win+n_pad,16)*gausswin(n_win+n_pad,16)');

for ix1=1:n_win_i
    wf=wind_func;
    i1=(ix1-1)*n_inc+1;
    i2=i1+n_win-1;
    if i2>n_i
        i_shift=i2-n_i;
        i2=n_i;
        i1=n_i-n_win+1;
        wf=[zeros(i_shift,n_win);wf(1:n_win-i_shift,:)];
    end
    for ix2=1:n_win_j
        wf2=wf;
        j1=(ix2-1)*n_inc+1;
        j2=j1+n_win-1;
        if j2>n_j
           j_shift=j2-n_j;
           j2=n_j;
           j1=n_j-n_win+1;
           wf2=[zeros(n_win,j_shift),wf2(:,1:n_win-j_shift)];
        end
        ph_bit(1:n_win,1:n_win)=ph(i1:i2,j1:j2);
        ph_fft=fft2(ph_bit);
        H=abs(ph_fft);
        H=ifftshift(filter2(B,fftshift(H))); % smooth response
        meanH=median(H(:));
        if meanH~=0
            H=H/meanH;
        end
        H=H.^alpha;
        ph_filt=ifft2(ph_fft.*H);
        ph_filt=ph_filt(1:n_win,1:n_win).*wf2;
        if strcmpi(low_flag,'y')
            ph_filt_low=ifft2(ph_fft.*L);
            ph_filt_low=ph_filt_low(1:n_win,1:n_win).*wf2;
        end
        if isnan(ph_filt(1,1))
            disp('filtered phase contains NaNs in goldstein_filt')
            keyboard
        end
        ph_out(i1:i2,j1:j2)=ph_out(i1:i2,j1:j2)+ph_filt;
        if strcmpi(low_flag,'y')
            ph_out_low(i1:i2,j1:j2)=ph_out_low(i1:i2,j1:j2)+ph_filt_low;
        end
    end
end

ph_out=abs(ph).*exp(j*angle(ph_out)); % reset magnitude
if strcmpi(low_flag,'y')
    ph_out_low=abs(ph).*exp(j*angle(ph_out_low)); % reset magnitude
end


function [membership,means,rms] = kmeansML(k,data,varargin)
% [membership,means,rms] = kmeansML(k,data,...)
%
% Multi-level kmeans.
% Tries very hard to always return k clusters.
%
% INPUT
%	k		Number of clusters
% 	data		dxn matrix of data points
%	'maxiter'	Max number of iterations. [30]
%	'dtol'		Min change in center locations. [0]
%	'etol'		Min percent change in RMS error. [0]
%	'ml'		Multi-level? [true]
%	'verbose'	Verbose level. [0]
%			    0 = none
%			    1 = textual
%			    2 = visual
%
% OUTPUT
% 	membership	1xn cluster membership vector
% 	means		dxk matrix of cluster centroids
%	rms		RMS error of model
%
% October 2002
% David R. Martin <dmartin@eecs.berkeley.edu>

% process options
maxIter = 30;
dtol = 0;
etol = 0;
ml = true;
verbose = 0;
for i = 1:2:numel(varargin),
  opt = varargin{i};
  if ~ischar(opt), error('option names not a string'); end
  if i==numel(varargin), error(sprintf('option ''%s'' has no value',opt)); end
  val = varargin{i+1};
  switch opt,
   case 'maxiter', maxIter = max(1,val);
   case 'dtol', dtol = max(0,val);
   case 'etol', etol = max(0,val);
   case 'ml', ml = val;
   case 'verbose', verbose = val;
   otherwise, error(sprintf('invalid option ''%s''',opt));
  end
end

[membership,means,rms] = ...
    kmeansInternal(k,data,maxIter,dtol,etol,ml,verbose,1);

function [membership,means,rms] = kmeansInternal(...
    k,data,maxIter,dtol,etol,ml,verbose,retry)

[d,n] = size(data);
perm = randperm(n);

% compute initial means
rate = 3;
minN = 50;
coarseN = round(n/rate);
if ~ml | coarseN < k | coarseN < minN,
  % pick random points as means
  means = data(:,perm(1:k));
else
  % recurse on random subsample to get means
  coarseData = data(:,perm(1:coarseN));
  [coarseMem,means] = ...
      kmeansInternal(k,coarseData,maxIter,dtol,etol,ml,verbose,0);
end

% Iterate.
iter = 0;
rms = inf;
if verbose>0, fwrite(2,sprintf('kmeansML: n=%d d=%d k=%d [',n,d,k)); end
while iter < maxIter,
  if verbose>0, fwrite(2,'.'); end
  iter = iter + 1;
  % Compute cluster membership and RMS error.
  rmsPrev = rms;
  [membership,rms] = computeMembership(data,means);
  % Compute new means and cluster counts.
  prevMeans = means;
  [means,counts] = computeMeans(k,data,membership);
  % The error should always decrease.
  if rms > rmsPrev, error('bug: rms > rmsPrev'); end
  % Check for convergence.
  rmsPctChange = 2 * (rmsPrev - rms) / (rmsPrev + rms + eps);
  maxMoved = sqrt(max(sum((prevMeans-means).^2)));
  if rmsPctChange <= etol & maxMoved <= dtol, break; end
  % Visualize.
  if verbose>1, kmeansVis(data,membership,means); end
end
[membership,rms] = computeMembership(data,means);
if verbose>0, fwrite(2,sprintf('] rms=%.3g\n',rms)); end

% If there's an empty cluster, then re-run kmeans.
% Retry a fixed number of times.
maxRetries = 3;
if find(counts==0),
  if retry < maxRetries,
    disp('Warning: Re-runing kmeans due to empty cluster.');
    [membership,means] = kmeansInternal( ...
        k,data,maxIter,dtol,etol,ml,verbose,retry+1);
  else
    disp('Warning: There is an empty cluster.');
  end
end

function [membership,rms] = computeMembership(data,means)
z = distSqr(data,means);
[d2,membership] = min(z,[],2);
rms = sqrt(mean(d2));

function [means,counts] = computeMeans(k,data,membership)
[d,n] = size(data);
means = zeros(d,k);
counts = zeros(1,k);
for i = 1:k,
  ind = find(membership==i);
  counts(i) = length(ind);
  means(:,i) = sum(data(:,ind),2) / max(1,counts(i));
end

%  for i = 1:n,
%    j = membership(i);
%    means(:,j) = means(:,j) + data(:,i);
%    counts(j) = counts(j) + 1;
%  end
%  for j = 1:k,
%    means(:,j) = means(:,j) / max(1,counts(j));
%  end


function L = evaluate_log_posterior(this, uv)
%EVALUATE_LOG_POSTERIOR computes the log-posterior (negative energy) of the
%   flow fields UV
%   Actually only proportional to the log posterior since the variance of neither the
%   spatial nor the data terms is considered
%
%   This is a member function of the class 'alt_ba_optical_flow'.
%
% Authors: Deqing Sun, Department of Computer Science, Brown University
% Contact: dqsun@cs.brown.edu
% $Date: 2009 $
% $Revision: $
%
% Copyright 2009-2010, Brown University, Providence, RI. USA
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

% spatial term
S = this.spatial_filters;
p = 0;

for i = 1:length(S)

    u_ = conv2(uv(:,:,1), S{i}, 'valid');
    v_ = conv2(uv(:,:,2), S{i}, 'valid');

    if isa(this.rho_spatial_u{i}, 'robust_function')

        p = p - sum(evaluate(this.rho_spatial_u{i}, u_(:)))...
            - sum(evaluate(this.rho_spatial_v{i}, v_(:)));

    elseif isa(this.rho_spatial_u{i}, 'gsm_density')

        p   = p + sum(evaluate_log(this.rho_spatial_u{i}, u_(:)'))...
                    + sum(evaluate_log(this.rho_spatial_v{i}, v_(:)'));

    else
        error('evaluate_log_posterior: unknown rho function!');
    end;
end;

% likelihood
It  = partial_deriv(this.images, uv, this.interpolation_method);

if isa(this.rho_data, 'robust_function')

    l   = -sum(evaluate(this.rho_data, It(:)));

elseif isa(this.rho_data, 'gsm_density')

    l   = sum(evaluate_log(this.rho_data, It(:)'));

else
    error('evaluate_log_posterior: unknown rho function!');
end;

L = this.lambda*p + l;

if this.display
    fprintf('spatial\t%3.2e\tdata\t%3.2e\n', this.lambda*p, l);
end;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Spherical Harmonic Modeling and Analysis Toolkit (SPHARM-MAT) is a 3D
% shape modeling and analysis toolkit.
% It is a software package developed at Shenlab in Center for Neuroimaging,
% Indiana University (SpharmMat@gmail.com, http://www.iupui.edu/~shenlab/)
% It is available to the scientific community as copyright freeware
% under the terms of the GNU General Public Licence.
%
% Copyright 2009, 2010, ShenLab, Center for Neuroimaging, Indiana University
%
% This file is part of SPHARM-MAT.
%
% SPHARM-MAT is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% SPHARM-MAT is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with SPHARM-MAT. If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% ============================================
%
% Goal: Create canonical spherical harmonic bases
%
% Li Shen
% 04/11/2002 - create
% 10/15/2002 - rename and modify
% 11/03/2008 - renamed by Sungeun Kim.

function Z = calculate_SPHARM_basis(vs, degree)

[PHI,THETA] = cart2sph(vs(:,1),vs(:,2),vs(:,3));
ind = find(PHI<0);
PHI(ind) = PHI(ind)+2*pi;
THETA = pi/2-THETA;
vertnum = size(THETA,1);

Z = spharm_basis(degree,THETA,PHI);

return;

function Z = spharm_basis(max_degree,theta,phi)

Z = []; vnum = size(theta,1);

% save calculations for efficiency
for k = 0:(2*max_degree)
    fact(k+1) = factorial(k);
end
for m = 0:max_degree
    exp_i_m_phi(:,m+1) = exp(i*m*phi);
    sign_m(m+1) = (-1)^(m);
end

for n = 0:max_degree

	% P = legendre(n,X) computes the associated Legendre functions of degree n and
	% order m = 0,1,...,n, evaluated at X. Argument n must be a scalar integer
	% less than 256, and X must contain real values in the domain -1<=x<=1.
	% The returned array P has one more dimension than X, and each element
	% P(m+1,d1,d2...) contains the associated Legendre function of degree n and order
	% m evaluated at X(d1,d2...).

    Pn = legendre(n,cos(theta'))';

    posi_Y = [];
    nega_Y = [];

    m= 0:n;
    v = sqrt(((2*n+1)/(4*pi))*(fact(n-m+1)./fact(n+m+1)));
    v = v(ones(1,vnum),:).*Pn(:,m+1).*exp_i_m_phi(:,m+1);
    posi_Y(:,m+1) = v; % positive order;
    nega_Y(:,n-m+1) = sign_m(ones(1,vnum),m+1).*conj(v); % negative order

    Z(:,end+1:end+n) = nega_Y(:,1:n);
    Z(:,end+1:end+n+1) = posi_Y(:,1:(n+1));
end

return;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   [alpha, beta, gamma] = rot2euler(R, convention)
%   Returns the Euler angles alpha, beta and gamma that yield a given matrix
%   R. The convention specifies the order and axes of rotations.
%   Currently, only the XYZ convention is supported.
%
%   In particular, the function computes the angles alpha, beta and gamma that
%   allow to compute R as.
%
%   R = Rot(alpha,'x')*Rot(beta,'y')*Rot(gamma,'z')
%
%   Author: Arturo Gil. Universidad Miguel Hernandez de Elche. email:
%   arturo.gil@umh.es date:   11/11/2020
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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 [sol1, sol2] = rot2euler(R, convention)

if convention=='XYZ'
    [sol1, sol2]=conventionXYZ(R);
elseif convention=='ZYX'
    [sol1, sol2]=conventionZYX(R);
else
    'Unknown convention. Only XYZ and ZYX are supported'
end

function [sol1, sol2] = conventionXYZ(R)
%'R(1,3)=sen(beta)=1??'
if abs(R(1,3)) == 1
    % degenerate case in which sen(beta)=+-1 and cos(beta)=0
    alpha1 = 0; % arbitrarily set alpha to zero
    alpha2 = pi; % arbitrarily set alpha to pi
    beta1 = asin(R(1,3));
    beta2 = pi-beta1;
    if sin(beta1) > 0
        gamma1 = atan2(R(2,1), -R(3,1));
        gamma2 = atan2(R(2,1), -R(3,1)) - alpha2;
    else
        gamma1 = atan2(R(2,1), R(3,1));
        gamma2 = atan2(R(2,1), R(3,1))+alpha2;
    end
else
    beta1 = asin(R(1,3));
    beta2 = pi-beta1;

    % standard way to compute alpha beta and gamma
    alpha1 = -atan2(R(2,3)/cos(beta1), R(3,3)/cos(beta1));
    alpha2 = -atan2(R(2,3)/cos(beta2), R(3,3)/cos(beta2));
    gamma1 = -atan2(R(1,2)/cos(beta1), R(1,1)/cos(beta1));
    gamma2 = -atan2(R(1,2)/cos(beta2), R(1,1)/cos(beta2));
end
% Normalize all angles to -pi, pi
sol1 = [alpha1, beta1, gamma1];
sol2 = [alpha2, beta2, gamma2];
sol1 = normalize_angles(sol1);
sol2 = normalize_angles(sol2);

function [sol1, sol2] = conventionZYX(R)
%R(3,1)=sin(beta)=+-1?? --> degenerate case
% degenerate case in which sen(beta)=+-1 and cos(beta)=0
if abs(R(3,1)) == 1
    alpha1 = 0; % arbitrarily set alpha to zero
    alpha2 = pi;%arbitrarily set to pi
    beta1 = asin(-R(3,1));
    beta2 = pi-beta1;
    if sin(beta1) > 0
        gamma1 = atan2(R(1,2), R(2,2));
        gamma2 = atan2(R(1,2), R(2,2)) + alpha2;
    else
        gamma1 = atan2(-R(1,2), R(2,2));
        gamma2 = atan2(-R(1,2), R(2,2)) - alpha2;
    end
else
    % standard way to compute alpha beta and gamma outside of the gimbal
    % lock
    beta1 = asin(-R(3,1));
    beta2 = pi-beta1;

    alpha1 = atan2(R(2,1)/cos(beta1), R(1,1)/cos(beta1));
    alpha2 = atan2(R(2,1)/cos(beta2), R(1,1)/cos(beta2));
    gamma1 = atan2(R(3,2)/cos(beta1), R(3,3)/cos(beta1));
    gamma2 = atan2(R(3,2)/cos(beta2), R(3,3)/cos(beta2));
end
% Normalize all angles to -pi, pi
sol1 = [alpha1, beta1, gamma1];
sol2 = [alpha2, beta2, gamma2];
sol1 = normalize_angles(sol1);
sol2 = normalize_angles(sol2);

function sol_norm = normalize_angles(sol)
sol_norm = [0 0 0];
for i=1:3
    sol_norm(i) = atan2(sin(sol(i)), cos(sol(i)));
end


function [p, f, t] = spm_mmtspec (x,Fs, freqs,timeres, timestep, NW)
% Moving multitaper based spectrogram
% FORMAT [p, f, t] = spm_mmtspec (x,Fs,freqs,timeres)
%
% x         input time series
% Fs        sampling frequency of input time series
% freqs     desired vector of frequencies for spectrogram eg. [6:1:30]
% timeres   desired time resolution for spectrogram, default T/16
%           where T is duration of x
%
% p         p(f, t) is estimate of power at freq f and time t
%
% Time series is split into a series of overlapping windows with 5% overlap.
% Desired frequency resolution is attained by zero padding
% as/if necessary. The taper approach is applied to each padded sample.
%
% Plot spectrogram using imagesc(t,f,p); axis xy
%___________________________________________________________________________
% Copyright (C) 2009 Wellcome Trust Centre for Neuroimaging

% Partha Mitra, Ken Harris and Will Penny
% $Id: spm_mmtspec.m 4021 2010-07-28 12:43:16Z vladimir $

nChannels = size(x, 2);
nSamples = size(x,1);

if (nargin<4 || isempty(timeres))
    timeres = nSamples/(16*Fs);
end

if (nargin<5 || isempty(timestep))
    percent_overlap=0.05;
else
    percent_overlap= 1-timestep/timeres;
end

if (nargin<6 || isempty(NW))
   NW=3;
end

if length(unique(diff(freqs))) > 1
    error('Varying frequency resolution is not supported.');
end

df=freqs(2)-freqs(1);
nFFT=round(Fs/df);

WinLength=round(Fs*timeres);
nOverlap=ceil(percent_overlap*WinLength);

nTapers = 2*NW -1;

% Now do some computations that are common to all spectrogram functions

winstep = WinLength - nOverlap;

% check for column vector input
if nSamples == 1
    x = x';
    nSamples = size(x,1);
    nChannels = 1;
end;

if nSamples < WinLength
    disp('Error in spm_mmtspec: win length must be less than number of samples');
    return;
end

% calculate number of FFTChunks per channel
nFFTChunks = round(((nSamples-WinLength)/winstep));
% turn this into time, using the sample frequency
t = winstep*(0:(nFFTChunks-1))'/Fs;

% set up f and t arrays
if ~any(any(imag(x)))    % x purely real
    if rem(nFFT,2),    % nfft odd
        select = [1:(nFFT+1)/2];
    else
        select = [1:nFFT/2+1];
    end
    nFreqBins = length(select);
else
    select = 1:nFFT;
end
f = (select - 1)'*Fs/nFFT;

% allocate memory now to avoid nasty surprises later
y=complex(zeros(nFreqBins, nFFTChunks, nChannels, nChannels)); % output array
Periodogram = complex(zeros(nFreqBins, nTapers, nChannels, nFFTChunks)); % intermediate FFTs
Temp1 = complex(zeros(nFFT, nTapers, nFFTChunks));
Temp2 = complex(zeros(nFFT, nTapers, nFFTChunks));
Temp3 = complex(zeros(nFFT, nTapers, nFFTChunks));
eJ = complex(zeros(nFFT, nFFTChunks));

% calculate Slepian sequences.
%Tapers=spm_dpss(WinLength,NW);
Tapers=spm_dpss(max(nFFT,WinLength),NW);
Tapers=Tapers(:,1:nTapers);

% Vectorized alogrithm for computing tapered periodogram with FFT
TaperingArray = repmat(Tapers, [1 1 nChannels]);
for j=1:nFFTChunks
    Segment = x((j-1)*winstep+[1:WinLength], :);

    if WinLength<nFFT,
        % Zero pad sample to attain desired freq resolution
        Segment=[Segment;zeros(nFFT-WinLength, nChannels)];
    end

    SegmentsArray = permute(repmat(Segment, [1 1 nTapers]), [1 3 2]);
    TaperedSegments = TaperingArray .* SegmentsArray;

    fftOut = fft(TaperedSegments, nFFT);
    Periodogram(:,:,:,j) = fftOut(select,:,:); %fft(TaperedSegments,nFFT);
end

% Now make cross-products of them to fill cross-spectrum matrix
for Ch1 = 1:nChannels
        Temp1 = reshape(Periodogram(:,:,Ch1,:), [nFreqBins,nTapers,nFFTChunks]);
        Temp2 = Temp1;
        Temp2 = conj(Temp2);
        Temp3 = Temp1 .* Temp2;
        eJ=sum(Temp3, 2);
        p(: ,:, Ch1) = eJ/nTapers;
end

% Remove frequencies outside user-specified range (0.1 to correct for small
% numerical differences)
ind=find(f>=(freqs(1)-0.1) & f <=(freqs(end)+0.1));
f=f(ind);
p= p(ind,:, :);


function [mean_err_rate, err_rates] = classif_mean_err_rate(labels,test_set,truth)

% Usage
%    [mean_err_rate,err_rates] =  classif_mean_err(labels,test_set,truth)
%
% Input
%    labels (int): The labels attributed to the testing instances.
%    test_set (int): The object indices of the testing instances.
%    truth: the actual labels of the testing instances
%
% Output
%    mean_err_rate (real): The mean error rate
%    recog_rate(real): array containing the individual error rates of
% each class.
%
% Description
%    This function computes the mean of the error rates of all the classes
% which are present in the dataset. We have mean_err_rate = 1 - mean_recog_rate
% where mean_recog_rate is the mean of all the recognition rate of all the
% classes which are present in the dataset and
% is a widely used measure of the performance of the classifier when the
% data set is highly unbalanced.
%
% See also
%    SVM_TEST, CLASSIF_ERR, CLASSIF_RECOG

if  isstruct(truth)
    src = truth;
    truth = [src.objects.class];
end

% Normally, the test_set contains samples of all classes
recog_rates = zeros(1,max(truth));
gdTruth = truth(:,test_set);

parfor k = 1:max(truth)

    mask = k == gdTruth;
    good_elts = find(labels==k & mask);

    mask1 = numel(find(mask==1));
    recog_rates(k)=numel(good_elts)/mask1;

end

err_rates = 1 - recog_rates;
mean_err_rate = 1 - mean(recog_rates);

end


function [out_img, criterion] = TVdenoising(img, method, num_steps, lambda, clear_img, alpha, showfigs)

[H, W] = size(img);
N = H * W;

%% method aspects
switch method
    case 'ROFalg1'
        alg = 1;
        Lone = 0;
        huber = 0;
    case 'ROFalg2'
        alg = 2;
        Lone = 0;
        huber = 0;
    case 'TVL1ROFalg1'
        alg = 1;
        Lone = 1;
        huber = 0;
    case 'HuberROFalg3'
        alg = 3;
        Lone = 0;
        huber = 1;
    case 'HuberL1ROFalg1'
        alg = 1;
        Lone = 1;
        huber = 1;
    otherwise
        disp(['Unknown method: ' method]);
        return;
end

%% parameters
L = sqrt(8);

switch alg
    case 0      % Arrow-Hurwics version of Alg 2 (AHMOD)
        tau = 1/L;
        sigma = 1/L;
        gamma = 0.35 * lambda;
        theta = 0;
    case 1
        tau = 0.01;
        sigma = 1/(tau * L * L);
        theta = 1;
    case 2
        tau = 1/L;
        sigma = 1/L;
        gamma = 0.35*lambda;
    case 3
        gamma = lambda;
        delta = alpha;
        mu = 2 * sqrt(gamma * delta) / L;
        theta = 1/(1+mu);
        tau = mu / (2 * gamma);
        sigma = mu / (2 * delta);
end

%% initial solution
% primal task variables
u = img(:);
ubar = u;

% dual task variable
p = zeros(N * 2, 1);

%% precomputed
nabla = make_derivatives_mine(H, W);
divop = nabla';
% divop = make_divop(H, W);
denom = 1 + tau * lambda;

%% initial criterion value
if (Lone)
    lambda_denom = 1;
else
    lambda_denom = 2;
end

criterion = zeros(1, num_steps+1);
criterion(1) = Fval(u, img, alpha, huber) + lambda / lambda_denom * Gval(u, img, Lone);

%% plot the initial state
if showfigs
    fh1 = sfigure;

    imshow([img reshape(u, H, W)]);

    fh2 = sfigure;
    plot(0, criterion(1), 'b-');
    xlabel('step');
    ylabel('J(u)');
end

%% main loop
for step = 1:num_steps
    disp(['step: ' num2str(step)]);

    % ------ update p^n+1 ------
    p = p + sigma * nabla * ubar;

    if huber
        p = p / (1 + sigma * alpha);
    end

    % projection of p onto L2 ball
    p_len = sqrt(p(1:N).^2 + p(N+1:end).^2);
    p_len = max(1, p_len);
    p = p ./ repmat(p_len, 2, 1);

    % ----- update u^n+1 ------
    divp = divop * p;
    u_tilde = u - tau * divp;

    if Lone
        dif = u_tilde - img(:);
        idx1 = dif > tau * lambda;
        idx2 = dif < -tau * lambda;
        idx3 = abs(dif) <= tau * lambda;
        u_new = zeros(size(u));
        u_new(idx1) = u_tilde(idx1) - tau * lambda;
        u_new(idx2) = u_tilde(idx2) + tau * lambda;
        u_new(idx3) = img(idx3);
    else
        if alg == 2 || alg == 0
            denom = 1 + tau * lambda;
        end
        u_new = (u_tilde + tau * lambda * img(:)) / denom;
    end

    % ----- update tau and sigma -----
    if alg == 2
        theta = 1/(sqrt(1+2*gamma*tau));
        tau = tau * theta;
        sigma = sigma / theta;
    end
    if alg == 0
        theta_tmp = 1/(sqrt(1+2*gamma*tau));
        tau = tau * theta_tmp;
        sigma = sigma / theta_tmp;
    end

    % update ubar^n+1
    ubar = u_new + theta * (u_new - u);

    u = u_new;

    % compute the criterion function value
    criterion(step+1) = Fval(u, clear_img, alpha, huber) + lambda / lambda_denom * Gval(u, img, Lone);

    % plot current result
    if showfigs
        sfigure(fh1);
        imshow([img reshape(u, H, W)]);
        drawnow;

        sfigure(fh2);
        plot(0:step, criterion(1:step+1), 'b-');
        xlabel('step');
        ylabel('J(u)');
        drawnow;
    end
end

out_img = u;


function COFMEn = COFMEn(data, m, r,fs)
%******************************************************
% $ This function is usded for calcualting the cofficient of fuzzy measure entropy (COFMEn) for physiological signal time sequence.
% $ Using COFMEn aims to improve the performance of COSEn.
%
% $ Reference: Liu C Y, Li K, Zhao L N, Liu F, Zheng D C, Liu C C and Liu S
% T. Analysis of heart rate variability using fuzzy measure entropy.
% Computers in Biology and Medicine, 2013, 43(2): 100-108
%
% $ Variable declaration:
% data is RR time series
% m is embedding dimension (usually m=1)
% r is threshold value (usually r=30 ms)
% local threshold r_l=0.2, global threshold r_g=0.2,
% local weight of sequence segments' similarity n_l=2
% global weight of sequence segments' similarity n_g=2
%
% $ Author:  Chengyu Liu (bestlcy@sdu.edu.cn)
%           Institute of Biomedical Engineering,
%           Shandong University
% $Last updated:  2015.10.15
%
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information

% data=[108,108,109,109,108,108,110,107,109,109,109,109,108,109,108,108,108,108,107,108,108,108,108,107,109,107,108,107,108,108,108,108,108,108,108,109,108,109,108,108,109,108,108,108,109,107,108,107,107,108,108,108,109,107,108,108,108,108,108,108]*4;
% data=[109,108,108,109,109,109,108,109,108,108,109,108,108,109,108,108,108,109,108,109,109,109,108,108,109,108,108,109,109,109]*4;
% m=1;
% r=30;

N=length(data);
for i=1:N-m
    x1(i,:)=data(i:i+m-1);
    x2(i,:)=data(i:i+m);
end

ratio=0.4;
if N>20
%     Thr=ratio*N;
    Thr=5;
else
    Thr=5;
end

Min_numerator=0;
kk=0;
while Min_numerator<Thr
    [Min_numerator,r]=ReGet_min_numerator(x1,x2,N,m,r,fs);
    kk=kk+1;
end
if kk==1 || kk>20
   if N<20
        r=r-1;
    else
        r=r-1000/fs;
    end
end

if Min_numerator==N-m
    while Min_numerator>=Thr
        [Min_numerator,r]=ReGet_min_numerator2(x1,x2,N,m,r,fs);
    end
    if r<0
        r=r+2;
    else
        r=r+1;
    end
end

x=data;
r_l   = r;
r_g   = r;
n_l   = 2;
n_g   = 2;
%% m=2
D_l   = zeros(N-m,1); % initiate the mean local distance vector
D_g   = zeros(N-m,1); % initiate the mean global distance vector
for i = 1:N-m
    distance_l = zeros(N-m,1); % initiate the local distance vector for the ith vector
    distance_g = zeros(N-m,1); % initiate the global distance vector for the ith vector
    for j = 1:N-m
        if j==i
            d_l = 0;
            d_g = 0;
        else
        d_l = max(abs(x(i:i+m-1)-mean(x(i:i+m-1))-x(j:j+m-1)+mean(x(j:j+m-1))));
        d_g = max(abs(x(i:i+m-1)-x(j:j+m-1)));
        end
        distance_l(j) = exp(-(d_l.^n_l/r_l));
        distance_g(j) = exp(-(d_g.^n_g/r_g));
    end
    D_l(i) = sum(distance_l)/(N-m-1);
    D_g(i) = sum(distance_g)/(N-m-1);
end
Bm_l = mean(D_l);
Bm_g = mean(D_g);

%% m=m+1
m=m+1;
D_l   = zeros(N-m,1);
D_g   = zeros(N-m,1);
for i = 1:N-m
    distance_l = zeros(N-m,1);
    distance_g = zeros(N-m,1);
    for j = 1:N-m
        if j==i
            d_l = 0;
            d_g = 0;
        else
        d_l = max(abs(x(i:i+m-1)-mean(x(i:i+m-1))-x(j:j+m-1)+mean(x(j:j+m-1))));
        d_g = max(abs(x(i:i+m-1)-x(j:j+m-1)));
        end
        distance_l(j) = exp(-(d_l.^n_l/r_l));
        distance_g(j) = exp(-(d_g.^n_g/r_g));
    end
    D_l(i) = sum(distance_l)/(N-m-1);
    D_g(i) = sum(distance_g)/(N-m-1);
end
Am_l = mean(D_l);
Am_g = mean(D_g);

%% Calculate local and global fuzzy measure entropy
FuzzyLMEn = -log(Am_l/Bm_l);  % local fuzzy measure entropy
FuzzyGMEn = -log(Am_g/Bm_g);  % global fuzzy measure entropy
%% Generate fuzzy measure entropy
COFMEn  = FuzzyLMEn+FuzzyGMEn+2*log(2*r/1000)-2*log(mean(data)/1000);


function [ nxy, bxy, fxy ] = pce_legendre_linear_assemble ( n, p )

%*****************************************************************************80
%
%% PCE_LEGENDRE_LINEAR_ASSEMBLE assembles a particular stochastic Galerkin matrix.
%
%  Discussion:
%
%    We wish to analyze a stochastic PDE of the form:
%
%      -div A(X,Y) grad U(X,Y) = F(X)
%
%    where
%
%      U is an unknown scalar function,
%      A is a given diffusion function,
%      F is a given right hand side function,
%      X represents dependence on spatial variables,
%      Y represents dependence on stochastic variables.
%
%    We let X be a space of finite element functions generated by piecewise linear
%    functions associated with a particular triangular dissection of the unit square.
%
%    We let Y be the space of polynomials over R^N with total degree at most P.
%
%    We seek solutions U in X cross Y using a polynomial chaos expansion approach.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of factors in the probability space.
%
%    Input, integer P, the maximum degree of the basis functions for the
%    probability space.
%
%    Output, integer NXY, the order of the matrix BXY.  NXY = NX * NY.
%
%    Output, real sparse BXY(NXY,NXY), the matrix.
%
%    Output, real FXY(NXY), the right hand side.
%
%  Local Parameters:
%
%    Local, real AREA_X, the area of the current element.
%
%    Local, integer BASIS_X_NUM, the number of finite element basis functions (3).
%
%    Local, integer BASIS_Y_DEGREE(1:N), describes a particular basis function
%    in Y, in terms of the degrees of its components.
%
%    Local, integer BASIS_Y_H(*), auxilliary "external" memory needed by
%    SUBCOMP_NEXT.
%
%    Local, logical BASIS_Y_MORE, is set FALSE before the first call to
%    SUBCOMP_NEXT.  SUBCOMP_NEXT returns the next subcomposition, and sets the
%    value of BASIS_Y_MORE to TRUE if there are even more subpositions that can
%    be produced, or FALSE if there are no more.
%
%    Local, integer BASIS_Y_T(*), auxilliary "external" memory needed by
%    SUBCOMP_NEXT.
%
%    Local, real BVEC(1:N,1:POINT_X_NUM), stores the KL expansion coefficients
%    of orders 1 through N at the spatial points POINT_X_P(1:2,1:POINT_X_NUM).
%
%    Local, real BZERO(1:POINT_X_NUM), stores the KL expansion coefficients
%    of order 0 at the spatial points POINT_X_P(1:2,1:POINT_X_NUM).
%
%    Local, real CLK(1:QUAD_X_NUM), contains the integral of the KL expansion
%    function for given values of the L-th probability basis function and the
%    K-th probability test function evaluated at the quadrature points in an
%    element.
%
%    Local, integer ELEMENT_X, the element being considered.
%
%    Local, integer ELEMENT_X_NUM, the number of elements in the finite element
%    problem.
%
%    Local, integer ELEMENT_X_NODE(1:3,1:ELEMENT_X_NUM);
%    ELEMENT_X_NODE(I,J) is the global index of local node I in element J.
%
%    Local, integer ELEMENT_X1_NUM, the number of (pairs) of elements in the
%    first spatial dimension in the finite element problem.
%
%    Local, integer ELEMENT_X2_NUM, the number of (pairs) of elements in the
%    second spatial dimension in the finite element problem.
%
%    Local, real FVEC(1:QUAD_X_NUM), stores the values of the function F which
%    is the right hand side of the PDE, at the quadrature points in an element.
%
%    Local, integer I, the index of the finite element test function
%    PHI(1:NX)(X).
%
%    Local, integer J, the index of the finite element basis function
%    PHI(1:NX)(X).
%
%    Local, integer K, the index of the probability space test function
%    PSI(1:NY)(Y).
%
%    Local, integer L, the index of the probability space basis function
%    PSI(1:NY)(Y).
%
%    Local, integer NODE_X_NUM, the number of nodes.
%
%    Local, real NODE_X_P(1:2,1:NODE_X_NUM), the coordinates of nodes.
%
%    Local, integer NODE_X1_NUM, the number of nodes in the first space
%    direction.
%
%    Local, integer NODE_X2_NUM, the number of nodes in the second space
%    direction.
%
%    Local, integer NX, the dimension of the finite element space.
%    NX = NODE_X_NUM for a finite element problem involving a scalar variable U.
%
%    Local, integer NY, = ( N + P)! / N! / P! = the dimension of the space Y,
%    the space of polynomials over R^N of total degree at most P.
%
%    Local, real PHI(1:BASIS_X_NUM,1:QUAD_X_NUM), the finite element basis
%    functions, evaluated at all the quadrature points in a particular element.
%
%    Local, real PHI_DX1(1:BASIS_X_NUM,1:QUAD_X_NUM), the derivative of the
%    finite element basis functions with respect to the first spatial variable,
%    evaluated at all the quadrature points in a particular element.
%
%    Local, real PHI_DX2(1:BASIS_X_NUM,1:QUAD_X_NUM), the derivative of the
%    finite element basis functions with respect to the second spatial variable,
%    evaluated at all the quadrature points in a particular element.
%
%    Local, real PHYS_X_P(1:2,1:QUAD_X_NUM), the "physical" coordinates of the
%    quadrature points in the current element.
%
%    Local, integer QUAD_X, the index of the current X quadrature point.
%
%    Local, integer QUAD_X_NUM, the order of the X quadrature rule.
%
%    Local, real QUAD_X_P(1:2,1:QUAD_X_NUM), the points for the X quadrature
%    rule.
%
%    Local, real QUAD_X_W(1:QUAD_X_NUM), the weights for the X quadrature rule.
%
%    Local, real T3(1:2,1:3), the coordinates of the nodes that define the
%    current element.
%
%    Local, real TABLE(1:P+1,1:P+1), a table of the values of integrals of the
%    1D basis functions in Z.  TABLE(D1+1,D2+1)
%    = Integral ( -1 <= Z <= +1 ) Z * PSI(D1)(Z) * PSI(D2)(Z) dZ.
%
%    Local, integer TEST_X_NUM, the number of finite element test functions (3).
%
%    Local, integer TEST_Y_DEGREE(1:N), describes a particular test function in
%    Y, in terms of the degrees of its components.
%
%    Local, integer TEST_Y_H(*), auxilliary "external" memory needed by
%    SUBCOMP_NEXT.
%
%    Local, logical TEST_Y_MORE, is set FALSE before the first call to
%    SUBCOMP_NEXT.  SUBCOMP_NEXT returns the next subcomposition, and sets the
%    value of TESST_Y_MORE to TRUE if there are even more subpositions that can
%    be produced, or FALSE if there are no more.
%
%    Local, integer TEST_Y_T(*), auxilliary "external" memory needed by
%    SUBCOMP_NEXT.
%
  FALSE = 0;
%
%  Some setup for the finite element calculation.
%
  basis_x_num = 3;
  test_x_num = 3;
  node_x1_num = 11;
  node_x2_num = 11;
  node_x_num = node_x1_num * node_x2_num;
  nx = node_x_num;

  node_x_p = grid_nodes_01 ( node_x1_num, node_x2_num );

  element_x1_num = node_x1_num - 1;
  element_x2_num = node_x2_num - 1;
  element_x_num = 2 * element_x1_num * element_x2_num;
  element_x_node = grid_t3_element ( element_x1_num, element_x2_num );

  quad_x_num = 13;
  [ quad_x_p, quad_x_w ] = triangle_rule_13 ( );
%
%  Some setup for the probability space.
%
  ny = i4_choose ( n + p, n );

  e = 1;
  table = legendre_linear_product ( p, e );

  for i = 1 : p + 1
    for j = 1 : p + 1
      if ( abs ( table(i,j) ) < 10000 * eps )
        table(i,j) = 0.0;
      end
    end
  end
%
%  Define the order of the matrix BXY,
%  Define BXY as a sparse matrix;
%  Define FXY as a column vector.
%
  nxy = nx * ny;
  bxy = sparse ( [], [], [], nxy, nxy );
  fxy(1:nxy,1) = 0.0;
%
%  LOOP L:
%  Generate the L-th basis function PSI(D,Y) in Y space.
%
  basis_y_degree(1:n) = 0;
  basis_y_more = FALSE;
  basis_y_h = [];
  basis_y_t = [];
  basis_y_n2 = [];
  basis_y_more2 = [];
  l = 0;

  while ( 1 )

    l = l + 1;

    [ basis_y_degree, basis_y_more, basis_y_h, basis_y_t, basis_y_n2, basis_y_more2 ] = ...
      subcomp_next ( p, n, basis_y_degree, basis_y_more, basis_y_h, basis_y_t, basis_y_n2, ...
      basis_y_more2 );
%
%  LOOP K:
%  Generate the K-th test function PSI(D,Y) in Y space.
%
    test_y_degree(1:n) = 0;
    test_y_more = FALSE;
    test_y_h = [];
    test_y_t = [];
    test_y_n2 = [];
    test_y_more2 = [];
    k = 0;

    while ( 1 )

      k = k + 1;

      [ test_y_degree, test_y_more, test_y_h, test_y_t, test_y_n2, test_y_more2 ] = ...
        subcomp_next ( p, n, test_y_degree, test_y_more, test_y_h, test_y_t, test_y_n2, ...
        test_y_more2 );

      for element_x = 1 : element_x_num
%
%  For ALL the quadrature points in this element, evaluate:
%  * PHI the basis functions;
%  * PHI_DX1 and PHI_DX2, basis function derivatives;
%  * PHYS_X_P, the physical coordinates of the quadrature points;
%  * BZERO and BVEC, the coefficient functions in the KL expansion for CLK;
%  * CLK, the KL function integrated against the probability basis and test functions;
%  * FVEC, the right hand side of the PDE.
%
        t3(1:2,1:3) = node_x_p(1:2,element_x_node(1:3,element_x));

        area_x = abs ( triangle_area_2d ( t3 ) );

        [ phi, phi_dx1, phi_dx2 ] = basis_mn_t3 ( t3, quad_x_num, quad_x_p );

        phys_x_p(1:2,1:quad_x_num) = reference_to_physical_t3 ( t3, quad_x_num, quad_x_p );

        [ bzero, bvec ] = b_evaluator ( n, quad_x_num, phys_x_p );

        if ( l == k )
          clk(1:quad_x_num) = bzero(1:quad_x_num);
        else
          clk(1:quad_x_num) = 0.0;
        end

        for i2 = 1 : n

          factor = 1.0;
          for i3 = 1 : n
            if ( i3 ~= i2 )
              if ( basis_y_degree(i3) ~= test_y_degree(i3) )
                factor = 0.0;
              end
            end
          end

          clk(1:quad_x_num) = clk(1:quad_x_num) ...
            + bvec(i2,1:quad_x_num) * table(basis_y_degree(i2)+1,test_y_degree(i2)+1)...
            * factor;

        end

        fvec = f_evaluator ( quad_x_num, phys_x_p );
%
%  Integrate over the finite element space X.
%
        for quad_x = 1 : quad_x_num
%
%  LOOP J:
%  All finite element basis functions J.
%  But we actually only look at those which are nonzero in this element.
%
          for basis_x = 1 : basis_x_num

            j = element_x_node(basis_x,element_x);
%
%  LOOP I:
%  Finite element test function I.
%  But we actually only look at those which are nonzero in this element.
%
            for test_x = 1 : test_x_num

              i = element_x_node(test_x,element_x);

              ik = ( k - 1 ) * node_x_num + i;
              jl = ( l - 1 ) * node_x_num + j;
%
%  If I is a boundary node, the equation associated with its test function
%  is replaced by a simple equation that enforces a zero Dirichlet condition.
%
              if ( i <= node_x1_num || ...
                        node_x1_num * ( node_x2_num - 1) + 1 <= i || ...
                        mod ( i, node_x1_num ) == 1 || ...
                        mod ( i, node_x1_num ) == 0 )

                if ( ik == jl )
                  bxy(ik,jl) = 1.0;
                  fxy(ik) = 0.0;
                end
%
%  If I is not a boundary node, the equation associated with its test function
%  generates a finite element equation.
%
              else

                bxy(ik,jl) = bxy(ik,jl) + quad_x_w(quad_x) * area_x ...
                  * clk(quad_x) ...
                  * ( phi_dx1(test_x,quad_x) * phi_dx1(basis_x,quad_x) ...
                    + phi_dx2(test_x,quad_x) * phi_dx2(basis_x,quad_x) );
%
%  The only nonzero result for the right hand side FXY occurs when K = 1,
%  since this is the single test function in Y which is the product of N
%  copies of the constant Legendre polynomial.
%
%  (This is true as long as there is no Y dependence in F(), and
%  assuming we are using Legendre polynomials.)
%
                if ( k == 1 )
                  psi0_integral = 1 / sqrt ( 2^n );
                  fxy(ik) = fxy(ik) + quad_x_w(quad_x) * area_x ...
                    * fvec(quad_x) * phi(test_x,quad_x) * psi0_integral;
                end

              end

            end
          end
        end

      end

      if ( test_y_more == FALSE )
        break
      end

    end

    if ( basis_y_more == FALSE )
      break
    end

  end

  return
end
function [ bzero, bvec ] = b_evaluator ( n, nx, x )

%*****************************************************************************80
%
%% B_EVALUATOR evaluates KL expansion coefficients for the diffusion coefficient.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of factors in the probability space.
%
%    Input, integer NX, the number of points.
%
%    Input, real X(1:2,1:NX), the point coordinates.
%
%    Output, real BZERO(1:NX), the zero-th order coefficient evaluated at X.
%
%    Output, real BVEC(1:N,1:NX), the coefficients of orders 1 to N,
%    evaluated at X.
%
  bzero(1:nx) = 1.0;
  bvec = zeros ( n, nx );
  for i = 1 : n
    bvec(i,1:nx) = sin ( i * x(1,1:nx) );
  end

  return
end
function [ phi, dphidx, dphidy ] = basis_mn_t3 ( t, n, p )

%*****************************************************************************80
%
%% BASIS_MN_T3: all bases functions at N points for a T3 element.
%
%  Discussion:
%
%    The routine is given the coordinates of the vertices of a triangle.
%    It works directly with these coordinates, and does not refer to a
%    reference element.
%
%    The sides of the triangle DO NOT have to lie along a coordinate
%    axis.
%
%    The routine evaluates the basis functions associated with each vertex,
%    and their derivatives with respect to X and Y.
%
%  Physical Element T3:
%
%            3
%           / \
%          /   \
%         /     \
%        /       \
%       1---------2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the vertices of the triangle.  It is common to list
%    these points in counter clockwise order.
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real P(2,N), the coordinates of the evaluation points.
%
%    Output, real PHI(3,N), the basis functions at the evaluation points.
%
%    Output, real DPHIDX(3,N), DPHIDY(3,N), the basis derivatives
%    at the evaluation points.
%
%  Local parameters:
%
%    Local, real AREA, is (twice) the area of the triangle.
%
  area = t(1,1) * ( t(2,2) - t(2,3) ) ...
       + t(1,2) * ( t(2,3) - t(2,1) ) ...
       + t(1,3) * ( t(2,1) - t(2,2) );

  phi(1,1:n) =     (  ( t(1,3) - t(1,2) ) * ( p(2,1:n) - t(2,2) )     ...
                    - ( t(2,3) - t(2,2) ) * ( p(1,1:n) - t(1,2) ) );
  dphidx(1,1:n) =   - ( t(2,3) - t(2,2) );
  dphidy(1,1:n) =     ( t(1,3) - t(1,2) );

  phi(2,1:n) =     (  ( t(1,1) - t(1,3) ) * ( p(2,1:n) - t(2,3) )     ...
                    - ( t(2,1) - t(2,3) ) * ( p(1,1:n) - t(1,3) ) );
  dphidx(2,1:n) =   - ( t(2,1) - t(2,3) );
  dphidy(2,1:n) =     ( t(1,1) - t(1,3) );

  phi(3,1:n) =     (  ( t(1,2) - t(1,1) ) * ( p(2,1:n) - t(2,1) )     ...
                    - ( t(2,2) - t(2,1) ) * ( p(1,1:n) - t(1,1) ) );
  dphidx(3,1:n) =   - ( t(2,2) - t(2,1) );
  dphidy(3,1:n) =     ( t(1,2) - t(1,1) );
%
%  Normalize.
%
  phi(1:3,1:n)    = phi(1:3,1:n) / area;
  dphidx(1:3,1:n) = dphidx(1:3,1:n) / area;
  dphidy(1:3,1:n) = dphidy(1:3,1:n) / area;

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

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

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

  else

    if ( 1 < t )
      h = 0;
    end

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

  end

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

  return
end
function fvec = f_evaluator ( n, x )

%*****************************************************************************80
%
%% F_EVALUATOR evaluates the finite element right hand side function F.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of point.
%
%    Input, real X(2,N), the point coordinates.
%
%    Output, real FVEC(N), the value of F at the points.
%
  fvec(1:n) = sin ( pi * x(1,1:n) ) .* sin ( pi * x(2,1:n) );

  return
end
function node_xy = grid_nodes_01 ( x_num, y_num )

%*****************************************************************************80
%
%% GRID_NODES_01 returns an equally spaced grid of nodes in the unit square.
%
%  Example:
%
%    X_NUM = 5
%    Y_NUM = 3
%
%    NODE_XY =
%    ( 0, 0.25, 0.5, 0.75, 1, 0,   0.25, 0.5, 0.75, 1,   0, 0.25, 0.5, 0.75, 1;
%      0, 0,    0,   0,    0, 0.5, 0.5,  0.5, 0.5,  0.5, 1, 1.0,  1.0, 1.0,  1 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer X_NUM, Y_NUM, the number of nodes in the X and Y directions.
%
%    Output, real NODE_XY(2,X_NUM*Y_NUM), the coordinates of the nodes.
%
  node_num = x_num * y_num;

  node_xy(1:2,1:node_num) = 0.0;

  for i = 1 : x_num
    node_xy(1,i:x_num:i+(y_num-1)*x_num) = ( i - 1 ) / ( x_num - 1 );
  end

  for j = 1 : y_num
    node_xy(2,1+(j-1)*x_num:j*x_num) = ( j - 1 ) / ( y_num - 1 );
  end

  return
end
function [ element_node ] = grid_t3_element ( nelemx, nelemy )

%*****************************************************************************80
%
%% GRID_T3_ELEMENT produces a grid of pairs of 3 node triangles.
%
%  Example:
%
%    Input:
%
%      NELEMX = 3, NELEMY = 2
%
%    Output:
%
%      ELEMENT_NODE =
%         1,  2,  5;
%         6,  5,  2;
%         2,  3,  6;
%         7,  6,  3;
%         3,  4,  7;
%         8,  7,  4;
%         5,  6,  9;
%        10,  9,  6;
%         6,  7, 10;
%        11, 10,  7;
%         7,  8, 11;
%        12, 11,  8.
%
%  Grid:
%
%    9---10---11---12
%    |\ 8 |\10 |\12 |
%    | \  | \  | \  |
%    |  \ |  \ |  \ |
%    |  7\|  9\| 11\|
%    5----6----7----8
%    |\ 2 |\ 4 |\ 6 |
%    | \  | \  | \  |
%    |  \ |  \ |  \ |
%    |  1\|  3\|  5\|
%    1----2----3----4
%
%  Reference Element T3:
%
%    |
%    1  3
%    |  |\
%    |  | \
%    S  |  \
%    |  |   \
%    |  |    \
%    0  1-----2
%    |
%    +--0--R--1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NELEMX, NELEMY, the number of elements along the
%    X and Y directions.  The number of elements generated will be
%    2 * NELEMX * NELEMY.
%
%    Output, integer ELEMENT_NODE(3,2*NELEMX*NELEMY), the nodes that form
%    each element.
%

%
%  Node labeling:
%
%    NW--NE
%     |\ |
%     | \|
%    SW--SE
%
  element = 0;
  element_node = zeros ( 3, 2 * nelemx * nelemy );
  for j = 1 : nelemy
    for i = 1 : nelemx

      sw = i     + ( j - 1 ) * ( nelemx + 1 );
      se = i + 1 + ( j - 1 ) * ( nelemx + 1 );
      nw = i     +   j       * ( nelemx + 1 );
      ne = i + 1 +   j       * ( nelemx + 1 );

      element = element + 1;

      element_node(1,element) = sw;
      element_node(2,element) = se;
      element_node(3,element) = nw;

      element = element + 1;

      element_node(1,element) = ne;
      element_node(2,element) = nw;
      element_node(3,element) = se;

    end
  end

  return
end
function value = i4_choose ( n, k )

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

  if ( mn < 0 )

    value = 0;

  elseif ( mn == 0 )

    value = 1;

  else

    mx = max ( k, n - k );
    value = mx + 1;

    for i = 2 : mn
      value = ( value * ( mx + i ) ) / i;
    end

  end

  return
end
function table = legendre_linear_product ( p, e )

%*****************************************************************************80
%
%% LEGENDRE_LINEAR_PRODUCT computes a linearly weighted Legendre product table.
%
%  Discussion:
%
%    Let L(i)(X) represent the Legendre polynomial of degree i.
%
%    For polynomial chaos applications, it is of interest to know the
%    value of the integrals of products of X with every possible pair
%    of basis functions.  That is, we'd like to form
%
%      Tij = Integral ( -1 <= X <= +1 ) X^E * L(i)(X) * L(j)(X) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer P, the maximum degree of the polyonomial factors.
%    0 <= P.
%
%    Input, integer E, the exponent of X in the integrand.
%    0 <= E.
%
%    Output, real TABLE(P+1,P+1), the table of integrals.  TABLE(I,J)
%    represents the weighted integral of X^E * L(i+1)(X) * L(j+1)(X).
%
  table(1:p+1,1:p+1) = 0.0;

  order = p + 1 + floor ( ( e + 1 ) / 2 );
  [ x_table, w_table ] = legendre_quadrature_rule ( order );

  for k = 1 : order

    x = x_table(k);
    l_table = legendre_polynomial ( p, x );
%
%  The following formula is an outer product in L_TABLE.
%
    if ( e == 0 )
      table(1:p+1,1:p+1) = table(1:p+1,1:p+1) ...
        + w_table(k) *       l_table(1:p+1)' * l_table(1:p+1);
    else
      table(1:p+1,1:p+1) = table(1:p+1,1:p+1) ...
        + w_table(k) * x^e * l_table(1:p+1)' * l_table(1:p+1);
    end

  end

  return
end
function l = legendre_polynomial ( p, x )

%*****************************************************************************80
%
%% LEGENDRE_POLYNOMIAL evaluates the Legendre polynomials L(0:P)(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Daniel Zwillinger, editor,
%    CRC Standard Mathematical Tables and Formulae,
%    30th Edition,
%    CRC Press, 1996.
%
%  Parameters:
%
%    Input, integer P, the highest evaluation degree.
%
%    Input, real X, the evaluation point.
%
%    Output, real L(1:P+1), the Legendre polynomials of order 0 through P at X.
%
  if ( p < 0 )
    l = [];
    return
  end
%
%  Allocate space.
%
  l(1:p+1) = 0.0;
%
%  Apply recursion.
%
  l(1) = 1.0;

  if ( 1 <= p )

    l(2) = x;

    for i = 2 : p

      l(i+1) = ( ( 2 * i - 1 ) * x * l(i)   ...
               - (     i - 1 ) *     l(i-1) ) ...
               / (     i     );

    end

  end
%
%  Normalize.
%
  l(1:p+1) = l(1:p+1) .* sqrt ( ( 1 : 2 : 2*p+1 ) / 2 );

  return
end
function [ xtab, weight ] = legendre_quadrature_rule ( order )

%*****************************************************************************80
%
%% LEGENDRE_QUADRATURE_RULE computes a Gauss-Legendre quadrature rule.
%
%  Discussion:
%
%    The integration interval is [ -1, 1 ].
%
%    The weight function is w(x) = 1.0.
%
%    The integral to approximate:
%
%      Integral ( -1 <= X <= 1 ) F(X) dX
%
%    The quadrature rule:
%
%      Sum ( 1 <= I <= NORDER ) WEIGHT(I) * F ( XTAB(I) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 May 2008
%
%  Author:
%
%    FORTRAN77 original version by Philip Davis, Philip Rabinowitz
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%  Parameters:
%
%    Input, integer ORDER, the order of the rule.
%    ORDER must be greater than 0.
%
%    Output, real XTAB(ORDER), the abscissas of the rule.
%
%    Output, real WEIGHT(ORDER), the weights of the rule.
%    The weights are positive, symmetric, and should sum to 2.
%
  xtab = zeros ( order, 1 );
  weight = zeros ( order, 1 );

  if ( order < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LEGENDRE_RULE - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of ORDER = %d\n', order );
    error ( 'LEGENDRE_RULE - Fatal error!' );
  end

  e1 = order * ( order + 1 );

  m = floor ( ( order + 1 ) / 2 );

  for i = 1 : floor ( ( order + 1 ) / 2 )

    mp1mi = m + 1 - i;

    t = ( 4 * i - 1 ) * pi / ( 4 * order + 2 );

    x0 = cos(t) * ( 1.0 - ( 1.0 - 1.0 / ( order ) ) / ( 8 * order * order ) );

    pkm1 = 1.0;
    pk = x0;

    for k = 2 : order
      pkp1 = 2.0 * x0 * pk - pkm1 - ( x0 * pk - pkm1 ) / k;
      pkm1 = pk;
      pk = pkp1;
    end

    d1 = order * ( pkm1 - x0 * pk );

    dpn = d1 / ( 1.0 - x0 * x0 );

    d2pn = ( 2.0 * x0 * dpn - e1 * pk ) / ( 1.0 - x0 * x0 );

    d3pn = ( 4.0 * x0 * d2pn + ( 2.0 - e1 ) * dpn ) / ( 1.0 - x0 * x0 );

    d4pn = ( 6.0 * x0 * d3pn + ( 6.0 - e1 ) * d2pn ) / ( 1.0 - x0 * x0 );

    u = pk / dpn;
    v = d2pn / dpn;
%
%  Initial approximation H:
%
    h = - u * ( 1.0 + 0.5 * u * ( v + u * ( v * v - d3pn / ( 3.0 * dpn ) ) ) );
%
%  Refine H using one step of Newton's method:
%
    p = pk + h * ( dpn + 0.5 * h * ( d2pn + h / 3.0 ...
      * ( d3pn + 0.25 * h * d4pn ) ) );

    dp = dpn + h * ( d2pn + 0.5 * h * ( d3pn + h * d4pn / 3.0 ) );

    h = h - p / dp;

    xtemp = x0 + h;

    xtab(mp1mi) = xtemp;

    fx = d1 - h * e1 * ( pk + 0.5 * h * ( dpn + h / 3.0 ...
      * ( d2pn + 0.25 * h * ( d3pn + 0.2 * h * d4pn ) ) ) );

    weight(mp1mi) = 2.0 * ( 1.0 - xtemp * xtemp ) / fx / fx;

  end

  if ( mod ( order, 2 ) == 1 )
    xtab(1) = 0.0;
  end
%
%  Shift the data up.
%
  nmove = floor ( ( order + 1 ) / 2 );
  ncopy = order - nmove;

  for i = 1 : nmove
    iback = order + 1 - i;
    xtab(iback) = xtab(iback-ncopy);
    weight(iback) = weight(iback-ncopy);
  end
%
%  Reflect values for the negative abscissas.
%
  for i = 1 : order - nmove
    xtab(i) = - xtab(order+1-i);
    weight(i) = weight(order+1-i);
  end

  return
end
function phy = reference_to_physical_t3 ( t, n, ref )

%*****************************************************************************80
%
%% REFERENCE_TO_PHYSICAL_T3 maps a reference point to a physical point.
%
%  Discussion:
%
%    Given the vertices of an order 3 physical triangle and a point
%    (XSI,ETA) in the reference triangle, the routine computes the value
%    of the corresponding image point (X,Y) in physical space.
%
%    Note that this routine may also be appropriate for an order 6
%    triangle, if the mapping between reference and physical space
%    is linear.  This implies, in particular, that the sides of the
%    image triangle are straight and that the "midside" nodes in the
%    physical triangle are halfway along the sides of
%    the physical triangle.
%
%    The T3 reference element is suggested by the following diagram:
%
%    |
%    1  3
%    |  |\
%    |  | \
%    S  |  \
%    |  |   \
%    |  |    \
%    0  1-----2
%    |
%    +--0--R--1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the coordinates of the vertices.  The vertices are assumed
%    to be the images of (0,0), (1,0) and (0,1) respectively.
%
%    Input, integer N, the number of points to transform.
%
%    Input, real REF(2,N), the coordinates of points in the reference space.
%
%    Output, real PHY(2,N), the coordinates of the corresponding points in the
%    physical space.
%
  phy = zeros ( 2, n );

  for i = 1 : 2

    phy(i,1:n) = t(i,1) * ( 1.0 - ref(1,1:n) - ref(2,1:n) ) ...
               + t(i,2) *        ref(1,1:n)                ...
               + t(i,3) *                     ref(2,1:n);
  end

  return
end
function [ a, more, h, t, n2, more2 ] = subcomp_next ( n, k, a, more, h, t, ...
  n2, more2 )

%*****************************************************************************80
%
%% SUBCOMP_NEXT computes the next subcomposition of N into K parts.
%
%  Discussion:
%
%    A composition of the integer N into K parts is an ordered sequence
%    of K nonnegative integers which sum to a value of N.
%
%    A subcomposition of the integer N into K parts is a composition
%    of M into K parts, where 0 <= M <= N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the integer whose subcompositions are desired.
%
%    Input, integer K, the number of parts in the subcomposition.
%
%    Input, integer A(K), the parts of the subcomposition.
%
%    Input, logical MORE, set to FALSE by the user to start the computation.
%
%    Input, integer H, T, N2, MORE2, internal parameters needed for the
%    computation.  The user may need to initialize these before the
%    very first call, but these initial values are not important.
%    The user should not alter these parameters once the computation
%    begins.
%
%    Output, integer A(K), the parts of the subcomposition.
%
%    Output, logical MORE, set to FALSE by the routine to terminate
%    the computation.
%
%    Output, integer H, T, N2, MORE2, the updated values of the two internal
%    parameters.
%

%
%  The first computation.
%
  if ( ~more )

    n2 = 0;
    a(1:k) = 0;
    more2 = 0;
    h = 0;
    t = 0;

    more = 1;
%
%  Do the next element at the current value of N.
%
  elseif ( more2 )

    [ a, more2, h, t ] = comp_next ( n2, k, a, more2, h, t );

  else

    more2 = 0;
    n2 = n2 + 1;

    [ a, more2, h, t ] = comp_next ( n2, k, a, more2, h, t );

  end
%
%  Termination occurs if MORE2 = FALSE and N2 = N.
%
  if ( ~more2 && n2 == n )
    more = 0;
  end

  return
end
function area = triangle_area_2d ( t )

%*****************************************************************************80
%
%% TRIANGLE_AREA_2D computes the area of a triangle in 2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the triangle vertices.
%
%    Output, real AREA, the absolute area of the triangle.
%
  area = 0.5 * abs ( ...
      t(1,1) * ( t(2,2) - t(2,3) ) ...
    + t(1,2) * ( t(2,3) - t(2,1) ) ...
    + t(1,3) * ( t(2,1) - t(2,2) ) );

  return
end
function [ x, w ] = triangle_rule_13 ( )

%*****************************************************************************80
%
%% TRIANGLE_RULE_13 sets a 13 point quadrature rule for triangles.
%
%  Discussion:
%
%    The Integration region is points (X,Y) such that
%
%      0 <= X,
%      0 <= Y, and
%      X + Y <= 1.
%
%    Graph:
%
%      ^
%    1 | *
%      | |\
%    Y | | \
%      | |  \
%    0 | *---*
%      +------->
%        0 X 1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gilbert Strang, George Fix,
%    An Analysis of the Finite Element Method,
%    Prentice Hall, 1973, page 184,
%    ISBN: 096140888X,
%    LC: TA335.S77.
%
%  Parameters:
%
%    Output, real X(2,13), the abscissas.
%
%    Output, real W(13), the weights.
%
  a = 0.479308067841923;
  b = 0.260345966079038;
  c = 0.869739794195568;
  d = 0.065130102902216;
  e = 0.638444188569809;
  f = 0.312865496004875;
  g = 0.048690315425316;
  h = 1.0 / 3.0;
  t = 0.175615257433204;
  u = 0.053347235608839;
  v = 0.077113760890257;
  w = -0.149570044467670;

  x(1:2,1:13) = [ a, b, b, c, d, d, e, e, f, f, g, g, h;
                  b, a, b, d, c, d, f, g, e, g, e, f, h ];

  w(1:13) = [ t, t, t, u, u, u, v, v, v, v, v, v, w ];

  return
end


% ATAN   Inverse tangent, result in radians.
%    ATAN(X) is the arctangent of the elements of X.
%
%    See also ATAN2, TAN, ATAND, ATAN2D.
%
%    Reference page in Doc Center
%       doc atan
%
%    Other functions named atan
%
%       codistributed/atan    gpuArray/atan    sym/atan    ts/atan
%


function legendre_set_test ( )

%*****************************************************************************80
%
%% LEGENDRE_SET_TEST tests LEGENDRE_SET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 November 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LEGENDRE_SET_TEST\n' );
  fprintf ( 1, '  LEGENDRE_SET returns points and weights of \n' );
  fprintf ( 1, '  Gauss-Legendre quadrature rules.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   N               1             X^4           Runge\n' );
  fprintf ( 1, '\n' );

  for n = 1 : 10
    [ x, w ] = legendre_set ( n );
    e1 = sum ( w(1:n,1) );
    e2 = w(1:n,1)' * x(1:n,1).^4;
    e3 = w(1:n,1)' * ( 1.0 ./ ( 1.0 + 25.0 * x(1:n,1).^2 ) );
    fprintf ( 1, '  %2d  %14.6g  %14.6g  %14.6g\n', n, e1, e2, e3 );
  end

  return
end


%% Machine Learning Online Class
%  Exercise 5 | Regularized Linear Regression and Bias-Variance
%
%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  exercise. You will need to complete the following functions:
%
%     linearRegCostFunction.m
%     learningCurve.m
%     validationCurve.m
%
%  For this exercise, you will not need to change any code in this file,
%  or any other files other than those mentioned above.
%

%% Initialization
clear ; close all; clc

%% =========== Part 1: Loading and Visualizing Data =============
%  We start the exercise by first loading and visualizing the dataset.
%  The following code will load the dataset into your environment and plot
%  the data.
%

% Load Training Data
fprintf('Loading and Visualizing Data ...\n')

% Load from ex5data1:
% You will have X, y, Xval, yval, Xtest, ytest in your environment
load ('ex5data1.mat');

% m = Number of examples
m = size(X, 1);

% Plot training data
plot(X, y, 'rx', 'MarkerSize', 10, 'LineWidth', 1.5);
xlabel('Change in water level (x)');
ylabel('Water flowing out of the dam (y)');

fprintf('Program paused. Press enter to continue.\n');
% pause;

%% =========== Part 2: Regularized Linear Regression Cost =============
%  You should now implement the cost function for regularized linear
%  regression.
%

theta = [1 ; 1];
J = linearRegCostFunction([ones(m, 1) X], y, theta, 1);

fprintf(['Cost at theta = [1 ; 1]: %f '...
         '\n(this value should be about 303.993192)\n'], J);

fprintf('Program paused. Press enter to continue.\n');
% pause;

%% =========== Part 3: Regularized Linear Regression Gradient =============
%  You should now implement the gradient for regularized linear
%  regression.
%

theta = [1 ; 1];
[J, grad] = linearRegCostFunction([ones(m, 1) X], y, theta, 1);

fprintf(['Gradient at theta = [1 ; 1]:  [%f; %f] '...
         '\n(this value should be about [-15.303016; 598.250744])\n'], ...
         grad(1), grad(2));

fprintf('Program paused. Press enter to continue.\n');
% pause;

%% =========== Part 4: Train Linear Regression =============
%  Once you have implemented the cost and gradient correctly, the
%  trainLinearReg function will use your cost function to train
%  regularized linear regression.
%
%  Write Up Note: The data is non-linear, so this will not give a great
%                 fit.
%

%  Train linear regression with lambda = 0
lambda = 0;
[theta] = trainLinearReg([ones(m, 1) X], y, lambda);

%  Plot fit over the data
plot(X, y, 'rx', 'MarkerSize', 10, 'LineWidth', 1.5);
xlabel('Change in water level (x)');
ylabel('Water flowing out of the dam (y)');
hold on;
plot(X, [ones(m, 1) X]*theta, '--', 'LineWidth', 2)
hold off;

fprintf('Program paused. Press enter to continue.\n');
% pause;

%% =========== Part 5: Learning Curve for Linear Regression =============
%  Next, you should implement the learningCurve function.
%
%  Write Up Note: Since the model is underfitting the data, we expect to
%                 see a graph with "high bias" -- slide 8 in ML-advice.pdf
%

lambda = 0;
[error_train, error_val] = ...
    learningCurve([ones(m, 1) X], y, ...
                  [ones(size(Xval, 1), 1) Xval], yval, ...
                  lambda);

plot(1:m, error_train, 1:m, error_val);
title('Learning curve for linear regression')
legend('Train', 'Cross Validation')
xlabel('Number of training examples')
ylabel('Error')
axis([0 13 0 150])

fprintf('# Training Examples\tTrain Error\tCross Validation Error\n');
for i = 1:m
    fprintf('  \t%d\t\t%f\t%f\n', i, error_train(i), error_val(i));
end

fprintf('Program paused. Press enter to continue.\n');
% pause;

%% =========== Part 6: Feature Mapping for Polynomial Regression =============
%  One solution to this is to use polynomial regression. You should now
%  complete polyFeatures to map each example into its powers
%

p = 8;

% Map X onto Polynomial Features and Normalize
X_poly = polyFeatures(X, p);
[X_poly, mu, sigma] = featureNormalize(X_poly);  % Normalize
X_poly = [ones(m, 1), X_poly];                   % Add Ones

% Map X_poly_test and normalize (using mu and sigma)
X_poly_test = polyFeatures(Xtest, p);
X_poly_test = bsxfun(@minus, X_poly_test, mu);
X_poly_test = bsxfun(@rdivide, X_poly_test, sigma);
X_poly_test = [ones(size(X_poly_test, 1), 1), X_poly_test];         % Add Ones

% Map X_poly_val and normalize (using mu and sigma)
X_poly_val = polyFeatures(Xval, p);
X_poly_val = bsxfun(@minus, X_poly_val, mu);
X_poly_val = bsxfun(@rdivide, X_poly_val, sigma);
X_poly_val = [ones(size(X_poly_val, 1), 1), X_poly_val];           % Add Ones

fprintf('Normalized Training Example 1:\n');
fprintf('  %f  \n', X_poly(1, :));

fprintf('\nProgram paused. Press enter to continue.\n');
pause;

%% =========== Part 7: Learning Curve for Polynomial Regression =============
%  Now, you will get to experiment with polynomial regression with multiple
%  values of lambda. The code below runs polynomial regression with
%  lambda = 0. You should try running the code with different values of
%  lambda to see how the fit and learning curve change.
%

lambda = 0;
[theta] = trainLinearReg(X_poly, y, lambda);

% Plot training data and fit
figure(1);
plot(X, y, 'rx', 'MarkerSize', 10, 'LineWidth', 1.5);
plotFit(min(X), max(X), mu, sigma, theta, p);
xlabel('Change in water level (x)');
ylabel('Water flowing out of the dam (y)');
title (sprintf('Polynomial Regression Fit (lambda = %f)', lambda));

figure(2);
[error_train, error_val] = ...
    learningCurve(X_poly, y, X_poly_val, yval, lambda);
plot(1:m, error_train, 1:m, error_val);

title(sprintf('Polynomial Regression Learning Curve (lambda = %f)', lambda));
xlabel('Number of training examples')
ylabel('Error')
axis([0 13 0 100])
legend('Train', 'Cross Validation')

fprintf('Polynomial Regression (lambda = %f)\n\n', lambda);
fprintf('# Training Examples\tTrain Error\tCross Validation Error\n');
for i = 1:m
    fprintf('  \t%d\t\t%f\t%f\n', i, error_train(i), error_val(i));
end

fprintf('Program paused. Press enter to continue.\n');
% pause;

%% =========== Part 8: Validation for Selecting Lambda =============
%  You will now implement validationCurve to test various values of
%  lambda on a validation set. You will then use this to select the
%  "best" lambda value.
%

[lambda_vec, error_train, error_val] = ...
    validationCurve(X_poly, y, X_poly_val, yval);

close all;
plot(lambda_vec, error_train, lambda_vec, error_val);
legend('Train', 'Cross Validation');
xlabel('lambda');
ylabel('Error');

fprintf('lambda\t\tTrain Error\tValidation Error\n');
for i = 1:length(lambda_vec)
  fprintf(' %f\t%f\t%f\n', ...
            lambda_vec(i), error_train(i), error_val(i));
end

fprintf('Program paused. Press enter to continue.\n');
pause;


function snowfall_display ( )

%*****************************************************************************80
%
%% SNOWFALL_DISPLAY plots yearly snowfall data side by side.
%
%  Discussion:
%
%    The file "snowfall.txt" contains snowfall records.
%    Each line contains the year, the snowfall in inches for 8 months, and
%    the total.
%
%    We want to plot the yearly snowfall curves for 2000 through 2010,
%    side by side in a 3D plot.
%
%    We can use PLOT3(X,Y,Z) to plot a single 3D curve.
%
%    We can use HOLD ON / HOLD OFF to force multiple 3D curves to appear
%    together.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 November 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SNOWFALL_DISPLAY\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Demonstrate how to display several curves side by side.\n' );
  fprintf ( 1, '  Here, each curve represents snowfall over a given year.\n' );
%
%  Load all the snowfall data.
%
  data = load ( 'snowfall.txt' );
%
%  Column 1 contains the years.
%
  years = data ( :, 1 );
  offset = years(1) - 1;
%
%  Columns 2 through 9 are snowfall amounts.
%
  snow = data ( :, 2:9);
%
%  Column 10 is the snowfall total.
%
  total = data ( :, 10 );
%
%  Create figure #1 and clear it.
%
  figure ( 1 )
  clf
%
%  Call HOLD ON so multiple curves share one image.
%
%  Call PLOT3 to draw each snowfall curve.
%
  hold on
  for year = 2000 : 2010
    x(1:8) = year;
    y(1:8) = ( 1 : 8 );
    z(1:8) = snow(year-offset,1:8);
    plot3 ( x, y, z, 'Linewidth', 3 );
  end
%
%  Annotate the plot.
%
  grid on
  xlabel ( 'Year' );
  ylabel ( 'Month' );
  set ( gca, 'yTick', 1 : 8 );
  set ( gca, 'yTickLabel', { 'Oct', 'Nov', 'Dec', 'Jan', 'Feb', 'Mar', 'Apr', 'May' } );
  zlabel ( 'Inches of Snow' );
  title ( 'Michigan Tech Snowfall Records', 'Fontsize', 16 );
%
%  Set the 3D viewing angles in degrees.
%
  azimuth = 45.0;
  elevation = 35.0;
  view ( azimuth, elevation );
%
%  Call HOLD OFF because we are done concatenating plots.
%
  hold off
%
%  Save a copy of the plot as a PNG file.
%
  filename = 'snowfall.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  PNG image saved as "%s"\n', filename );

  return
end


function cordic_test001 ( )

%*****************************************************************************80
%
%% TEST001 demonstrates the use of COSSIN_CORDIC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 June 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST001:\n' );
  fprintf ( 1, '  COSSIN_CORDIC computes the cosine and sine of an angle\n' );
  fprintf ( 1, '  using the CORDIC algorithm.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      A    N           Cos(A)     Cos(A)      Difference\n' );
  fprintf ( 1, '                     Tabulated   CORDIC\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, a, c1 ] = cos_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '\n' );

    for n = 0 : 5 : 25

      v = cossin_cordic ( a, n );
      c2 = v(1);
      d = c1 - c2;

      fprintf ( 1, '  %12f  %4d  %16.8f  %16.8f  %9e\n', a, n, c1, c2, d );

    end

  end

  return
end


function [ a, b ] = p23_lim ( dim_num )

%*****************************************************************************80
%
%% P23_LIM returns the integration limits for problem 23.
%
%  Discussion:
%
%    Because the integration region is the interior of the unit simplex,
%    the integration limits simply specify the limits of a box containing
%    the integration region.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Output, real A(DIM_NUM), B(DIM_NUM), the lower and upper
%    limits of integration.
%
  a(1:dim_num) = 0.0;
  b(1:dim_num) = 1.0;

  return
end


% Demo script on various ways to create an ellipse.
%
%   output = create_ellipse_demo(input)
%
%   Example
%   create_ellipse_demo
%
%   See also
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% INRAE - BIA Research Unit - BIBS Platform (Nantes)
% Created: 2022-09-09,    using Matlab 9.12.0.1884302 (R2022a)
% Copyright 2022 INRAE.

%% Use explicit representation

center = [50 50];
radii = [40 20];
theta = 30;
elli0 = [center radii theta];

figure; hold on; axis square; axis([0 100 0 100]);
drawEllipse(elli0, 'lineWidth', 2, 'color', 'b');

%% Fit an ellipse to a set of points

% choose several points on the ellipse, and add some noise
nPoints = 100;
ti = rand(nPoints, 1) * 2 * pi;
pts = ellipsePoint(elli0, ti) + randn(nPoints, 2) * 2;

% fit the ellipse to the set of points
elli = fitEllipse(pts);

% display points and fit result
figure; hold on; axis square; axis([0 100 0 100]);
drawPoint(pts, 'linewidth', 1, 'color', 'b');
drawEllipse(elli, 'lineWidth', 2, 'color', 'm');

%% Equivalent ellipse from a set of points

% generate random points within the ellipse,
% with a density equal to 1 (1 point per unit square on average)
nPoints = round(ellipseArea(elli));
pts0 = zeros(nPoints, 2);
for iPoint = 1:nPoints
    while true
        pt = rand([1 2]) * 100;
        if isPointInEllipse(pt, elli0)
            pts(iPoint,:) = pt;
            break;
        end
    end
end

% computes equivalent ellipse
elli = equivalentEllipse(pts);

% display result
figure; hold on; axis square; axis([0 100 0 100]);
drawPoint(pts, 'b.');
drawEllipse(elli, 'lineWidth', 2, 'color', 'm');
drawEllipseAxes(elli, 'lineWidth', 2, 'color', 'm');


%{
    Tests the extended "analysis" form of basis pursuit de-noising

    min_1 alpha*||W_1 x||_1 + beta*|| W_2 x ||_1
s.t.
    || A(x) - b || <= eps

The solvers solve a regularized version

see also test_sBP.m and test_sBPDN.m and test_sBPDN_W.m

%}

% Before running this, please add the TFOCS base directory to your path
myAwgn = @(x,snr) x + ...
        10^( (10*log10(sum(abs(x(:)).^2)/length(x(:))) - snr)/20 )*randn(size(x));

% Try to load the problem from disk
fileName = fullfile('reference_solutions','basispursuit_WW_problem1_smoothed_noisy');
randn('state',34324);
rand('state',34324);

N = 512;
M = round(N/2);
K = round(M/5);

A = randn(M,N);
x = zeros(N,1);

% introduce a sparsifying transform "W"
d  = round(4*N);     % redundant
Wf = @(x) dct(x,d);  % zero-padded DCT
downsample  = @(x) x(1:N,:);
Wt = @(y) downsample( idct(y) );    % transpose of W
W  = Wf(eye(N));     % make an explicit matrix for CVX

% and add another transform:
d2 = round(2*N);
W2 = randn(d2,N);
if exist([fileName,'.mat'],'file')
    load(fileName);
    fprintf('Loaded problem from %s\n', fileName );
else

    % Generate a new problem

    alpha = 1;
    beta = 2;

    % the signal x consists of several pure tones at random frequencies
    for k = 1:K
        x = x + randn()*sin( rand()*pi*(1:N) + 2*pi*rand() ).';
    end

    b_original = A*x;
    snr = 40;  % SNR in dB
    b   = myAwgn(b_original,snr);
    EPS = norm(b-b_original);
    x_original = x;

    mu = .01*norm(Wf(x),Inf);
    x0 = zeros(N,1);

    % get reference via CVX
    tic
    cvx_begin
        cvx_precision high
        variable xcvx(N,1)
        minimize alpha*norm(W*xcvx,1) + beta*norm(W2*xcvx,1) + ...
            mu/2*sum_square(xcvx-x0)
        subject to
            norm(A*xcvx - b ) <= EPS
    cvx_end
    time_IPM = toc;
    x_ref = xcvx;
    objF        = @(x)alpha*norm(W*x,1) +beta*norm(W2*x,1)+ mu/2*norm(x-x0).^2;
    obj_ref     = objF(x_ref);

    save(fileName,'x_ref','b','x_original','mu',...
        'EPS','b_original','obj_ref','x0','time_IPM','snr',...
        'd','d2','alpha','beta');
    fprintf('Saved data to file %s\n', fileName);

end

[M,N]           = size(A);
norm_x_ref      = norm(x_ref);
norm_x_orig     = norm(x_original);
er_ref          = @(x) norm(x-x_ref)/norm_x_ref;
er_signal       = @(x) norm(x-x_original)/norm_x_orig;
resid           = @(x) norm(A*x-b)/norm(b);  % change if b is noisy

fprintf('\tA is %d x %d\n', M, N );
fprintf('\tl1-norm solution and original signal differ by %.2e (mu = %.2e)\n', ...
    norm(x_ref - x_original)/norm(x_original),mu );

%% Call the TFOCS solver
objF            = @(x)alpha*norm(W*x,1) +beta*norm(W2*x,1)+ mu/2*norm(x-x0).^2;
infeasF         = @(x)norm(A*x-b) - EPS;
er              = er_ref;  % error with reference solution (from IPM)
opts            = [];
opts.errFcn     = { @(f,dual,primal) er(primal), ...
                    @(f,dual,primal) obj_ref - f, ...
                    @(f,dual,primal) infeasF(primal) };
opts.maxIts     = 2000;
opts.tol        = 1e-10;
% opts.normA2     = norm(A*A');
% opts.normW2     = norm(W'*W);
z0  = [];   % we don't have a good guess for the dual
tic;
[ x, out, optsOut ] = solver_sBPDN_WW( A, alpha,W,beta,W2,b, EPS, mu, x0, z0, opts );
time_TFOCS = toc;
fprintf('x is sub-optimal by %.2e, and infeasible by %.2e\n',...
    objF(x) - obj_ref, infeasF(x) );

fprintf('Solution has %d nonzeros.  Error vs. IPM solution is %.2e\n',...
    nnz(x), er(x) );

% Check that we are within allowable bounds
if out.err(end,1) < 1e-3
    disp('Everything is working');
else
    error('Failed the test');
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

%-----------------------------------------------------------------------
% This script demonstrates the use of function LINPROG on the basis of
% the C-VaR portfolio optimization problen given in:
% Uryasev, Rockafellar: Optimization of Conditional Value-at-Risk(1999)

clear
close all
clc
warning 'off'

tic

% load matrix data
% of annualized linear stock returns
load 'LinRetMat'

% M: number of samples
% N: number of assets
[M,N] = size(data);
% sample mean
mu = mean(data)';

% confidence level
beta = 0.99;

% number portfolios
NumPorts = 50;
% target returns
Rstar = min(mu):(max(mu)-min(mu))/(NumPorts-1):max(mu);
% objective function vector
f = objfCVaR(beta, M, N);
% constraints matrices
[A, b, Aeq, beq, lb, ub] = constCVaR(data, mu, Rstar(1));

CVaR    = zeros(NumPorts,1);
weights = zeros(N,NumPorts);

for i = 1:NumPorts
    beq(1) = -Rstar(i);
    [optimvar, CVaR(i)] = linprog(f, A, b, Aeq, beq, lb, ub);
    weights(:,i) = optimvar(1:N);
end

toc
warning 'on'

% Create figure
figure1 = figure('Color',[1 1 1]);
colormap('gray');

% plot efficient frontier
plot(CVaR,Rstar,'k','LineWidth',1.5)
legend('Mean-CVaR Frontier','Location','NorthWest')
set(gca,'xlim',[0 CVaR(end)],'ylim',[0 Rstar(end)])
title('Mean-CVaR Frontier','FontSize',17)
xlabel('$\beta$-CVaR','Interpreter','latex','FontName','Helvetia','FontSize',16)
ylabel('$\mu$','Interpreter','latex','FontName','Helvetia','FontSize',16,'Rotation',0)
ticks = get(gca,'YTick');
set(gca,'YTickLabel',[num2str(ticks'*100),repmat('%',length(ticks),1)])
ticks = get(gca,'XTick');
set(gca,'XTickLabel',[num2str(ticks'*100),repmat('%',length(ticks),1)])


function Cmatrix = correlation_3d_to_2d(C)
% Turn an k x k x n matrix of correlations into a 2-d flat matrix
%
% Cmatrix = correlation_3d_to_2d(C)
%
% C = [k x k x n],  each [k x k] matrix is inter-variable correlation
% matrix, one correlation matrix for each of n replicates.

% get data for prediction
%-----------------------------------

[j, k, n] = size(C);

if j ~= k, error('Not a series of correlation matrices'); end

E = eye(j);

nout = j * (j - 1) ./ 2;
Cmatrix = zeros(n, nout);

for i = 1:n

    Cmatrix(i, :) = squareform(C(:, :, i) - E);

end

end


function cg_rc_test02 ( )

%*****************************************************************************80
%
%% CG_RC_TEST02 tests CG_RC with the Wathen matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 January 2013
%
%  Author:
%
%    John Burkardt
%
  n = 79;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CG_RC_TEST02\n' );
  fprintf ( 1, '  Use CG_RC to solve a linear system\n' );
  fprintf ( 1, '  involving the Wathen matrix.\n' );

  nx = 5;
  ny = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  NX = %d\n', nx );
  fprintf ( 1, '  NY = %d\n', ny );
  fprintf ( 1, '  N  = %d\n', n );

  a = wathen ( nx, ny, n );

  seed = 123456789;
  [ x_exact, seed ] = r8vec_uniform_01 ( n, seed );

  size ( x_exact )
  size ( a )
  b(1:n) = a(1:n,1:n) * x_exact(1:n);

  x = zeros ( n, 1 );
%
%  Parameters we need for the stopping test.
%
  it = 0;
  it_max = 30;
  tol = 1.0E-05;
  bnrm2 = norm ( b(1:n) );
%
%  Set parameters for CG_RC.
%
  r = zeros ( n, 1 );
  z = zeros ( n, 1 );
  p = zeros ( n, 1 );
  q = zeros ( n, 1 );
  job = 1;
%
%  Repeatedly call CG_RC, and on return, do what JOB tells you.
%
  while ( 1 )

    [ x, r, z, p, q, job ] = cg_rc ( n, b, x, r, z, p, q, job );
%
%  Compute q = A * p.
%
    if ( job == 1 )

      q = a * p;
%
%  Solve M * z = r;
%
    elseif ( job == 2 )

      for i = 1 : n
        z(i) = r(i) / a(i,i);
      end
%
%  Compute r = r - A * x.
%
    elseif ( job == 3 )

      r = r - a * x;
%
%  Stopping test.
%
    elseif ( job == 4 )

      rnrm2 = norm ( r );

      if ( bnrm2 == 0.0 )
        if ( rnrm2 <= tol )
          break
        end
      else
        if ( rnrm2 <= tol * bnrm2 )
          break
        end
      end

      it = it + 1;

      if ( it_max <= it )
        fprintf ( 1, '\n' );
        fprintf ( 1, '  Iteration limit exceeded.\n' );
        fprintf ( 1, '  Terminating early.\n' );
        break
      end

    end

    job = 2;

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of iterations was %d\n', it );
  fprintf ( 1, '  Estimated error is %g\n', rnrm2 );
  err = max ( abs ( x_exact(1:n) - x(1:n) ) );
  fprintf ( 1, '  Loo error is %g\n', err );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     I      X(I)         X_EXACT(I)        B(I)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %14f  %14f  %14f\n', ...
      i, x(i), x_exact(i), b(i) );
  end

  return
  end


function suborder = lyness_suborder ( rule, suborder_num )

%*****************************************************************************80
%
%% LYNESS_SUBORDER returns the suborders for a Lyness rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    James Lyness, Dennis Jespersen,
%    Moderate Degree Symmetric Quadrature Rules for the Triangle,
%    Journal of the Institute of Mathematics and its Applications,
%    Volume 15, Number 1, February 1975, pages 19-32.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%
%    Input, integer SUBORDER_NUM, the number of suborders
%    of the rule.
%
%    Output, integer SUBORDER(SUBORDER_NUM), the suborders
%    of the rule.
%
  if ( rule == 0 )
    suborder = [ 1 ]';
  elseif ( rule == 1 )
    suborder = [ 3 ]';
  elseif ( rule == 2 )
    suborder = [ 1, 3 ]';
  elseif ( rule == 3 )
    suborder = [ 1, 3 ]';
  elseif ( rule == 4 )
    suborder = [ 1, 3, 3 ]';
  elseif ( rule == 5 )
    suborder = [ 3, 3  ]';
  elseif ( rule == 6 )
    suborder = [ 1, 3, 6  ]';
  elseif ( rule == 7 )
    suborder = [ 3, 3, 3 ]';
  elseif ( rule == 8 )
    suborder = [ 1, 3, 3 ]';
  elseif ( rule == 9 )
    suborder = [ 1, 3, 3, 3 ]';
  elseif ( rule == 10 )
    suborder = [ 3, 3, 6 ]';
  elseif ( rule == 11 )
    suborder = [ 1, 3, 3, 3, 6 ]';
  elseif ( rule == 12 )
    suborder = [ 1, 3, 3, 6 ]';
  elseif ( rule == 13 )
    suborder = [ 1, 3, 3, 6 ]';
  elseif ( rule == 14 )
    suborder = [ 1, 3, 3, 3, 6 ]';
  elseif ( rule == 15 )
    suborder = [ 1, 3, 3, 3, 6 ]';
  elseif ( rule == 16 )
    suborder = [ 3, 3, 3, 3, 3, 6 ]';
  elseif ( rule == 17 )
    suborder = [ 1, 3, 3, 3, 6 ]';
  elseif ( rule == 18 )
    suborder = [ 1, 3, 3, 3, 3, 6 ]';
  elseif ( rule == 19 )
    suborder = [ 1, 3, 3, 3, 3, 3, 6 ]';
  elseif ( rule == 20 )
    suborder = [ 3, 3, 3, 3, 3, 6, 6 ]';
  elseif ( rule == 21 )
    suborder = [ 1, 3, 3, 3, 3, 3, 6, 6 ]';
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LYNESS_SUBORDER - Fatal error!\n' );
    fprintf ( 1, '  Illegal RULE = %d\n', rule );
    error ( 'LYNESS_SUBORDER - Fatal error!' );
  end

  return
end


% Convert an adjacency matrix of a general graph to the adjacency matrix of
%          a simple graph (symmetric, no loops, no double edges, no weights)
%
% INPUTS: adjacency matrix, nxn
% OUTPUTs: adjacency matrix (nxn) of the corresponding simple graph
%
% Other routines used: symmetrize.m
% GB: last updated, Sep 6 2014

function adj=adj2simple(adj)

adj=adj>0; % make all edges weight 1
adj = symmetrize(adj);
adj = adj - diag(diag(adj)); % clear the diagonal (selfloops)


function hammersley_test01 ( )

%*****************************************************************************80
%
%% TEST01 tests I4_TO_HAMMERSLEY_SEQUENCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  I4_TO_HAMMERSLEY_SEQUENCE computes N elements of\n' );
  fprintf ( 1, '  a Hammersley sequence on a single call.\n' );
  fprintf ( 1, '  All arguments are specified explicitly.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  In this example, we compute the first 10 elements\n' );
  fprintf ( 1, '  of a "classical" Hammersley sequence, and then\n' );
  fprintf ( 1, '  the "last" 10 elements.\n' );

  nmax = 1000;

  dim_num = 4;
  n = 10;
  step = 1;
  seed(1:dim_num) = 0;
  leap(1:dim_num) = 1;
  base(1) = -nmax;
  for i = 2 : dim_num
    base(i) = prime ( i - 1 );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  DIM_NUM = %12d\n', dim_num );
  fprintf ( 1, '  N =    %12d\n', n );
  fprintf ( 1, '  STEP = %12d\n', step );
  i4vec_transpose_print ( dim_num, seed, '  SEED = ' );
  i4vec_transpose_print ( dim_num, leap, '  LEAP = ' );
  i4vec_transpose_print ( dim_num, base, '  BASE = ' );

  r = i4_to_hammersley_sequence ( dim_num, n, step, seed, leap, base );

  fprintf ( 1, '\n' );
  fprintf ( 1, '    STEP   Hammersley\n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %6d  ', step+j-1 );
    for i = 1 : dim_num
      fprintf ( 1, '%12f  ', r(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  We can jump ahead in the sequence by changing STEP.\n' );

  step = nmax - n + 1;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  STEP = %12d\n', step );

  r = i4_to_hammersley_sequence ( dim_num, n, step, seed, leap, base );

  fprintf ( 1, '\n' );
  fprintf ( 1, '    STEP   Hammersley\n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %6d  ', step+j-1 );
    for i = 1 : dim_num
      fprintf ( 1, '%12f  ', r(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


% GP_COV_SE - Squared exponential covariance function for Gaussian
% processes.
%
% [K, DK_LOGTHETA, DK_X2] = GP_COV_SE(X1, X2, LOGTHETA)

% Last modified 2010-10-06
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function [K, dK_logtheta, dK_x2] = gp_cov_se(x1, x2, logtheta)

% 2-norm distances
if ~isempty(x2)
  % Distances for covariances
  n1 = cols(x1);
  n2 = cols(x2);
  D = zeros(n1,n2);
  for i=1:n1
    for j=1:n2
      D(i,j) = norm(x1(:,i) - x2(:,j));
    end
  end
else
  % Distances for variances
  n = rows(x1);
  D = zeros(n,1);
end

% Covariance matrix
D2 = D.^2;
K = exp(logtheta(1)) * exp(-0.5*exp(-2*logtheta(2))*D2);

% Gradient for hyperparameters
if nargout >= 2
  dK_logtheta = zeros([size(D), 2]);
  dK_logtheta(:,:,1) = K;
  dK_logtheta(:,:,2) = K .* (-0.5*D2*exp(-2*logtheta(2))) .* (-2);
end

% Gradients for inputs x2
if nargout >= 3
  if isempty(x2)
    error('Can''t calculate gradient: x2 not given');
  end
  d = rows(x2); % dimensionality of inputs
  m = cols(x1); % number of other inputs
  n = cols(x2); % number of inputs
  dK_x2 = zeros([d,m,n]);
  for i=1:m
    for j=1:n
      dK_x2(:,i,j) = K(i,j) * (-0.5*exp(-2*logtheta(2))) * 2*(x2(:,j)-x1(:,i));
    end
  end
end

function [K, dK] = gpK_sqexp(D, p1, p2)

% [K, dK] = gpK_sqexp(D, p1, p2)
%
% Squared exponential covariance function for GP.
%
% p1 is the scale
% p2 is the length
%
K = p1^2*exp(-0.5*D.^2/(p2^2));

if nargout >= 2
  dK = zeros([size(D), 2]);
  dK(:,:,1) = K .* 2 / p1;
  dK(:,:,2) = K .* (-0.5*D.^2) .* (-2*p2^(-3));
end


function g = p10_g ( n, x )

%*****************************************************************************80
%
%% P10_G evaluates the gradient for problem 10.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real G(N), the gradient of the objective function.
%
  g = zeros ( n, 1 );

  g(1) = 2.0 * x(1) - 2000000.0 + 2.0 * x(1) * x(2) * x(2) - 4.0 * x(2);

  g(2) = 2.0 * x(2) - 0.000004 + 2.0 * x(1)^2 * x(2) - 4.0 * x(1);

  return
end


function [pass, u1, u2, info1, info2] = test_scalarODEdamping(pref)
% A nonlinear CHEBOP test. This test tests a scalar ODE, where no breakpoints
% occur. It solves the problem using chebcolloc1, chebcolloc2 and ultraS
% discretizations. The problem solved requires damping for the Newton iteration
% to converge.
%
% Asgeir Birkisson, May 2014.

%% Setup
dom = [0 pi];
if ( nargin == 0 )
    pref = cheboppref;
end

N = chebop(@(x,u) .05*diff(u,2) + cos(5*x).*sin(u), dom);
N.lbc = @(u) u - 2;
N.rbc = @(u) u - 3;
rhs = 0;

pref.bvpTol = 1e-10;

% Try different discretizations:

%% Start with chebcolloc2
pref.discretization = @chebcolloc2;
pref.bvpTol = 1e-13;
[u1, info1] = solvebvp(N, rhs, pref);
err(1) = norm(N(u1));

%% Change to chebcolloc1
pref.discretization = @chebcolloc1;
[u2, info2] = solvebvp(N, rhs, pref);
err(2) = norm(N(u2));

%% Change to ultraS
pref.discretization = @ultraS;
pref.bvpTol = 1e-12;
[u3, info3] = solvebvp(N, rhs, pref);
err(3) = norm(N(u3));

%% Did we pass?
% To pass, both residuals have to be small, but we should not expect u1 and u2
% to be identical!

tol = 1e-9;
pass = err < tol;

end


function [Fs spec] = PTSpec2d(Y, F, psd)
%% [Fs spec] = PTSpec2d(Y, F, psd)
%   computes standard fft on input data Y. F is sample frequency in Hz.

% ----------------------------------------------------------------------------------
% "THE BEER-WARE LICENSE" (Revision 42):
% <brian.white@queensu.ca> wrote this file. As long as you retain this notice you
% can do whatever you want with this stuff. If we meet some day, and you think
% this stuff is worth it, you can buy me a beer in return. -Brian White
% ----------------------------------------------------------------------------------

if psd
    % N = length(Y);
    % [psdx,Fs] = periodogram(Y,[], N-1, F*1000,'psd'); % power psd
    % [spec,Fs] = pspectrum(Y, F*1000, 'FrequencyResolution', 10);

    N = length(Y);
    Fs = ((F*1000)*(0:(N/2))/N);
    Y = Y.*hann(N)';
    Y = fft(Y);
    psdx = abs(Y).^2 / (F*1000*N); % (1/(F*N)) * abs(Y).^2  is exactly same as  abs(Y).^2 / (F*N), to
    psdx(2:end-1) = 2*psdx(2:end-1);
    psdx = psdx(1:N/2+1);
%     % scale to dB
     spec = 10 * log10(psdx)';
else
    N = length(Y);
    Fs = ((F*1000)*(0:(N/2))/N);
    Y = Y.*hann(N)';
    Y = fft(Y);
    spec = abs(Y) / N; % (1/(F*N)) * abs(Y).^2  is exactly same as  abs(Y).^2 / (F*N), to
    spec = spec(1:N/2+1)';
end

end