function points = getSpacedPoints(X1, X2, N, spacing)
%GETSPACEDPOINTS     Create vector of log or linear spaced points.
%
% DESCRIPTION:
%       getSpacedPoints generates a row vector of either logarithmically
%       or linearly spaced points between X1 and X2. When spacing is set to
%       'linear', the function is identical to the inbuilt linspace
%       function. When spacing is set to 'log', the function is similar to
%       the inbuilt logspace function, except that X1 and X2 define the
%       start and end numbers, not decades. For logarithmically spaced
%       points, X1 must be > 0. If N < 2, X2 is returned.
%
% USAGE:
%       points = getSpacedPoints(X1, X2)
%       points = getSpacedPoints(X1, X2, N)
%       points = getSpacedPoints(X1, X2, N, spacing)
%
% INPUTS:
%       X1          - starting points value
%       X2          - ending points value (where X2 > X1)
%
% OPTIONAL INPUTS:
%       N           - number of points in the vector (default = 100)
%       spacing     - 'log' or 'linear' spaced values (default = 'linear')
%
% OUTPUTS:
%       points      - row vector of equally spaced points
%
% ABOUT:
%       author      - Bradley E. Treeby
%       date        - 14th July 2005
%       last update - 1st December 2012
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave 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.
%
% k-Wave 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 Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% check for number of points input
if nargin < 3
    N = 100;
end

% check for spacing input
if nargin < 4
    spacing = 'linear';
end

% check if the end point is larger than the start point
if X2 <= X1
    error('X2 must be larger than X1');
end

% force N to be an integer
N = round(N);

if (N < 2)
    % return the end point if N < 2
    points = X2;
elseif (N == 2)
    % return the start and end points if N = 2
    points(1) = X1;
    points(2) = X2;
else

    % update X1 and X2 values for log spaced variables
    if (strcmp(spacing, 'log'))

        % check that X1 is greater than 0
        if X1 <= 0
            error('X1 must be > 0 for log spaced points');
        end

        X1 = log10(X1);
        X2 = log10(X2);
    end

    % create step variable and points range
    step = (X2 - X1)/(N - 1);
    points = X1:step:X2;

    % update log spaced points
    if (strcmp(spacing, 'log'))
        points = 10.^(points);
    end
end


%
% This is the main script to finds a (near) optimal solution to the Traveling
% Salesman Problem (TSP), by setting up a Simulated Annealing (SA) to search
% for the shortest route (least distance for the salesman to travel to each
% city exactly once and return to the starting city).
%

clear;clc;

load china; % geographic information
plotcities(province, border, city); % draw the map of China

numberofcities = length(city);      % number of cities
% distance matrix: dis(i,j) is the distance between city i and j.
dis = distancematrix(city);

global h;
temperature = 1000;                 % Initialize the temperature.
cooling_rate = 0.94;                % cooling rate
iterations = 1;                     % Initialize the iteration number.

% Initialize random number generator with "seed".
rand('seed',0);

% Initialize the route by generate a sequence of random
route = randperm(numberofcities);
% This is objective function, the total distance for the routes.
previous_distance = totaldistance(route,dis);

% This is a flag used to cool the current temperature after 100 iterations
temperature_iterations = 1;
% This is a flag used to plot the current route after 200 iterations
plot_iterations = 1;

% plot the current route
plotroute(city, route, previous_distance, temperature);

while 1.0 < temperature
    % generate randomly a neighbouring solution
    temp_route = perturb(route,'reverse');
    % compute total distance of the temp_route
    current_distance = totaldistance(temp_route, dis);
    % compute change of distance
    diff = current_distance - previous_distance;

    % Metropolis Algorithm
    if (diff < 0) || (rand < exp(-diff/(temperature)))
        route = temp_route;         %accept new route
        previous_distance = current_distance;

        % update iterations
        temperature_iterations = temperature_iterations + 1;
        plot_iterations = plot_iterations + 1;
        iterations = iterations + 1;
    end

    % reduce the temperature every 100 iterations
    if temperature_iterations >= 100
       temperature = cooling_rate*temperature;
       temperature_iterations = 0;
    end

    %  plot the current route every 200 iterations
    if plot_iterations >= 200
       plotroute(city, route, previous_distance, temperature);
       plot_iterations = 0;
    end
end

% plot and output final solution
plotroute(city, route, previous_distance, temperature);
% sth. wrong with function fpdfprinter
fpdfprinter('Final Solution')


function [mu ul ll] = circ_mean(alpha, w, dim)
%
% mu = circ_mean(alpha, w)
%   Computes the mean direction for circular data.
%
%   Input:
%     alpha	sample of angles in radians
%     [w		weightings in case of binned angle data]
%     [dim  compute along this dimension, default is 1]
%
%     If dim argument is specified, all other optional arguments can be
%     left empty: circ_mean(alpha, [], dim)
%
%   Output:
%     mu		mean direction
%     ul    upper 95% confidence limit
%     ll    lower 95% confidence limit
%
% PHB 7/6/2008
%
% References:
%   Statistical analysis of circular data, N. I. Fisher
%   Topics in circular statistics, S. R. Jammalamadaka et al.
%   Biostatistical Analysis, J. H. Zar
%
% Circular Statistics Toolbox for Matlab

% By Philipp Berens, 2009
% berens@tuebingen.mpg.de - www.kyb.mpg.de/~berens/circStat.html

if nargin < 3
  dim = 1;
end

if nargin < 2 || isempty(w)
  % if no specific weighting has been specified
  % assume no binning has taken place
	w = ones(size(alpha));
else
  if size(w,2) ~= size(alpha,2) || size(w,1) ~= size(alpha,1)
    error('Input dimensions do not match');
  end
end

% compute weighted sum of cos and sin of angles
r = sum(w.*exp(1i*alpha),dim);

% obtain mean by
mu = angle(r);

% confidence limits if desired
if nargout > 1
  t = circ_confmean(alpha,0.05,w,[],dim);
  ul = mu + t;
  ll = mu - t;
end


function y = r8vec_sct ( n, x )

%*****************************************************************************80
%
%% R8VEC_SCT computes a "slow" cosine transform of an R8VEC.
%
%  Discussion:
%
%    This routine is provided for illustration and testing.  It is inefficient
%    relative to optimized routines that use fast Fourier techniques.
%
%      Y(1) = Sum ( 1 <= J <= N ) X(J)
%
%      For 2 <= I <= N-1:
%
%        Y(I) = 2 * Sum ( 1 <= J <= N ) X(J)
%          * cos ( PI * ( I - 1 ) * ( J - 1 ) / ( N - 1 ) )
%
%      Y(N) = Sum ( X(1:N:2) ) - Sum ( X(2:N:2) )
%
%    Applying the routine twice in succession should yield the original data,
%    multiplied by 2 * ( N + 1 ).  This is a good check for correctness
%    and accuracy.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of data values.
%
%    Input, real X(N), the data sequence.
%
%    Output, real Y(N), the transformed data.
%
  for i = 1 : n

    y(i) = x(1) / 2.0;

    for j = 2 : n - 1
      angle = pi * mod ( ( i - 1 ) * ( j - 1 ), 2 * ( n - 1 ) ) / ( n - 1 );
      y(i) = y(i) + x(j) * cos ( angle );
    end

    j = n;

    angle = pi * mod ( ( i - 1 ) * ( j - 1 ), 2 * ( n - 1 ) ) / ( n - 1 );

    y(i) = y(i) + x(n) * cos ( angle ) / 2.0;

  end

  y(1:n) = 2.0 * y(1:n) * sqrt ( n / ( n - 1 ) );

  return
end


function [ess,m] = spm_mci_ess (x,p)
% Compute Effective Sample Size
% FORMAT [ess,m] = spm_mci_ess (x,p)
%
% x      Univariate time series
% p      Maximum lag for autocovariance estimation
%
% ess    Effective Sample Size
% m      Number of lags used in ESS estimate
%
% This routine is based on the Initial Positive Sequence estimate
% proposed in C. Geyer (1992) Practical Markov Chain Monte Carlo,
% Statistical Science, 7(4):473-511.
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_mci_ess.m 6697 2016-01-27 14:57:28Z spm $

N=length(x);

try, pmax=p; catch, pmax=min(ceil(N/10),256); end

for i=1:pmax,
    y(:,i)=x(pmax-i+1:end-i);
end
C=cov(y);
c=C(1,:);
gamma=c(2:end);
gamma0=c(1);
r=gamma/gamma0;

G=[];
for j=1:floor(pmax/2)-1,
    % Sum of adjacent pairs of autocovariances
    G(j)=gamma(2*j)+gamma(2*j+1);
end

if ~isempty(G)
    % Find minimum j such that all G's up to j are positive
    Gneg=find(G<0);
    if isempty(Gneg)
        m=length(G);
    else
        m1=min(Gneg);
        m=m1-1;
    end
else
    m=0;
end

ess=N/(1+2*sum(r(1:2*m)));

% figure;
% plot(c);
% title('Autocovariance');
%
% figure
% plot(G);
% title('Sum of adjacent covariances');


function v = haar_1d_inverse ( n, u )

%*****************************************************************************80
%
%% HAAR_1D_INVERSE inverts the Haar transform of a vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 March 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the length of the vector.
%    N must be a power of 2.
%
%    Input, real U(N,1), the vector to be transformed.
%
%    Output, real V(N,1), the transformed vector.
%
  v = u(:);

  s = sqrt ( 2.0 );

  w = zeros ( n, 1 );

  m = 1;

  while ( m * 2 <= n )

    w(1:2:2*m-1) = ( v(1:m) + v(1+m:m+m) ) / s;
    w(2:2:2*m)   = ( v(1:m) - v(1+m:m+m) ) / s;

    v(1:2*m) = w(1:2*m);
    m = m * 2;

  end

  return
end


function table = p_power_product ( p, e )

%*****************************************************************************80
%
%% P_POWER_PRODUCT: power products for Legendre polynomial P(n,x).
%
%  Discussion:
%
%    Let P(n,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 powers of X with every possible pair
%    of basis functions.  That is, we'd like to form
%
%      Tij = Integral ( -1 <= X <= +1 ) X^E * P(i,x) * P(j,x) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 March 2012
%
%  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(1:p+1,1:p+1) = 0.0;

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

  for k = 1 : order

    x = x_table(k);
    l_table = p_polynomial_value ( 1, 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  [coreness,kn] = kcoreness_centrality_bu(CIJ)
%KCORENESS_CENTRALITY_BU       K-coreness centrality
%
%   [coreness,kn] = kcoreness_centrality_bu(CIJ)
%
%   The k-core is the largest subgraph comprising nodes of degree at least
%   k. The coreness of a node is k if the node belongs to the k-core but
%   not to the (k+1)-core. This function computes the coreness of all nodes
%   for a given binary undirected connection matrix.
%
%   input:          CIJ,        connection/adjacency matrix (binary, undirected)
%
%   output:    coreness,        node coreness.
%                    kn,        size of k-core
%
%   References: e.g. Hagmann et al. (2008) PLoS Biology
%
%   Olaf Sporns, Indiana University, 2007/2008/2010/2012

N = size(CIJ,1);

% determine if the network is undirected - if not, compute coreness on the
% corresponding undirected network
CIJund = CIJ+CIJ';
if (any(CIJund(:)>1))
    CIJ = double(CIJund>0);
end;

coreness = zeros(1,N); kn = zeros(1,N);
for k=1:N
    [CIJkcore,kn(k)] = kcore_bu(CIJ,k);
    ss = sum(CIJkcore)>0;
    coreness(ss) = k;
end;


function [filtered_signal,filtb,filta]=lopass_butterworth(inputsignal,cutoff_freq,Fs,order)
% Low-pass Butterworth filter
% [filtered_signal,filtb,filta] = lopass_butterworth(inputsignal,cutoff_freq,Fs,order)
%
% This is simply a set of built-in Matlab functions, repackaged for ease of
% use by Chad Greene, October 2012.
%
% INPUTS:
% inputsignal = input time series
% cutoff_freq = filter corner frequency
% Fs = data sampling frequency
% order = order of Butterworth filter
%
% OUTPUTS:
% filtered_signal = the filtered time series
% filtb, filta = filter numerator and denominator (optional)
%
% EXAMPLE 1:
% load train
% t = (1:length(y))/Fs;
% y_filt = lopass_butterworth(y,900,Fs,4); % cut off at 900 Hz
% figure
% plot(t,y,'b',t,y_filt,'r')
% xlabel('time in seconds')
% box off
% legend('unfiltered','filtered')
% sound(y,Fs)      % play original time series
% pause(2)         % pause two seconds
% sound(y_filt,Fs) % play filtered time series
%
%
% EXAMPLE 2:
% load train
% t = (1:length(y))/Fs;
% [y_filt,filtb,filta] = lopass_butterworth(y,900,Fs,4); % cut off at 900 Hz
% [h1,f1] = freqz(filtb,filta,256,Fs);
%
% figure
% subplot(3,1,1)
% plot(t,y,'b',t,y_filt,'r')
% xlabel('time in seconds')
% box off
% text(0,.1,' time series','units','normalized')
%
% subplot(3,1,2)
% AX = plotyy(f1,10*log10(abs(h1)),f1,angle(h1),'semilogx');
% set(get(AX(1),'ylabel'),'string','gain (dB)')
% set(get(AX(2),'ylabel'),'string','phase (rad)')
% xlim(AX(1),[min(f1) max(f1)])
% xlim(AX(2),[min(f1) max(f1)])
% text(0,.1,' filter response','units','normalized')
% box off
%
% [Pxx,f] = pwelch(y,512,256,[],Fs,'onesided');
% [Pxxf,f_f]= pwelch(y_filt,512,256,[],Fs,'onesided');
% subplot(3,1,3)
% semilogx(f,10*log10(Pxx))
% hold on
% semilogx(f_f,10*log10(Pxxf),'r')
% xlabel('frequency (Hz)')
% ylabel('PSD (dB)')
% xlim([min(f1) max(f1)])
% box off
% legend('unfiltered','filtered','location','northwest')
% legend boxoff

nyquist_freq = Fs/2;  % Nyquist frequency
Wn=cutoff_freq/nyquist_freq;    % non-dimensional frequency
[filtb,filta]=butter(order,Wn,'low'); % construct the filter
filtered_signal=filtfilt(filtb,filta,inputsignal); % filter the data with zero phase


function [is,os,str] = strengths_dir(CIJ)
%STRENGTHS_DIR      In-strength and out-strength
%
%   [is,os,str] = strengths_dir(CIJ);
%
%   Node strength is the sum of weights of links connected to the node. The
%   instrength is the sum of inward link weights and the outstrength is the
%   sum of outward link weights.
%
%   Input:      CIJ,    directed weighted connection matrix
%
%   Output:     is,     node instrength
%               os,     node outstrength
%               str,    node strength (instrength + outstrength)
%
%   Notes:  Inputs are assumed to be on the columns of the CIJ matrix.
%
%
%   Olaf Sporns, Indiana University, 2002/2006/2008

% compute strengths
is = sum(CIJ,1);    % instrength = column sum of CIJ
os = sum(CIJ,2)';   % outstrength = row sum of CIJ
str = is+os;        % strength = instrength+outstrength


function [T, Eps] = estimateRigidTransform(x, y)
% ESTIMATERIGIDTRANSFORM
%   [T, EPS] = ESTIMATERIGIDTRANSFORM(X, Y) estimates the rigid transformation
%   that best aligns x with y (in the least-squares sense).
%
%   Reference: "Estimating Rigid Transformations" in
%   "Computer Vision, a modern approach" by Forsyth and Ponce (1993), page 480
%   (page 717(?) of the newer edition)
%
%   Input:
%       X: 3xN, N 3-D points (N>=3)
%       Y: 3xN, N 3-D points (N>=3)
%
%   Output
%       T: the rigid transformation that aligns x and y as:  xh = T * yh
%          (h denotes homogenous coordinates)
%          (corrspondence between points x(:,i) and y(:,i) is assumed)
%
%       EPS: the smallest singular value. The closer this value it is
%          to 0, the better the estimate is. (large values mean that the
%          transform between the two data sets cannot be approximated
%          well with a rigid transform.
%
%   Babak Taati, 2003
%   (revised 2009)

if nargin ~= 2
    error('Requires two input arguments.')
end

if size(x,1)~=3 || size(y,1)~=3
    error('Input point clouds must be a 3xN matrix.');
end

if size(x, 2) ~= size(y,2)
    error('Input point clouds must be of the same size');
end

if size(x,2)<3 || size(y,2)<3
    error('At least 3 point matches are needed');
end

pointCount = length(x); % since x has N=3+ points, length shows the number of points

x_centroid = sum(x,2) / pointCount;
y_centroid = sum(y,2) / pointCount;

x_centrized = [x(1,:)-x_centroid(1) ; x(2,:)-x_centroid(2); x(3,:)-x_centroid(3)];
y_centrized = [y(1,:)-y_centroid(1) ; y(2,:)-y_centroid(2); y(3,:)-y_centroid(3)];

R12 = y_centrized' - x_centrized';
R21 = x_centrized - y_centrized;
R22_1 = y_centrized  + x_centrized;
R22 = crossTimesMatrix(R22_1(1:3,:));

B = zeros(4, 4);
A = zeros(4, 4, pointCount);
for ii=1:pointCount
    A(1:4,1:4,ii) = [0, R12(ii,1:3); R21(1:3,ii), R22(1:3,1:3,ii)];
    B = B + A(:,:,ii)' * A(:,:,ii);
end

[~, S, V] = svd(B);
quat = V(:,4);
rot = quat2rot(quat);

T1 = [eye(3,3), -y_centroid ; 0 0 0 1];
T2 = [rot, [0; 0; 0]; 0 0 0 1];
T3 = [eye(3,3), x_centroid ;  0 0 0 1];

T = T3 * T2 * T1;
Eps = S(4,4);


function theta = circle_segment_angle_from_chord_angles ( omega1, omega2 )

%*****************************************************************************80
%
%% CIRCLE_SEGMENT_ANGLE_FROM_CHORD_ANGLES computes the angle of a circle segment.
%
%  Discussion:
%
%    Begin with a circle of radius R.  Choose two points P1 and P2 on the
%    circle, and draw the chord P1:P2.  This chord divides the circle
%    into two pieces, each of which is called a circle segment.
%    Consider one of the pieces.  The "angle" of this segment is the angle
%    P1:C:P2, where C is the center of the circle.  Let Q be the point on
%    the chord P1:P2 which is closest to C.  The "height" of the segment
%    is the distance from Q to the perimeter of the circle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 May 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real OMEGA1, OMEGA2, the angles of the points P1 and P2.
%    OMEGA1 <= OMEGA2.
%
%    Output, real THETA, the angle of the circle segment.
%    Essentially, THETA = OMEGA2 - OMEGA1.
%
  while ( omega2 < omega1 )
    omega2 = omega2 + 2.0 * pi;
  end

  theta = omega2 - omega1;

  return
end


function yp = p07_fun ( neqn, t, y )

%*****************************************************************************80
%
%% P07_FUN evaluates the function for problem P07.
%
%  Discussion:
%
%    y1' = -y1 +   y2
%    y2' =  y1 - 2 y2 + y3
%    y3' =         y2 - y3
%    y1(0) = 2
%    y2(0) = 0
%    y3(0) = 1
%
%    3 equations.
%    Enright and Pryce nonstiff problem #B2.
%    Autonomous.
%
%    Note that the quantity (y1+y2+y3) is conserved by the exact solution.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Wayne Enright, John Pryce,
%    Algorithm 648,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 1, pages 28-34.
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the derivative
%    function.
%
%    Output, real YP(NEQN), the value of the derivative function.
%
  yp = zeros ( neqn, 1 );

  yp(1) = - y(1) +       y(2);
  yp(2) =   y(1) - 2.0 * y(2) + y(3);
  yp(3) =                y(2) - y(3);

  return
end


%% Rotational Approximation and Interpolation
%
%%
% On this page, we want to cover the topic of function approximation from
% discrete values on the Rotation group. To simulate this, we have stored some
% nodes and corresponding function values which we can load. The csv-file
% contains the Euler angles $\phi_1$, $\Phi$ and $\phi_2$ of the nodes and the function
% value in the fourth column. Lets import these data using the function
% <orientation.load.html |load|>

fname = fullfile(mtexDataPath, 'orientation', 'dubna.csv');
[nodes, S] = orientation.load(fname,'columnNames',{'phi1','Phi','phi2','values'});

%%
% The second output |S| is a struct that contains a field |S.values| with
% the function values from the fourth column. Next, we can make a section
% plot to see, what we are dealing with

plotSection(nodes, S.values,'all');

%%
% Now, we want to find a function which coincides with the given function
% values in the nodes reasonably well.

%% Interpolation
%
%%
% Interpolation is done by the <SO3Fun.interpolate |interpolate|> command
% of class <SO3Fun.SO3Fun |SO3Fun|>

SO3F = SO3Fun.interpolate(nodes, S.values,'exact');
plot(SO3F)

%%
% The interpolation is done by lsqr. Hence the error is not in machine
% precision.
norm(SO3F.eval(nodes) - S.values)

%%
% If we don't restrict ourselfs to the given function values in the nodes, we have more
% freedom, which can be seen in the case of approximation.

%% Approximation
%
% In contrast to interpolation we are now not restricted to the function
% values in the nodes but still want to keep the error reasonably small.
%
%%
% One way is to interpolate the function similary as before, without the
% option |'exact'|.
%
%%
% Another way is to approximate the rotational function with a series of
% <WignerFunctions.html Wigner-D functions> (Harmonic series).
% We don't take as many Wigner-D functions as there are nodes,
% such that we are in the overdetermined case. In that way we don't have a
% chance of getting the error in the nodes zero but hope for a smoother
% approximation. This can be achieved by the <SO3FunHarmonic.approximation |approximation|>
% command of the class <SO3FunHarmonic.SO3FunHarmonic |SO3FunHarmonic|>

SO3F2 = SO3FunHarmonic.approximation(nodes, S.values);
plot(SO3F2)

%%
% Plotting this function, we can immidiately see, that we have a much
% smoother function. But one has to keep in mind that the error in the data
% nodes is not zero as in the case of interpolation.

norm(eval(SO3F, nodes) - S.values)

%%
% But this may not be of great importance like in the case of function
% approximation from noisy function values, where we don't know the exact
% function values anyways.

%%
%
% The strategy underlying the |approximation|-command
% to obtain such an approximation works via Wigner-D functions
% (<SO3FunHarmonicSeries Basics of rotational harmonics>). For that,
% we seek for so-called Fourier-coefficients ${\bf \hat f} = (\hat
% f^{0,0}_0,\dots,\hat f^{N,N}_N)^T$ such that
%
% $$ g(x) = \sum_{n=0}^N\sum_{k,l = -n}^n \hat f_n^{k,l} D_n^{k,l}(x) $$
%
% approximates our function. A basic strategy to achieve this is through
% least squares, where we minimize the functional
%
% $$ \sum_{m=1}^M|f(x_m)-g(x_m)|^2 $$
%
% for the data nodes $x_m$, $m=1,\dots,M$, $f(x_m)$ the target function
% values and $g(x_m)$ our approximation evaluated in the given data nodes.
%
% This can be done by the |lsqr| function of Matlab, which efficiently
% seeks for roots of the derivative of the given functional (also known as
% normal equation). In the process we compute the matrix-vector product
% with the Fourier-matrix multible times, where the Fourier-matrix is given
% by
%
% $$ F = [D_n^{k,l}(x_m)]_{m = 1,\dots,M;~n = 0,\dots,N,\,k,l = -n,\dots,n}. $$
%
% This matrix-vector product can be computed efficiently with the use of
% the nonequispaced SO(3) Fourier transform
% <https://www-user.tu-chemnitz.de/~potts/nfft/nfsoft.php NSOFT>
% or faster by the combination of an Wigner-transform together with a
% <https://www-user.tu-chemnitz.de/~potts/nfft/index.php NFFT>.
%
% We end up with the Fourier-coefficients of our approximation $g$, which
% describe our approximation.
%


function [U, R, V] = cod(A, tol)
%COD    Complete orthogonal decomposition.
%       [U, R, V] = COD(A, TOL) computes a decomposition A = U*T*V,
%       where U and V are unitary, T = [R 0; 0 0] has the same dimensions as
%       A, and R is upper triangular and nonsingular of dimension rank(A).
%       Rank decisions are made using TOL, which defaults to approximately
%       LENGTH(A)*NORM(A)*EPS.
%       By itself, COD(A, TOL) returns R.

%       Reference:
%       G. H. Golub and C. F. Van Loan, Matrix Computations, third
%       edition, Johns Hopkins University Press, Baltimore, Maryland,
%       1996; sec. 5.4.2.

[m, n] = size(A);

% QR decomposition.
[U, R, P] = qr(A);    % AP = UR
V = P';               % A = URV;
if nargin == 1, tol = max(m,n)*eps*abs(R(1,1)); end  % |R(1,1)| approx NORM(A).

% Determine r = effective rank.
r = sum(abs(diag(R)) > tol);
r = r(1);             % Fix for case where R is vector.
R = R(1:r,:);         % Throw away negligible rows (incl. all zero rows, m>n).

if r ~= n

   % Reduce nxr R' =  r  [L]  to lower triangular form: QR' = [Lbar].
   %                 n-r [M]                                  [0]

   [Q, R] = trap2tri(R');
   V = Q*V;
   R = R';

end

if nargout <= 1, U = R; end


function [ti,fi]=midpoint(t1,f1,t2,f2,k)
%MIDPOINT Mid-point construction used in the interference diagram.
%	[TI,FI]=MIDPOINT(T1,F1,T2,F2,K) gives the coordinates in the
%	time-frequency plane of the interference-term corresponding to
%	the points (T1,F1) and (T2,F2), for a distribution in the
%	affine class perfectly localized on power-law group-delays of
%	the form : tx(nu)=t0+c nu^(K-1).
%
%	T1 : time-coordinate of the first point
%	F1 : frequency-coordinate of the first point (>0)
%	T2 : time-coordinate of the second point
%	F2 : frequency-coordinate of the second point (>0)
%	K  : power of the group-delay law
%	  K = 2    : Wigner-Ville
%	  K = 1/2  : D-Flandrin
%	  K = 0    : Bertrand (unitary)
%	  K = -1   : Unterberger (active)
%	  K = inf  : Margenau-Hill-Rihaczek
%	TI : time-coordinate of the interference term
%	FI : frequency-coordinate of the interference term
%
%	See also PLOTSID.

%	P. Flandrin, September 1995 - F. Auger, April 1996.
%	Copyright (c) 1996 by CNRS (France).
%
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if f1<=0 | f2<=0,
 error('F1 and F2 must be >0');
end
[rt1,ct1]=size(t1);
[rt2,ct2]=size(t2);
[rf1,cf1]=size(f1);
[rf2,cf2]=size(f2);
if (rt1~=rt2|rt1~=rf1|rt1~=rf2) | (ct1~=ct2|ct1~=cf1|ct1~=cf2),
 error('T1, T2, F1 and F2 must have the same size');
end
if rt1>ct1,
 error('T1 must be a row-vector');
elseif rt2>ct2,
 error('T2 must be a row-vector');
elseif rf2>cf2,
 error('F2 must be a row-vector');
elseif rf1>cf1,
 error('F1 must be a row-vector');
end

if (k==2),
 fi=(f1+f2)/2;
 ti=(t1+t2)/2;
elseif (k==inf),
 ti=[t1;t2];
 fi=[f2;f1];
else
 I=find(abs(f1-f2)>sqrt(eps));
 if length(I)~=0,
  if (k==1),
   fi(I)=exp( (f1(I).*(log(f1(I))-1)-f2(I).*(log(f2(I))-1)) ./ ...
 	(f1(I)-f2(I)));
   ti(I)=(t1(I).*f1(I)-t2(I).*f2(I)) ./ (f1(I)-f2(I)) - ...
 	(t1(I)-t2(I)) ./ (log(f1(I))-log(f2(I)));
  elseif (k==0),
   fi(I)=(f1(I)-f2(I))./(log(f1(I))-log(f2(I)));
   ti(I)=(t1(I).*f1(I)-t2(I).*f2(I)) ./ (f1(I)-f2(I)) + ...
         f1(I) .* f2(I) .* (t2(I)-t1(I)) .* ...
    	(log(f1(I))-log(f2(I))) ./ (f2(I)-f1(I)).^2;
  else
   t0(I)=(t1(I).*f2(I).^(k-1)-t2(I).*f1(I).^(k-1)) ./ ...
 	(f2(I).^(k-1)-f1(I).^(k-1));
   fi(I)=((f1(I).^k-f2(I).^k) ./ (f1(I)-f2(I))/k).^(1/(k-1));
   ti(I)=t0(I)+(t2(I)-t1(I)) ./ (f2(I).^(k-1)-f1(I).^(k-1)) .*fi(I).^(k-1);
  end
 end
end


% KM_DEMO_KCCA Demo file for kernel canonical correlation analysis
% algorithm.
%
% This script takes two multi-dimensional variables and uses kernel CCA to
% map them onto a single latent 1-D variable. The kernel matrices are
% decomposed using incomplete Cholesky decomposition in order to allow
% large data sets. This demo includes 3 flavors of the KCCA generalized
% eigenvalue problem, all yielding very similar results.
%
% Author: Steven Van Vaerenbergh (steven *at* gtas.dicom.unican.es), 2012.
%
% The algorithm in this file is based on the following publications:
% D. R. Hardoon, S. Szedmak and J. Shawe-Taylor, "Canonical Correlation
% Analysis: An Overview with Application to Learning Methods", Neural
% Computation, Volume 16 (12), Pages 2639--2664, 2004.
% F. R. Bach, M. I. Jordan, "Kernel Independent Component Analysis", Journal
% of Machine Learning Research, 3, 1-48, 2002.
%
% This file is part of the Kernel Methods Toolbox for MATLAB.
% https://github.com/steven2358/kmbox

close all; clear
rs = 1; % seed for random generator
rng('default')
rng(rs)

%% PARAMETERS
N = 1000;	% number of samples. method's complexity is O(NM^2)
Mmax = 50;  % max. M (number of components in incomplete Cholesky decomp.)
reg = 1E-5; % regularization
kerneltype = 'gauss';   % kernel type
kernelpar = 1;  % kernel parameter

%% PROGRAM
tic

% generate data
s = randn(N,1);	% latent signal
r1 = randn(N,1); r2 = randn(N,1);	% random (helper) variables

% option 1: two multi-dimensional variables that are mappable onto s
x1 = [tanh(r1-s)+0.1*r1 r1+3*s-1/10*(sin(3*s))];
x2 = [s - 2*(1-exp(-r2))./(1+exp(-r2)) r2.*s tanh(r2+s)];

% option 2: two invertible 1D nonlinear transformations
% x1 = tanh(0.8*s)+0.1*s;	% moderate saturation
% x2 = -1/10*(sin(s*3)+1.1*s*3);   % stairway

% clean up: remove mean
x1 = x1-repmat(mean(x1),N,1);
x2 = x2-repmat(mean(x2),N,1);

% normalize variance (to improve anisotropy) or any other preprocessing
x1 = x1*sqrt(diag(1./diag(x1'*x1)));
x2 = x2*sqrt(diag(1./diag(x2'*x2)));

% KCCA
[y1,y2,beta] = km_kcca(x1,x2,kerneltype,kernelpar,reg,1,'ICD',Mmax);

% scale the estimated signals to compare without the scalar ambiguity
scaling = sqrt(var(s))/sqrt(var(y1))*sign(s(1))*sign(y1(1));

% mean square errors
error1 = s-scaling*y1;
error2 = s-scaling*y2;
MSE1 = sum(error1.^2)/N;
MSE2 = sum(error2.^2)/N;

toc
%% OUTPUT

figure; hold all
plot(s)
plot(scaling*y1);
plot(scaling*y2);
legend('latent variable','projection 1','projection 2')

fprintf('2x %d data points\n',N)
fprintf('Canonical correlation: %f\n',beta)
fprintf('MSE1: %f\n',MSE1);
fprintf('MSE2: %f\n',MSE2);
fprintf('\n')

% figure;plot(sort(diag(real(betas))))  % check eigenvalues
% figure;plot(sort(diag(betas))); % imaginary part due to numerical error


%HOMLINE Homogeneous line from two points
%
% L = HOMLINE(X1, Y1, X2, Y2) is a vector (3x1) which describes a line in
% homogeneous form that contains the two Euclidean points (X1,Y1) and (X2,Y2).
%
% Homogeneous points X (3x1) on the line must satisfy L'*X = 0.
%
% See also PLOT_HOMLINE.

% Copyright (C) 1993-2014, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB 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.
%
% RTB 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 RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

% TODO, probably should be part of a HomLine class.

function l = homline(x1, y1, x2, y2)

    l = cross([x1 y1 1], [x2 y2 1]);

    % normalize so that the result of x*l' is the pixel distance
    % from the line
    l = l / norm(l(1:2));


function voronoi_neighbor_test01 ( )

%*****************************************************************************80
%
%% VORONOI_NEIGHBOR_TEST01 demonstrates VORONOI_NEIGHBORS
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 December 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'VORONOI_NEIGHBORS_TEST01:\n' );
  fprintf ( 1, '  Select a random set of points in the unit square.\n' );
  fprintf ( 1, '  Compute the Voronoi diagram.\n' );
  fprintf ( 1, '  Have VORONOI_NEIGHBORS determine the neighbors.\n' );

  n = 8;
  x = rand ( n, 2 );
%
%  Compute and display the Voronoi diagram.
%
  figure ( );

  hold on
  voronoi ( x(:,1), x(:,2) );
  for i = 1 : n
    txt = sprintf ( '%d', i );
    text ( x(i,1), x(i,2), txt, 'FontSize', 12 );
  end
  hold off

  filename = 'voronoi_neighbors_test01.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Voronoi diagram saved as "%s".\n', filename );
%
%  Compute and list the finite Voronoi edges.
%
  [ V, C ] = voronoin ( x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Voronoi edges:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : length ( C )
    disp ( C{i} )
  end
%
%  Compute the Voronoi neighbors.
%
  vn = voronoi_neighbors ( x );
%
%  To print the matrix, make a full version.
%
  vn = full ( vn );
  i4mat_print ( n, n, vn, '  Voronoi adjacency:' )

  return
end


function y = trapmf(x, params)
%TRAPMF Trapezoidal membership function.
%   TRAPMF(X, PARAMS) returns a matrix which is the trapezoidal
%   membership function evaluated at X. PARAMS = [A B C D] is a 4-element
%   vector that determines the break points of this membership function.
%   We require that A <= B and C <= D. If B >= C, this membership
%   function becomes a triangular membership function that could have
%   a height less than unity. (See the example below.)
%
%   For example:
%
%       x = (0:0.1:10)';
%       y1 = trapmf(x, [2 3 7 9]);
%       y2 = trapmf(x, [3 4 6 8]);
%       y3 = trapmf(x, [4 5 5 7]);
%       y4 = trapmf(x, [5 6 4 6]);
%       plot(x, [y1 y2 y3 y4]);
%       set(gcf, 'name', 'trapmf', 'numbertitle', 'off');
%
%   See also DSIGMF, EVALMF, GAUSS2MF, GAUSSMF, GBELLMF, MF2MF, PIMF, PSIGMF,
%   SIGMF, SMF, TRIMF, ZMF.

%   Roger Jang, 6-28-93, 10-5-93, 4-14-94.
%   Copyright 1994-2002 The MathWorks, Inc.
%   $Revision: 1.22 $  $Date: 2002/04/14 22:21:13 $

if nargin ~= 2
    error('Two arguments are required by the trapezoidal MF.');
elseif length(params) < 4
    error('The trapezoidal MF needs at least four parameters.');
end

a = params(1); b = params(2); c = params(3); d = params(4);

if a > b,
    error('Illegal parameter condition: a > b');
elseif c > d,
    error('Illegal parameter condition: c > d');
end

y1 = zeros(size(x));
y2 = zeros(size(x));

% Compute y1
index = find(x >= b);
if ~isempty(index),
    y1(index) = ones(size(index));
end
index = find(x < a);
if ~isempty(index),
    y1(index) = zeros(size(index));
end
index = find(a <= x & x < b);
if ~isempty(index) & a ~= b,
    y1(index) = (x(index)-a)/(b-a);
end

% Compute y2
index = find(x <= c);
if ~isempty(index),
    y2(index) = ones(size(index));
end
index = find(x > d);
if ~isempty(index),
    y2(index) = zeros(size(index));
end
index = find(c < x & x <= d);
if ~isempty(index) & c ~= d,
    y2(index) = (d-x(index))/(d-c);
end

% Compute y
y = min(y1, y2);


function [ w, xy ] = triangle_unit_o03 ( )

%*****************************************************************************80
%
%% TRIANGLE_UNIT_O03 returns a 3 point quadrature rule for the unit triangle.
%
%  Discussion:
%
%    This rule is precise for monomials through degree 2.
%
%    The integration region is:
%
%      0 <= X
%      0 <= Y
%      X + Y <= 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carlos Felippa,
%    A compendium of FEM integration formulas for symbolic work,
%    Engineering Computation,
%    Volume 21, Number 8, 2004, pages 867-890.
%
%  Parameters:
%
%    Output, real W(3), the weights.
%
%    Output, real XY(2,3), the abscissas.
%
  w(1:3,1) = [ ...
    0.33333333333333333333, ...
    0.33333333333333333333, ...
    0.33333333333333333333 ];

  xy(1:2,1:3) = [ ...
    0.66666666666666666667,  0.16666666666666666667; ...
    0.16666666666666666667,  0.66666666666666666667; ...
    0.16666666666666666667,  0.16666666666666666667 ]';
  return
end


function r = trirnd(varargin)
%TRIRND Pseudorandom numbers drawn from the triangular distribution
%
%   R = IOSR.STATISTICS.TRIRND(N) returns an N-by-N matrix containing
%   pseudorandom values drawn from the triangular distribution constrained
%   to (-1,1) and mode = 0.
%
%   IOSR.STATISTICS.TRIRND(M,N) or IOSR.STATISTICS.TRIRND([M,N]) returns an
%   M-by-N matrix.
%
%   IOSR.STATISTICS.TRIRND(M,N,P,...) or
%   IOSR.STATISTICS.TRIRND([M,N,P,...]) returns an M-by-N-by-P-by-...
%   array.
%
%   IOSR.STATISTICS.TRIRND returns a scalar.
%
%   TRIRND(SIZE(A)) returns an array the same size as A.
%
%   Note: The size inputs M, N, P, ... should be nonnegative integers.
%   Negative integers are treated as 0.
%
%   The sequence of numbers produced by TRIRND is determined by the
%   settings of the uniform random number generator that underlies RAND,
%   RANDI, and RANDN. Control that shared random number generator using
%   RNG.
%
%   See also IOSR.STATISTICS.LAPRND, RAND, RANDN, RANDI, RNG.

%   Based on code (Matlab FE File ID: #13705) written by Elvis Chen, 2007.

%   Copyright 2016 University of Surrey.

    % Generate traingular noise
    u1 = rand(varargin{:})-0.5;
    u2 = rand(varargin{:})-0.5;
    r = u1+u2;

end


%% Structured grid
% This is a sample code showing how to use ConstructProjector2D().
% This example is going to setup a transformation matrix based on forth
% order polynomials in 2D.
% Pay attention that no data is needed to construct the interpolator. Only
% the coordinates of the points on source and destination grid.
clear;clc;
%% Initializing Part I
disp('- Initializing')
xMin=0;
xMax=2*pi;
yMin=0;
yMax=2*pi;

nx=40;
ny=40;

nPoly=4; % We gonna fit a fourth order polynomial,
         % i.e. sum_{n,m=0}^{n,m=nPoly} x^n*y^m
nInterp=30; % nPoly=4 requires 25 points. However,
            % we are going to use 30 points. This would be
            % the least-square fit of the surface.
            % Note that: min(nInterp)=(nPoly+1)^2. Otherwise it won't work.
%% Turning off the warnings
% Depending on the parameter that you choose the matrices might be nearly
% singular. In this example, despite being nearly singular, they are not
% affecting the final interpolation. Therefore, I set the warning to off.
% But generally do not turn it off and if you got this warning check if it
% is going to affect your results or not.
disp('- Turning the warnings off')
warning('OFF','MATLAB:nearlySingularMatrix');

%% Generating the Source Grid
disp('- Generating the source grid.')
[xn,yn]=meshgrid(linspace(xMin,xMax,nx),linspace(yMin,yMax,ny));

%% Finding the cell centers
disp('- Generating the destination grid')
xc=(xn(1:ny-1,1:nx-1)+xn(2:ny,1:nx-1)+xn(1:ny-1,2:nx)+xn(2:ny,2:nx))*0.25;
yc=(yn(1:ny-1,1:nx-1)+yn(2:ny,1:nx-1)+yn(1:ny-1,2:nx)+yn(2:ny,2:nx))*0.25;

figure
surface(xn,yn,zeros(size(xn)));
hold on
plot(xn,yn,'k.');
plot(xc,yc,'b.');
axis tight;
axis square;
title('Source Grid, black dots, & the destination grid, blue dots.', ...
      'FontName','Arial','FontSize',12,'FontWeight','Bold');
%% Constructing the Interpolant
% Note: that we do not need the data on the source grid to create the
% interpolant.
disp('- Constructing the interpolant')
P=ConstructProjector2D(xn(:),yn(:),xc(:),yc(:),nPoly,nInterp);

%% Generarting some Data
disp('- Generating some data and interpolating')
F1=@(x,y) (sin(sqrt(x.^2+y.^2)));
zn=F1(xn,yn);
zc_interp=reshape(P*zn(:),size(xc));
zc_Analytic=F1(xc,yc);
RMSE=sqrt(mean((zc_Analytic(:)-zc_interp(:)).^2));

figure
surface(xc,yc,zc_interp,'EdgeColor','none');
title(['nPoly: ' num2str(nPoly) ', RMSE= ' num2str(RMSE)]);
axis tight

%% Generating some more data
disp('- Generating multiple data field on the source grid and interpolating them.')
F2=@(x,y) (sin(x).*cos(y));
F3=@(x,y) (exp(-sqrt(x.^2+y.^2)));
F4=@(x,y,x0,y0) (exp(-sqrt((x-x0).^2+(y-y0).^2)));

zn(:,:,1)=F1(xn,yn);
zn(:,:,2)=F2(xn,yn);
zn(:,:,3)=F3(xn,yn);
zn(:,:,4)=F4(xn,yn,mean(mean(xn)),mean(mean(yn)));

zc_Analytic(:,:,1)=F1(xc,yc);
zc_Analytic(:,:,2)=F2(xc,yc);
zc_Analytic(:,:,3)=F3(xc,yc);
zc_Analytic(:,:,4)=F4(xc,yc,mean(mean(xn)),mean(mean(yn)));

% Now interpolating
% Note that the same interpolant is used for all data fields and they can
% be all interpolated with one sparse matrix multiplication.
zc_interp=P*reshape(zn,nx*ny,4); % There are 4 data fields.
zc_interp=reshape(zc_interp,(nx-1),(ny-1),4); % these two commands can be combined in one.
                                              % They were separated for clarity.

% calculating the RMSE and plotting
RMSE=zeros(4,1);
figure
for i=1:4
  RMSE(i)=sqrt(mean((reshape(zc_Analytic(:,:,i),(nx-1)*(ny-1),1)-reshape(zc_interp(:,:,i),(nx-1)*(ny-1),1)).^2));
  subplot(2,2,i);
  surface(xc,yc,squeeze(zc_interp(:,:,i)),'EdgeColor','none');
  title(['F' num2str(i) ', nPoly: ' num2str(nPoly) ', RMSE:' num2str(RMSE(i))])
  axis tight
  axis square
end


function values = coeffs2vals(coeffs)
%COEFFS2VALS   Convert Fourier coefficients to values at N equally spaced
%points between [-1 1), where N is the number of coefficients.
%   V = COEFFS2VALS(C) returns the values of the trignometric polynomial
%   as follows:
%   If N is odd
%       F(x) = C(1)*z^(-(N-1)/2) + C(2)*z^(-(N-1)/2-1) + ... + C(N)*z^((N-1)/2)
%   If N is even
%       F(x) = C(1)*z^(-N/2) + C(2)*z^(-N/2+1) + ... + C(N)*z^(N/2-1)
%   where z = exp(1i*pi*x) and -1 <= x <= 1.
%
%   If the input C is an (N+1)xM matrix then V = COEFFS2VALS(C) returns the
%   (N+1)xM matrix of values V such that V(i,j) is the ith value of the
%   trignometric polynomial corresponding to the jth column.
%
% See also VALS2COEFFS, TRIGPTS.

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

% *Note about symmetry*.  Some of the code below is designed to
% enforce two symmetries whose failure might disturb users:
% COEFFS exactly real ==> VALUES exactly hermitian
% COEFFS exactly imaginary ==> VALUES exactly skew-hermitian
% This is necessary because the MATLAB FFT code does not
% exactly preserve symmetries.

% Get the length of the input:
n = size(coeffs, 1);

% Trivial case (constant or empty):
if ( n <= 1 )
    values = coeffs;
    return
end

% The coefficients are for interpolation defined on [-pi,pi), but the FFT
% works for values on [0,2*pi). To fix the coefficients for this we just need to
% assign c_k = (-1)^k c_k, with k=-(N-1)/2:(N-1)/2 for N odd, and
% k = -N/2:N/2-1 for N even.
if ( mod(n, 2) )
    even_odd_fix = (-1).^(-(n-1)/2:(n-1)/2).';
else
    even_odd_fix = (-1).^((-n/2):(n/2-1)).';
end
coeffs = bsxfun(@times, coeffs, even_odd_fix);

% test for symmetry
isHerm = max(abs(imag(coeffs)),[],1) == 0;
isSkew = max(abs(real(coeffs)),[],1) == 0;

% Shift the coefficients properly.
values = ifft(ifftshift(n*coeffs, 1), [], 1);

% correct if symmetric
vals = [values;values(1,:)];
hermvals = (vals+flipud(conj(vals)))/2;
skewvals = (vals-flipud(conj(vals)))/2;
values(:,isHerm) = hermvals(1:end-1,isHerm);
values(:,isSkew) = skewvals(1:end-1,isSkew);

end


function OUT = datatreat(DATA,vtreat,nlag)
% =======================================================================
% Treat a time series with the specified method. If changes are computed
% the function assumes 1 lag (unless otherwise specified)
% =======================================================================
% OUT = datatreat(DATA,vtreat,lag)
% -----------------------------------------------------------------------
% INPUT
%	- DATA: matrix DATA(T,N)
%   - vtreat: 0 No treatment
%             1	Log
%             2 Difference
%             3	Log Difference
%             4 Percent change
% -----------------------------------------------------------------------
% OPTIONAL INPUT
%   - lag: number of lags for changes
% -----------------------------------------------------------------------
% OUTPUT
%	- OUT: matrix DATA(T,N) of treated data. The first "nlag"
%     observations are NaNs
% -----------------------------------------------------------------------
% EXAMPLE
%   x = [1 2; -3 4; 5 6; 7 8; 9 10];
%   OUT = datatreat(x,3)
% =======================================================================
% VAR Toolbox 3.0
% Ambrogio Cesa-Bianchi
% ambrogiocesabianchi@gmail.com
% March 2012. Updated November 2020
% -----------------------------------------------------------------------

% Check input
if ~exist('nlag','var')
    nlag = 1;
end

% Set matrices
[nobs, nvar] = size(DATA);
OUT = nan(nobs,nvar);

% No treatment
if vtreat==0
    OUT = DATA;

% Log
elseif vtreat==1
    if ~isempty(DATA<0), warning('Negative numbers set to NaN before taking logs'), end
    DATA(DATA<0) = NaN;
    OUT = log(DATA);

% Difference
elseif vtreat==2
    OUT(nlag+1:end,:) = DATA(1+nlag:end,:) - DATA(1:end-nlag,:);

% Log difference
elseif vtreat==3
    if ~isempty(DATA<0), warning('Negative numbers set to NaN before taking logs'), end
    DATA(DATA<0) = NaN;
    OUT(nlag+1:end,:) = log(DATA(1+nlag:end,:))-log(DATA(1:end-nlag,:));

% Percent change
elseif vtreat==4
    OUT(nlag+1:end,:) = DATA(1+nlag:end,:)./DATA(1:end-nlag,:)-1;
end


function kappa = surfaceCurvature(kappa1, kappa2, theta)
%SURFACECURVATURE Curvature on a surface from angle and principal curvatures.
%
%   usage:
%   KAPPA = surfaceCurvature(KAPPA1, KAPPA2, THETA)
%   return the curvature KAPPA of surface with respect to direction THETA.

%   KAPPA1 and KAPPA2 are the principal curvatures of the surface at the
%   considered point. THETA is angle of direction relative to angle of
%   first principal curvature KAPPA1.
%
%   Examples:
%   K = surfaceCurvature(KAPPA1, KAPPA2, 0) returns KAPPA1.
%   K = surfaceCurvature(KAPPA1, KAPPA2, pi/2) returns KAPPA2.
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 07/04/2004.
%

%   HISTORY
%   20/04/2004 change name and add doc.
%   14/06/2004 correct creation date

kappa = kappa1 * cos(theta).^2 + kappa2 * sin(theta).^2;


function [f,P,prob] = lomb(t,h,ofac,hifac)
% LOMB(T,H,OFAC,HIFAC) computes the Lomb normalized periodogram (spectral
% power as a function of frequency) of a sequence of N data points H,
% sampled at times T, which are not necessarily evenly spaced. T and H must
% be vectors of equal size. The routine will calculate the spectral power
% for an increasing sequence of frequencies (in reciprocal units of the
% time array T) up to HIFAC times the average Nyquist frequency, with an
% oversampling factor of OFAC (typically >= 4).
%
% The returned values are arrays of frequencies considered (f), the
% associated spectral power (P) and estimated significance of the power
% values (prob).  Note: the significance returned is the false alarm
% probability of the null hypothesis, i.e. that the data is composed of
% independent gaussian random variables.  Low probability values indicate a
% high degree of significance in the associated periodic signal.
%
% Although this implementation is based on that described in Press,
% Teukolsky, et al. Numerical Recipes  In C, section 13.8, rather than using
% trigonometric rercurrences, this takes advantage of MATALB's array
% operators to calculate the exact spectral power as defined in equation
% 13.8.4 on page 577.  This may cause memory issues for large data sets and
% frequency ranges.
%
% Example
%    [f,P,prob] = lomb(t,h,4,1);
%    plot(f,P)
%    [Pmax,jmax] = max(P)
%    disp(['Most significant period is ',num2str(1/f(jmax)),...
%         ' with FAP of ',num2str(prob(jmax))])
%
% Written by Dmitry Savransky 21 May 2008

%sample length and time span
N = length(h);
T = max(t) - min(t);

%mean and variance
mu = mean(h);
s2 = var(h);

%calculate sampling frequencies
f = (1/(T*ofac):1/(T*ofac):hifac*N/(2*T)).';

%angular frequencies and constant offsets
w = 2*pi*f;
tau = atan2(sum(sin(2*w*t.'),2),sum(cos(2*w*t.'),2))./(2*w);

%spectral power
cterm = cos(w*t.' - repmat(w.*tau,1,length(t)));
sterm = sin(w*t.' - repmat(w.*tau,1,length(t)));
P = (sum(cterm*diag(h-mu),2).^2./sum(cterm.^2,2) + ...
     sum(sterm*diag(h-mu),2).^2./sum(sterm.^2,2))/(2*s2);

%estimate of the number of independent frequencies
M=2*length(f)/ofac;

%statistical significane of power
prob = M*exp(-P);
inds = prob > 0.01;
prob(inds) = 1-(1-exp(-P(inds))).^M;


function value = normal_ms_moment ( order, mu, sigma )

%*****************************************************************************80
%
%% NORMAL_MS_MOMENT evaluates moments of the Normal PDF.
%
%  Discussion:
%
%    The formula was posted by John D Cook.
%
%    Order  Moment
%    -----  ------
%      0    1
%      1    mu
%      2    mu^2 +         sigma^2
%      3    mu^3 +  3 mu   sigma^2
%      4    mu^4 +  6 mu^2 sigma^2 +   3      sigma^4
%      5    mu^5 + 10 mu^3 sigma^2 +  15 mu   sigma^4
%      6    mu^6 + 15 mu^4 sigma^2 +  45 mu^2 sigma^4 +  15      sigma^6
%      7    mu^7 + 21 mu^5 sigma^2 + 105 mu^3 sigma^4 + 105 mu   sigma^6
%      8    mu^8 + 28 mu^6 sigma^2 + 210 mu^4 sigma^4 + 420 mu^2 sigma^6 + 105 sigma^8
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ORDER, the order of the moment.
%    0 <= ORDER.
%
%    Input, real MU, the mean of the distribution.
%
%    Input, real SIGMA, the standard deviation of the distribution.
%
%    Output, real VALUE, the value of the central moment.
%
  j_hi = floor ( order / 2 );

  value = 0.0;
  for j = 0 : j_hi
    value = value ...
      + r8_choose ( order, 2 * j ) ...
      * r8_factorial2 ( 2 * j - 1 ) ...
      * mu ^ ( order - 2 * j ) * sigma ^ ( 2 * j );
  end

  return
end


function corr_coef = RegreesionTID2013(objectiveValues,mos)
%this script is used to calculate the pearson linear correlation
%coefficient and root mean sqaured error after regression

%get the objective scores computed by the IQA metric and the subjective
%scores provided by the dataset
% matData = load('VSIOnTID2013.mat');
% VSIOnTID2013 = matData.VSIOnTID2013;
% objectiveValues = VSIOnTID2013(:,1);
% mos = VSIOnTID2013(:,2);

%plot objective-subjective score pairs
% p = plot(objectiveValues,mos,'+');
% set(p,'Color','blue','LineWidth',1);

%initialize the parameters used by the nonlinear fitting function
beta(1) = 10;
beta(2) = 0;
beta(3) = mean(objectiveValues);
beta(4) = 0.1;
beta(5) = 0.1;
%fitting a curve using the data
[bayta ehat,J] = nlinfit(objectiveValues,mos,@logistic,beta);
%given an objective value, predict the correspoing mos (ypre) using the fitted curve
[ypre junk] = nlpredci(@logistic,objectiveValues,bayta,ehat,J);

RMSE = sqrt(sum((ypre - mos).^2) / length(mos));%root meas squared error
corr_coef = corr(mos, ypre, 'type','Pearson'); %pearson linear coefficient

%draw the fitted curve
% t = min(objectiveValues):0.01:max(objectiveValues);
% [ypre junk] = nlpredci(@logistic,t,bayta,ehat,J);
% hold on;
% p = plot(t,ypre);
% set(p,'Color','black','LineWidth',2);
% legend('Images in TID2013','Curve fitted with logistic function', 'Location','NorthWest');
% xlabel('Objective score by VSI');
% ylabel('MOS');


function prob_test094 ( )

%*****************************************************************************80
%
%% TEST094 tests INVERSE_GAUSSIAN_MEAN, INVERSE_GAUSSIAN_SAMPLE, INVERSE_GAUSSIAN_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST094\n' );
  fprintf ( 1, '  For the Inverse Gaussian PDF:\n' );
  fprintf ( 1, '  INVERSE_GAUSSIAN_MEAN computes the mean;\n' );
  fprintf ( 1, '  INVERSE_GAUSSIAN_SAMPLE samples;\n' );
  fprintf ( 1, '  INVERSE_GAUSSIAN_VARIANCE computes the variance.\n' );

  a = 2.0;
  b = 3.0;

  check = inverse_gaussian_check ( a, b );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST094 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  mean = inverse_gaussian_mean ( a, b );
  variance = inverse_gaussian_variance ( a, b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A =             %14f\n', a );
  fprintf ( 1, '  PDF parameter B =             %14f\n', b );
  fprintf ( 1, '  PDF mean =                    %14f\n', mean );
  fprintf ( 1, '  PDF variance =                %14f\n', variance );

  for i = 1 : nsample
    [ x(i), seed ] = inverse_gaussian_sample ( a, b, seed );
  end

  mean = r8vec_mean ( nsample, x );
  variance = r8vec_variance ( nsample, x );
  xmax = max ( x(1:nsample) );
  xmin = min ( x(1:nsample) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sample size =     %6d\n', nsample );
  fprintf ( 1, '  Sample mean =     %14f\n', mean );
  fprintf ( 1, '  Sample variance = %14f\n', variance );
  fprintf ( 1, '  Sample maximum =  %14f\n', xmax );
  fprintf ( 1, '  Sample minimum =  %14f\n', xmin );

  return
end


function interp_Function_Coeffs(x1,x2)

mat1 = [1 0 0 0 0 0 0 0 0 0 0 0];
mat2 = [0 1 0 0 0 0 0 0 0 0 0 0];
mat3 = [0 0 1 0 0 0 0 0 0 0 0 0];
mat4 = [1 x1 x1^2 x1^3 -1 -x1 -x1^2 -x1^3 0 0 0 0];
mat5 = [0 1 2*x1 3*x1^2 0 -1 -2*x1 -3*x1^2 0 0 0 0];
mat6 = [0 0 2 6*x1 0 0 -2 -6*x1 0 0 0 0];

mat7 = [0 0 0 0 1 x2 x2^2 x2^3 -1 -x2 -x2^2 -x2^3];
mat8 = [0 0 0 0 0 1 2*x2 3*x2^2 0 -1 -2*x2 -3*x2^2];
mat9 = [0 0 0 0 0 0 2 6*x2 0 0 -2 -6*x2];

mat10= [0 0 0 0 0 0 0 0 1 1 1 1];
mat11= [0 0 0 0 0 0 0 0 0 1 2 3];
mat12= [0 0 0 0 0 0 0 0 0 0 2 6];

mat = [mat1; mat2; mat3; mat4; mat5; mat6; mat7; mat8; mat9; mat10; mat11; mat12];

rhs = [0 0 0 0 0 0 0 0 0 1 0 0]';

%mat1 = [-x1^2 0 x1^3 x1^2 x1 1];
%mat2 = [-2*x1 0 3*x1^2 2*x1 1 0];
%mat3 = [-2 0 6*x1 2 0 0];
%mat4 = [0 (x2-1)^2 x2^3 x2^2 x2 1];
%mat5 = [0 2*(x2-1) 3*x2^2 2*x2 1 0];
%mat6 = [0 2 6*x2 2 0 0];
%mat = [mat1; mat2; mat3; mat4; mat5; mat6];
%rhs = [0 0 0 1 0 0]';

coeffs = mat\rhs

a0 = coeffs(1);
a1 = coeffs(2);
a2 = coeffs(3);
a3 = coeffs(4);
b0 = coeffs(5);
b1 = coeffs(6);
b2 = coeffs(7);
b3 = coeffs(8);
c0 = coeffs(9);
c1 = coeffs(10);
c2 = coeffs(11);
c3 = coeffs(12);

ds = 0.01;
x = 0:ds:1;

figure(1)
for i=1:length(x)
    plot(x(i),g(coeffs,x(i),x1,x2),'*'); hold on;
end

figure(2)
for i=1:length(x)
    plot(x(i),gP(coeffs,x(i),x1,x2),'*'); hold on;
end

figure(3)
for i=1:length(x)
    plot(x(i),gPP(coeffs,x(i),x1,x2),'*'); hold on;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: evaluate interpolating polynomial
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function val = g(coeffs,x,x1,x2)

if x<=x1
    val = coeffs(1) + coeffs(2)*x + coeffs(3)*x^2 + coeffs(4)*x^3;
elseif x<=x2
    val = coeffs(5) + coeffs(6)*x + coeffs(7)*x^2 + coeffs(8)*x^3;
else
    val = coeffs(9) + coeffs(10)*x + coeffs(11)*x^2 + coeffs(12)*x^3;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: evaluate interpolating polynomial 1st derivative
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function val = gP(coeffs,x,x1,x2)

if x<=x1
    val = coeffs(2) + 2*coeffs(3)*x + 3*coeffs(4)*x^2;
elseif x<=x2
    val = coeffs(6) + 2*coeffs(7)*x + 3*coeffs(8)*x^2;
else
    val = coeffs(10) + 2*coeffs(11)*x + 3*coeffs(12)*x^2;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: evaluate interpolating polynomial 2nd derivative
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function val = gPP(coeffs,x,x1,x2)

if x<=x1
    val = 2*coeffs(3) + 6*coeffs(4)*x;
elseif x<=x2
    val = 2*coeffs(7) + 6*coeffs(8)*x;
else
    val = 2*coeffs(11) + 6*coeffs(12)*x;
end

%
% figure(1)
% for i=1:length(x1_part)
%     x = x1_part(i);
%     plot(x,a*x^2,'ro'); hold on;
% end
%
% for i=1:length(x2_part)
%     x = x2_part(i);
%     plot(x,c*x^3+d*x^2+g*x+h,'*'); hold on;
% end
%
% for i=1:length(x3_part)
%    x = x3_part(i);
%    plot(x,-b*(x-1)^2+1,'go'); hold on;
% end
%
% xtotal = 0:ds:1;
% for i=1:length(xtotal)
%    x = xtotal(i);
%    plot(x,0.5*(1+tanh(4.3*(x-0.5))),'m*'); hold on;
% end
% title('function vals');
%
%
% figure(2)
% for i=1:length(x1_part)
%     x = x1_part(i);
%     plot(x,2*a*x,'ro'); hold on;
% end
%
% for i=1:length(x2_part)
%     x = x2_part(i);
%     plot(x,3*c*x^2+2*d*x+g,'*'); hold on;
% end
%
% for i=1:length(x3_part)
%    x = x3_part(i);
%    plot(x,-2*b*(x-1),'go'); hold on;
% end
% title('1st deriv');
%
%
% figure(3)
% for i=1:length(x1_part)
%     x = x1_part(i);
%     plot(x,2*a,'ro'); hold on;
% end
%
% for i=1:length(x2_part)
%     x = x2_part(i);
%     plot(x,6*c*x+2*d,'*'); hold on;
% end
%
% for i=1:length(x3_part)
%    x = x3_part(i);
%    plot(x,-2*b,'go'); hold on;
% end
% title('2nd deriv');


function histmat  = hist2D(x, y, xedges, yedges)
% function histmat  = hist2D(x, y, xedges, yedges)
%
% Extract 2D histogram data containing the number of events
% of [x , y] pairs that fall in each bin of the grid defined by
% xedges and yedges. The edges are vectors with monotonically
% non-decreasing values.
%
%EXAMPLE
%
% events = 1000000;
% x1 = sqrt(0.05)*randn(events,1)-0.5; x2 = sqrt(0.05)*randn(events,1)+0.5;
% y1 = sqrt(0.05)*randn(events,1)+0.5; y2 = sqrt(0.05)*randn(events,1)-0.5;
% x= [x1;x2]; y = [y1;y2];
%
%For linearly spaced edges:
% xedges = linspace(-1,1,64); yedges = linspace(-1,1,64);
% histmat = hist2(x, y, xedges, yedges);
% figure; pcolor(xedges,yedges,histmat'); colorbar ; axis square tight ;
%
%For nonlinearly spaced edges:
% xedges_ = logspace(0,log10(3),64)-2; yedges_ = linspace(-1,1,64);
% histmat_ = hist2(x, y, xedges_, yedges_);
% figure; pcolor(xedges_,yedges_,histmat_'); colorbar ; axis square tight ;

% University of Debrecen, PET Center/Laszlo Balkay 2006
% email: balkay@pet.dote.hu

if nargin ~= 4
    error ('The four input arguments are required!');
    return;
end
if any(size(x) ~= size(y))
    error ('The size of the two first input vectors should be same!');
    return;
end

[xn, xbin] = histc(x,xedges);
[yn, ybin] = histc(y,yedges);

%xbin, ybin zero for out of range values
% (see the help of histc) force this event to the
% first bins
xbin(find(xbin == 0)) = inf;
ybin(find(ybin == 0)) = inf;

xnbin = length(xedges);
ynbin = length(yedges);

if xnbin >= ynbin
    xy = ybin*(xnbin) + xbin;
      indexshift =  xnbin;
else
    xy = xbin*(ynbin) + ybin;
      indexshift =  ynbin;
end

%[xyuni, m, n] = unique(xy);
xyuni = unique(xy);
xyuni(end) = [];
hstres = histc(xy,xyuni);
clear xy;

histmat = zeros(ynbin,xnbin);
histmat(xyuni-indexshift) = hstres;


% Averaging Quaternions
% Since quaternions are not regular vectors, but rather representations
% of orientation, an average quaternion cannot just be obtained by taking
% a weighted mean. This function implements the work done by paper by
% F. Landis Merkley to calculate the average quaternion. The algorithm
% explained by F. Landis Markley at:
% http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf
% For this particular implementation, I would also like to reference Mandar
% Harshe:
% http://www-sop.inria.fr/members/Mandar.Harshe/knee-joint/html/index.html
%
% This algorithm is compared by rotqrmean from VoiceBox and found to
% produce quite similar results, yet it is more elegant, much simpler to
% implement and follow. (Though, there might be difference in signs)
%
% Usage :
% Q is an Mx4 matrix, where each row stores a quaternion to be averaged.
% In return, the function outputs Qavg, which is a single quaternion
% corresponding to the average.
%
% Tolga Birdal

function [Qwavg]=weighted_avg_quat(Q, weights)

% Form the symmetric accumulator matrix
A=zeros(4,4);
M=size(Q,1);

for i=1:M
    q=Q(i,:)';
    A=q*q'*weights(i)+A; % rank 1 update
end

% scale
A=A/sum(weights);

% Get the eigenvector corresponding to largest eigen value
[Qwavg, Eval]=eigs(A,1);

end


function M=axisAng2RotMat(u,theta,handed)
%%AXISANG2ROTMAT Get a rotation matrix to rotate a 3D vector an angle of
%                theta counterlockwise (right-handed) or clockwise
%                (left-handed) about an axis given by the unit vector u.
%
%INPUTS: u A unit vector representing the axis about which a vector should
%          be rotated using the rotation matrix.
%    theta The angle in radians by which a vector is to be rotated is to be
%          rotated about the axis u when multiplied by the rotation matrix.
%   handed The handedness of the rotation angle. If omitted, it is assumed
%          that the rotation is right-handed (the standard). Possible
%          values are
%          'right' The default if omitted. The rotation is right-handed.
%           'left' The rotation is left-handed. The rotation angle is
%                  counterclockwise when one is looking in the same
%                  direction that the rotation axis points.
%
%OUTPUTS: M The rotation matrix such that M*v rotates the vector v by the
%           desired rotation.
%
%To rotate a 3D vector x an angle of theta about the axis u, simply
%evaluate M*x.
%
%The rotation matrix is obtained by obtaining the unit quaternion for the
%rotation and turning it into the corresponding rotation matrix as
%described in  [1].
%
%REFERENCES:
%[1] David F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%December 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(handed))
    handed='right';
end

q=axisAng2Quat(u,theta);
M=quat2RotMat(q,handed);

end

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


function [ a, more ] = compnz_next ( n, k, a, more )

%*****************************************************************************80
%
%% COMPNZ_NEXT computes the compositions of the integer N into K nonzero parts.
%
%  Discussion:
%
%    A composition of the integer N into K nonzero parts is an ordered sequence
%    of K positive 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 4+1+1 but 5+1+0 is not allowed since it includes a zero part.
%
%    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.
%
%  Example:
%
%    The 10 compositions of 6 into three nonzero parts are:
%
%      4 1 1,  3 2 1,  3 1 2,  2 3 1,  2 2 2,  2 1 3,
%      1 4 1,  1 3 2,  1 2 3,  1 1 4.
%
%  Modified:
%
%    01 December 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Albert Nijenhuis and Herbert Wilf,
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer N, the integer whose compositions are desired.
%
%    Input, integer K, the number of parts in the composition.
%    K must be no greater than N.
%
%    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.
%
%    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.
%
  persistent h;
  persistent t;
%
%  We use the trick of computing ordinary compositions of (N-K)
%  into K parts, and adding 1 to each part.
%
  if ( n < k )
    more = 0;
    a(1:k) = -1;
    return
  end

  if ( ~more )

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

  else

    a(1:k) = a(1:k) - 1;

    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 - k ) );

  a(1:k) = a(1:k) + 1;


function c=cross7(a,b)
%%CROSS7 Find the cross product of two 7-dimensional vectors. The cross
%        product has the properties:
%        1) cross7(a,b) is a bilinear function of a and b.
%        2) cross7(a,b) is perpendicular to both a and b.
%           Thus, dot(cross7(a,b),a)=0 and dot(cross7(a,b),b)=0
%        3) norm(cross7(a,b))^2=norm(a)^2*norm(b)^2-dot(a,b)^2
%        Just as two forms of the 3D cross product exist (left-handed and
%        right-handed) 480 forms of the 7-dimensional cross product exist.
%        This function implements a cross product defined in terms of an
%        orthonormal basis using asymmetry as in [1].
%
%INPUTS: a A 7 by numVecs matrix of numVecs vectors.
%        b A 7 by numVecs matrix of numVecs vectors.
%
%OUTPUTS: c The 7 by numVecs cross products of a and b, aXb, for each of
%           the vectors in a and b.
%
%As noted in [2], it is only possible for a cross product between two
%vectors to have all of the properties listed above in three and seven
%dimensions.
%
%As noted in 1, the Jacobi identity does not hold. This means that
%cross7(cross7(a,b),c)+cross7(cross7(b,c),a)+cross7(cross7(c,a),b) does
%not necessarily equal zero. However, the following identities can be
%proven:
%cross7(a,b)=-cross7(b,a)
%dot(a,cross7(b,c))=dot(b,cross7(c,a))=dot(c,cross7(a,b))
%cross7(a,cross7(a,b))=dot(a,b)*a-norm(a)^2*b
%cross7(cross7(a,b),cross7(a,c))=cross7(cross7(cross7(a,b),c),a)+cross7(cross7(cross7(b,c),a),a)+cross7(cross7(cross7(c,a),a),b)
%
%REFERENCES:
%[1] P. Lounesto, "Octonians and triality," Advances in Clifford Algebras,
%    vol. 11, no. 2, pp. 191-213, Dec. 2001.
%[2] W. S. Massey, "Cross products of vectors in higher dimensional
%    Euclidean spaces," The American Mathematical Monthly, vol. 90, no. 10,
%    pp. 697-701, Dec. 1983.
%
%December 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numVecs=size(a,2);
c=zeros(7,numVecs);

c(1,:)=+0              +a(2,:).*b(4,:)  +a(3,:).*b(7,:)  -a(4,:).*b(2,:)  +a(5,:).*b(6,:)  -a(6,:).*b(5,:)  -a(7,:).*b(3,:);
c(2,:)=-a(1,:).*b(4,:) +0               +a(3,:).*b(5,:)  +a(4,:).*b(1,:)  -a(5,:).*b(3,:)  +a(6,:).*b(7,:)  -a(7,:).*b(6,:);
c(3,:)=-a(1,:).*b(7,:) -a(2,:).*b(5,:)  +0               +a(4,:).*b(6,:)  +a(5,:).*b(2,:)  -a(6,:).*b(4,:)  +a(7,:).*b(1,:);
c(4,:)=+a(1,:).*b(2,:) -a(2,:).*b(1,:)  -a(3,:).*b(6,:)  +0               +a(5,:).*b(7,:)  +a(6,:).*b(3,:)  -a(7,:).*b(5,:);
c(5,:)=-a(1,:).*b(6,:) +a(2,:).*b(3,:)  -a(3,:).*b(2,:)  -a(4,:).*b(7,:)  +0               +a(6,:).*b(1,:)  +a(7,:).*b(4,:);
c(6,:)=+a(1,:).*b(5,:) -a(2,:).*b(7,:)  +a(3,:).*b(4,:)  -a(4,:).*b(3,:)  -a(5,:).*b(1,:)  +0               +a(7,:).*b(2,:);
c(7,:)=+a(1,:).*b(3,:) +a(2,:).*b(6,:)  -a(3,:).*b(1,:)  +a(4,:).*b(5,:)  -a(5,:).*b(4,:)  -a(6,:).*b(2,:)  +0;

end

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


function [snrdB,ptotdB,psigdB,pnoisedB] = calcSNR(vout,f,fB,w,N)
% SNR calculation in the time domain (P. Malcovati, S. Brigati)
% function [snrdB,ptotdB,psigdB,pnoisedB] = calcSNR(vout,f,fB,w,N)
% vout: Sigma-Delta bit-stream taken at the modulator output
% f:    Normalized signal frequency (fs -> 1)
% fB:	Base-band frequency bins
% w:	windowing vector
% N:    samples number
%
% snrdB:     SNR in dB
% ptotdB:    Bit-stream power spectral density (vector)
% psigdB:    Extracted signal power spectral density (vector)
% pnoisedB:  Noise power spectral density (vector)
%
fB=ceil(fB);
signal=(N/sum(w))*sinusx(vout(1:N).*w,f,N);	% Extracts sinusoidal signal
noise=vout(1:N)-signal;			            % Extracts noise components
stot=((abs(fft((vout(1:N).*w)'))).^2);		% Bit-stream PSD
ssignal=(abs(fft((signal(1:N).*w)'))).^2;	% Signal PSD
snoise=(abs(fft((noise(1:N).*w)'))).^2;		% Noise PSD
pwsignal=sum(ssignal(1:fB));                % Signal power
pwnoise=sum(snoise(1:fB));		            % Noise power
snr=pwsignal/pwnoise;
snrdB=dbp(snr);
norm=sum(stot)/sum(vout(1:N).^2)*N;								% PSD normalization
if nargout > 1
	ptot=stot/norm;
	ptotdB=dbp(ptot);
end

if nargout > 2
	psig=ssignal/norm;
	psigdB=dbp(psig);
end

if nargout > 3
	pnoise=snoise/norm;
	pnoisedB=dbp(pnoise);
end


% Filter design
% FIR Filter Design via Spectral Factorization and Convex Optimization
% EE364 lecture, Filter design and equalization
%
%  fir_chebychev_design.m              - Chebychev design of an FIR filter given a desired H(w)
%  one_over_f_filter.m                 - Design a 1/f spectrum shaping (pink-noise) filter
%  equalizer_design.m                  - Equalizer design example
%  iir_mag_design_bandpass_max_atten.m - Maximize stopband attenuation of a bandpass IIR filter
%  fir_lin_phase_lowpass_max_atten.m   - Maximize stopband attenuation of a linear phase lowpass FIR filter
%  fir_mag_design_lowpass_max_atten.m  - Maximize stopband attenuation of a lowpass FIR filter (magnitude design)
%  iir_mag_design_lowpass_max_atten.m  - Maximize stopband attenuation of a lowpass IIR filter
%  fir_lin_phase_lowpass_min_order.m   - Minimize order of a linear phase lowpass FIR filter
%  fir_mag_design_lowpass_min_order.m  - Minimize order of a lowpass FIR filter (magnitude design)
%  fir_lin_phase_lowpass_min_ripple.m  - Minimize stopband ripple of a linear phase lowpass FIR filter
%  fir_lin_phase_lowpass_min_trans.m   - Minimize transition bandwidth of a linear phase lowpass FIR filter
%  spectral_fact.m                     - Spectral factorization using Kolmogorov 1939 approach.
help Contents


function varargout = createDodecahedron()
%CREATEDODECAHEDRON Create a 3D mesh representing a dodecahedron.
%
%   [V, E, F] = createDodecahedron;
%   Create a 3D mesh representing a dodecahedron
%   V is the 20-by-3 array of vertex coordinates
%   E is the 30-by-2 array of edge vertex indices
%   F is the 12-by-5 array of face vertex indices
%
%   [V, F] = createDodecahedron;
%   Returns only the vertices and the face vertex indices.
%
%   MESH = createDodecahedron;
%   Returns the data as a mesh structure, with fields 'vertices', 'edges'
%   and 'faces'.
%
%   Example
%   [v, e, f] = createDodecahedron;
%   drawMesh(v, f);
%
%   Use values given by P. Bourke, see:
%   http://local.wasp.uwa.edu.au/~pbourke/geometry/platonic/
%   faces are re-oriented to have normals pointing outwards.
%
%   See also
%   meshes3d, drawMesh
%   createCube, createOctahedron, createIcosahedron, createTetrahedron
%

%   ---------
%   author : David Legland
%   e-mail: david.legland@inra.fr
%   INRA - TPV URPOI - BIA IMASTE
%   created the 29/07/2010.
%

%   HISTORY

% golden ratio
phi = (1+sqrt(5))/2;

% coordinates pre-computations
b = 1 / phi ;
c = 2 - phi ;

% use values given by P. Bourke, see:
% http://local.wasp.uwa.edu.au/~pbourke/geometry/platonic/
tmp = [ ...
 c  0  1 ;   b  b  b ;   0  1  c  ; -b  b  b  ; -c  0  1 ;  ...
-c  0  1 ;  -b -b  b ;   0 -1  c  ;  b -b  b  ;  c  0  1 ;   ...
 c  0 -1 ;   b -b -b ;   0 -1 -c  ; -b -b -b  ; -c  0 -1 ;  ...
-c  0 -1 ;  -b  b -b ;   0  1 -c  ;  b  b -b  ;  c  0 -1 ; ...
 0  1 -c ;   0  1  c ;   b  b  b  ;  1  c  0  ;  b  b -b ; ...
 0  1  c ;   0  1 -c ;  -b  b -b  ; -1  c  0  ; -b  b  b ; ...
 0 -1 -c ;   0 -1  c ;  -b -b  b  ; -1 -c  0  ; -b -b -b ; ...
 0 -1  c ;   0 -1 -c ;   b -b -b  ;  1 -c  0  ;  b -b  b ; ...
 1  c  0 ;   b  b  b ;   c  0  1  ;  b -b  b  ;  1 -c  0 ;  ...
 1 -c  0 ;   b -b -b ;   c  0 -1  ;  b  b -b  ;  1  c  0 ; ...
-1  c  0 ;  -b  b -b ;  -c  0 -1  ; -b -b -b  ; -1 -c  0 ; ...
-1 -c  0 ;  -b -b  b ;  -c  0  1  ; -b  b  b  ; -1  c  0 ;  ...
];

% extract coordinates of unique vertices
[verts, M, N] = unique(tmp, 'rows', 'first'); %#ok<ASGLU>

% compute indices of face vertices, put result in a 12-by-5 index array
ind0 = reshape((1:60), [5 12])';
faces = N(ind0);

% extract edges from faces
edges = [reshape(faces(:, 1:5), [60 1]) reshape(faces(:, [2:5 1]), [60 1])];
edges = unique(sort(edges, 2), 'rows');

% format output
varargout = formatMeshOutput(nargout, verts, edges, faces);


function [J, grad] = cofiCostFunc(params, Y, R, num_users, num_movies, ...
                                  num_features, lambda)
%COFICOSTFUNC Collaborative filtering cost function
%   [J, grad] = COFICOSTFUNC(params, Y, R, num_users, num_movies, ...
%   num_features, lambda) returns the cost and gradient for the
%   collaborative filtering problem.
%

% Unfold the U and W matrices from params
X = reshape(params(1:num_movies*num_features), num_movies, num_features);
Theta = reshape(params(num_movies*num_features+1:end), ...
                num_users, num_features);

% You need to return the following values correctly
J = 0;
X_grad = zeros(size(X));
Theta_grad = zeros(size(Theta));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the cost function and gradient for collaborative
%               filtering. Concretely, you should first implement the cost
%               function (without regularization) and make sure it is
%               matches our costs. After that, you should implement the
%               gradient and use the checkCostFunction routine to check
%               that the gradient is correct. Finally, you should implement
%               regularization.
%
% Notes: X - num_movies  x num_features matrix of movie features
%        Theta - num_users  x num_features matrix of user features
%        Y - num_movies x num_users matrix of user ratings of movies
%        R - num_movies x num_users matrix, where R(i, j) = 1 if the
%            i-th movie was rated by the j-th user
%
% You should set the following variables correctly:
%
%        X_grad - num_movies x num_features matrix, containing the
%                 partial derivatives w.r.t. to each element of X
%        Theta_grad - num_users x num_features matrix, containing the
%                     partial derivatives w.r.t. to each element of Theta
%

%sum(R .* (((X * Theta') .- Y) .^ 2)) 1*4 matrix
J = 1 / 2 * sum(sum(R .* (((X * Theta') .- Y) .^ 2)));
J = J + lambda / 2 * sum(sum(X .^ 2)) + lambda / 2 * sum(sum(Theta .^ 2));

% (((X * Theta') .- Y) * Theta)

X_grad = (R .* ((X * Theta') .- Y)) * Theta;
X_grad = X_grad + lambda .* X;
Theta_grad = (R .* ((X * Theta') .- Y))' * X;
Theta_grad = Theta_grad + lambda .* Theta;

% =============================================================

grad = [X_grad(:); Theta_grad(:)];

end


function [ ns, xyz ] = sphere_cubed_grid_points_face ( n, i1, j1, k1, i2, ...
  j2, k2, ns, xyz )

%*****************************************************************************80
%
%% SPHERE_CUBED_GRID_POINTS_FACE: points on a face of a cubed sphere grid.
%
%  Discussion:
%
%    This function generates points on a face of a cubed sphere grid, and
%    appneds them to a growing list.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of sections into which each face of
%    the cube is to be divided.
%
%    Input, integer I1, J1, K1, I2, J2, K2, the logical indices, between 0
%    and N, of two corners of the face grid.  It is guaranteed that I1 <= I2,
%    J1 <= J2, and K1 <= K2.
%
%    Input, integer NS, the initial number of points.
%
%    Input, real XYZ(NS,3), distinct points on the unit sphere
%    generated by a cubed sphere grid.
%
%    Output, integer NS, the final number of points.
%
%    Output, real XYZ(NS,3), distinct points on the unit sphere
%    generated by a cubed sphere grid, including the points just genereated.
%
  for i = i1 : i2

    if ( i1 < i2 )
      xc = tan ( ( 2 * i - n ) * 0.25 * pi / n );
    elseif ( i1 == 0 )
      xc = -1.0;
    elseif ( i1 == n )
      xc = +1.0;
    else
      xc = 0.0;
    end

    for j = j1 : j2

      if ( j1 < j2 )
        yc = tan ( ( 2 * j - n ) * 0.25 * pi / n );
      elseif ( j1 == 0 )
        yc = -1.0;
      elseif ( j1 == n )
        yc = +1.0;
      else
        yc = 0.0;
      end

      for k = k1 : k2

        if ( k1 < k2 )
          zc = tan ( ( 2 * k - n ) * 0.25 * pi / n );
        elseif ( k1 == 0 )
          zc = -1.0;
        elseif ( k1 == n )
          zc = +1.0;
        else
          zc = 0.0;
        end

        xyzn = sqrt ( xc^2 + yc^2 + zc^2 );

        ns = ns + 1;
        xyz(ns,1) = xc / xyzn;
        xyz(ns,2) = yc / xyzn;
        xyz(ns,3) = zc / xyzn;

      end
    end
  end

  return
end


function a = diagonal ( m, n, x )

%*****************************************************************************80
%
%% DIAGONAL returns the DIAGONAL matrix.
%
%  Formula:
%
%    if ( I = J )
%      A(I,J) = X(I)
%    else
%      A(I,J) = 0
%
%  Example:
%
%    M = 5, N = 5, X = ( 1, 2, 3, 4, 5 )
%
%    1 0 0 0 0
%    0 2 0 0 0
%    0 0 3 0 0
%    0 0 0 4 0
%    0 0 0 0 5
%
%  Square Properties:
%
%    A is banded, with bandwidth 1.
%
%    A is nonsingular if, and only if, each X(I) is nonzero.
%
%    The inverse of A is a diagonal matrix with diagonal values 1/X(I).
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    LAMBDA(1:N) = X(1:N).
%
%    The matrix of eigenvectors of A is the identity matrix.
%
%    det ( A ) = product ( 1 <= I <= N ) X(I).
%
%    Because A is diagonal, it has property A (bipartite).
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of A.
%
%    Input, real X(min(M,N)), the diagonal entries of A.
%
%    Output, real A(M,N), the matrix.
%
  a = zeros ( m, n );

  for i = 1 : m
    for j = 1 : n

      if ( i == j )
        a(i,j) = x(i);
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


function h = p28_h ( n, x )

%*****************************************************************************80
%
%% P28_H evaluates the Hessian for problem 28.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real H(N,N), the N by N Hessian matrix.
%
  h = zeros ( n, n );

  r = sqrt ( x(1)^2 + x(2)^2 );

  rx1 = x(1) / r;
  rx2 = x(2) / r;

  rx1x1 = x(2)^2 / r^3;
  rx1x2 = - x(1) * x(2) / r^3;
  rx2x1 = - x(1) * x(2) / r^3;
  rx2x2 = x(1)^2 / r^3;
%
%  F = A * B
%  dFdX1 = ( Ar * B + A * Br ) * Rx1
%  d2FdX1dX1 = ( Arr * B + Ar * Br ) * Rx1^2 + ( Ar * B + A * Br ) * Rx1x1
%  etc
%
  a = sqrt ( r );
  ar = 0.5 / sqrt ( r );
  arr = - 0.25 / sqrt ( r^3 );

  b = 1.0 + ( sin ( 50.0 * r^0.2 ) )^2;
  br = 10.0 * sin ( 100.0 * r^0.2 ) * r^(-0.8);
  brr = 200.0 * cos ( 100.0 * r^0.2 ) * r^(-1.6) ...
    - 10.0 * sin ( 100.0 * r^0.2 ) * 0.8 * r^(-1.8);

  h(1,1) = ( arr * b + 2.0 * ar * br + a * brr ) * rx1 * rx1 ...
    + ( ar * b + a * br ) * rx1x1;

  h(1,2) = ( arr * b + 2.0 * ar * br + a * brr ) * rx1 * rx2 ...
    + ( ar * b + a * br ) * rx1x2;

  h(2,1) = ( arr * b + 2.0 * ar * br + a * brr ) * rx2 * rx1 ...
    + ( ar * b + a * br ) * rx2x1;

  h(2,2) = ( arr * b + 2.0 * ar * br + a * brr ) * rx2 * rx2 ...
    + ( ar * b + a * br ) * rx2x2;

  return
end


function [v]=tetVolMeanEst(F,V)

% function [v]=tetVolMeanEst(F,V)
% ------------------------------------------------------------------------
%
% This function calculates the volume of an ideal regular tetrahedron with
% edge lengths (all equal) that match the mean edge lengths occuring for
% the input surface defined by F (faces) and V (vertices).

%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 2014/10/17
%------------------------------------------------------------------------
%%

[edgeLengths]=patchEdgeLengths(F,V);
edgeLengthsMean=mean(edgeLengths);
meanProposedVolume=edgeLengthsMean^3./(6*sqrt(2)); %For a regular tetrahedron
v=meanProposedVolume;

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function hEllipse = ellipsedraw(hand,a,b,x0,y0,phi,lineStyle)
%ELLIPSEDRAW can draw an arbitrary ellipse with given parameters.
%   The properties of that ellipse plot can be customized
%   by setting the ellipse handle.
%
%       hEllipse = ellipsedraw(hand,a,b,x0,y0,phi,lineStyle)
%
%   Input parameters:
%       hand        Parent handle for ellipse
%       a           Value of the major axis
%       b           Value of the minor axis
%       x0          Abscissa of the center point of the ellipse
%       y0          Ordinate of the center point of the ellipse
%       phi         Angle between x-axis and the major axis
%       lineStyle   Definition of the plotted line style
%
%   Output:
%       hEllipse    Handle of the ellipse
%
%   Simple usage:
%       ellipsedraw(5,3);
%       ellipsedraw(5,3,'g--');
%       ellipsedraw(5,3,pi/4);
%
%   Complete usage:
%       h = ellipsedraw(5,3,1,-2,pi/4,'r-.');
%       set(h,'LineWidth',2);

% Designed by: Lei Wang, <WangLeiBox@hotmail.com>, 25-Mar-2003.
% Last Revision: 01-Apr-2003.
% Dept. Mechanical & Aerospace Engineering, NC State University.
% Copyright (c)2003, Lei Wang <WangLeiBox@hotmail.com>
%$Revision: 1.0 $  $ 4/1/2003 5:42:24 PM $

if (nargin < 3)||(nargin > 7),
    error('Too few or too many arguments.');

elseif nargin == 3
    x0 = 0;     y0 = 0;
    phi = 0;    lineStyle = 'b-';

elseif nargin == 4
    if ischar(x0) == 1
        lineStyle = x0;
        x0 = 0; y0 = 0;
        phi = 0;
    else
        phi = x0;
        x0 = 0; y0 = 0;
        lineStyle = 'b-';
    end

elseif nargin == 5
    phi = 0;    lineStyle = 'b-';

elseif nargin == 6
    lineStyle = 'b-';
end

theta = [-0.03:0.01:2*pi];

% Parametric equation of the ellipse
%----------------------------------------
 x = a*cos(theta);
 y = b*sin(theta);

% Coordinate transform
%----------------------------------------
 X = cos(phi)*x - sin(phi)*y;
 Y = sin(phi)*x + cos(phi)*y;
 X = X + x0;
 Y = Y + y0;

% Plot the ellipse
%----------------------------------------
 hEllipse = plot(hand,X,Y,lineStyle);

 %axis equal;


function a = gk323 ( m, n )

%*****************************************************************************80
%
%% GK323 returns the GK323 matrix.
%
%  Discussion:
%
%    This matrix is occasionally known as the "Todd" matrix.
%
%  Formula:
%
%    A(I,J) = abs ( I - J )
%
%  Example:
%
%    N = 5
%
%     0  1  2  3  4
%     1  0  1  2  3
%     2  1  0  1  2
%     3  2  1  0  1
%     4  3  2  1  0
%
%  Rectangular Properties:
%
%    A is integral: int ( A ) = A.
%
%    A is a special case of the Fiedler matrix.
%
%  Square Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    det ( A ) = (-1)^(N-1) * 2^(N-2) * ( N - 1 ).
%
%    A has a dominant positive eigenvalue, and N-1 real negative eigenvalues.
%
%    If N = 2 mod 4, then -1 is an eigenvalue, with an eigenvector
%    of the form ( 1, -1, -1, 1, 1, -1, -1, 1, ... ).
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%   09 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Gregory, David Karney,
%    Example 3.23,
%    A Collection of Matrices for Testing Computational Algorithms,
%    Wiley, New York, 1969, page 51,
%    LC: QA263.G68.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns
%    of the matrix.
%
%    Output, real A(M,N), the matrix.
%
  a = zeros ( m, n );

  for i = 1 : m
    for j = 1 : n
      a(i,j) = abs ( i - j );
    end
  end

  return
end


%function chol_comparison
% Test what is the best way to evaluate Cholesky(-like) decomposition.

n = 1000;

% Large and ill-conditioned covariance matrix
C = gpcov(log(100), 1:n, 1:n);

% Regular Cholesky with diagonal addition
tic
Lchol = chol(C + 100*eps*eye(n), 'lower');
Lchol(1:10, 1:10)
toc

% LDL with negative values to zero
tic
[Lldl, Dldl] = ldl(C);
%Dldl(Dldl<0) = 0;
%Lldl = Lldl * sqrt(Dldl);
d = diag(Dldl);
d(d<0) = 0;
Lldl(1:10,1:10)
Lldl = bsxfun(@times, Lldl, sqrt(d(:))');
toc

% My LDL with negative values to zero
tic
[Lmyldl, Dmyldl] = ldl(C);
%Dldl(Dldl<0) = 0;
%Lldl = Lldl * sqrt(Dldl);
d = diag(Dmyldl);
d(d<0) = 0;
Lmyldl(1:10,1:10)
Lmyldl = bsxfun(@times, Lmyldl, sqrt(d(:))');
toc
%return

% Matlab's cholcov based on eigenvalue decomposition
% Note that this solution does not, in general, give triangular matrix!!!
% Also, this is very slow!
tic
Lcholcov = cholcov(C)';
Lcholcov = [Lcholcov, zeros(n, n-cols(Lcholcov))];
toc

% Errors:
error_chol = norm(C - Lchol*Lchol')
error_ldl = norm(C - Lldl*Lldl')
error_myldl = norm(C - Lmyldl*Lmyldl')
%error_ldl = norm(C - Lldl*Dldl*Lldl')
error_cholcov = norm(C - Lcholcov*Lcholcov')

% What other measures could be used?
% How accurate these methods are in solving linear equations?

x = randn(n,1);
y = C * x;

opts.LT = true;
opts.TRANSA = false;
x_chol = linsolve(Lchol, y, opts);
opts.TRANSA = true;
x_chol = linsolve(Lchol, x_chol, opts);

opts.LT = true;
opts.TRANSA = false;
x_ldl = linsolve(Lldl, y, opts);
opts.TRANSA = true;
x_ldl = linsolve(Lldl, x_ldl, opts);

opts.LT = true;
opts.TRANSA = false;
x_myldl = linsolve(Lmyldl, y, opts);
opts.TRANSA = true;
x_myldl = linsolve(Lmyldl, x_myldl, opts);

opts.LT = false;
opts.TRANSA = false;
x_cholcov = linsolve(Lcholcov, y, opts);
opts.TRANSA = true;
x_cholcov = linsolve(Lcholcov, x_cholcov, opts);

error_chol = norm(x - x_chol)
error_ldl = norm(x - x_ldl)
error_myldl = norm(x - x_myldl)
error_cholcov = norm(x - x_cholcov)


function z = cumint3(x,y)
%
%   z = cumint3(x,y)
%
% This returns a vector z the same size as x and y
% which is the cumulative integral of y with respect
% to x, with the lower integration limit set to x(1) and
% the upper limit ranging from x(1) to x(n). The
% successive intervals in x need not be of equal lengths,
% though none should be of zero length. A third order
% approximation is made so that for up to cubic polynomials
% the values will be exact except for rounding.
% x and y must be column vectors of the same length
% and have at least four elements. Note that with only
% z = b.*g below, this would be computing matlab's
% 'cumtrapz' trapezoidal integration, the rest of the
% expression in z serving to carry out the additional
% third order approximation.  RAS - 03/11/08

% Test arguments
[n,m] = size(x);
if any([n,m]~=size(y)) | m~=1
 error('x and y must be column vectors of equal length.')
end
if n<4
 error('There must be at least four points.')
end

% Compute third order integral
xe = [x(4);x;x(n-3)]; ye = [y(4);y;y(n-3)]; % Provide for endpoints
x0 = xe(1:n-1); x1 = xe(2:n); x2 = xe(3:n+1); x3 = xe(4:n+2);
y0 = ye(1:n-1); y1 = ye(2:n); y2 = ye(3:n+1); y3 = ye(4:n+2);
a = x1-x0; b = x2-x1; c = x3-x2;
d = y1-y0; e = y2-y1; f = y3-y2;
g = (y1+y2)/2;

% Each z value will be the integral from x1 to x2 of a cubic
% polynomial running through (x0,y0),(x1,y1),(x2,y2),(x3,y3).
z = b.*g+1/12*b.^2.*(+c.*b.*(2*c+b).*(c+b).*d ...
    -a.*c.*(c-a).*(2*c+2*a+3*b).*e-a.*b.*(2*a+b).*(a+b).*f) ...
    ./(a.*c.*(a+b).*(c+b).*(c+a+b));

% Obtain cumulative integral values
z = [0;cumsum(z)];

return


% Synthesis of a stationary time signal given sinusoids parameters
%
% Octave compatible
%
% Inputs
%  sins   : matrix of size(3,N) [freq;amp;phase]
%  fs     : [Hz] Sampling frequency
%  winlen : length of the time signal to synthesize
%  derive : if deriv=1, synthesize the derivative of the signal
%
% Outputs
%  s       : The synthesized signal
%
% Copyright (c) 2012 University of Crete - Computer Science Department
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function [s t] = sin2sig(sins, fs, winlen, deriv)
    if nargin<4; deriv=0; end

%      t = (0:winlen-1)/fs;
%      t = (0:winlen-1)/fs;
    % The time reference used for the phase in sins is in the window center
    t = (-(winlen-1)/2:(winlen-1)/2)/fs;

    if deriv==0
        A = cos(2*pi*t'*sins(1,:) + ones(length(t),1)*sins(3,:));
        A = ones(length(t),1)*sins(2,:).*A;
        s = sum(A,2);
    elseif deriv==1
        A = -sin(2*pi*t'*sins(1,:) + ones(length(t),1)*sins(3,:));
        A = ones(length(t),1)*(sins(2,:).*(2*pi*partials(1,:))).*A;
        s = sum(A,2);
    end

return


function varargout = zettl(X)
% Zettl function
%
%   ZETTL([x1, x2]) returns the value of the Zettle function at the
%   specified points. [x1] and [x2] may be vectors. The search domain
%   is
%
%               -5 < x_i < 5
%
%   The global minimum is
%
%               f(x1, x2) = f(-0.0299, 0) = -0.003791

% Author: Rody P.S. Oldenhuis
% Delft University of Technology
% E-mail: oldenhuis@dds.nl
% Last edited 20/Jul/2009

    % if no input is given, return dimensions, bounds and minimum
    if (nargin == 0)
        varargout{1} = 2;        % # dims
        varargout{2} = [-5, -5]; % LB
        varargout{3} = [+5, +5]; % UB
        varargout{4} = [-2.989597760285287e-002, 0]; % solution
        varargout{5} = -3.791237220468656e-003; % function value at solution

    % otherwise, output function value
    else
        % keep all values in the search domain
        X(X < -5) = inf;        X(X > 5) = inf;

        % split input vector X into x1, x2
        if size(X, 1) == 2
            x1 = X(1, :);        x2 = X(2, :);
        else
            x1 = X(:, 1);        x2 = X(:, 2);
        end

        % output function value
        varargout{1} = (x1.^2 + x2.^2 - 2*x1).^2 + x1/4;
    end

end


function [varargout]=ellipsefit(x,y)
%ELLIPSEFIT Stable Direct Least Squares Ellipse Fit to Data.
% [Xc,Yc,A,B,Phi,P]=ELLIPSEFIT(X,Y) finds the least squares ellipse that
% best fits the data in X and Y. X and Y must have at least 5 data points.
% Xc and Yc are the x- and y-axis center of the ellipse respectively.
% A and B are the major and minor axis of the ellipse respectively.
% Phi is the radian angle of the major axis with respect to the x-axis.
% P is a vector containing the general conic parameters of the ellipse.
% The conic representation of the ellipse is given by:
%
% P(1)*x^2 + P(2)*x*y + P(3)*y^2 + P(4)*x + P(5)*y + P(6) = 0
%
% S=ELLIPSEFIT(X,Y) returns the output data in a structure with field names
% equal to the variable names given above, e.g., S.Xc, S.Yc, S.A, S.B,
% S.Phi and S.P
%
% Reference: R. Halif and J. Flusser, "Numerically Stable Direct Least
% Squares FItting of Ellipses," Department of Software Engineering, Charles
% University, Czech Republic, 2000.

% Conversion from conic to conventional ellipse equation inspired by
% fit_ellipse.m on MATLAB Central

% D.C. Hanselman, University of Maine, Orono, ME 04469
% Mastering MATLAB 7
% 2005-02-28
% Rotation angle fixed 2005-08-09

%--------------------------------------------------------------------------
x=x(:); % convert data to column vectors
y=y(:);
if numel(x)~=numel(y) || numel(x)<5
   error('X and Y Must be the Same Length and Contain at Least 5 Values.')
end

D1=[x.*x x.*y y.*y]; % quadratic terms
D2=[x y ones(size(x))]; % linear terms
S1=D1'*D1;
S2=D1'*D2;

[Q2,R2]=qr(D2,0);
if condest(R2)>1.0e10
   warning('ellipsefit',...
      'Data is Poorly Conditioned and May Not Represent an Ellipse.')
end
T=-R2\(R2'\S2'); % -inv(S3) * S2'

M=S1+S2*T;
CinvM=[M(3,:)/2; -M(2,:); M(1,:)/2];
[V,na]=eig(CinvM);
c=4*V(1,:).*V(3,:) - V(2,:).^2;
A1=V(:,c>0);
P=[A1; T*A1];

% correct signs if needed
if ~isempty(P),
  P=sign(P(1))*P;

  Phi=atan(P(2)/(P(3)-P(1)))/2;
  c=cos(Phi);
  s=sin(Phi);

  % rotate the ellipse parallel to x-axis
  Pr=zeros(6,1);
  Pr(1)=P(1)*c*c - P(2)*c*s + P(3)*s*s;
  Pr(2)=2*(P(1)-P(3))*c*s + (c^2-s^2)*P(2);
  Pr(3)=P(1)*s*s + P(2)*s*c + P(3)*c*c;
  Pr(4)=P(4)*c - P(5)*s;
  Pr(5)=P(4)*s + P(5)*c;
  Pr(6)=P(6);

  % extract other data
  XcYc=[c s;-s c]*[-Pr(4)/(2*Pr(1));-Pr(5)/(2*Pr(3))];
  Xc=XcYc(1);
  Yc=XcYc(2);
  F=-Pr(6) + Pr(4)^2/(4*Pr(1)) + Pr(5)^2/(4*Pr(3));
  AB=sqrt(F./Pr(1:2:3));
  A=AB(1);
  B=AB(2);
  Phi=-Phi;
  if A<B % x-axis not major axis, so rotate it pi/2
    Phi=Phi-sign(Phi)*pi/2;
    A=AB(2);
    B=AB(1);
  end
  S.Xc=Xc;
  S.Yc=Yc;
  S.A=A;
  S.B=B;
  S.Phi=Phi;
  S.P=P;
else
  S.Xc=nan;
  S.Yc=nan;
  S.A=nan;
  S.B=nan;
  S.Phi=nan;
  S.P=nan;
end
if nargout==1
  varargout{1}=S;
else
  outcell=struct2cell(S);
  varargout=outcell(1:nargout);
end


function vol = tetrahedronVolume(vertices, varargin)
%TETRAHEDRONVOLUME Signed volume of a tetrahedron.
%
%   VOL = tetrahedronVolume(TETRA)
%   Comptues the siged volume of the tetrahedron TETRA defined by a 4-by-4
%   array representing the polyhedron vertices.
%
%   Example
%     vi = [0 0 0;1 0 0;0 1 0;0 0 1];
%     tetrahedronVolume(vi)
%     ans =
%         0.1667
%
%     [V F] = createTetrahedron;
%     tetrahedronVolume(V)
%     ans =
%         -.3333
%
%   See also
%   meshes3d, createTetrahedron, meshVolume
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2012-04-05,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2012 INRA - Cepia Software Platform.

if nargin == 2
    tetras = varargin{1};
    nTetras = size(tetras, 1);
    vol = zeros(nTetras, 1);
    for i = 1:nTetras
        tetra = tetras(i,:);
        vol(i) = det(bsxfun(@minus, vertices(tetra(2:4),:), vertices(tetra(1),:))) / 6;
    end
    return;
end

% control on inputs
if nargin == 4
    vertices = [vertices ; varargin{1} ; varargin{2} ; varargin{3}];
end

if size(vertices, 1) < 4
    error('Input vertex array requires at least 4 vertices');
end

% compute volume of tetrahedron, using first vertex as origin
vol = det(vertices(2:4,:) - vertices([1 1 1],:)) / 6;