function [x, y, h, hm, xm, ym] = generate_terrain(n, mesh_size, h0, r0, rr)

% [x, y, h, hm, xm, ym] = generate_terrain(n, mesh_size, h0, r0, rr)
%
% This function generates a series of points that approximate terrain
% according to a simple algorithm and very few parameters.
%
% Inputs:
% n         - Number of iterations of algorithm to perform, akin to the
%             order of magnitudes represented by the variation. Anything
%             beyond 7 will produce detail too fine to notice, while the
%             algorithm will require ~3 times longer for every iteration.
% mesh_size - Size of the output mesh (e.g., 512 for 512-by-512)
% h0        - Initial elevation
% r0        - Initial roughness (how much terrain can vary in a step)
% rr        - Roughness roughness (how much roughness can vary in a step)
%
% For an example, see terrain_generation_introduction.m or
% html/terrain_generation_introduction.html.
%
% Outputs:
% x, y, and h    - Vectors of points comprising terrain
% xm, ym, and hm - Meshes over landscape, useful for surf(...)
%
% Tucker McClure
% Copyright 2012, The MathWorks, Inc.

    % Set some defaults if the user didn't provide inputs.
    if nargin < 1, n         = 7;            end % Iterations
    if nargin < 2, mesh_size = 513;          end % Output mesh size
    if nargin < 3, h0        = 0.1 * rand(); end % Elevation
    if nargin < 4, r0        = 0.1 * rand(); end % Roughness
    if nargin < 5, rr        = 0.1 * rand(); end % Roughness roughness

    n0 = randi(5);     % Number of initial points
    m  = 3;            % How many points grow from each old point
    nf = n0 * (m+1)^n; % Total number of points

    % Create initial x, y, and height coordinates and roughness map.
    x = [randn(n0, 1);           zeros(nf-n0, 1)];
    y = [randn(n0, 1);           zeros(nf-n0, 1)];
    h = [r0 * randn(n0, 1) + h0; zeros(nf-n0, 1)];
    r = [rr * randn(n0, 1) + r0; zeros(nf-n0, 1)];

    % Create new points from old points n times.
    for k = 1:n

        % Calculate the new variance for the x, y random draws and for the
        % h, r random draws.
        dxy = 0.75^k;
        dh  = 0.5^k;

        % Number of new points to generate
        n_new = m * n0;

        % Parents for new points
        parents = reshape(repmat(1:n0, m, 1), [n_new, 1]);

        % Calculate indices for new and existing points.
        new = (n0+1):(n0+n_new);
        old = 1:n0;

        % Generate new x/y values.
        theta  = 2*pi * rand(n_new, 1);
        radius = dxy * (rand(n_new, 1) + 1);
        x(new) = x(parents) + radius .* cos(theta);
        y(new) = y(parents) + radius .* sin(theta);

        % Interpolate to find nominal new r and h values and add noise to
        % roughness and height maps.
        r(new) =   interpolate(x(old), y(old), r(old), x(new), y(new)) ...
                 + (dh * rr) .* randn(n_new, 1);
        h(new) =   interpolate(x(old), y(old), h(old), x(new), y(new)) ...
                 + (dh/dxy) * radius .* r(new) .* randn(n_new, 1);

        % Add up the new points.
        n0 = n_new + n0;

    end

    % Normalize the distribution of the points about the median.
    x = (x - median(x))/std(x);
    y = (y - median(y))/std(y);

    % If the user wants a mesh output, we can do that too. Create a mesh
    % over the significant part and interpolate over it.
    if nargout > 3 && ~isempty(mesh_size)
        [xm, ym] = meshgrid(linspace(-1, 1, mesh_size));
        hm = interpolate(x, y, h, xm, ym);
    end

end

% Perform our particular type of interpolation.
function vn = interpolate(x0, y0, v0, xn, yn)

    % Make an interpolator for height or roughness. We'll add safe far-away
    % points so we'll always be interpolating and not extrapolating.
    int = TriScatteredInterp([100*[-1 -1 1 1]'; x0], ...
                             [100*[-1 1 -1 1]'; y0], ...
                             [zeros(4, 1);      v0], 'linear'); %#ok<R%#ok<MSNU> EMFF1>

	% Perform the actual interpolation.
	vn = int(xn, yn);

end


function dist = circle_imp_point_dist_signed_2d ( r, center, p )

%*****************************************************************************80
%
%% CIRCLE_IMP_POINT_DIST_SIGNED_2D: signed distance ( implicit circle, point ) in 2D.
%
%  Discussion:
%
%    The signed distance is zero if the point is on the circle.
%    The signed distance is negative if the point is inside the circle.
%
%    An implicit circle in 2D satisfies the equation:
%
%      ( X - CENTER(1) )**2 + ( Y - CENTER(2) )**2 = R**2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the circle.
%
%    Input, real CENTER(2,1), the center of the circle.
%
%    Input, real P(2,1), the point to be checked.
%
%    Output, real DIST, the signed distance of the point
%    to the circle.  If the point is inside the circle, the signed distance
%    is negative.
%
  dim_num = 2;

  r2 = sqrt ( sum ( ( p(1:dim_num,1) - center(1:dim_num,1) ).^2 ) );

  dist = r2 - r;

  return
end


function y = sinspace(d1, d2, n, factor)
%SINSPACE cosine spaced vector.
%   SINSPACE(X1, X2) generates a row vector of 100 sine spaced points
%   between X1 and X2.
%
%   SINSPACE(X1, X2, N) generates N points between X1 and X2.
%
%   A sine spaced vector clusters the elements toward X2:
%    X1 |      |      |      |     |    |   |  | || X2
%
%   Make n negative to cluster the elements toward X1:
%    X1 || |  |   |    |     |      |      |      | X2
%
%   For -2 < N < 2, SINSPACE returns X2.
%
%   SINSPACE(X1, X2, N, W) clusters the elements to a lesser degree as
%   dictated by W. W = 0 returns a normal sine spaced vector. W = 1 is the
%   same as LINSPACE(X1, X2, N). Experiment with W < 0 and W > 1 for
%   different clustering patterns.
%
%   Author:     Sky Sartorius
%
%   See also COSSPACE, LINSPACE, LOGSPACE.

if nargin == 2
    n = 100;
end
if nargin < 4
    factor = false;
end

if n<0
    n = floor(double(-n));
    y = d2 + (d1-d2)*sin(pi/2*(1-(0:(n-1))/(n-1)));
else
    n = floor(double(n));
    y = d1 + (d2-d1)*sin(pi/(2*(n-1))*(0:n-1));
end

if factor
    y = (1-factor)*y+factor*[d1+(0:n-2)*(d2-d1)/(n-1) d2];
end


function cdf = levy_cdf ( x, a, b )

%*****************************************************************************80
%
%% LEVY_CDF evaluates the Levy CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument of the CDF.
%    Normally, A <= X.
%
%    Input, real A, B, the parameters of the PDF.
%    0 < B.
%
%    Output, real CDF, the value of the PDF.
%
  if ( b <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  LEVY_PDF - Fatal error!\n' );
    fprintf ( 1, '  Input parameter B <= 0.0\n' );
    error ( 'LEVY_PDF - Fatal error!' );
  end

  if ( x <= a )
    cdf = 0.0;
  else
    cdf = 1.0 - error_f ( sqrt ( b / ( 2.0 * ( x - a ) ) ) );
  end

  return
end


% foo.m
%
% This m-file shows an example of how to use ApEn function
%
% More specifically, it generates three simulated data with different
% complexity (sine, chirp, and Gaussian noise), and plots the ApEn values
% with varying r

%-------------------------------------------------------------------------
% coded by Kijoon Lee, kjlee@ntu.edu.sg
% Mar 21st, 2012
%-------------------------------------------------------------------------
clear all

m = 2;      % embedded dimension
tau = 1;    % time delay for downsampling
N = 1000;
t = 0.001*(1:N);

% generate simulated data
sint = sin(2*pi*10*t);      % sine curve
chirpt = chirp(t,0,1,150);  % chirp signal
whitet = randn(1,N);        % white noise

% calculate standard deviations
sd1 = std(sint);
sd2 = std(chirpt);
sd3 = std(whitet);

% specify the range of r
rnum = 30;
result = zeros(3,rnum);

% main calculation and display
figure
for i = 1:rnum
    r = i*0.02;
    result(1,i) = ApEn(m, r*sd1, sint, tau);
    result(2,i) = ApEn(m,r*sd2,chirpt, tau);
    result(3,i) = ApEn(m,r*sd3,whitet, tau);
end

r = 0.02*(1:rnum);
plot(r,result(1,:),'o-',r,result(2,:),'o-',r,result(3,:),'o-')
axis([0 rnum*0.02 0 1.05*max(result(:))])
legend('sin','chirp','white noise')
title(['ApEn, m=' num2str(m) ', \tau=' num2str(tau)],'fontsize',14)
xlabel r


function a = clement1 ( n )

%*****************************************************************************80
%
%% CLEMENT1 returns the CLEMENT1 matrix.
%
%  Formula:
%
%    if ( J = I+1 )
%      A(I,J) = sqrt(I*(N-I))
%    else if ( I = J+1 )
%      A(I,J) = sqrt(J*(N-J))
%    else
%      A(I,J) = 0
%
%  Example:
%
%    N = 5
%
%       .    sqrt(4)    .       .       .
%    sqrt(4)    .    sqrt(6)    .       .
%       .    sqrt(6)    .    sqrt(6)    .
%       .       .    sqrt(6)    .    sqrt(4)
%       .       .       .    sqrt(4)    .
%
%  Properties:
%
%    A is tridiagonal.
%
%    A is banded, with bandwidth 3.
%
%    Because A is tridiagonal, it has property A (bipartite).
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    The diagonal of A is zero.
%
%    A is singular if N is odd.
%
%    About 64 percent of the entries of the inverse of A are zero.
%
%    The eigenvalues are plus and minus the numbers
%
%      N-1, N-3, N-5, ..., (1 or 0).
%
%    If N is even,
%
%      det ( A ) = (-1)^(N/2) * (N-1) * (N+1)^(N/2)
%
%    and if N is odd,
%
%      det ( A ) = 0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Clement,
%    A class of triple-diagonal matrices for test purposes,
%    SIAM Review,
%    Volume 1, 1959, pages 50-52.
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  for i = 1 : n
    for j = 1 : n

      if ( j == i + 1 )
        a(i,j) = sqrt ( i * ( n - i ) );
      elseif ( i == j + 1 )
        a(i,j) = sqrt ( j * ( n - j ) );
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


% Computes log(besseli(0,x)) robustly.  Computing it directly causes
% numerical problems at high x, but the function has asymptotic linear
% behaviour, which we approximate here for high x.
%
% author: Daniel C Alexander (d.alexander@ucl.ac.uk)
%

function lb0 = logbesseli0(x)

% For very large arguments to besseli, we approximate log besseli using a
% linear model of the asymptotic behaviour.
% The linear parameters come from this command:
% app=regress(log(besseli(0,500:700))',[ones(201,1) (500:700)']);
app = [-3.61178295877576 0.99916157999904];

lb0 = zeros(length(x), 1);
exact = find(x<700);
approx = find(x>=700);
lb0(exact) = log(besseli(0, x(exact)));
%lb0(approx) = x(approx)*app(2) + app(1);

% This is a more standard approximation.  For large x, I_0(x) -> exp(x)/sqrt(2 pi x).
lb0(approx) = x(approx) - log(2*pi*x(approx))/2;


function Ahat = nearestSPD(A)
% nearestSPD - the nearest (in Frobenius norm) Symmetric Positive Definite matrix to A
% usage: Ahat = nearestSPD(A)
%
% From Higham: "The nearest symmetric positive semidefinite matrix in the
% Frobenius norm to an arbitrary real matrix A is shown to be (B + H)/2,
% where H is the symmetric polar factor of B=(A + A')/2."
%
% http://www.sciencedirect.com/science/article/pii/0024379588902236
%
% arguments: (input)
%  A - square matrix, which will be converted to the nearest Symmetric
%    Positive Definite Matrix.
%
% Arguments: (output)
%  Ahat - The matrix chosen as the nearest SPD matrix to A.

if nargin ~= 1
  error('Exactly one argument must be provided.')
end

% test for a square matrix A
[r,c] = size(A);
if r ~= c
  error('A must be a square matrix.')
elseif (r == 1) && (A <= 0)
  % A was scalar and non-positive, so just return eps
  Ahat = eps;
  return
end

% symmetrize A into B
B = (A + A')/2;

% Compute the symmetric polar factor of B. Call it H.
% Clearly H is itself SPD.
[U,Sigma,V] = svd(B);
H = V*Sigma*V';

% get Ahat in the above formula
Ahat = (B+H)/2;

% ensure symmetry
Ahat = (Ahat + Ahat')/2;

% test that Ahat is in fact PD. if it is not so, then tweak it just a bit.
p = 1;
k = 0;
while p ~= 0
  [R,p] = chol(Ahat);
  k = k + 1;
  if p ~= 0
    % Ahat failed the chol test. It must have been just a hair off,
    % due to floating point trash, so it is simplest now just to
    % tweak by adding a tiny multiple of an identity matrix.
    mineig = min(eig(Ahat));
    Ahat = Ahat + (-mineig*k.^2 + eps(mineig))*eye(size(A));
  end
end


%Function that creates different kernels

function func=kernelCreator()
	%Xtrain is the training set
	%Xtest is the test set

	func.SE=@SE;
	func.powExp=@powExp;

end

function [yhat,conf95,K,dd]=SE(Xtrain,ytrain,Xtest,l,A,nugget)
	%Squared Exponential kernel:
	%K(x,x')=exp(-0.5*||x-x'||^2/l^2)
	%l is the length parameter

	%yhat gives the mean of the prediction
	%conf95 is a matrix with two columns with the
	%lower 95 and upper 95 confidence interval

	dd=distance(Xtrain,Xtrain);
	k=dd;
	ks=distance(Xtrain,Xtest);
	kss=distance(Xtest,Xtest);
	%Creating the covariance matrices
	scl=2*l^2;
	K=A*exp(-k/scl)+nugget*eye(size(dd));
	Ks=A*exp(-ks/scl);
	Kss=A*exp(-kss/scl);
	U=chol(K);
	aux=U'\ytrain;
	v=U\aux;

	yhat=Ks'*v;
	yhat;
	%Calculating the confidence intervales
	covariance=Kss-Ks'*inv(K)*Ks;
	diag(covariance);
	temp=sqrt(diag(covariance));
	temp;
	%pause()
	conf95(:,1)=yhat-temp; %lower 95
	conf95(:,2)=yhat+temp;

end

function yhat=powExp(Xtrain,ytrain,Xtest,l,p,nugget)
	%PowerExponentialKernel: exp(-\|x-x'\|^{p}/l^{2})
	%K(x,x')=min(x,x');

	dd=distance(Xtrain,Xtrain);
	k=dd;
	ks=distance(Xtrain,Xtest);
	kss=distance(Xtest,Xtest);
	%Creating the covariance matrices
	scl=l^2;
	K=exp(-(k).^(p/2)/scl)+nugget*eye(size(dd));
	Ks=exp(-(ks).^(p/2)/scl);
	Kss=exp(-(kss).^(p/2)/scl);
	U=chol(K);
	aux=U'\ytrain;
	v=U\aux;

	yhat=Ks'*v;
end

function D=distance(Xtrain,Xtest)
	%Xtrain, Xtest
	%D(i,j) is the euclidean distance between training point i
	%and test point j

	[n,d]=size(Xtrain);
	[t,d]=size(Xtest);

	D=Xtrain.^2*ones(d,t)+ones(n,d)*(Xtest').^2-2*Xtrain*Xtest';
end


function numgrad = computeNumericalGradient(J, theta)
% numgrad = computeNumericalGradient(J, theta)
% theta: a vector of parameters
% J: a function that outputs a real-number. Calling y = J(theta) will return the
% function value at theta.

% Initialize numgrad with zeros   2x1  这是输入X的行数，其实也就是参数W的个数
numgrad = zeros(size(theta));

%% ---------- YOUR CODE HERE --------------------------------------
% Instructions:
% Implement numerical gradient checking, and return the result in numgrad.
% (See Section 2.3 of the lecture notes.)
% You should write code so that numgrad(i) is (the numerical approximation to) the
% partial derivative of J with respect to the i-th input argument, evaluated at theta.
% I.e., numgrad(i) should be the (approximately) the partial derivative of J with
% respect to theta(i).
%
% Hint: You will probably want to compute the elements of numgrad one at a time.

epsilon = 0.000001;
% 2 x2  得到参数的个数
numW = size(theta,1);
I = eye(numW);
I =  I * epsilon;
for j = 1:numW
    numgrad(j) = (J(theta+I(:,j)) - J(theta-I(:,j))) / (2 * epsilon);
end

%% ---------------------------------------------------------------
end


function [I J] = itril(sz, k)
% function [I J] = itril(sz, k) % OR
% I = itril(sz, k)
%
% Return the subindices [I J] (or linear indices I if single output call)
% in the purpose of extracting an lower triangular part of the matrix of
% the size SZ. Input k is optional shifting. For k=0, extract from the main
% diagonal. For k>0 -> above the diagonal, k<0 -> below the diagonal
%
% This returnd same as [...] = find(tril(ones(sz),k))
% - Output is a column and sorted with respect to linear indice
% - No intermediate matrix is generated, that could be useful for large
%   size problem
% - Mathematically, A(itril(size(A)) is called (lower) "half-vectorization"
%   of A
%
% Example:
%
% A = [ 7     5     4
%       4     2     3
%       9     1     9
%       3     5     7 ]
%
% I = itril(size(A))  % gives [1 2 3 4 6 7 8 11 12]'
% A(I)                % gives [7 4 9 3 2 1 5  9  7]' OR A(tril(A)>0)
%
% Author: Bruno Luong <brunoluong@yahoo.com>
% Date: 21/March/2009

if isscalar(sz)
    sz = [sz sz];
end
m=sz(1);
n=sz(2);

% Main diagonal by default
if nargin<2
    k=0;
end

nc = min(n,m+k); % number of columns of the triangular part
lo = max((1:nc).'-k,1); % lower row indice for each column
hi = m + zeros(nc,1); % upper row indice for each column

if isempty(lo)
    I = zeros(0,1);
    J = zeros(0,1);
else
    c=cumsum([0; hi-lo]+1); % cumsum of the length
    I = accumarray(c(1:end-1), (lo-[0; hi(1:end-1)]-1), ...
                   [c(end)-1 1]);
    I = cumsum(I+1); % row indice
    J = cumsum(accumarray(c,1));
    J = J(1:end-1); % column indice
end

if nargout<2
    % convert to linear indices
    I = sub2ind([m n], I, J);
end

end % itril


function result = polygon_integral_yy ( n, v )

%*****************************************************************************80
%
%% POLYGON_INTEGRAL_YY integrates the function Y*Y over a polygon.
%
%  Discussion:
%
%    The polygon is bounded by the points (X(1:N), Y(1:N)).
%
%    INTEGRAL = (1/12) * sum ( 1 <= I <= N )
%      - ( Y(I)^3 + Y(I)^2 * Y(I-1) + Y(I) * Y(I-1)^2 + Y(I-1)^3 )
%      * ( X(I) - X(I-1) )
%
%    where X(0) and Y(0) should be replaced by X(N) and Y(N).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 March 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    S F Bockman,
%    Generalizing the Formula for Areas of Polygons to Moments,
%    American Mathematical Society Monthly,
%    1989, pages 131-132.
%
%  Parameters:
%
%    Input, integer N, the number of vertices of the polygon.
%    N should be at least 3 for a nonzero result.
%
%    Input, real V(2,N), the coordinates of the vertices
%    of the polygon.  These vertices should be given in
%    counter-clockwise order.
%
%    Output, real RESULT, the value of the integral.
%
  result = 0.0;

  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'POLYGON_INTEGRAL_YY - Warning!\n' );
    fprintf ( 1, '  The number of polygonal vertices must be\n' );
    fprintf ( 1, '  at least 3, but the input polygon has N = %d\n', n );
    error ( 'POLYGON_INTEGRAL_YY - Warning!' );
  end

  for i = 1 : n

    if ( i == 1 )
      im1 = n;
    else
      im1 = i - 1;
    end

    result = result - ( v(2,i).^3 + v(2,i).^2 * v(2,im1) ...
      + v(2,i) * v(2,im1).^2 + v(2,im1).^3 ) * ( v(1,i) - v(1,im1) );

  end

  result = result / 12.0;

  return
end


function x=pentsolve(A,b)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% pentsolve.m
%
% Solve a pentadiagonal system Ax=b where A is a strongly nonsingular matrix
%
% If A is not a pentadiagonal matrix, results will be wrong
%
% Reference: G. Engeln-Muellges, F. Uhlig, "Numerical Algorithms with C"
%               Chapter 4. Springer-Verlag Berlin (1996)
%
% Written by Greg von Winckel 3/15/04
% Contact: gregvw@chtm.unm.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[M,N]=size(A);

% Check dimensions
if M~=N
    error('Matrix must be square');
    return;
end

if length(b)~=M
    error('Matrix and vector must have the same number of rows');
    return;
end

x=zeros(N,1);

% Check for symmetry
if A==A'    % Symmetric Matrix Scheme

    % Extract bands
    d=diag(A);
    f=diag(A,1);
    e=diag(A,2);

    alpha=zeros(N,1);
    gamma=zeros(N-1,1);
    delta=zeros(N-2,1);
    c=zeros(N,1);
    z=zeros(N,1);

    % Factor A=LDL'
    alpha(1)=d(1);
    gamma(1)=f(1)/alpha(1);
    delta(1)=e(1)/alpha(1);

    alpha(2)=d(2)-f(1)*gamma(1);
    gamma(2)=(f(2)-e(1)*gamma(1))/alpha(2);
    delta(2)=e(2)/alpha(2);

    for k=3:N-2
        alpha(k)=d(k)-e(k-2)*delta(k-2)-alpha(k-1)*gamma(k-1)^2;
        gamma(k)=(f(k)-e(k-1)*gamma(k-1))/alpha(k);
        delta(k)=e(k)/alpha(k);
    end

    alpha(N-1)=d(N-1)-e(N-3)*delta(N-3)-alpha(N-2)*gamma(N-2)^2;
    gamma(N-1)=(f(N-1)-e(N-2)*gamma(N-2))/alpha(N-1);
    alpha(N)=d(N)-e(N-2)*delta(N-2)-alpha(N-1)*gamma(N-1)^2;

    % Update Lx=b, Dc=z

    z(1)=b(1);
    z(2)=b(2)-gamma(1)*z(1);

    for k=3:N
        z(k)=b(k)-gamma(k-1)*z(k-1)-delta(k-2)*z(k-2);
    end

    c=z./alpha;

    % Backsubstitution L'x=c
    x(N)=c(N);
    x(N-1)=c(N-1)-gamma(N-1)*x(N);

    for k=N-2:-1:1
        x(k)=c(k)-gamma(k)*x(k+1)-delta(k)*x(k+2);
    end

else        % Non-symmetric Matrix Scheme

    % Extract bands
    d=diag(A);
    e=diag(A,1);
    f=diag(A,2);
    h=[0;diag(A,-1)];
    g=[0;0;diag(A,-2)];

    alpha=zeros(N,1);
    gam=zeros(N-1,1);
    delta=zeros(N-2,1);
    bet=zeros(N,1);

    c=zeros(N,1);
    z=zeros(N,1);

    % Factor A=LR
    alpha(1)=d(1);
    gam(1)=e(1)/alpha(1);
    delta(1)=f(1)/alpha(1);
    bet(2)=h(2);
    alpha(2)=d(2)-bet(2)*gam(1);
    gam(2)=( e(2)-bet(2)*delta(1) )/alpha(2);
    delta(2)=f(2)/alpha(2);

    for k=3:N-2
        bet(k)=h(k)-g(k)*gam(k-2);
        alpha(k)=d(k)-g(k)*delta(k-2)-bet(k)*gam(k-1);
        gam(k)=( e(k)-bet(k)*delta(k-1) )/alpha(k);
        delta(k)=f(k)/alpha(k);
    end

    bet(N-1)=h(N-1)-g(N-1)*gam(N-3);
    alpha(N-1)=d(N-1)-g(N-1)*delta(N-3)-bet(N-1)*gam(N-2);
    gam(N-1)=( e(N-1)-bet(N-1)*delta(N-2) )/alpha(N-1);
    bet(N)=h(N)-g(N)*gam(N-2);
    alpha(N)=d(N)-g(N)*delta(N-2)-bet(N)*gam(N-1);

    % Update b=Lc
    c(1)=b(1)/alpha(1);
    c(2)=(b(2)-bet(2)*c(1))/alpha(2);

    for k=3:N
        c(k)=( b(k)-g(k)*c(k-2)-bet(k)*c(k-1) )/alpha(k);
    end

    % Back substitution Rx=c
    x(N)=c(N);
    x(N-1)=c(N-1)-gam(N-1)*x(N);

    for k=N-2:-1:1
        x(k)=c(k)-gam(k)*x(k+1)-delta(k)*x(k+2);
    end

end


function [ lam, mu ] = cycle_floyd ( f, x0 )

%*****************************************************************************80
%
%% CYCLE_FLOYD finds a cycle in an iterated mapping using Floyd's method.
%
%  Discussion:
%
%    Suppose we a repeatedly apply a function f(), starting with the argument
%    x0, then f(x0), f(f(x0)) and so on.  Suppose that the range of f is finite.
%    Then eventually the iteration must reach a cycle.  Once the cycle is reached,
%    succeeding values stay within that cycle.
%
%    Starting at x0, there is a "nearest element" of the cycle, which is
%    reached after MU applications of f.
%
%    Once the cycle is entered, the cycle has a length LAM, which is the number
%    of steps required to first return to a given value.
%
%    This function uses Floyd's method to determine the values of MU and LAM,
%    given F and X0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Knuth,
%    The Art of Computer Programming,
%    Volume 2, Seminumerical Algorithms,
%    Third Edition,
%    Addison Wesley, 1997,
%    ISBN: 0201896842,
%    LC: QA76.6.K64.
%
%  Parameters:
%
%    Input, integer F( integer I ), the name of the function
%    to be analyzed.
%
%    Input, integer X0, the starting point.
%
%    Output, integer LAM, the length of the cycle.
%
%    Output, integer MU, the index in the sequence starting
%    at X0, of the first appearance of an element of the cycle.
%
  tortoise = f ( x0 );
  hare = f ( tortoise );

  while ( tortoise ~= hare )
    tortoise = f ( tortoise );
    hare = f ( f ( hare ) );
  end

  mu = 0;
  tortoise = x0;

  while ( tortoise ~= hare )
    tortoise = f ( tortoise );
    hare = f ( hare );
    mu = mu + 1;
  end

  lam = 1
  hare = f ( tortoise )
  while ( tortoise ~= hare )
    hare = f ( hare );
    lam = lam + 1;
  end

  return
end


function [auc,fpr,tpr] = fastAUC(labels,scores,plot_flag)
% function [auc,fpr,tpr] = myauc(labels,scores,plot_flag)
%
% This function calculates m AUC values for m ranked lists.
% n is the number of ranked items.
% m is the number of different rankings.
%
% Input:  labels is nXm binary logical.
%         scores is nXm real. For a high AUC the higher scores should have
%         labels==1.
%         plot_flag: binary flag, if TRUE then m ROC curves will be plotted
%         (default FALSE).
%
% Output: auc is mX1 real, the Area Under the ROC curves.
%         fpr is nXm real, the false positive rates.
%         tpr is nXm real, the true positive rates.

if ~exist('plot_flag','var')
    plot_flag = 0;
end
if ~islogical(labels)
    error('labels input should be logical');
end
if ~isequal(size(labels),size(scores))
    error('labels and scores should have the same size');
end
[n,m] = size(labels);
num_pos = sum(labels);
if any(num_pos==0)
    error('no positive labels entered');
end
if any(num_pos==n)
    error('no negative labels entered');
end

[~,scores_si] = sort(scores,'descend');
clear scores
scores_si_reindex = scores_si+ones(n,1)*(0:m-1)*n;
l = labels(scores_si_reindex);
clear scores_si labels

tp = cumsum(l==1,1);
fp = repmat((1:n)',[1 m])-tp;

num_neg = n-num_pos;
fpr = bsxfun(@rdivide,fp,num_neg); %False Positive Rate
tpr = bsxfun(@rdivide,tp,num_pos); %True Positive Rate

%Plot the ROC curve
if plot_flag==1
    plot(fpr,tpr);
    xlabel('False Positive');
    ylabel('True Positive');
end

auc = sum(tpr.*[(diff(fp)==1); zeros(1,m)])./num_neg;


function j = jacobi_symbol ( q, p )

%*****************************************************************************80
%
%% JACOBI_SYMBOL evaluates the Jacobi symbol (Q/P).
%
%  Discussion:
%
%    If P is prime, then
%
%      Jacobi Symbol (Q/P) = Legendre Symbol (Q/P)
%
%    Else
%
%      let P have the prime factorization
%
%        P = Product ( 1 <= I <= N ) P(I)**E(I)
%
%      Jacobi Symbol (Q/P) =
%
%        Product ( 1 <= I <= N ) Legendre Symbol (Q/P(I))**E(I)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Daniel Zwillinger,
%    CRC Standard Mathematical Tables and Formulae,
%    30th Edition,
%    CRC Press, 1996, pages 86-87.
%
%  Parameters:
%
%    Input, integer Q, an integer whose Jacobi symbol with
%    respect to P is desired.
%
%    Input, integer P, the number with respect to which the Jacobi
%    symbol of Q is desired.  P should be 2 or greater.
%
%    Output, integer J, the Jacobi symbol (Q/P).
%    Ordinarily, J will be -1, 0 or 1.
%    -2, not enough factorization space.
%    -3, an error during Legendre symbol calculation.
%

%
%  P must be greater than 1.
%
  if ( p <= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'JACOBI_SYMBOL - Fatal error!\n' );
    fprintf ( 1, '  P must be greater than 1.\n' );
    l = -2;
    return
  end
%
%  Decompose P into factors of prime powers.
%
  [ nfactor, factor, power, nleft ] = i4_factor ( p );

  if ( nleft ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'JACOBI_SYMBOL - Fatal error!\n' );
    fprintf ( 1, '  Not enough factorization space.\n' );
    j = -2;
    error ( 'JACOBI_SYMBOL - Fatal error!' );
  end
%
%  Force Q to be nonnegative.
%
  qq = q;

  while ( qq < 0 )
    qq = qq + p;
  end
%
%  For each prime factor, compute the Legendre symbol, and
%  multiply the Jacobi symbol by the appropriate factor.
%
  j = 1;
  for i = 1 : nfactor
    pp = factor(i);
    l = legendre_symbol ( qq, pp );
    if ( l < -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'JACOBI_SYMBOL - Fatal error!\n' );
      fprintf ( 1, '  Error during Legendre symbol calculation.\n' );
      j = -3;
      error ( 'JACOBI_SYMBOL - Fatal error!' );
    end
    j = j * l^power(i);
  end

  return
end


function [ x, seed ] = r4_normal_01 ( seed )

%*****************************************************************************80
%
%% R4_NORMAL_01 returns a unit pseudonormal R4.
%
%  Discussion:
%
%    The standard normal probability distribution function (PDF) has
%    mean 0 and standard deviation 1.
%
%    The Box-Muller method is used, which is efficient, but
%    generates two values at a time.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X, a sample of the standard normal PDF.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  [ r1, seed ] = r4_uniform_01 ( seed );
  [ r2, seed ] = r4_uniform_01 ( seed );
  x = sqrt ( -2.0 * log ( r1 ) ) * cos ( 2.0 * pi * r2 );

  return
end


function [q, pval] = ljungbox(data, lags)
% Ljung-Box tests for the presence of serial correlation in up to q lags.  Returns LAGS Ljung-Box
% statistics tests, one for tests for each lag between 1 and LAGS. Under the null of no serial
% correlation and assuming homoskedasticity, the Ljung-Box test statistic is asymptotically
% distributed X2(q)
%
% USAGE:
%  [Q,PVAL] = ljungbox(DATA,LAGS)
%
% INPUTS:
%  DATA      - A T by 1 vector of data
%  LAGS      - The maximum number of lags to compute the LB.  The statistic and pval will be
%                returned for all sets of lags up to and including LAGS
%
% OUTPUTS:
%  Q         - A LAGS by 1 vector of Q statistics
%  PVAL      - A LAGS by 1 set of appropriate pvals
%
% COMMENTS:
%  This test statistic is common but often inappropriate since it assumes homoskedasticity.  For a
%  heteroskedasticity consistent serial correlation test, see lmtest1
%
% SEE ALSO:
%  LMTEST1, SACF, SPACF

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 1/1/2007

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin~=2
    error('2 inputs required.')
end
T=length(data);
if T<=lags
    error('At least LAGS observations requires')
end
if size(data,1)~=T,
    data=data';
end
if size(data,2)~=1
    error('DATA must be a column vector')
end
if ~isscalar(lags) && lags>0 && floor(lags)==lags
    error('LAGS must be a positive integer')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

ac = sacf(data,lags,0,0);

q = zeros(lags,1);
T = length(data);
for L=1:lags
    q(L)=T*(T+2)*sum(ac(1:L).^2./(T-(1:L)'));
end
pval = 1 - chi2cdf(q,(1:lags)');


function s = Simpson2(a, b)
%% Simpson Rule for a prestablished f(x)
% a: is the lower limit of integration
% b: is the upper limit of integration
% for this excersice f(x)=x^2

h=(b-a)/4;
x(1)=a;
x(2)=a+(b-a)*1/4;
x(3)=a+(b-a)*1/2;
x(4)=a+(b-a)*3/4;
x(5)=b;

f(1)=myfunction(x(1));
f(2)=myfunction(x(2));
f(3)=myfunction(x(3));
f(4)=myfunction(x(4));
f(5)=myfunction(x(5));

s=h/3*sum([1 4 2 4 1].*f);


function [P L U Q]=GaElCoPi(A)

%Gaussian Elimination with Complete Pivoting
%version 1.0

%P*A*Q=L*U
%Alvaro Moraes
%KAUST 2009

%Example of usage:
%A=randn(40);
%[P L U Q]=GaElCoPi(A);
%norm(P*A*Q-L*U)

% or this (it takes some time)
% choose the size of A and
% the number of simulations
% for k=1:1000
%  k
%  A=randn(128);
%  [P L U Q]=GaElCoPi(A);
%  N1(k) =norm(P*A*Q-L*U);
%  %compute the growth factor
%  %rho(k)=max(max(abs(U)))/max(max(abs(A)))
% end
% plot(N1)
% max(abs(N1)) %must be small xxe-014

%and also check the worst scenario for the Partial Pivoting:
% m=128; %(matrix 22.4 from Trefethen and Baum)
% A=-1*tril(ones(m))+2*eye(m);
% A(:,m)=ones(m,1);
%[P L U Q]=GaElCoPi(A);
%norm(P*A*Q-L*U)

[n n]=size(A);
L=zeros(n);
%v and w record the permutations
%of the rows and cols respect.
v=1:n; w=1:n;

for k=1:n-1
     %in the next three lines
     %we obtain  the max of abs(A(v(k:n),w(k:n)))
     %and its coordinates (imr,imc)
     [m,mc]=max(abs(A(v(k:n),w(k:n))));
     [m,c]=max(m);
     imc=c; imr=mc(c);
     %next we transform this coordinates to the
     %coordinates of A
     imr=imr+k-1;
     imc=imc+k-1;
     %now, we perform the permutations
     v([k imr])=v([imr k]);
     w([k imc])=w([imc k]);
     %next, the gaussian elimination step
    for i=k+1:n
        L(v(i),w(k))=A(v(i),w(k))/A(v(k),w(k));
        A(v(i),:)=A(v(i),:)-L(v(i),w(k))*A(v(k),:);
    end
end
%by last, we use v and w to define P, Q, L and U
P=eye(n);P=P(v,:);
Q=eye(n);Q=Q(:,w);
L=L(v,w)+ eye(n);
U=A(v,w);


function[fs]=morsespace(varargin)
%MORSESPACE  Logarithmically-spaced frequencies for generalized Morse wavelets.
%
%   F=MORSESPACE(GAMMA,BETA,N) generates a frequency array for the
%   generalized Morse wavelet transform of an N-point time series. The
%   wavelets are specified by GAMMA and BETA.
%
%   LOG(F) is uniformly spaced, following convention for wavelet analysis.
%
%   F has units of *radians* per sample point, and is a column vector with
%   the frequencies arranged in decending order.
%
%   In this usage, the frequencies F are determined using default settings,
%   described below, which should be appropriate for most applications.
%
%   Additional control over the frequency array F can be obtained using the
%   following alternate usages.
%
%   For details on this calculation, see Lilly (2016).
%   __________________________________________________________________
%
%   High- and low-frequency specification
%
%   F=MORSESPACE(GAMMA,BETA,HIGH,LOW) explicitly sets the high-frequency
%   and low-frequency cutoffs for the frequency array.
%
%   The first (largest) value of F is then just smaller than HIGH and the
%   smallest is just larger than LOW.
%
%   HIGH and LOW have units of *radian* per unit sample point.
%   __________________________________________________________________
%
%   High-frequency cutoff
%
%   The highest frequency can be set to be the minimum of a specified value
%   and a cutoff frequency based on a Nyquist overlap condition.
%
%   F=MORSESPACE(GAMMA,BETA,{ETA,HIGH},LOW) sets the highest frequency
%   to be the minimum of the specified value HIGH, and the largest
%   frequency for which the wavelet will satisfy the threshold level ETA.
%
%   Here ETA be a number between zero and one specifying the ratio of a
%   frequency-domain wavelet at the Nyquist frequency to its peak value.
%
%   Note that in this usage, {ETA,HIGH} is a cell array with two entries.
%
%   The simplified usage F=MORSESPACE(GAMMA,BETA,N) corresponds to the
%   choice ETA=0.1, so that by default, the highest-frequency wavelet
%   will decay to at least 10% of its peak value at the Nyquist frequency.
%   __________________________________________________________________
%
%   Low-frequency cutoff
%
%   The lowest frequency can be set to a cutoff frequency based on an
%   endpoint overlap condition.
%
%   F=MORSESPACE(GAMMA,BETA,HIGH,{P,N}) sets the lowest frequency such that
%   the lowest-frequency wavelet will reach some number P times its central
%   window width at the ends of the time series.
%
%   P is called the packing number.  A choice of P=1 corresponds to
%   roughly 95% of the time-domain wavelet energy being contained within
%   the time series endpoints for a wavelet at the center of the domain.
%
%   F=MORSESPACE(GAMMA,BETA,HIGH,{P,N,LOW}) alternately chooses the maximum
%   of the R-level cutoff frequency, and a specified low frequency LOW.
%
%   The simplified usage F=MORSESPACE(GAMMA,BETA,N) corresponds to the
%   default value P=5.  At the lowest frequency, five wavelets will then
%   fit into the time series, with 5% energy overlap between them.
%   __________________________________________________________________
%
%   Wavelet density
%
%   F=MORSESPACE(GAMMA,BETA,HIGH,LOW,D) controls the number of points in
%   the frequency array through the 'density' D.
%
%   Higher values of D mean more overlap in the frequency domain. The
%   default value of the density is D=4.
%
%   When D=1, the peak of one wavelet is located at the half-power points
%   of the adjacent wavelet. D=4 means that four other wavelets will occur
%   between the peak of one wavelet and its half-power point.
%   __________________________________________________________________
%
%   See also MORSEWAVE, WAVETRANS.
%
%   Usage: f=morsespace(ga,be,N);
%          f=morsespace(ga,be,high,low,D);
%          f=morsespace(ga,be,{eta,high},low,D);
%          f=morsespace(ga,be,{eta,high},{p,N},D);
%          f=morsespace(ga,be,{eta,high},{p,N,low},D);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2009--2016 J.M. Lilly --- type 'help jlab_license' for details

%   Note then when HIGH and LOW are explicitly input, the time series
%   length N is no longer input as the first argument.

if strcmpi(varargin{1},'--t')
  morsespace_test;return
end

if strcmpi(varargin{1},'--f')
  type makefigs_morsespace;
  makefigs_morsespace
  return
end

%/***************************************************************
%Sort out input arguments
% if ischar(varargin{end})
%     str=varargin{end};
%     varargin=varargin(1:end-1);
% else
%     str='peak';
% end

ga=varargin{1};
be=varargin{2};
D=4;

if length(varargin)==3
    N=varargin{3};

    %Default choices
    low={5,N};
    high={0.1,pi};
else
    high=varargin{3};
    low=varargin{4};
    if length(varargin)==5
        D=varargin{5};
    end
end

%\***************************************************************

if iscell(high)
    %Recall pi is Nyquist
    high=min(high{2},morsehigh(ga,be,high{1}));
    %high
end

if iscell(low)
    if length(low)==2
        low{3}=0;
    end
    low=max(low{3},morsespace_low(ga,be,low{1},low{2}));
end

r=1+frac(1,D*morseprops(ga,be));
N=floor(frac(log(frac(high,low)),log(r)));
fs=high*ones(N+1,1)./r.^[0:N]';

% if length(fs)>1000
%     warning('F is longer than 1000 points... ')
% end

% if strfind(str,'ene')
%      [fm,fe,fi] = morsefreq(ga,be);
%      fs=frac(fe,fm).*fs;
% end

function[fmin]=morsespace_low(ga,be,r,N)

p=morseprops(ga,be);
fmin=frac(2*sqrt(2)*p*r,N);

function[]=morsespace_test
morsespace_hightest;
morsespace_lowtest;

function[]=morsespace_hightest
%Test for high-frequncy cutoff

ga=3;
be=4;
eta=0.1;
ompeak=morsefreq(ga,be);
N=100;

dom=ompeak/N+zeros(4*N,1);
om=cumsum(dom,1);

%morse=frac(1,2)*morseafun(ga,be).*(om.^be).*exp(-om.^ga);

fhigh=morsehigh(ga,be,eta);
[psi,morse] = morsewave(10000,ga,be,fhigh);
reporttest('MORSESPACE high-frequency cutoff',aresame(morse(end/2)./2,eta,1e-3))

function[]=morsespace_lowtest
%Test for low-frequency cutoff

N=1001;
ga1=(1/3:1:11);
be1=(1:1:10);
%ga1=(1/3:.5:11);
%be1=(1:.5:10);

[ga,be]=meshgrid(ga1,be1);
vcolon(ga,be);
[a,sigt,sigo]=morsebox(ga,be);

psi=zeros(N,length(ga));
for i=1:length(ga)
    fs{i}=morsespace(ga(i),be(i),{0.95,pi},{1,N/10},4);
    psi(:,i)=morsewave(N,ga(i),be(i),fs{i}(end),'energy');
end
%figure,plot(sum(squared(psi((end+1)/2-50:(end+1)/2+50,:)),1))
bool=aresame(median(sum(squared(psi((end+1)/2-50:(end+1)/2+50,:)))),0.95,0.01);

reporttest('MORSESPACE low-frequency cutoff, P approximates 95% energy',bool)

%fs=morsespace(2,2,10000);
%psi=morsewave(10000,2,2,fs(end),'energy');
%Can make a figure with the wavelet shifted by 2000 points each time


function b = isPointInCircle(point, circle, varargin)
%ISPOINTINCIRCLE Test if a point is located inside a given circle
%
%   B = isPointInCircle(POINT, CIRCLE)
%   Returns true if point is located inside the circle, i.e. if distance to
%   circle center is lower than the circle radius.
%
%   B = isPointInCircle(POINT, CIRCLE, TOL)
%   Specifies the tolerance value
%
%   Example:
%   isPointInCircle([1 0], [0 0 1])
%   isPointInCircle([0 0], [0 0 1])
%   returns true, whereas
%   isPointInCircle([1 1], [0 0 1])
%   return false
%
%   See also:
%   circles2d, isPointOnCircle
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 07/04/2004.
%

%   HISTORY
%   22/05/2009 rename to isPointInCircle, add psb to specify tolerance

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

d = sqrt(sum(power(point - circle(:,1:2), 2), 2));
b = d-circle(:,3)<=tol;


close all;
clear all;
clc;
rng('default');
png_export = true;
pdf_export = false;
% Create the directory for storing images
[status_code,message,message_id] = mkdir('bin');

mf = spx.graphics.Figures();

% Signal space
N = 256;
% Number of measurements
M = 64;
% Sparsity level
K = 10;
% Construct the signal generator.
gen  = spx.data.synthetic.SparseSignalGenerator(N, K);
% Generate bi-uniform signals
x = gen.biUniform(1, 2);
% Sensing matrix
Phi = spx.dict.simple.gaussian_dict(M, N);
% Measurement vectors
y = Phi.apply(x);
% GOMP solver instance
solver = spx.pursuit.single.GOMP(Phi, K);
%solver.Verbose = true;
% Solve the sparse recovery problem
result = solver.solve(y);
% Solution vector
z = result.z;

stats = spx.commons.sparse.recovery_performance(Phi, K, y, x, z);
spx.commons.sparse.print_recovery_performance(stats);

mf.new_figure('GOMP solution');
subplot(411);
stem(x, '.');
title('Sparse vector');
subplot(412);
stem(z, '.');
title('Recovered sparse vector');
subplot(413);
stem(abs(x - z), '.');
title('Recovery error');
subplot(414);
stem(y, '.');
title('Measurement vector');


function [ value, en ] = wew_a ( x )

%*****************************************************************************80
%
%% WEW_A estimates Lambert's W function.
%
%  Discussion:
%
%    For a given X, this routine estimates the solution W of Lambert's
%    equation:
%
%      X = W * EXP ( W )
%
%    This routine has higher accuracy than WEW_B.
%
%  Modified:
%
%    11 June 2014
%
%  Reference:
%
%    Fred Fritsch, R Shafer, W Crowley,
%    Algorithm 443: Solution of the transcendental equation w e^w = x,
%    Communications of the ACM,
%    October 1973, Volume 16, Number 2, pages 123-124.
%
%  Parameters:
%
%    Input, real X, the argument of W(X)
%
%    Output, real VALUE, the estimated value of W(X).
%
%    Output, real EN, the last relative correction to W(X).
%
  c1 = 4.0 / 3.0;
  c2 = 7.0 / 3.0;
  c3 = 5.0 / 6.0;
  c4 = 2.0 / 3.0;
%
%  Initial guess.
%
  f = log ( x );

  if ( x <= 6.46 )

    wn = x .* ( 1.0 + c1 * x ) ./ ( 1.0 + x .* ( c2 + c3 * x ) );
    zn = f - wn - log ( wn );

  else

    wn = f;
    zn = - log ( wn );

  end
%
%  Iteration 1.
%
  temp = 1.0 + wn;
  y = 2.0 * temp .* ( temp + c4 * zn ) - zn;
  wn = wn .* ( 1.0 + zn .* y ./ ( temp .* ( y - zn ) ) );
%
%  Iteration 2.
%
  zn = f - wn - log ( wn );
  temp = 1.0 + wn;
  temp2 = temp + c4 * zn;
  en = zn .* temp2 ./ ( temp .* temp2 - 0.5 * zn );
  wn = wn .* ( 1.0 + en );

  value = wn;

  return
end


function idx = findClosestCentroids(X, centroids)
%FINDCLOSESTCENTROIDS computes the centroid memberships for every example
%   idx = FINDCLOSESTCENTROIDS (X, centroids) returns the closest centroids
%   in idx for a dataset X where each row is a single example. idx = m x 1
%   vector of centroid assignments (i.e. each entry in range [1..K])
%

% Set K
K = size(centroids, 1);

% You need to return the following variables correctly.
idx = zeros(size(X,1), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Go over every example, find its closest centroid, and store
%               the index inside idx at the appropriate location.
%               Concretely, idx(i) should contain the index of the centroid
%               closest to example i. Hence, it should be a value in the
%               range 1..K
%
% Note: You can use a for-loop over the examples to compute this.
%

for i=1:length(X)
    distance = inf;
    for j=1:K
        kDist = norm(X(i, :) - centroids(j, :));
        if (kDist < distance)
            distance = kDist;
            idx(i) = j;
        end
    end
end

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

end


function g = sigmoidGradient(z)
%SIGMOIDGRADIENT returns the gradient of the sigmoid function
%evaluated at z
%   g = SIGMOIDGRADIENT(z) computes the gradient of the sigmoid function
%   evaluated at z. This should work regardless if z is a matrix or a
%   vector. In particular, if z is a vector or matrix, you should return
%   the gradient for each element.

g = zeros(size(z));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the gradient of the sigmoid function evaluated at
%               each value of z (z can be a matrix, vector or scalar).

g = sigmoid(z).*(1 - sigmoid(z));

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

end


% Entropy: Returns entropy (in bits) of each column of 'X'
% by Will Dwinnell
%
% H = Entropy(X)
%
% H = row vector of calculated entropies (in bits)
% X = data to be analyzed
%
% Example: Measure sample entropy of observations of variables with
%   1, 2, 3 and 4 bits of entropy.
%
% Note: Estimated entropy values are slightly less than true, due to
% finite sample size.
%
% X = ceil(repmat([2 4 8 16],[1e3,1]) .* rand(1e3,4));
% Entropy(X)
%
% Last modified: Nov-12-2006

function H = Entropy(X)

% Establish size of data
[n m] = size(X);

% Housekeeping
H = zeros(1,m);

for Column = 1:m,
    % Assemble observed alphabet
    Alphabet = unique(X(:,Column));

    % Housekeeping
    Frequency = zeros(size(Alphabet));

    % Calculate sample frequencies
    for symbol = 1:length(Alphabet)
        Frequency(symbol) = sum(X(:,Column) == Alphabet(symbol));
    end

    % Calculate sample class probabilities
    P = Frequency / sum(Frequency);

    % Calculate entropy in bits
    % Note: floating point underflow is never an issue since we are
    %   dealing only with the observed alphabet
    H(Column) = -sum(P .* log2(P));
end

% God bless Claude Shannon.

% EOF


function JTotal=polAngGradient(xG,systemType,lRx)
%%POLANGGRADIENT Determine the gradient of the angle of a 2D polar
%           measurement with respect to position (gradient components for
%           velocity etc. are zero and are not provided).Atmospheric and
%           other propagation effects are not taken into account. The
%           measurement can be monostatic or bistatic, but the location of
%           the transmitter does not matter.
%
%INPUTS: xG A 2XN set of Cartesian locations in the format [x;y] where the
%          gradient is desired.
% systemType An optional parameter specifying the axis from which the
%          azimuth angle is measured. It is assumed that the azimuth
%          angle is given in radians. Possible values are
%          0 (The default if omitted) The azimuth angle is
%             counterclockwise from the x axis.
%          1 The azimuth angle is measured clockwise from the y axis.
%      lRx The 2X1 [x;y] location vector of the receiver in Cartesian
%          coordinates. If this parameter is omitted or an empty matrix is
%          passed, then the receiver is assumed to be at the origin.
%
%OUTPUTS: JTotal A 1X2XN set of gradient matrices of the polar angle taken
%           with respect to the components [x,y] in 2D in that order for
%           each of the N points in xG.
%
%The conversion utilizing bistatic polar measurements in 2D is similar to
%that using bistatic r-u-v measurements in 3D, which is discussed in [1].
%Basic differentiation was analytically performed to obtain the expressions
%used in this file.
%
%Note that normally we would convert the point to the local coordinate
%system as
%xLocal=M*(xG(1:2,curPoint)-lRx(1:2))
%then compute the gradient J, and then undo the effects of the rotation
%matrix M in the end to the global coordinates as JTotal=J*M.
%However, that is not necessary with polar angles, due to cancellations.
%Say that M=[m11,m12;m21,m22] is a rotation matrix. Then, the partial
%derivative with respect to x is explicitly:
%((m12*m21-m11*m22)*y)/((m11^2+m21^2)*x^2+2*(m11*m12+m21*m22)*x*y+(m12^2+m22^2)*y^2)
%However, one will see that because M is a rotation matrix and M*M'=eye(2)
%then m12*m21-m11*m22=-1, m11^2+m21^2=m12^2+m22^2=1, and m11*m12+m21*m22=0,
%which is the same as if no rotation matrix were present. Basically, all M
%does is add a constant to the direction angle; it does not change any of
%the derivatives. Thus, M does not matter for the Jacobian.
%
%EXAMPLE:
%Here, we verify that a numerically differentiated Jacobian is consistent
%with the analytic one produced by this function.
% x=[100;-1000];
% lRx=[500;20];
% epsVal=1e-5;
% systemType=0;
% M=randRotMat(2);
% J=polAngGradient(x,systemType,lRx);
%
% theta=getPolAngle(x,systemType,lRx,M);
% thetadX=getPolAngle(x+[epsVal;0],systemType,lRx,M);
% thetadY=getPolAngle(x+[0;epsVal],systemType,lRx,M);
%
% JNumDiff=[(thetadX-theta)/epsVal;(thetadY-theta)/epsVal];
% max(abs((J(:)-JNumDiff(:))./J(:)))
%The relative error will be on the order of 1e-8, indicating good agreement
%between the numerical Jacobian vector and the actual Jacobian vector.
%
%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.
%
%February 2017 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

N=size(xG,2);

%M does not matter, so we do not need to check whether it is given.

if(nargin<3||isempty(lRx))
    lRx=zeros(2,1);
end

if(nargin<2||isempty(systemType))
    systemType=0;
end

JTotal=zeros(1,2,N);
for curPoint=1:N
    %Convert the point into the local coordinate system of the receiver.
    xLocal=xG(1:2,curPoint)-lRx(1:2);

    xL=xLocal(1);
    yL=xLocal(2);

    J=zeros(1,2);
    switch(systemType)
        case 0
            %Derivative with respect to x.
            J(1,1)=-yL/(xL^2+yL^2);

            %Derivative with respect to y.
            J(1,2)=xL/(xL^2+yL^2);
        case 1
            %Derivative with respect to x.
            J(1,1)=yL/(xL^2+yL^2);

            %Derivative with respect to y.
            J(1,2)=-xL/(xL^2+yL^2);
        otherwise
            error('Invalid system type specified.')
    end

    %Undo the rotation to move the gradient into the global coordinate
    %system.
    JTotal(:,:,curPoint)=J;
end
end

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


function PaV = calc_Pa(H0,betaV,dkV)
%Actuarial probability of complication using Cox model

PaV = 1 - exp(-H0 .* exp(betaV.*dkV));

%Test-1:
% tol = 10^-5;
% expectedp10 = 0.1;
% H0=0.01;
% B = 0.05;
% d10 = 47.096;
% p10 = calc_Pa(H0,B,d10);
% assertAlmostEqual(p10,expectedp10,tol);

end


% Flush out the MATLAB.
close all;
clc;
clear all;

% Read the desired image file.
ImageData = imread('ct_scan.pnm');

% Add noise to oriiginal image.
ImageData = imnoise(ImageData,'Salt & Pepper', 0.04);

% Display the original image.
figure,imshow(ImageData);
title(' Original Image with noise: ');

% Take the input of the angle from user.
Angle = input('Enter the Angle = ');

% Convert the degree to radian.
AngleR = Angle*pi/180;

% Create the conversion matrix.
CMatrix = [+cos(AngleR) +sin(AngleR); -sin(AngleR) +cos(AngleR)];

% Calculate the size of the image.
[X,Y,Z] = size(ImageData);

Temp = round( [1 1; 1 Y; X 1; X Y]*CMatrix );

Temp = bsxfun(@minus, Temp, min(Temp)) + 1;

OutputImage = zeros([max(Temp) Z],class(ImageData));

% Implementation of rotaton function.
for a = 1:size(OutputImage,1)

    for b = 1:size(OutputImage,2)
        Rotation = ([a b]-Temp(1,:))*CMatrix.';

        if all(Rotation >= 1) && all(Rotation <= [X Y])
            CL = ceil(Rotation);

            FL = floor(Rotation);

            A = [...
                ((CL(2)-Rotation(2))*(CL(1)-Rotation(1))),...
                ((Rotation(2)-FL(2))*(Rotation(1)-FL(1)));

                ((CL(2)-Rotation(2))*(Rotation(1)-FL(1))),...
                ((Rotation(2)-FL(2))*(CL(1)-Rotation(1)))];

            Color = bsxfun(@times, A, double(ImageData(FL(1):CL(1),FL(2):CL(2),:)));

            OutputImage(a,b,:) = sum(sum(Color),2);
        end

    end

end

% Display the output image.
figure, imshow(OutputImage);
title(' Final Image: ');


function Rets = Simulate_Jump_Diffusion_Log_Returns_func( N_sim, dt, r, q, sigma, jumpModel, jumpParams )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Simulates Log Returns of Jump Diffusion Models with jumps (including simple Black-Scholes, no jumps)
% Returns: paths of dimension (N_sim, M+1), since they include S_0
%          ... Simulates N_sim paths, each row is a full path starting from S_0, ending with S_M (M+1 points in path)
% Author: Justin Lars Kirkby
%
% -----------------
% Params
% -----------------
% N_sim = # paths
% M = #time steps on [0,T], time step is dt=T/M, so each path has M+1 points
% T = time to maturity, ie path is on [0,T]
% S_0 = initial underlying value (e.g. S_0=100)
% r = interst rate (e.g. r = 0.05)
% q = dividend yield (e.g. q = 0.05)
% sigma = diffusion parameter (e.g. sigma = 0.2)
%
%===================================
% jumpModel: 0 = NoJumps, 1 = NormalJumps, 2 = DEJumps, 3 = MixedNormalJumps
%===================================
% jumpParams = paramters container containing all necessary params for models,
%            : if jumpModel = 0, no jump params are needed
%            : if jumpModel > 0, jumpParams must contain lambda, kappa, and any other model specific params (see below)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%==============================
% Initialize Jump Model Params and JumpFunc (function handle)
%==============================
%%% NOTE:  Jump Model is of the form in LOG space
%%% X(m+1) = X(m) + drift + Brownian Component + sum(Jumps on [m,m+1])
%%% By Jump we mean log(Y), e.g. in Merton Model, Jump ~ Normal (since we are in log space )

if jumpModel > 0 %ie if there are jumps in the model
    lambda = jumpParams.lambda;
    kappa  = jumpParams.kappa;

    Zeta = r - q - lambda*kappa;  %NOTE: we are redefining r to include compensation for jump component
    lamdt = lambda*dt;

    if jumpModel == 1 %Normal Jumps, e.g. Merton
        muJ  = jumpParams.muJ;
        sigJ = jumpParams.sigJ;
        JumpFunc = @(n) sum(muJ +sigJ*randn(n,1)); %Generates n independent jumps and sums them

    elseif jumpModel == 2 %Double Exponenial Jumps
        p_up = jumpParams.p_up;
        eta1 = jumpParams.eta1;
        eta2 = jumpParams.eta2;
        JumpFunc = @(n) sum(DoubleExpoRnd(n,p_up, eta1,eta2));

    elseif jumpModel == 3 %Mixed normal Jumps
        p_up = jumpParams.p_up;
        a1 = jumpParams.a1;  b1 = jumpParams.b1;
        a2 = jumpParams.a2;  b2 = jumpParams.b2;
        JumpFunc = @(n) sum(MixedNormalRnd(n, p_up, a1,b1,a2,b2));
    end
else
    Zeta = r - q;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Sigsqdt = sigma*sqrt(dt);
drift = (Zeta - .5*sigma^2)*dt;

if jumpModel == 0  % Just a drifted Brownian motion
    Rets = drift + Sigsqdt*randn(N_sim,1);
else
    Poi = PoissonRnd(N_sim, lamdt);  %Generate Poisson Column Vector of size N_Sim
    for n = 1:N_sim
        if Poi(n)>0
            Poi(n) = JumpFunc(Poi(n));  %JumpFunc(Poi(n)) sums up Poi(n) many jumps from the jump distribution
        end
    end

    Rets = drift + Poi + Sigsqdt*randn(N_sim,1);
end

end


function quad = gauss_legendre_integrate_fast ( f, n )

%*****************************************************************************80
%
%% GAUSS_LEGENDRE_INTEGRATE_FAST applies a Gauss-Legendre quadrature rule.
%
%  Discussion:
%
%    The integration is carried out on the standard interval [-1,1].
%
%    The computation should be very efficient in MATLAB.
%
%  Modified:
%
%    03 March 2007
%
%  Author:
%
%    Lloyd Trefethen
%
%  Reference:
%
%    Lloyd Trefethen,
%    Is Gauss Quadrature Better than Clenshaw-Curtis?
%    SIAM Review,
%    Volume 50, Number 1, 2008, pages 67-87.
%
%  Parameters:
%
%    Input, function F ( x ), the function to be integrated.
%
%    Input, integer N, the order of the quadrature rule.
%
%    Output, real QUAD, the estimate for the integral of F(X).
%
  beta = 0.5 ./ sqrt ( 1 - ( 2 * ( 1:n ) ) .^ (-2) );

  tridiag = diag ( beta, 1 ) + diag ( beta, -1 );

  [ eigval, eigvec ] = eig ( tridiag );

  x = diag ( eigvec );
  [ x, i ] = sort ( x );
  w = 2 * eigval(1,i) .^ 2;

  quad = w * feval ( f, x );

  return
end


function value = c8mat_norm_fro ( m, n, a )

%*****************************************************************************80
%
%% C8MAT_NORM_FRO returns the Frobenius norm of a C8MAT.
%
%  Discussion:
%
%    The Frobenius norm is defined as
%
%      C8MAT_NORM_FRO = sqrt (
%        sum ( 1 <= I <= M ) sum ( 1 <= j <= N ) A(I,J) * A(I,J) )
%
%    The matrix Frobenius norm is not derived from a vector norm, but
%    is compatible with the vector L2 norm, so that:
%
%      c8vec_norm_l2 ( A * x ) <= c8mat_norm_fro ( A ) * c8vec_norm_l2 ( x ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 February 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in A.
%
%    Input, integer N, the number of columns in A.
%
%    Input, complex A(M,N), the matrix whose norm is desired.
%
%    Output, real VALUE, the norm of A.
%
  value = ...
    sqrt ...
    ( ...
      sum ...
      ( ...
        sum ...
        ( ...
          ( ...
            abs ...
            ( ...
              a(1:m,1:n) ...
            ) ...
          ) .^ 2 ...
        ) ...
      ) ...
   );

  return
end


function jac = p12_jac ( neqn, t, y )

%*****************************************************************************80
%
%% P12_JAC evaluates the jacobian for problem p12.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the jacobian.
%
%    Output, real JAC(NEQN,NEQN), the jacobian matrix.
%
  jac = zeros ( neqn, neqn );

  for i = 1 : neqn - 1
    jac(i,i) = - i;
  end

  for i = 2 : neqn
    jac(i,i-1) = i - 1;
  end

  return
end


function geometry_test068 ( )

%*****************************************************************************80
%
%% GEOMETRY_TEST068 tests the SPHERE_DISTANCE routines.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  ntest = 6;

  name = [ ...
       'Atlanta, Georgia  '; ...
       'North Pole        '; ...
       'South Pole        '; ...
       'Timbuktu          '; ...
       'San Antonio, Texas'; ...
       'Savannah, Georgia ' ];
  lat_d =  [ 33, 90, -90, 16, 29, 32 ];
  lat_m =  [ 11,  0,   0, 49, 25,  5 ];
  long_d = [ 82,  0,   0,  3, 98, 81 ];
  long_m = [ 34,  0,   0,  0, 30,  6 ];
  radius = 3957.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'GEOMETRY_TEST068\n' );
  fprintf ( 1, '  POINTS_DIST_SPHERE_3D measures the distance between\n' );
  fprintf ( 1, '  two points on a sphere.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  All tests uses RADIUS = %f\n', radius );
  fprintf ( 1, '  which is the radius of the earth in miles.\n' );
  fprintf ( 1, '\n' );

  for i = 1 : ntest-1

    lat1 = dms_to_radians ( lat_d(i), lat_m(i), 0.0 );
    long1 = dms_to_radians ( long_d(i), long_m(i), 0.0 );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Distance from %s\n', name(i,:) );

    for j = i+1 : ntest

      lat2 = dms_to_radians ( lat_d(j), lat_m(j), 0.0 );
      long2 = dms_to_radians ( long_d(j), long_m(j), 0.0 );

      dist1 = sphere_distance1 ( lat1, long1, lat2, long2, radius );
      dist2 = sphere_distance2 ( lat1, long1, lat2, long2, radius );
      dist3 = sphere_distance3 ( lat1, long1, lat2, long2, radius );

      fprintf ( 1, '             to %s is %16.8f  %16.8f  %16.8f\n', ...
        name(j,:), dist1, dist2, dist3 );

    end

  end

  return
end


function [ u, it_num ] = monogrid_poisson_1d ( n, a, b, ua, ub, force, exact )

%*****************************************************************************80
%
%% MONOGRID_POISSON_1D solves a 1D PDE, using the Gauss-Seidel method.
%
%  Discussion:
%
%    This routine solves a 1D boundary value problem of the form
%
%      - U''(X) = F(X) for A < X < B,
%
%    with boundary conditions U(A) = UA, U(B) = UB.
%
%    The Gauss-Seidel method is used.
%
%    This routine is provided primarily for comparison with the
%    multigrid solver.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 July 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    William Hager,
%    Applied Numerical Linear Algebra,
%    Prentice-Hall, 1988,
%    ISBN13: 978-0130412942,
%    LC: QA184.H33.
%
%  Parameters:
%
%    Input, integer N, the number of intervals.
%
%    Input, real A, B, the left and right endpoints of the region.
%
%    Input, real UA, UB, the left and right boundary values.
%
%    Input, function value = FORCE ( x ), the name of the function
%    which evaluates the right hand side.
%
%    Input, function value = EXACT ( x ), the name of the function
%    which evaluates the exact solution.
%
%    Output, integer IT_NUM, the number of iterations.
%
%    Output, real U(N+1,1), the computed solution.
%

%
%  Initialization.
%
  tol = 0.0001;
%
%  Set the nodes.
%
  x = ( linspace ( a, b, n + 1 ) )';
%
%  Set the right hand side.
%
  r = zeros ( n + 1, 1 );

  r(1) = ua;
  r(2:n) = force ( x(2:n) ) / n / n;
  r(n+1) = ub;

  u = zeros ( n + 1, 1 );

  it_num = 0;
%
%  Gauss-Seidel iteration.
%
  while ( 1 )

    it_num = it_num + 1;

    [ u, d1 ] = gauss_seidel ( n + 1, r, u );

    if ( d1 <= tol )
      break
    end

  end

  return
end


function [ loglik ] = ml_LogLikelihood_gmm(X,Priors,Mu,Sigma)
%LOGLIKELIHOOD_GMM
%
%   input------------------------------------------------------------------
%
%       o X : (D x N), set of N data points of dimension  D
%       o Priors:   1 x K array representing the prior probabilities of the
%               K GMM components.
%       o Mu:       D x K array representing the centers of the K GMM components.
%       o Sigma:    D x D x K array representing the covariance matrices of the
%                   K GMM components.
%
%   output ----------------------------------------------------------------
%
%       o loglik : (N x 1) , loglikelihood
%

N = size(X,2);
K = size(Priors,2);

Pxi = zeros(N,K);
for i=1:K
    Pxi(:,i) = ml_gaussPDF(X, Mu(:,i), Sigma(:,:,i));
end

%Compute the log likelihood
F            = Pxi*Priors';
F(F<realmin) = realmin;
F            = Pxi*Priors';
loglik       = sum(log(F));

end


function point = intersectLinePlane(line, plane, varargin)
%INTERSECTLINEPLANE Intersection point between a 3D line and a plane.
%
%   PT = intersectLinePlane(LINE, PLANE)
%   Returns the intersection point of the given line and the given plane.
%   LINE:  [x0 y0 z0 dx dy dz]
%   PLANE: [x0 y0 z0 dx1 dy1 dz1 dx2 dy2 dz2]
%   PT:    [xi yi zi]
%   If LINE and PLANE are parallel, return [NaN NaN NaN].
%   If LINE (or PLANE) is a matrix with 6 (or 9) columns and N rows, result
%   is an array of points with N rows and 3 columns.
%
%   PT = intersectLinePlane(LINE, PLANE, TOL)
%   Specifies the tolerance factor to test if a line is parallel to a
%   plane. Default is 1e-14.
%
%   Example
%     % define horizontal plane through origin
%     plane = [0 0 0   1 0 0   0 1 0];
%     % intersection with a vertical line
%     line = [2 3 4  0 0 1];
%     intersectLinePlane(line, plane)
%     ans =
%        2   3   0
%     % intersection with a line "parallel" to plane
%     line = [2 3 4  1 2 0];
%     intersectLinePlane(line, plane)
%     ans =
%       NaN  NaN  NaN
%
%   See also
%   lines3d, planes3d, points3d, clipLine3d
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2005-02-17
% Copyright 2005-2022 INRA - TPV URPOI - BIA IMASTE

% extract tolerance if needed
tol = 1e-14;
if nargin > 2
    tol = varargin{1};
end

% unify sizes of data
nLines  = size(line, 1);
nPlanes = size(plane, 1);

% N planes and M lines not allowed
if nLines ~= nPlanes && min(nLines, nPlanes) > 1
    error('MatGeom:geom3d:intersectLinePlane', ...
        'Input must have same number of rows, or one must be 1');
end

% plane normal
n = crossProduct3d(plane(:,4:6), plane(:,7:9));

% difference between origins of plane and line
dp = bsxfun(@minus, plane(:, 1:3), line(:, 1:3));

% dot product of line direction with plane normal
denom = sum(bsxfun(@times, n, line(:,4:6)), 2);

% relative position of intersection point on line (can be inf in case of a
% line parallel to the plane)
t = sum(bsxfun(@times, n, dp),2) ./ denom;

% compute coord of intersection point
point = bsxfun(@plus, line(:,1:3),  bsxfun(@times, [t t t], line(:,4:6)));

% set indices of line and plane which are parallel to NaN
par = abs(denom) < tol;
point(par,:) = NaN;


% TERNARY.COORDS calculate rectangular coordinates of fractions on a ternary plot
%   [X, Y] = TERNARY.COORDS(FA, FB) returns the rectangular X and Y coordinates
%   for the point with a fraction defined by FA and FB.  It is assumed that
%   FA and FB are sensible fractions.
%
%   [X, Y] = TERNARY.COORDS(FA, FB, FC) returns the same.  FC is assumed to be
%   the remainder when subtracting FA and FB from 1.

% Author: Carl Sandrock 20050211

% Modifications

% Modifiers

function [x, y] = coords(fA, fB, fC)
if nargin < 3
    fC = 1 - (fA + fB);
end

y = fB*sin(deg2rad(60));
x = fA + y*cot(deg2rad(60));%fA+fb*cos(deg2rad(60))
return


function W = compute_mesh_weight(vertex,face,type,options)

% compute_mesh_weight - compute a weight matrix
%
%   W = compute_mesh_weight(vertex,face,type,options);
%
%   W is sparse weight matrix and W(i,j)=0 is vertex i and vertex j are not
%   connected in the mesh.
%
%   type is either
%       'combinatorial': W(i,j)=1 is vertex i is conntected to vertex j.
%       'distance': W(i,j) = 1/d_ij^2 where d_ij is distance between vertex
%           i and j.
%       'conformal': W(i,j) = cot(alpha_ij)+cot(beta_ij) where alpha_ij and
%           beta_ij are the adjacent angle to edge (i,j)
%
%   If options.normalize=1, the the rows of W are normalize to sum to 1.
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
[vertex,face] = check_face_vertex(vertex,face);

nface = size(face,1);
n = max(max(face));

verb = getoptions(options, 'verb', n>5000);

if nargin<3
    type = 'conformal';
end

switch lower(type)
    case 'combinatorial'
        W = triangulation2adjacency(face);
    case 'distance'
        W = my_euclidean_distance(triangulation2adjacency(face),vertex);
        W(W>0) = 1./W(W>0);
        W = (W+W')/2;
    case 'conformal'
        % conformal laplacian
        W = sparse(n,n);
        ring = compute_vertex_face_ring(face);
        for i = 1:n
            if verb
                progressbar(i,n);
            end
            for b = ring{i}
                % b is a face adjacent to a
                bf = face(:,b);
                % compute complementary vertices
                if bf(1)==i
                    v = bf(2:3);
                elseif bf(2)==i
                    v = bf([1 3]);
                elseif bf(3)==i
                    v = bf(1:2);
                else
                    error('Problem in face ring.');
                end
                j = v(1); k = v(2);
                vi = vertex(:,i);
                vj = vertex(:,j);
                vk = vertex(:,k);
                % angles
                alpha = myangle(vk-vi,vk-vj);
                beta = myangle(vj-vi,vj-vk);
                % add weight
                W(i,j) = W(i,j) + cot( alpha );
                W(i,k) = W(i,k) + cot( beta );
            end
        end
    otherwise
        error('Unknown type.')
end

if isfield(options, 'normalize') && options.normalize==1
    W = diag(sum(W,2).^(-1)) * W;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function beta = myangle(u,v);

du = sqrt( sum(u.^2) );
dv = sqrt( sum(v.^2) );
du = max(du,eps); dv = max(dv,eps);
beta = acos( sum(u.*v) / (du*dv) );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function W = my_euclidean_distance(A,vertex)

if size(vertex,1)<size(vertex,2)
    vertex = vertex';
end

[i,j,s] = find(sparse(A));
d = sum( (vertex(i,:) - vertex(j,:)).^2, 2);
W = sparse(i,j,d);


function [pval m] = circ_otest(alpha, sz, w)
%
% [pval, m] = circ_otest(alpha,sz,w)
%   Computes Omnibus or Hodges-Ajne test for non-uniformity of circular data.
%   H0: the population is uniformly distributed around the circle
%   HA: the population is not distributed uniformly around the circle
%
%   Alternative to the Rayleigh and Rao's test. Works well for unimodal,
%   bimodal or multimodal data. If requirements of the Rayleigh test are
%   met, the latter is more powerful.
%
%   Input:
%     alpha	sample of angles in radians
%     [sz   step size for evaluating distribution, default 1 degree
%     [w		number of incidences in case of binned angle data]

%   Output:
%     pval  p-value
%     m     minimum number of samples falling in one half of the circle
%
% PHB 3/16/2009
%
% References:
%   Biostatistical Analysis, J. H. Zar
%   A bivariate sign test, J. L. Hodges et al., 1955
%   A simple test for uniformity of a circular distribution, B. Ajne, 1968
%
% Circular Statistics Toolbox for Matlab

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

if size(alpha,2) > size(alpha,1)
	alpha = alpha';
end

if nargin < 2 || isempty(sz)
  sz = circ_ang2rad(1);
end

if nargin < 3
  w = ones(size(alpha));
else
  if length(alpha)~=length(w)
    error('Input length does not match.')
  end
  w =w(:);
end

alpha = mod(alpha,2*pi);
n = sum(w);
dg = 0:sz:pi;

m1 = zeros(size(dg));
m2 = zeros(size(dg));
for i=1:length(dg)
  m1(i) = sum((alpha > dg(i) & alpha < pi + dg(i)).*w);
  m2(i) = n - m1(i);
end
m = min(min([m1;m2]));

if n > 50
  % approximation by Ajne (1968)
  A = pi*sqrt(n) / 2 / (n-2*m);
  pval = sqrt(2*pi) / A * exp(-pi^2/8/A^2);
else
  % exact formula by Hodges (1955)
  pval = 2^(1-n) * (n-2*m) * nchoosek(n,m);
end


function y = gprctile(x,p)
%GPRCTILE Percentile(s) of a grouped sample.
%  In some scientific works, once the data have been gathered from a
%  population of interest, it is often difficult to get a sense of what
%  the data indicate when they are presented in an unorganized fashion.
%  Assembling the raw data into a meaningful form, such as a frequency
%  distribution, makes the data easier to understand and interpret. It is
%  in the context of frequency distributions that the importance of
%  conveying in a succinct way numerical information contained in the data
%  is encountered.
%  So, grouped data is data that has been organized into groups known as
%  classes.  The raw dataset can be organized by constructing a table
%  showing the frequency distribution of the variable (whose values are
%  given in the raw dataset). Such a frequency table is often referred to
%  as grouped data.
%  Here, we developed a m-code to calculate the percentile(s) of a grouped
%  data. One can input the returns or modified vectors n and xout
%  containing the frequency counts and the bin locations of the hist
%  m-function, in a column form matrix.
%
%  Percentile calculation uses the straight forward formula,
%
%                  P = L + I*(N*P/100 - C)/F
%
%  where:
%  L = lower limit of the interval containing the percentile
%  I = width of the interval containing the percentile
%  N = total number of data
%  P = interested percentile
%  C = cumulative frequency corresponding to the previous percentile class
%  F = number of cases in the interval containing the percentile
%
%  Syntax: function y = gprctile(x,p)
%
%  Inputs:
%       x - data matrix (Size of matrix must be n-by-2; absolut frequency=
%           column 1, class mark=column 2)
%       p - is a scalar or a vector of percent values
%  Outputs:
%       y  - percentile(s) of the values in x
%
%  Example: From the example given at
%  http://www.emathzone.com/tutorials/basic-statistics/frequency-distributi
%  on-of-discrete-data.html
%  We are interested to get the percentile values 15,30,60,80.
%
%  Data: 2,4,6,1,3,5,3,7,8,6,4,7,4,4,2,1,3,6,4,2,5,7,9,1,2,10,1,8,9,2,3,1,
%  2,3,4,4,4,6,6,5,5,4,5,8,5,4,3,3,2,5,0,5,9,9,8,10,0,4,10,10,1,1,2,2,1,8,
%  6,9,10
%
%  Using [A,B] = hist(x,11)
%
%  x = [B' A']; p = [15,30,60,80];
%
%                     ----------------
%                        MC       F
%                     ----------------
%                      0.4545     2
%                      1.3636     8
%                      2.2727     9
%                      3.1818     7
%                      4.0909    11
%                      5.0000     8
%                      5.9091     6
%                      6.8182     3
%                      7.7273     5
%                      8.6364     5
%                      9.5455     5
%                     ----------------
%
%  Calling on Matlab the function:
%          y = gprctile(x,p)
%
%  Answer is:
%
%  y = 1.8535    2.9481    5.0455    7.4909
%
%  Created by A. Trujillo-Ortiz, R. Hernandez-Walls and R. Preciado-Perez
%             Facultad de Ciencias Marinas
%             Universidad Autonoma de Baja California
%             Apdo. Postal 453
%             Ensenada, Baja California
%             Mexico.
%             atrujo@uabc.edu.mx
%
%  Copyright (C)  September 15, 2012.
%
%  To cite this file, this would be an appropriate format:
%  Trujillo-Ortiz, A., R. Hernandez-Walls and R. Preciado-Perez. (2012).
%     gprctile:Percentile(s) of a grouped sample. [WWW document].
%     URL http://www.mathworks.com/matlabcentral/fileexchange/
%     38228-gprctile
%
%  Reference:
%  Cann, A. J. (2003), Maths from Scratch for Biologists. John Willey &
%              Sons Ltd. Chichester:England.
%

c = size(x,2);

if c ~= 2
    error('stats:gprctile:BadData','X must have two colums.');
end

if ~isvector(p) || numel(p) == 0
    error('stats:gprctile:BadProbs', ...
          'P must be a scalar or a non-empty vector.');
elseif any(p < 0 | p > 100) || ~isreal(p)
    error('stats:gprctile:BadPercents', ...
          'P must take real values between 0 and 100');
end

mc = x(:,1); %class mark
f = x(:,2); %absolut frequency
a = mc(2) - mc(1); %class interval
fa = cumsum(f); %cumulative absolut frequency
n = fa(end); %sample size

y = [];
for i = 1:length(p)
    r(i) = n*p(i)/100; %interested ordened data (iod)
    cl(i) = min(find(fa > r(i))); %class where the iod are
    l(i) = mc(cl(i));
    L(i) = l(i) - 0.5*a; %lower class boundary
    A(i) =(a/f(cl(i)));
    if (cl(i) - 1) == 0;
        B(i) = r(i);
    else
        B(i) = (r(i) - fa(cl(i) - 1));
    end

    yy = L(i) + A(i)*B(i); %calculation by a linear interpolation
    y = [y yy];
end

return,


function [H,R] = myqr (A)
%MYQR QR factorization using Householder reflections
% uses function [v,beta,xnorm] = hmake1 (x)
% and function hx = happly (v, beta, x)
%
% Example
%   [H,R] = myqr (A)
% See also: testall

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

[m n] = size (A) ;

H = zeros (m,n) ;
R = zeros (m,n) ;

for k = 1:n

    % apply prior H's
    % fprintf ('\n-----------------init %d\n', k) ;
    x = A (:,k) ;
    for i = 1:k-1
        v = H(((i+1):m),i) ;
        v = [1 ; v] ;                                                       %#ok
        beta = H (i,i) ;
        % n1 = norm (x (i:m)) ;
        x (i:m) = happly (v, beta, x (i:m)) ;
        % n2 = norm (x (i:m)) ;
        % fprintf ('=============== i %d %g %g\n', i, n1, n2) ;
        % beta
        % v'
        % X = x'
        % pause
        % i
        % x
    end
    % k
    % x

    % make Hk
    % fprintf ('x(k:m) = ') ; x (k:m)
    [v,beta,xnorm] = hmake1 (x (k:m)) ;

    H (k,k) = beta ;
    H (k+1:m, k) = v (2:end) ;

    R (1:(k-1),k) = x (1:(k-1)) ;
    R (k,k) = xnorm ;
    % full (R)
    % pause
end

% s2 = svd (full (R)) ;
% [s1 s2 s1-s2]


function [col4row,row4col,gain]=linBottleneckAssign(C,solSel)
%%LINBOTTLENECKASSIGN Solve the linear bottleneck assignment problem for a
%          square cost matrix. The problem being solved can be formulated
%          as
%          gain=minimize_phi max_i C_{i,phi(i)} where phi is a permutation
%                                               vector.
%          It can also be formulated as:
%          gain=minimize_{x} max_{i,j} C_{i,j}*x_{i,j}
%          subject to
%          \sum_{j=1}^{n}x_{i,j}=1 for all i
%          \sum_{i=1}^{n}x_{i,j}=1 for all j
%          x_{i,j}=0 or 1.
%
%INPUTS: C An nXn cost matrix that does not contain any NaNs.
%   solSel Multiple optimal solutions typically exist to this problem. This
%          parameter specifies which solution to choose. Possible values
%          are:
%          0 (The default if omitted or an empty matrix is passed) Choose
%            a feasible solution without any particular property.
%          1 Choose the optimal solution that also minimizes
%            sum_i C_{i,phi(i)}.
%          2 Choose the optimal solution that also maximizes
%            sum_i C_{i,phi(i)}.
%
%OUTPUTS: col4row An nX1 vector where the entry in each element is an
%                 assignment of the element in that row to a column. This
%                 is equivalent to phi above.
%         row4col A nX1 vector where the entry in each element is an
%                 assignment of the element in that column to a row.
%            gain The value of the cost function of the linear bottleneck
%                 assignment problem at the optimal point.
%
%This function implements Algorithm 6.1 of Chapter 6.2 of [1]. This is an
%algorithm based on thresholding. The choice of which solution is obtained
%in the end depends on whether one uses maxCardBipartMatching in the final
%step, or whether one modifies the cost matrix in the final step according
%to G (forbidden assignments become either Inf or -Inf) and then uses
%assign2D to solve the problem.
%
%EXAMPLE:
%This is example 6.5 from Chapter 6.2 of [1].
% C=[8,2,3,3;
%    2,7,5,8;
%    0,9,8,4;
%    2,5,6,3];
% [col4row,row4col,gain]=linBottleneckAssign(C)
% %Verify the value of the cost function for the permutation.
% n=size(C,1);
% costVal=-Inf;
% for curRow=1:n
%     cij=C(curRow,col4row(curRow));
%     if(cij>costVal)
%         costVal=cij;
%     end
% end
% costVal
%One will see that gain=5 and costVal agrees with the gain. The cost value
%is five even though the col4row assignment is not identical to the in the
%book. Solution variations depend on variations to the algiruthm used for
%the perfect matching. Here, maxCardBipartMatching is used.
%
%REFERENCES:
%[1] R. Burkard, M. Dell'Amico, and S. Martello,Assignment Problems.
%    Philadelphia: Society for Industrial and Applied Mathematics, 2009.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(solSel))
    solSel=0;
end

n=size(C,1);

c0=min(C(:));
c1=max(C(:));

if(c0==c1)
    col4row=(1:n).';
    row4col=(1:n).';
    gain=c0;
    return;
end

sel=C<c1&C>c0;
CSel=C(sel);
if(isempty(CSel))
    %This instance can occur, for example, if the matrix contains only two
    %values, such as if C is the identity matrix.
    cStar=c1;
else
    while(~isempty(CSel))
        cStar=medianHi(CSel(:));
        [c0,c1]=feasCheck(C,cStar,c0,c1);

        sel=C<c1&C>c0;
        CSel=C(sel);
    end
end

if(cStar~=c0)
    [~,c1]=feasCheck(C,c0,c0,c1);
end

G=(C<=c1);
switch(solSel)
    case 0
        [col4row,row4col]=maxCardBipartMatching(G);
    case 1
        CNew=Inf(n,n);
        CNew(G)=C(G);
        [col4row,row4col]=assign2D(CNew,false);
        if(isempty(col4row))%If the cost wouldn't have been finite.
            [col4row,row4col]=maxCardBipartMatching(G);
        end
    case 2
        CNew=-Inf(n,n);
        CNew(G)=C(G);
        [col4row,row4col]=assign2D(CNew,true);
        if(isempty(col4row))%If the cost wouldn't have been finite.
            [col4row,row4col]=maxCardBipartMatching(G);
        end
    otherwise
        error('Unknown solution selected.')
end
gain=c1;

end

function [c0,c1]=feasCheck(C,cStar,c0,c1)

G=(C<=cStar);
col4row=maxCardBipartMatching(G);

if(all(col4row~=0))
    %It is feasible.
    c1=cStar;
else
    %It is infeasible.
    c0=cStar;
end
end

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


function c = correlation_power ( n, rho, rho0, e )

%*****************************************************************************80
%
%% CORRELATION_POWER evaluates the power correlation function.
%
%  Discussion:
%
%    The power correlation is
%
%      C(rho) = ( 1 - |rho| )^e  if 0 <= |rho| <= 1
%             = 0                otherwise
%
%      The constraint on the exponent is 2 <= e.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of arguments.
%
%    Input, real RHO(N,1), the arguments.
%    0.0 <= RHO.
%
%    Input, real RHO0, the correlation length.
%    0.0 < RHO0.
%
%    Input, real E, the exponent.
%    E has a default value of 2;
%    2 <= E.
%
%    Output, real C(N,1), the correlations.
%
  if ( nargin < 4 )
    e = 2.0;
  end

  c = zeros ( n, 1 );

  rho1 = abs ( rho ( : ) ) / rho0;

  i = find ( rho1 <= 1 );

  c(i) = ( 1.0 - rho1(i) ) .^ e;

  return
end


function sn = GetSn(Y, range_ff, method)
%% Estimate noise standard deviation

%% inputs:
%   Y: N X T matrix, fluorescence trace
%   range_ff : 1 x 2 vector, nonnegative, max value <= 0.5, range of frequency (x Nyquist rate) over which the spectrum is averaged
%   method: string, method of averaging: Mean, median, exponentiated mean of logvalues (default)

%% outputs:
%   sn: scalar, std of the noise

%% Authors: Pengcheng Zhou, Carnegie Mellon University, 2016
% adapted from the MATLAB implemention by Eftychios Pnevmatikakis and the
% Python implementation from Johannes Friedrich

%% References
% Pnevmatikakis E. et.al., Neuron 2016, Simultaneous Denoising, Deconvolution, and Demixing of Calcium Imaging Data

%% input arguments
if ~exist('range_ff', 'var') || isempty(range_ff)
    range_ff = [.25, .5];
end
if ~exist('method', 'var') || isempty(method)
    method = 'logmexp';
end
if any(size(Y)==1)
    Y = reshape(Y, [], 1);
else
    Y = Y';
end

%% estimate the noise
[psdx, ff] = pwelch(double(Y), [],[],[], 1);
indf = and(ff>=range_ff(1), ff<=range_ff(2));
switch method
    case 'mean'
        sn=sqrt(mean(psdx(indf, :)/2));
    case 'median'
        sn=sqrt(median(psdx(indf,:)/2));
    case 'logmexp'
        sn = sqrt(exp(mean(log(psdx(indf,:)/2))));
    otherwise
        fprintf('wrong method! use logmexp instead.\n');
        sn = sqrt(exp(mean(log(psdx(indf,:)/2))));
end
sn = sn';


% Black-Scholes implied vol
%
% sigma=impvol(C,S,K,r,t,T,q,tol)
%
% C: call price (scalar or vector)
% S: Stock price (scalar or vector)
% K: Strike price (scalar or vector)
% r: Interest rate (scalar or vector)
% t: Time now (scalar or vector)
% T: Maturity date (scalar or vector)
% [q]: Dividend yield (scalar or vector). Default=0;
% [tol]: Tolerance. Default=1e-6

function sigma=impvol(C,S,K,r,t,T,q,tol)

T=T-t;

if nargin<8
    tol=1e-6;
end

if nargin<7 || isempty(q)
    q=0;
end

F=S*exp((r-q).*T);
G=C.*exp(r.*T);

alpha=log(F./K)./sqrt(T);
beta=0.5*sqrt(T);

% Now we need to solve G=F Phi(d1)- K Phi(d2) where
% d1=alpha/sigma+beta and d2=alpha/sigma-beta

a=beta.*(F+K);
b=sqrt(2*pi)*(0.5*(F-K)-G);
c=alpha.*(F-K);

disc=max(0,b.^2-4*a.*c);

sigma0=(-b+sqrt(disc))./(2*a);

sigma=NewtonMethod(sigma0);

    function s1=NewtonMethod(s0)

        s1=s0;
        count=0;
        f=@(x) call(S,K,r,x,0,T,q)-C;
        fprime=@(x) call_vega(S,K,r,x,0,T,q);

        max_count=1e3;

        while max(abs(f(s1)))>tol && count<max_count
            count=count+1;

            s0=s1;
            s1=s0-f(s0)./fprime(s0);
        end

        if max(abs(f(s1)))>tol
            disp('Newton method did not converge')
        end
    end

end


function out = planesBisector(plane1, plane2)
% PLANESBISECTOR  Bisector plane between two other planes
%
%   BIS = planesBisector(PL1, PL2);
%   Returns the planes that contains the intersection line between PL1 and
%   PL2 and that bisect the dihedral angle of PL1 and PL2.
%   Note that computing the bisector of PL2 and PL1 (in that order) returns
%   the same plane but with opposite orientation.
%
%   Example
%     % Draw two planes together with their bisector
%     pl1 = createPlane([3 4 5], [1 2 3]);
%     pl2 = createPlane([3 4 5], [2 -3 4]);
%     % compute bisector
%     bis = planesBisector(pl1, pl2);
%     % setup display
%     figure; hold on; axis([0 10 0 10 0 10]);
%     set(gcf, 'renderer', 'opengl')
%     view(3);
%     % draw the planes
%     drawPlane3d(pl1, 'g');
%     drawPlane3d(pl2, 'g');
%     drawPlane3d(bis, 'b');
%
%   See also
%   planes3d, dihedralAngle, intersectPlanes
%
%   Author: Ben X. Kang
%   Dept. Orthopaedics & Traumatology
%   Li Ka Shing Faculty of Medicine
%   The University of Hong Kong
%   Pok Fu Lam, Hong Kong
%

% Let the two planes be defined by equations
%
%  a1*x + b1*y + c1*z + d1 = 0
%
% and
%
%  a2*x + b2*y + c2*z + d2 = 0
%
% in which vectors [a1,b1,c1] and [a2,b2,c2] are normalized to be of unit
% length (a^2+b^2+c^2 = 1). Then
%
%  (a1+a2)*x + (b1+b2)*y + (c1+c2)*z + (d1+d2) = 0
%
% is the equation of the desired plane which bisects the dihedral angle
% between the two planes.  These coefficients cannot be all zero because
% the two given planes are not parallel.
%
% Notice that there is a second solution to this problem
%
%  (a1-a2)*x + (b1-b2)*y + (c1-c2)*z + (d1-d2) = 0
%
% which also is a valid plane and orthogonal to the first solution. One of
% these planes bisects the acute dihedral angle, and the other the
% supplementary obtuse dihedral angle, between the two given planes.

P1 = plane1(1:3);			% a point on the plane
n1 = planeNormal(plane1);	% the normal of the plane
% d1 = -dot(n1, P1);		% for line equation

P2 = plane2(1:3);
n2 = planeNormal(plane2);
% d2 = -dot(n2, P2);

if ~isequal(P1(1:3), P2(1:3))
	L = intersectPlanes(plane1, plane2);	% intersection of the given two planes
	Pt = L(1:3);							% a point on the line intersection
% 	v2 = cross(n1-n2, L(4:6));				% another vector lie on the bisect plane
% 	out = [v1, v2]';
else
	Pt = P1(1:3);
end

% use column-wise vector
out = createPlane(Pt, n1 - n2);

%%  EOF  %%


function [A,B,Ypredict] = bls(X,Y,nhidden,algorithm)
% BLS  Bottleneck least squares aka sparse orthogonalized least squares
%
% X: ninput x nsamples input data matrix
% Y: noutput x nsamples output data matrix
% nhidden: dimension of hidden
%
% A: noutput x nhidden weight matrix
% B: ninput x nhidden weight matrix

if nargin < 3,
  nhidden = min(size(Y,1),2);
end

if nargin < 4,
  algorithm = 'direct';
end

[ninput,nsamples] = size(X);

Sxx = symmetric(X*X')/nsamples;

switch algorithm

  case 'direct'
    Syx = Y*X'/nsamples;
    Sxy = Syx';
    Sxxinv = symmetric(pinv(Sxx));
    S = symmetric(Syx * Sxxinv * Sxy);
    opts.disp = 0;
    opts.issym = 'true';
    [A,D] = eigs(S,nhidden,'LM',opts);   % first nhidden eigenvectors of S
    B = Sxxinv*Sxy*A;

  case 'indirect'          % same result and subspace as 'direct', but then different linear combination

    Syx = Y*X'/nsamples;
    Sxy = Syx';
    Sxxinv = symmetric(pinv(Sxx));

    % initialize randomly

    B = randn(ninput,nhidden);

    iter = 0;
    maxiter = 1000;
    tol = nhidden*ninput*(1e-10);
    Bold = B;
    while iter < maxiter,
      Z = B'*X;
      %		Szz = symmetric(Z*Z')/nsamples;
      Szz = symmetric(B'*Sxx*B);
      %		Syz = symmetric(Y*Z')/nsamples;
      Syz = Syx*B;
      A = Syz*pinv(Szz);

      B = Sxxinv*Sxy*A*pinv(A'*A);
      if sumsqr(B - Bold) < tol,
        fprintf('done after %d iterations!!\n',iter+1);
        iter = maxiter;
      else
        iter = iter + 1;
        Bold = B;
      end
    end

  case 'sequential'      % same result as 'direct', up to minus sign per hidden unit

    R = Y;
    noutput = size(Y,1);
    A = zeros(noutput,nhidden);
    B = zeros(ninput,nhidden);
    for i=1:nhidden,
      [A(:,i),B(:,i)] = bls(X,R,1,'direct');
      if i < nhidden,
        R = R - A(:,i)*B(:,i)'*X;
      end
    end

end

% transform to make latent variable orthonormal

Szz = symmetric(B'*Sxx*B);
Szzinv = symmetric(pinv(Szz));
sqrtSzzinv = chol(Szzinv(end:-1:1,end:-1:1));    % such that the rescaled B(i,:) is a linear combi of the unscaled B(1:i,:); this only makes sense for the direct method
sqrtSzzinv = sqrtSzzinv(end:-1:1,end:-1:1);
B = B*sqrtSzzinv;
A = A*pinv(sqrtSzzinv);

if nargout > 2,
  Ypredict = A*B'*X;
end

function A = symmetric(A)

A = (A+A')/2;


function [Jacobian, Joint] = ARM_2DOF(L1, L2, theta1, theta2, draw, orgX, orgY)
%% Function Configuration
if (nargin == 5)
    orgX = 0;
    orgY = 0;
end

%% Modelling of the Manipulator
X(1)= orgX+ L1*cosd(theta1);
Y(1)= orgY+ L1*sind(theta1);
X(2)= X(1)+ L2*cosd(theta1 + theta2);
Y(2)= Y(1)+ L2*sind(theta1 + theta2);

% Storing the Joint Locations of the Manipulator
Joint = [orgX, orgY; X(1), Y(1); X(2), Y(2)];

% Storing the Jacobian matrix for the current Manipulator configuration
Jacobian = [-L1*sind(theta1) - L2*sind(theta1+theta2), -L2*sind(theta1+theta2);
    L1*cosd(theta1)+ L2*cosd(theta1+theta2), L2*cosd(theta1+theta2)];

% If it was mentioned in the parameter 'draw' then draw the manipulator.
if draw == true
    plot([orgX,X(1)],[orgY,Y(1)]);
    hold on
    plot([X(1),X(2)],[Y(1),Y(2)]);
    axis([-(L1 + L2), (L1 + L2), -(L1 + L2), (L1 + L2)]);
    hold off;
    title('2 DOF Planar Manipulator');
    legend('1st Link of Arm', '2nd Link of Arm');
    drawnow;
end

end


function [v,usediters] = projfunc( s, k1, k2, nn )

% Solves the following problem:
% Given a vector s, find the vector v having sum(abs(v))=k1
% and sum(v.^2)=k2 which is closest to s in the euclidian sense.
% If the binary flag nn is set, the vector v is additionally
% restricted to being non-negative (v>=0).
%
% Written 2.7.2004 by Patrik O. Hoyer
%

% Problem dimension
N = length(s);

% If non-negativity flag not set, record signs and take abs
if ~nn,
    isneg = s<0;
    s = abs(s);
end

% Start by projecting the point to the sum constraint hyperplane
v = s + (k1-sum(s))/N;

% Initialize zerocoeff (initially, no elements are assumed zero)
zerocoeff = [];

j = 0;
while 1,

    % This does the proposed projection operator
    midpoint = ones(N,1)*k1/(N-length(zerocoeff));
    midpoint(zerocoeff) = 0;
    w = v-midpoint;
    a = sum(w.^2);
    b = 2*w'*v;
    c = sum(v.^2)-k2;
    alphap = (-b+real(sqrt(b^2-4*a*c)))/(2*a);
    v = alphap*w + v;

    if all(v>=0),
	% We've found our solution
	usediters = j+1;
	break;
    end

    j = j+1;

    % Set negs to zero, subtract appropriate amount from rest
    zerocoeff = find(v<=0);
    v(zerocoeff) = 0;
    tempsum = sum(v);
    v = v + (k1-tempsum)/(N-length(zerocoeff));
    v(zerocoeff) = 0;

end

% If non-negativity flag not set, return signs to solution
if ~nn,
    v = (-2*isneg + 1).*v;
end

% Check for problems
if max(max(abs(imag(v))))>1e-10,
    error('Somehow got imaginary values!');
end


function inside = circle_sector_contains_point_2d ( r, center, theta1, theta2, p )

%*****************************************************************************80
%
%% CIRCLE_SECTOR_CONTAINS_POINT_2D : is a point inside a circular sector?
%
%  Discussion:
%
%    A circular sector is formed by a circular arc, and the two straight line
%    segments that join its ends to the center of the circle.
%
%    A circular sector is defined by
%
%      ( X - CENTER(1) )**2 + ( Y - CENTER(2) )**2 = R**2
%
%    and
%
%      Theta1 <= Theta <= Theta2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the circle.
%
%    Input, real CENTER(2), the center of the circle.
%
%    Input, real THETA1, THETA2, the angles defining the arc,
%    in radians.  Normally, THETA1 < THETA2.
%
%    Input, real P(2), the point to be checked.
%
%    Output, logical INSIDE, is TRUE if the point is inside or on the
%    circular sector, FALSE otherwise.
%
  inside = 0;
%
%  Is the point inside the (full) circle?
%
  if ( ( p(1) - center(1) ) * ( p(1) - center(1) ) ...
     + ( p(2) - center(2) ) * ( p(2) - center(2) ) <= r * r )
%
%  Is the point's angle within the arc's range?
%  Try to force the angles to lie between 0 and 2 * PI.
%
    theta = r8_atan ( p(2) - center(2), p(1) - center(1) );

    if ( r8_modp ( theta  - theta1,  2.0 * pi ) <= ...
         r8_modp ( theta2 - theta1,  2.0 * pi ) )

      inside = 1;

    end

  end

  return
end


function vs = sphere01_triangle_vertices_to_centroid ( v1, v2, v3 )

%*****************************************************************************80
%
%% SPHERE01_TRIANGLE_VERTICES_TO_CENTROID gets a spherical triangle centroid.
%
%  Discussion:
%
%    A unit sphere centered at 0 in 3D satisfies the equation:
%
%      X*X + Y*Y + Z*Z = 1
%
%    A spherical triangle is specified by three points on the sphere.
%
%    The (true) centroid of a spherical triangle is the point
%
%      VT = (XT,YT,ZT) = Integral ( X, Y, Z ) dArea / Integral 1 dArea
%
%    Note that the true centroid does NOT, in general, lie on the sphere.
%
%    The "flat" centroid VF is the centroid of the planar triangle defined by
%    the vertices of the spherical triangle.
%
%    The "spherical" centroid VS of a spherical triangle is computed by
%    the intersection of the geodesic bisectors of the triangle angles.
%    The spherical centroid lies on the sphere.
%
%    VF, VT and VS lie on a line through the center of the sphere.  We can
%    easily calculate VF by averaging the vertices, and from this determine
%    VS by normalizing.
%
%    Of course, we still will not have actually computed VT, which lies
%    somewhere between VF and VS!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real V1(3), V2(3), V3(3), the vertices of the triangle.
%
%    Output, real VS(3), the coordinates of the "spherical
%    centroid" of the spherical triangle.
%
  vs(1:3) = ( v1(1:3) + v2(1:3) + v3(1:3) ) / 3.0;

  norm = sqrt ( sum ( vs(1:3).^2 ) );

  vs(1:3) = vs(1:3) / norm;

  return
end


function [ zfilt ] = gaussfilt( t,z,sigma )
%Apply a Gaussian filter to a time series
%   Inputs: t = independent variable, z = data at points t, and
%       sigma = standard deviation of Gaussian filter to be applied.
%   Outputs: zfilt = filtered data.
%
%   written by James Conder. Aug 22, 2013

n = length(z);  % number of data
zfilt = 0*z;    % initalize output vector

%%% get distances between points for proper weighting
w = 0*t;
w(2:end-1) = 0.5*(t(3:end)-t(1:end-2));
w(1) = t(2)-t(1);
w(end) = t(end)-t(end-1);

%%% check if sigma smaller than data spacing
iw = find(w > 2*sigma, 1);
if ~isempty(iw)
    disp('WARNING: sigma smaller than half node spacing')
    disp('May lead to unstable result')
    iw = w > 2.5*sigma;
    w(iw) = 2.5*sigma;
    % this correction leaves some residual for spacing between 2-3sigma.
    % otherwise ok.
    % In general, using a Gaussian filter with sigma less than spacing is
    % a bad idea anyway...
end

%%% loop over points
a = 1/(sqrt(2*pi)*sigma);
sigma2 = sigma*sigma;
for i = 1:n
    filter = a*exp(-0.5*((t - t(i)).^2)/(sigma2));
    zfilt(i) = sum(w.*z.*filter);
end

%%% clean-up edges - mirror data for correction
ss = 2.4*sigma;   % distance from edge that needs correcting

% left edge
tedge = min(t);
iedge = find(t < tedge + ss);
nedge = length(iedge);
for i = 1:nedge;
    dist = t(iedge(i)) - tedge;
    include = find( t > t(iedge(i)) + dist);
    filter = a*exp(-0.5*((t(include) - t(iedge(i))).^2)/(sigma2));
    zfilt(iedge(i)) = zfilt(iedge(i)) + sum(w(include).*filter.*z(include));
end

% right edge
tedge = max(t);
iedge = find(t > tedge - ss);
nedge = length(iedge);
for i = 1:nedge;
    dist = tedge - t(iedge(i));
    include = find( t < t(iedge(i)) - dist);
    filter = a*exp(-0.5*((t(include) - t(iedge(i))).^2)/(sigma2));
    zfilt(iedge(i)) = zfilt(iedge(i)) + sum(w(include).*filter.*z(include));
end

end