classdef RiceD
%%RICED Functions to handle the Rice distribution.
%Implemented methods are: mean, var, PDF, CDF, rand
%
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

methods(Static)

function val=mean(s,sigma)
%%MEAN Obtain the mean of the Rice distribution for given noncentrality
%      and scale parameters.
%
%INPUTS: s The noncentrality parameter of the distribution.
%    sigma The scale parameter of the distribution.
%
%OUTPUTS: val The mean of the Rice distribution.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    param=-s^2/(2*sigma^2);
    val=sigma*sqrt(pi/2)*exp(param/2)*((1-param)*besseli(0,-param/2)-param*besseli(1,-param/2));
end

function val=var(s,sigma)
%%VAR Obtain the variance of the Rice distribution for given noncentrality
%     and scale parameters.
%
%INPUTS: s The noncentrality parameter of the distribution.
%    sigma The scale parameter of the distribution.
%
%OUTPUTS: val The variance of the Rice distribution.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    param=-s^2/(2*sigma^2);
    L=exp(param/2)*((1-param)*besseli(0,-param/2)-param*besseli(1,-param/2));
    val=2*sigma^2+s^2-pi*(sigma^2/2)*L^2;
end

function val=PDF(x,s,sigma)
%%PDF Evaluate the Rice probability distribution function at one or more
%     desired points.
%
%INPUTS: x The point(s) at which the Rice PDF is to be evaluated. Note that
%          x>=0.
%        s The noncentrality parameter of the distribution.
%    sigma The scale parameter of the distribution.
%
%OUTPUTS: val The value(s) of the Rice PDF with parameters s and sigma
%             evaluated at x.
%
%The PDF of the Rice distribution is given in Chapter 2.1.4 of [1].
%
%REFERENCES:
%[1] J. G. Proakis, Digital Communications. Ed. 4, Boston, MA:
%    McGraw Hill, 2001.
%
%September 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    %We are evaluating
    %val=(x/sigma^2).*exp(-(x.^2+s^2)/(2*sigma^2)).*besseli(0,x*s/sigma^2);
    %However, if x*s/sigma^2 is large, then this function could return
    %NaNs. large doesn't have to be all that large. For example, s=30,
    %sigma=1, x=100. means that ratio is 3000. Thus, we use besseli with
    %the third argument to get a scaled value, then take the logarithm, do
    %everything else in the logarithmic domain and take the exponent to
    %undo the logarithm.

    besselArg=x*s/sigma^2;
    val=exp(log(besseli(0,besselArg,1))+besselArg-(x.^2+s^2)/(2*sigma^2)+log(x/sigma^2));
end

function val=CDF(x,s,sigma)
%%CDF Evaluate the cumulative distribution function of the Rice
%     distribution at desired points.
%
%INPUTS: x The point(s) at which the Rice CDF is to be evaluated. Note that
%          x>=0.
%        s The noncentrality parameter of the distribution.
%    sigma The scale parameter of the distribution.
%
%OUTPUTS: prob The value(s) of the CDF of the Rice distribution with
%              parameters k and theta evaluated at x.
%
%The CDF of the Rice distribution can be expressed in terms of Marcum's Q
%function as shown in Chapter 2.1.4 of [1].
%
%REFERENCES:
%[1] J. G. Proakis, Digital Communications. Ed. 4, Boston, MA:
%    McGraw Hill, 2001.
%
%February 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
    numPoints=length(x(:));
    val=zeros(size(x));

    for curPoint=1:numPoints
        val(curPoint)=1-MarcumQ(1,s/sigma,x(curPoint)/sigma);
    end
end

function vals=rand(N,s,sigma)
%%RAND Generate Rice-distributed random variables with the given
%      parameters.
%
%INPUTS: N If N is a scalar, then rand returns an NXN matrix of random
%          variables. If N=[M,N1] is a two-element row vector, then rand
%          returns an MXN1 matrix of random variables.
%        s The noncentrality parameter of the distribution.
%    sigma The scale parameter of the distribution.
%
%OUTPUTS: vals A matrix whose dimensions are determined by N of the
%              generated Rice random variables.
%
%This generates Rice distributed random variables by transforming normally
%distributed random variables using the identity given in Chapter 2.1.4 of
%[1].
%
%REFERENCES:
%[1] J. G. Proakis, Digital Communications. Ed. 4, Boston, MA:
%    McGraw Hill, 2001.
%
%September 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(isscalar(N))
        dims=[N, N];
    else
        dims=N;
    end

    m=s/sqrt(2);

    X=sigma*randn(dims)+m;
    Y=sigma*randn(dims)+m;

    vals=sqrt(X.^2+Y.^2);
end

end
end

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


function p = gaussProb(X, mu, Sigma)
% Multivariate Gaussian distribution, pdf
% X(i,:) is i'th case
% *** In the univariate case, Sigma is the variance, not the standard
% deviation! ***

% This file is from pmtk3.googlecode.com

d = size(Sigma, 2);
X  = reshape(X, [], d);  % make sure X is n-by-d and not d-by-n
X = bsxfun(@minus, X, rowvec(mu));
logp = -0.5*sum((X/(Sigma)).*X, 2);
logZ = (d/2)*log(2*pi) + 0.5*logdet(Sigma);
logp = logp - logZ;
p = exp(logp);

end


function h = r8mat_house_form ( n, v )

%*****************************************************************************80
%
%% R8MAT_HOUSE_FORM constructs a Householder matrix from its compact form.
%
%  Discussion:
%
%    H(v) = I - 2 * v * v' / ( v' * v )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real V(N), the vector defining the Householder matrix.
%
%    Output, real H(N,N), the Householder matrix.
%

%
%  Compute the L2 norm of V.
%
  beta = sum ( v(1:n).^2 );
%
%  Form the matrix H.
%
  h = r8mat_identity ( n );

  for i = 1 : n
    for j = 1 : n
      h(i,j) = h(i,j) - 2.0 * v(i) * v(j) / beta;
    end
  end

  return
end


function [ X ] = LineFaceIntersection( faceX, faceN, lineX, lineD )
%LINEFACEINTERSECTION Intersection of a plane and a line
%   faceX: a point on the plane
%   faceN: the normal direction of the plane
%   lineX: a point on the line
%   lineD: direction of the line
%

% A = dot(faceN,lineD,2)/lineD(1);
% B = -dot(faceX,faceN,2) + (dot(lineX(2:3),faceN(2:3),2)) - lineX(1)/lineD(1)*(dot(lineD(2:3),faceN(2:3),2));
A = sum(faceN.*lineD)/lineD(1);
B = -sum(faceX.*faceN) + (sum(lineX(2:3).*faceN(2:3),2)) - lineX(1)/lineD(1)*(sum(lineD(2:3).*faceN(2:3),2));

x = -B/A;

y = (x-lineX(1))/lineD(1)*lineD(2) + lineX(2);
z = (x-lineX(1))/lineD(1)*lineD(3) + lineX(3);
X = [x y z];
end


function centroids = computeCentroids(X, idx, K)
%COMPUTECENTROIDS returs the new centroids by computing the means of the
%data points assigned to each centroid.
%   centroids = COMPUTECENTROIDS(X, idx, K) returns the new centroids by
%   computing the means of the data points assigned to each centroid. It is
%   given a dataset X where each row is a single data point, a vector
%   idx of centroid assignments (i.e. each entry in range [1..K]) for each
%   example, and K, the number of centroids. You should return a matrix
%   centroids, where each row of centroids is the mean of the data points
%   assigned to it.
%

% Useful variables
[m n] = size(X);

% You need to return the following variables correctly.
centroids = zeros(K, n);

% ====================== YOUR CODE HERE ======================
% Instructions: Go over every centroid and compute mean of all points that
%               belong to it. Concretely, the row vector centroids(i, :)
%               should contain the mean of the data points assigned to
%               centroid i.
%
% Note: You can use a for-loop over the centroids to compute this.
%

numberOfElementsHavingCentroid_k = zeros(K,1);
sumOfElementsHavingCentroid_k = zeros(K,n);
for i = 1:size(idx,1)
	z = idx(i);
	numberOfElementsHavingCentroid_k(z) += 1;
	sumOfElementsHavingCentroid_k(z,:) += X(i,:);
end

centroids = sumOfElementsHavingCentroid_k./numberOfElementsHavingCentroid_k;

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

end


function dist = shape_point_dist_2d ( center, p1, nside, p )

%*****************************************************************************80
%
%% SHAPE_POINT_DIST_2D: distance ( regular shape, point ) in 2D.
%
%  Discussion:
%
%    The "regular shape" is assumed to be an equilateral and equiangular
%    polygon, such as the standard square, pentagon, hexagon, and so on.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CENTER(2,1), the center of the shape.
%
%    Input, real P1(2,1), the first vertex of the shape.
%
%    Input, integer NSIDE, the number of sides in the shape.
%
%    Input, real P(2,1), the point to be checked.
%
%    Output, real DIST, the distance from the point to the shape.
%

%
%  Determine the angle subtended by a single side.
%
  sector_angle = 360.0 / nside;
%
%  How long is the half-diagonal?
%
  radius = sqrt ( sum ( ( p1(1:2,1) - center(1:2,1) ).^2 ) );
%
%  If the radius is zero, then the shape is a point and the computation is easy.
%
  if ( radius == 0.0 )
    dist = sqrt ( sum ( ( p(1:2,1) - center(1:2,1) ).^2 ) );
    return;
  end
%
%  If the test point is at the center, then the computation is easy.
%  The angle subtended by any side is ( 2 * PI / NSIDE ) and the
%  nearest distance is the midpoint of any such side.
%
  if ( sqrt ( sum ( ( p1(1:2,1) - center(1:2,1) ).^2 ) ) == 0.0 )
    dist = radius * cos ( pi / nside );
    return
  end
%
%  Determine the angle between the ray to the first corner,
%  and the ray to the test point.
%
  angle = angle_deg_2d ( p1, center, p );
%
%  Determine the sector of the point.
%
  sector_index = floor ( angle / sector_angle ) + 1;
%
%  Generate the two corner points that terminate the SECTOR-th side.
%
  angle2 = ( sector_index - 1 ) * sector_angle;
  angle2 = degrees_to_radians ( angle2 );

  pa = vector_rotate_base_2d ( p1, center, angle2 );

  angle2 = ( sector_index ) * sector_angle;
  angle2 = degrees_to_radians ( angle2 );

  pb = vector_rotate_base_2d ( p1, center, angle2 );
%
%  Determine the distance from the test point to the line segment that
%  is the SECTOR-th side.
%
  dist = segment_point_dist_2d ( pa, pb, p );

  return
end


function [ n_data, n, c ] = tau_values ( n_data )

%*****************************************************************************80
%
%% TAU_VALUES returns some values of the Tau function.
%
%  Discussion:
%
%    TAU(N) is the number of divisors of N, including 1 and N.
%
%    In Mathematica, the function can be evaluated by:
%
%      DivisorSigma[1,n]
%
%  First values:
%
%     N   TAU(N)
%
%     1    1
%     2    2
%     3    2
%     4    3
%     5    2
%     6    4
%     7    2
%     8    4
%     9    3
%    10    4
%    11    2
%    12    6
%    13    2
%    14    4
%    15    4
%    16    5
%    17    2
%    18    6
%    19    2
%    20    6
%
%  Formula:
%
%    If the prime factorization of N is
%
%      N = P1^E1 * P2^E2 * ... * PM^EM,
%
%    then
%
%      TAU(N) = ( E1 + 1 ) * ( E2 + 1 ) * ... * ( EM + 1 ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the argument of the Tau function.
%
%    Output, integer C, the value of the Tau function.
%
  n_max = 20;

  c_vec = [ ...
    1,  2,  2,  3,  2,  4,  2,  4,  3,  4, ...
    2, 12, 12,  4, 18, 24,  2,  8, 14, 28 ];

  n_vec = [ ...
      1,   2,   3,   4,   5,   6,   7,   8,   9,  10, ...
     23,  72, 126, 226, 300, 480, 521, 610, 832, 960 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    n = 0;
    c = 0;
  else
    n = n_vec(n_data);
    c = c_vec(n_data);
  end

  return
end


function x = hypersphere_to_cartesian ( m, n, c, r, theta )

%*****************************************************************************80
%
%% HYPERSPHERE_TO_CARTESIAN: hypersphere to Cartesian coordinate transform.
%
%  Discussion:
%
%    We allow the trivial case M = 1; in that case alone, the value R
%    must be assumed to have a sign.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 May 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%    1 <= M.
%
%    Input, integer N, the number of points to transform.
%
%    Input, real C(M,1), the center of the hypersphere.
%
%    Input, real R(N,1), the radius of the points on the hypersphere.
%    Except for the trivial case M = 1, R is assumed nonnegative.
%
%    Input, real THETA(M-1,N), the coordinate angles of the points,
%    measured in radians.
%
%    Output, real X(M,N), the Cartesian coordinates of the points.
%
  x = zeros ( m, n );
%
%  Make R a row vector.
%
  r = ( r(:) )';
%
%  Handle special case of M = 1.
%
  if ( m == 1 )
    x(1:1,1:n) = repmat ( c(1:1,1:1), 1, n ) ...
               + repmat ( r(1:1,1:n), 1, 1 );
    return
  end

  x(1:m,1:n) = repmat ( r(1:1,1:n), m, 1 );

  for i1 = 1 : m - 1
    x(i1,1:n) = x(i1,1:n) .* cos ( theta(i1,1:n) );
    for i2 = i1 + 1 : m
      x(i2,1:n) = x(i2,1:n) .* sin ( theta(i1,1:n) );
    end
  end
%
%  Add the center.
%
  x(1:m,1:n) = x(1:m,1:n) + repmat ( c(1:m,1:1), 1, n );

  return
end


function dist = line_exp_point_dist_3d ( p1, p2, p )

%*****************************************************************************80
%
%% LINE_EXP_POINT_DIST_3D: distance ( explicit line, point ) in 3D.
%
%  Discussion:
%
%    The explicit form of a line in 3D is:
%
%      ( P1, P2 ) = ( (X1,Y1,Z1), (X2,Y2,Z2) ).
%
%    Thanks to Francois Struempfer for pointing out a parenthesis mistake in the
%    computation of the Euclidean norm.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(3,1), P2(3,1), two points on the line.
%
%    Input, real P(3,1), the point whose distance from the line is
%    to be measured.
%
%    Output, real DIST, the distance from the point to the line.
%
  dim_num = 3;

  bot = sum ( ( p2(1:dim_num,1) - p1(1:dim_num,1) ).^2 );

  if ( bot == 0.0 )

    pn(1:dim_num,1) = p1(1:dim_num,1);
%
%  (P-P1) dot (P2-P1) = Norm(P-P1) * Norm(P2-P1) * Cos(Theta).
%
%  (P-P1) dot (P2-P1) / Norm(P-P1)**2 = normalized coordinate T
%  of the projection of (P-P1) onto (P2-P1).
%
  else

    t = sum ( ( p(1:dim_num,1) - p1(1:dim_num,1) )' ...
            * ( p2(1:dim_num,1) - p1(1:dim_num,1) ) ) / bot;

    pn(1:dim_num,1) = p1(1:dim_num,1) + t * ( p2(1:dim_num,1) - p1(1:dim_num,1) );

  end
%
%  Now compute the distance between the projection point and P.
%
  dist = sqrt ( sum ( ( p(1:dim_num,1) - pn(1:dim_num,1) ).^2 ) );

  return
end


% Exercise 4.57: Capacity of a communication channel
% Boyd & Vandenberghe "Convex Optimization"
% Joëlle Skaf - 04/24/08
%
% We consider a discrete memoryless communication channel, with input
% X(t) \in {1,...,n}, and output Y(t) \in {1,...,m}, for t = 1,2,...
% The relation between the input and output is given statistically:
%           p_ij = Prob(Y(t)=i|X(t)=j), i=1,...,m,  j=1,...,n
% The matrix P is called the channel transition matrix.
% The channel capacity C is given by
%           C = sup{ I(X;Y) | x >= 0, sum(x) = 1},
% I(X;Y) is the mutual information between X and Y, and it can be shown
% that:     I(X;Y) = c'*x - sum_{i=1}^m y_i*log_2(y_i)
% where     c_j = sum_{i=1}^m p_ij*log_2(p_ij), j=1,...,m

% Input data
rand('state', 0);
n = 15;
m = 10;
P = rand(m,n);
P = P./repmat(sum(P),m,1);
c = sum(P.*log2(P))';

% Channel capacity
cvx_begin
    variable x(n)
    y = P*x;
    maximize (c'*x + sum(entr(y))/log(2))
    x >= 0;
    sum(x) == 1;
cvx_end
C = cvx_optval;

% Results
display(['The channel capacity is: ' num2str(C) ' bits.'])


function [rmag, dec] = geodet4 (lat, alt)

% geodetic to geocentric coordinates

% input

%  lat  = geodetic latitude (radians)
%         (+north, -south; -pi/2 <= lat <= +pi/2)
%  alt  = geodetic altitude (kilometers)

% output

%  rmag = geocentric position magnitude (kilometers)
%  dec  = geocentric declination (radians)
%         (+north, -south; -pi/2 <= dec <= +pi/2)

% global constants

%  req  = equatorial radius (kilometers)
%  flat = flattening factor (non-dimensional)

% Orbital Mechanics with Matlab

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

global req flat

% "normalize" altitude

hhat = alt / req;

hp1 = hhat + 1;

% calculate trig terms

s2lat = sin(2 * lat);

c2lat = cos(2 * lat);

s4lat = sin(4 * lat);

c4lat = cos(4 * lat);

% geocentric distance (kilometers)

rho = hp1 + (0.5 * (c2lat - 1)) * flat ...
      + ((1/(4 * hp1) + (1/16)) * (1 - c4lat)) * flat * flat;

rmag = req * rho;

% geocentric declination (radians)

dec = lat + (-s2lat/hp1) * flat  ...
      + (-s2lat/(2 * (hp1 * hp1)) + (1/(4 * hp1 * hp1) + 1/(4 * hp1))...
      * s4lat) * flat * flat;


function [theta, J_history] = gradientDescentMulti(X, y, theta, alpha, num_iters)
%GRADIENTDESCENTMULTI Performs gradient descent to learn theta
%   theta = GRADIENTDESCENTMULTI(x, y, theta, alpha, num_iters) updates theta by
%   taking num_iters gradient steps with learning rate alpha

% Initialize some useful values
m = length(y); % number of training examples
J_history = zeros(num_iters, 1);

for iter = 1:num_iters

    % ====================== YOUR CODE HERE ======================
    % Instructions: Perform a single gradient step on the parameter vector
    %               theta.
    %
    % Hint: While debugging, it can be useful to print out the values
    %       of the cost function (computeCostMulti) and gradient here.
    %

  predictions =  X * theta;
    updates = X' * (predictions - y);
    theta = theta - alpha * (1/m) * updates;

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

    % Save the cost J in every iteration
    J_history(iter) = computeCostMulti(X, y, theta);

end

end


function A = makejcf(n, e, m, X)
%MAKEJCF   A matrix with specified Jordan canonical form.
%          MAKEJCF(N, E, M) is a matrix having the Jordan canonical form
%          whose i'th Jordan block is of dimension M(i) with eigenvalue E(i),
%          and where N = SUM(M).
%          Defaults: E = 1:N, M = ONES(SIZE(E)) with M(1) so that SUM(M) = N.
%          The matrix is constructed by applying a random similarity
%          transformation to the Jordan form.
%          Alternatively, the matrix used in the similarity transformation
%          can be specified as a fifth parameter.
%          In particular, MAKEJCF(N, E, M, EYE(N)) returns the Jordan form
%          itself.
%          NB: The JCF is very sensitive to rounding errors.

if nargin < 2, e = 1:n; end
if nargin < 3, m = ones(size(e)); m(1) = m(1) + n - sum(m); end

if length(e) ~= length(m)
   error('Parameters E and M must be of same dimension.')
end

if sum(m) ~= n, error('Block dimensions must add up to N.'), end

A = zeros(n);
j = 1;
for i=1:max(size(m))
    if m(i) > 1
        Jb = gallery('jordbloc',m(i),e(i));
    else
        Jb = e(i);  % JORDBLOC fails in n = 1 case.
    end
    A(j:j+m(i)-1,j:j+m(i)-1) = Jb;
    j = j + m(i);
end

if nargin < 4
   X = randn(n);
end
A = X\A*X;


function [ pp, normal, seed ] = plane_normal_uniform_nd ( dim_num, seed )

%*****************************************************************************80
%
%% PLANE_NORMAL_UNIFORM_ND generates a random normal plane in ND.
%
%  Discussion:
%
%    The normal form of a plane is:
%
%      PP is a point on the plane,
%      N is a normal vector to the plane.
%
%    The point PP will be chosen at random inside the unit sphere.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, integer SEED, a seed for the random number generator.
%
%    Output, real PP(DIM_NUM), a point on the plane.
%
%    Output, real NORMAL(DIM_NUM), the unit normal vector.
%

%
%  Pick PP as a random point inside the unit sphere in ND.
%
  [ pp, seed ] = ball_unit_sample_nd ( dim_num, seed );
%
%  Get values from a standard normal distribution.
%
  [ normal, seed ] = r8vec_normal_01 ( dim_num, seed );
%
%  Compute the length of the vector.
%
  norm = sqrt ( sum ( normal(1:dim_num).^2 ) );
%
%  Normalize the vector.
%
  normal(1:dim_num) = normal(1:dim_num) / norm;

  return
end


function C=circulantMatrix(v,reverseDir)
%%CIRCULANTMATRIX Create a circulant matrix where the first row is given by
%            v. A circulatn matrix is a matrix where each row going down
%            from the top rotates the elements of the previous row by 1.
%
%INPUTS: v A 1XN or NX1 vector containing the elemtns that go into the
%          first row.
% reverseDir If true, this boolean parameter indicates that each subsequent
%          row should be left-shifted from the previous one. If this
%          parameter is omitted or an empty matrix is passed, then the
%          default is false.
%
%OUTPUTS: C The NXN circulant matrix whose first row is v.
%
%EXAMPLE:
% CF=circulantMatrix(1:5,false)
% CR=circulantMatrix(1:5,true)
%The results are:
%CF=[1,2,3,4,5;
%    5,1,2,3,4;
%    4,5,1,2,3;
%    3,4,5,1,2;
%    2,3,4,5,1];
%CR=[1,2,3,4,5;
%    2,3,4,5,1;
%    3,4,5,1,2;
%    4,5,1,2,3;
%    5,1,2,3,4];
%
%October 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(nargin<2||isempty(reverseDir))
        reverseDir=false;
    end

    N=length(v);
    v=v(:);
    x=[v(N:-1:1,1);flipud(v(2:end))];
    if(reverseDir)
        idx=bsxfun(@plus,mod(0:-1:-((N-1)),N)',(N:-1:1));
    else
        idx=bsxfun(@plus,(0:(N-1))',(N:-1:1));
    end

    C=x(idx);
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 [x,iflaw]=fmpar(N,p1,p2,p3)
%FMPAR	Parabolic frequency modulated signal.
%	[X,IFLAW]=FMPAR(N,P1,P2,P3) generates a signal with
%	parabolic frequency modulation law.
%	X(T) = exp(j*2*pi(A0.T + A1/2.T^2 +A2/3.T^3))
%
%	N  : the number of points in time
%	P1 : if NARGIN=2, P1 is a vector containing the three
%	    coefficients [A0 A1 A2] of the polynomial instantaneous phase.
%	    If NARGIN=4, P1 (as P2 and P3) is a time-frequency point of
%	    the form [Ti Fi].
%	    The coefficients (A0,A1,A2) are then deduced such that
%	    the frequency modulation law fits these three points.
%	P2,P3 : same as P1 if NARGIN=4.       (optional)
%	X     : time row vector containing the modulated signal samples
%	IFLAW : instantaneous frequency law
%
%	Examples :
%	 [X,IFLAW]=fmpar(128,[1 0.4],[64 0.05],[128 0.4]);
%	 subplot(211);plot(real(X));subplot(212);plot(IFLAW);
%	 [X,IFLAW]=fmpar(128,[0.4 -0.0112 8.6806e-05]);
%	 subplot(211);plot(real(X));subplot(212);plot(IFLAW);
%
%	See also FMCONST, FMHYP, FMLIN, FMSIN, FMODANY, FMPOWER.

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

if (nargin <= 1),
 error ( 'The number of parameters must be at least 2.' );
elseif (N <= 0),
 error ('The signal length N must be strictly positive' );
elseif nargin == 2 ;
  if length(p1) ~= 3
    error('Bad number of coefficients for P1');
  end
  a0 = p1(1) ; a1 = p1(2) ; a2 = p1(3) ;
elseif nargin == 4 ;
  if (length(p1) ~= 2) |(length(p2) ~= 2) |(length(p3) ~= 2),
    error('Bad number of coefficients for P1, P2, P3');
  end
  if p1(1)>N | p1(1)<1,
   error ('P1(1) must be between 1 and N');
  elseif p2(1)>N | p2(1)<1,
   error ('P2(1) must be between 1 and N');
  elseif p3(1)>N | p3(1)<1,
   error ('P3(1) must be between 1 and N');
  elseif p1(2)<0,
   error ('P1(2) must be > 0');
  elseif p2(2)<0,
   error ('P2(2) must be > 0');
  elseif p3(2)<0,
   error ('P3(2) must be > 0');
  end
  Y = [p1(2) p2(2) p3(2)] ;
  X = [1 1 1;p1(1) p2(1) p3(1);p1(1)^2 p2(1)^2 p3(1)^2] ;
  coef = Y*inv(X) ;
  a0 = coef(1) ;
  a1 = coef(2) ;
  a2 = coef(3) ;
end

t=1:N;

phi = 2*pi*(a0*t + a1/2*t.^2 + a2/3*t.^3) ;
iflaw = (a0 + a1*t + a2*t.^2).' ;

aliasing = find(iflaw<0 | iflaw>0.5) ;
if isempty(aliasing) == 0
  disp(['!!! WARNING: signal is undersampled or has negative frequencies']) ;
end

x = exp(i*phi).';


function [Xco Tau MT] = HC_CrankNicolson_h (Tin,TC,DE,HC,SE,DI,TS,TT,TBC,BC)
%
% DESCRIPTION:
%
% Crank-Nicolson numerical method for one dimensional unsteady state
% heat transfer by conduction for homogenous material
%
% Basic PDE equation :
%
%     dT         d^2 T
%   ------ = TD ------             for 0 < x < DI  and  tau > 0
%    dtau        dx^2
%
% Principle of method :
%       ^
%  time |
%       |      m-1     m     m+1
%       |       |      |      |
%       |    ---o------x------o-----   p+1
%       |  dtau |      |      |
%       |    ---o------o------o-----   p
%       |  dtau |      |      |
%       |    ---+------+------+-----   p-1
%       |       |  dx  |  dx  |
%       |
%       +-------------------------------->
%                                        x
% Space description:
%                  DI
%   |<------------------------------>|
%   +   +   +   +   +  . . .     +   +
%   1   2   3   4   5           n-1  n
%   |<->|
%     SE
%
% Difference equation :
%
%   T(m,p+1) - T(m,p)     TD
%  ------------------- = -------( T(m-1,p+1) - 2.T(m,p+1) + T(m+1,p+1) )
%       dTau             2.dx.dx
%
%                         TD
%                      + -------( T(m-1,p) - 2.T(m,p) + T(m+1,p) )
%                        2.dx.dx
%
%  System of equations:
%
% (1+M).T(2,p+1) - M/2.T(3,p+1) =
%                 =  M/2.T(1,p) + (1-M).T(2,p) + M/2.T(3,p) +  M/2.T(1,p+1)
%  . . .
%  for m <2,n-1>
% - M/2.T(m-1,p+1) + (1+M).T(m,p+1)- M/2.T(m+1,p+1) =
%                             =  M/2.T(m-1,p) + (1-M).T(m,p) + M/2.T(m+1,p)
%  . . .
%  - M/2.T(n-2,p+1) + (1+M).T(n-1,p+1) =
%              =  M/2.T(n-2,p) + (1-M).T(n-1,p) + M/2.T(n,p) + M/2.T(n,p+1)
%
%  Matrix notation:
%
%   A.T = b
%
%  The elements of A are given by
%
%   A(n-2,n-2) = 0 except for:
%
%   A(1,1)     = 1 + M;
%   A(1,2)     = - M/2;
%
%   A(m,m-1)   = - M/2;
%   A(m,m)     = 1 + M;
%   A(m,m+1)   = - M/2;   for m <2,n-3>
%
%   A(n-2,n-3) = - M/2;
%   A(n-2,n-2) = 1 + M;
%
%  and the elements of b are given by
%
%   b(1)   = M/2*Tin(1)   + (1-M)*Tin(2)   + M/2*Tin(3) + M/2*T(1);
%
%   b(m)   = M/2*Tin(m)   + (1-M)*Tin(m+1) + M/2*Tin(m+2);    for m <2,n-3>
%
%   b(n-2) = M/2*Tin(n-2) + (1-M)*Tin(n-1) + M/2*Tin(n) + M/2*T(n);
%
% INPUTS:
%
% Tin  - initialization temperatures                        [ K           ]
% TC   - thermal conductivity                               [ W.m^-1.K^-1 ]
% DE   - density                                            [ kg.m^-3     ]
% HC   - specific heat capacity                             [ kg.m^-3     ]
% SE   - size of element                                    [ m           ]
% DI   - distance                                           [ m           ]
% TS   - time step                                          [ s           ]
% TT   - total time of simulation                           [ s           ]
% TBC  - type of boundary condition
%        (1 = first-type, 2 = second-type)                  [ -           ]
% BC   - boundary condition
%         - first-type : BC(1) = T(1),  BC(2) = T(n)        [ K           ]
%         - second-type: BC(1) = iQ(1), BC(2) = iQ(n)       [ W.m^-2      ]
%
% OUTPUTS:
%
% Xco  - vector of X coordinate                             [ m           ]
% Tau  - vector of time                                     [ s           ]
% MT   - matrix of temperatures                             [ K           ]
%
% AUXILIARY VARIABLE:
%
% r    - time  step  number                                 [ -           ]
% n    - space point number                                 [ -           ]
% TD   - thermal diffusivity                                [ m^2.m^-1    ]
% M    - modul (Fourier number)                             [ -           ]
% A    - matrix (n-2,n-2)                                   [ -           ]
% b    - vector (n-2)                                       [ -           ]
%
% Copyright (C) 2013, Technical University of Kosice
% Author  : Zecova Monika, Terpak Jan
% Revision: 27.08.2013
%
  r = round(TT/TS) + 1;
  n = round(DI/SE) + 1;

  Xco = 0.0:SE:DI;
  Tau = 0.0:TS:TT;
  MT  = zeros(r,n);
  MT(1,:) = Tin;

  for j=2:r
    [T]     = CrankNicolson (Tin,TC,DE,HC,SE,TS,TBC,BC);
    Tin     = T;
    MT(j,:) = Tin;
  end
end
function [T] = CrankNicolson (Tin,TC,DE,HC,SE,TS,TBC,BC)

  TD = TC/(DE*HC);
  M  = TD*TS/(SE*SE);

  T = Tin;

  if TBC == 1
     T(1)   = BC(1);
     T(end) = BC(2);
  end

  if TBC == 2
     T(1)   = T(2)     - BC(1)*SE/TC;
     T(end) = T(end-1) - BC(2)*SE/TC;
  end

  k = length(Tin)-2;
  A = zeros(k,k);
  b = zeros(k,1);

  for i=1:k
      b(i) = M/2*Tin(i) + (1-M)*Tin(i+1)   + M/2*Tin(i+2) ;
  end

  A(1,1) = (1+M);
  A(1,2) = - M/2;
  b(1)   = b(1) + M/2*T(1);

  for i=2:k-1
      A(i,i-1) = - M/2;
      A(i,i)   = 1 + M;
      A(i,i+1) = - M/2;
  end

  A(k,k-1) = - M/2;
  A(k,k)   = 1 + M;
  b(k)     = b(k) + M/2*T(end);

  x = A\b;

  for i=2:length(Tin)-1
      T(i) = x(i-1);
  end

end


function [W phi]=train_rbf(X,Y,Xc,k_i,basisfunction)
%trains a radial basis function
%X is a N_p by N_dim matrix of training data
%Y is a N_p by N_dim matrix of training data
%Xc is a N_r by N_dim matrix of rbf centres
%basisfunction may be 'gaussian' or 'polyharmonicspline'
%k_i is a prescaler for 'gaussian' rbf and function order for
%'polyharmonicspline'. See k_i(i)=0 for constant bias

%    Copyright Travis Wiens 2008
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%    If you would like to request that this software be licensed under a less
%    restrictive license (i.e. for commercial closed-source use) please
%    contact Travis at travis.mlfx@nutaksas.com

if nargin<4
    k_i=1;
end

if nargin<5
    basisfunction='gaussian';
end

N_r=size(Xc,1);%number of centres

W=zeros(N_r,1);%weight matrix
[z phi]=sim_rbf(Xc,X,W,k_i,basisfunction);%simulate rbf
A=pinv(phi'*phi)*phi';%do inverse
W=A*Y;%find weights


function mc = moc ( a, b, n, f )

%*****************************************************************************80
%
%% MOC estimates the modulus of continuity of a function over an interval.
%
%  Discussion;
%
%    The modulus of continuity function MC(T) for a function F(X) over an
%    interval [A,B] is defined as
%
%      MC(T) = max | F(X+DX) - F(X) | for 0 <= DX <= T, and X and X+DX in [A,B].
%
%    The modulus of continuity function is a monotone increasing function,
%    with MC(0) = 0.
%
%    This function estimates the modulus of continuity based on a discrete
%    set of data at N equally spaced points in the interval [A,B].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 October 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the left and right endpoints of the interval.
%
%    Input, integer N, the number of equally spaced sample points.
%
%    Input, function F(x), a handle to the function.
%
%    Output, real MC(N), the modulus of continuity function estimated at
%    0, H or less, 2*H or less, ..., (N-1)*H or less.
%

%  Compute the maximum difference with a separation DX of exactly 0*H, 1*H, 2*H,
%  ..., (N-1)*H.
%
  mc1 = moc1 ( a, b, n, f );
%
%  Compute the maximum difference with a separation DX of 0*H or less,
%  1*H or less, 2*H or less, ..., (N-1)*H or less.
%
  mc = zeros ( n, 1 );
  for i = 1 : n - 1
    mc(i+1) = max ( mc(i), mc1(i+1) );
  end

  return
end


function Nimg = Gscale(img,levels,gsize,sigma);
%
%  Function to generate a gaussian-pyramid for the given input image
%
% Input:
%        img: input image-matrix grayscale
%        levels:  number of levels of the pyramid
%        gsize: size of the gaussian kernel [w h] ([5 5] normally provides a smooth output)
%        sigma:  sigma for gaussian kernel
% Output:
%        Nimg:  is a struct consisting of images from each level
%            :  Nimg.img;
% Usage:
%      im = imread('cameraman.tif');
%      Nimg = Gscale(im,3,[5 5],1.6);
%      i = 2; %select a level
%      figure; imshow(Nimg(i).img);
%
% Author: Pranam Janney                     Date: 24th July 2006 15:39
% Email: pranamjanney@yahoo.com
%
% Revised Version 1.0.1                     Date: 04th August 2006, 10:50
%

%guassian filter  with a sigma=1.6
g = fspecial('gaussian',gsize,sigma);

%pyramid
for i = 1:levels
    if i == 1
        im = imfilter(img,g,'conv');
        Nimg(i).img = im;
    else
        %perform guassian filtering
        im = imfilter(Nimg(i-1).img,g,'conv');
        %perform downsampling (horizontal)
        im1 = im(:,1:2:size(Nimg(i-1).img,2));
        %vertical
        im2 = im1(1:2:size(Nimg(i-1).img,1),:);
        %store it in a struct format
        Nimg(i).img = im2;
    end
end

%End


% Y = spharm_fast(theta,phi,l,m,p0)
%
% Orthonormal spherical harmonic function (Arfken, p. 681) for quantum
% numbers l and m. Angles theta and phi should be given in radians.
% p0 contains the values of the Legendre polynomial
%
function Y = spharm_fast(theta,phi,l,m,p0)

% Copyright (c) 2016, Elekta Oy
% ---------------------------------------
%
% Redistribution and use of the Software in source and binary forms, with or without
% modification, are permitted for non-commercial use.
%
% The Software is provided "as is" without warranties of any kind, either express or
% implied including, without limitation, warranties that the Software is free of defects,
% merchantable, fit for a particular purpose. Developer/user agrees to bear the entire risk
% in connection with its use and distribution of any and all parts of the Software under this license.
%

%p0 = legendre(l,cos(theta));
p = p0(2:end);  % Values m > 1
%
% Arfken, p. 681. The Condon-Shortley phase is included in the
% Legendre polynomials
%
%scale = sqrt((2*l+1)*prod(1:(l-m))/(4*pi*prod(1:(l+m))));
if m < 0
  Y = ((-1)^m)*sqrt((2*l+1)*prod(1:(l+m))/(4*pi*prod(1:(l-m))))*p(-m);
  %Y = scale*((-1)^m)*(prod(1:(l-abs(m)))/prod(1:(l+abs(m))))*p(-m);
elseif m > 0
  Y = sqrt((2*l+1)*prod(1:(l-m))/(4*pi*prod(1:(l+m))))*p(m);
  %Y = scale*p(m);
else
  Y = sqrt((2*l+1)/(4*pi))*p0(1);
  %Y = scale*p0(1);
end
Y = Y*complex(cos(m*phi),sin(m*phi));


function [mellin,beta]=fmt(X,fmin,fmax,N);
%FMT    Fast Fourier Mellin Transform.
%       [MELLIN,BETA]=FMT(X,FMIN,FMAX,N) computes the Fast Mellin
%       Transform of signal X.
%
%       X : signal in time (Nx=length(X)).
%       FMIN,FMAX : respectively lower and upper frequency bounds of
%        the analyzed signal. These parameters fix the equivalent
%        frequency bandwidth (expressed in Hz). When unspecified, you
%        have to enter them at the command line from the plot of the
%        spectrum. FMIN and FMAX must be >0 and <=0.5.
%       N : number of analyzed voices. N must be even
%				 (default : automatically determined).
%       MELLIN : the N-points Mellin transform of signal S.
%       BETA : the N-points Mellin variable.
%
%       Examples :
%        sig=altes(128,0.05,0.45);
%	 [MELLIN,BETA]=fmt(sig,0.05,0.5,128);
%        plot(BETA,real(MELLIN));
%
%       See also IFMT, FFT, IFFT.

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

if (nargin == 0),
 error('At least one parameter required');
end;

[xrow,xcol] = size(X);

if (nargin==2),
 disp('FMIN will not be taken into account. Determine it with FMAX');
 disp('     from the following plot of the spectrum.');
elseif nargin==3,
 N=[];
elseif (nargin==4 & rem(N,2)~=0),
 error('N must be even');
end;
if (xcol==0)|(xcol>2),
 error('X must have one or two columns');
end

Mt = length(X);
Z  = hilbert(real(X));
M  = (Mt+rem(Mt,2))/2;

if nargin<=2,                                   % fmin,fmax,N unspecified
 STF = fft(fftshift(X)); Nstf=length(STF);
 sp = (abs(STF(1:Nstf/2))).^2; Maxsp=max(sp);
 f = linspace(0,0.5,Nstf/2+1) ; f = f(1:Nstf/2);
 plot(f,sp) ; grid;
 xlabel('Normalized frequency');
 title('Analyzed signal energy spectrum');
 indmin=min(find(sp>Maxsp/1000));
 indmax=max(find(sp>Maxsp/1000));
 fmindflt=max([0.001 0.05*fix(f(indmin)/0.05)]);
 fmaxdflt=0.05*ceil(f(indmax)/0.05);
 txtmin=['Lower frequency bound [',num2str(fmindflt),'] : '];
 txtmax=['Upper frequency bound [',num2str(fmaxdflt),'] : '];
 fmin = input(txtmin); fmax = input(txtmax);
 if fmin==[], fmin=fmindflt; end
 if fmax==[], fmax=fmaxdflt; end
end

if fmin >= fmax
 error('FMAX must be greater or equal to FMIN');
elseif fmin<=0.0 | fmin>0.5,
 error('FMIN must be > 0 and <= 0.5');
elseif fmax<=0.0 | fmax>0.5,
 error('FMAX must be > 0 and <= 0.5');
end

B = fmax-fmin;       		% Bandwidth of the signal X
R = B/((fmin+fmax)/2);		% Relative bandwidth of X

Nq= ceil((B*Mt*(1+2/R)*log((1+R/2)/(1-R/2)))/2);
Nmin = Nq-rem(Nq,2);
Ndflt = 2^nextpow2(Nmin);
if nargin<=2,
 Ntxt=['Number of frequency samples (>=',num2str(Nmin),') [',num2str(Ndflt),'] : '];
 N = input(Ntxt);
end

if N~=[],
 if (N<Nmin),
  dispstr=['Warning : the number of analyzed voices (N) should be >= ',num2str(Nmin)];
  disp(dispstr);
 end
else
 N=Ndflt;
end

% Geometric sampling of the analyzed spectrum
No2 = N/2;
k = (1:No2);
q = (fmax/fmin)^(1/(No2-1));
t = (1:Mt)-M-1;
geo_f  = fmin*(exp((k-1).*log(q)));
tfmatx = zeros(Mt,N);
tfmatx = exp(-2*j*pi*t'*geo_f);
ZS = Z.'*tfmatx;
ZS(No2+1:N) = zeros(1,N-No2);

% Mellin transform computation of the analyzed signal
p = 0:(N-1);
L = log(fmin)/log(q);
mellin = N*log(q)*fftshift(ifft(ZS)).*exp(j*2*pi*L*(p/N-1/2));
beta   = (p/N-1/2)./log(q);

% Normalization
SP = fft(hilbert(real(X)));
indmin = 1+round(fmin*(xrow-2));
indmax = 1+round(fmax*(xrow-2));
SPana = SP(indmin:indmax);
nu = (indmin:indmax)'/N;
SPp = SPana./nu;
Normsig = sqrt(SPp'*SPana);

mellin = mellin*Normsig/norm(mellin);


clear

% problem specification
m = 400; n = 500; r = 10;
t = 1; esr = ceil(t*r);
fprintf('[m n r esr] = [%i, %i, %i, %i]\n',m,n,r,esr);

% data
% Xo = rand(m,r); Yo = rand(r,n);
Xo = abs(randn(m,r)); Yo = abs(randn(r,n));

d = ones(r,1).*(1:r)';
M = Xo*spdiags(d,0,r,r)*Yo;

% set solver options
opts.tol = 1e-5;
opts.maxit = 500;
opts.print = 1;

% call solver

sr = 0.5;
Omega = randsample(m*n, sr*m*n);
A = M(Omega);

%exact rank-estimate case
t0 = tic;
[X,Y,Out] = mc_nmf(A,Omega,esr,m,n,opts);
time = toc(t0);
relerr = norm(X*Y-M,'fro')/norm(M,'fro');
fprintf('RelErr = %5.2e, %4.2e Sec.\n',relerr,time);
%%
[numr,numc] = size(M);
I = randi([0 1],numr,numc);
Omega = find(I);
A = M(Omega);

%%
subplot(1,2,1),imagesc(M);
subplot(1,2,2),imagesc(X*Y);
%%

%over-estimate case
esr = ceil(1.5*r);
t0 = tic;
fprintf('[m n r esr] = [%i, %i, %i, %i]\n',m,n,r,esr);
[X,Y,Out] = mc_nmf(A,Omega,esr,m,n,opts);
time = toc(t0);
relerr = norm(X*Y-M,'fro')/norm(M,'fro');
fprintf('RelErr = %5.2e, %4.2e Sec.\n',relerr,time);


function value = cube_monomial ( a, b, expon )

%*****************************************************************************80
%
%% CUBE_MONOMIAL integrates a monomial over a cube in 3D.
%
%  Discussion:
%
%    This routine integrates a monomial of the form
%
%      product ( 1 <= dim <= 3 ) x(dim)^expon(dim)
%
%    The combination 0^0 should be treated as 1.
%
%    The integration region is:
%      A(1) <= X <= B(1)
%      A(2) <= Y <= B(2)
%      A(3) <= Z <= B(3)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A(3), B(3), the lower and upper limits.
%
%    Input, integer EXPON(3), the exponents.
%
%    Output, real VALUE, the integral of the monomial.
%
  for i = 1 : 3

    if ( expon(i) == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CUBE_MONOMIAL - Fatal error!\n' );
      fprintf ( 1, '  Exponent of -1 encountered.\n' );
      error ( 'CUBE_MONOMIAL - Fatal error!' );
    end

  end

  value = 1.0;

  for i = 1 : 3

    value = value * ( b(i) ^ ( expon(i) + 1 ) - a(i) ^ ( expon(i) + 1 ) ) ...
      / ( expon(i) + 1 );

  end

  return
end


#!/usr/bin/env octave
%% Machine Learning Online Class - Exercise 1: Linear Regression

%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  linear exercise. You will need to complete the following functions
%  in this exericse:
%
%     warmUpExercise.m
%     plotData.m
%     gradientDescent.m
%     computeCost.m
%     gradientDescentMulti.m
%     computeCostMulti.m
%     featureNormalize.m
%     normalEqn.m
%
%  For this exercise, you will not need to change any code in this file,
%  or any other files other than those mentioned above.
%
% x refers to the population size in 10,000s
% y refers to the profit in $10,000s
%

%% Initialization
clear all; close all; clc

%% ==================== Part 1: Basic Function ====================
% Complete warmUpExercise.m
fprintf('Running warmUpExercise ... \n');
fprintf('5x5 Identity Matrix: \n');
warmUpExercise()

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

%% ======================= Part 2: Plotting =======================
fprintf('Plotting Data ...\n')
data = csvread('ex1data1.txt');
X = data(:, 1); y = data(:, 2);
m = length(y); % number of training examples

% Plot Data
% Note: You have to complete the code in plotData.m
plotData(X, y);

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

%% =================== Part 3: Gradient descent ===================
fprintf('Running Gradient Descent ...\n')

X = [ones(m, 1), data(:,1)]; % Add a column of ones to x
theta = zeros(2, 1); % initialize fitting parameters

% Some gradient descent settings
iterations = 1500;
alpha = 0.01;

% compute and display initial cost
computeCost(X, y, theta)

% run gradient descent
theta = gradientDescent(X, y, theta, alpha, iterations);

% print theta to screen
fprintf('Theta found by gradient descent: ');
fprintf('%f %f \n', theta(1), theta(2));

% Plot the linear fit
hold on; % keep previous plot visible
plot(X(:,2), X*theta, '-')
legend('Training data', 'Linear regression')
hold off % don't overlay any more plots on this figure

% Predict values for population sizes of 35,000 and 70,000
predict1 = [1, 3.5] *theta;
fprintf('For population = 35,000, we predict a profit of %f\n',...
    predict1*10000);
predict2 = [1, 7] * theta;
fprintf('For population = 70,000, we predict a profit of %f\n',...
    predict2*10000);

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

%% ============= Part 4: Visualizing J(theta_0, theta_1) =============
fprintf('Visualizing J(theta_0, theta_1) ...\n')

% Grid over which we will calculate J
theta0_vals = linspace(-10, 10, 100);
theta1_vals = linspace(-1, 4, 100);

% initialize J_vals to a matrix of 0's
J_vals = zeros(length(theta0_vals), length(theta1_vals));

% Fill out J_vals
for i = 1:length(theta0_vals)
    for j = 1:length(theta1_vals)
	  t = [theta0_vals(i); theta1_vals(j)];
	  J_vals(i,j) = computeCost(X, y, t);
    end
end

% Because of the way meshgrids work in the surf command, we need to
% transpose J_vals before calling surf, or else the axes will be flipped
J_vals = J_vals';
% Surface plot
figure;
surf(theta0_vals, theta1_vals, J_vals)
xlabel('\theta_0'); ylabel('\theta_1');

% Contour plot
figure;
% Plot J_vals as 15 contours spaced logarithmically between 0.01 and 100
contour(theta0_vals, theta1_vals, J_vals, logspace(-2, 3, 20))
xlabel('\theta_0'); ylabel('\theta_1');
hold on;
plot(theta(1), theta(2), 'rx', 'MarkerSize', 10, 'LineWidth', 2);

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


function [total_error, max_error] = eq_area_error(dim,N)
%EQ_AREA_ERROR Total area error and max area error per region of an EQ partition
%
%Syntax
% [total_error, max_error] = eq_area_error(dim,N)
%
%Description
% [TOTAL_ERROR, MAX_ERROR] = EQ_AREA_ERROR(dim,N) does the following:
% 1) uses the recursive zonal equal area sphere partitioning algorithm to
%    partition the unit sphere S^dim into N regions,
% 2) sets TOTAL_ERROR to be the absolute difference between the total area of
%    all regions of the partition, and the area of S^dim, and
% 3) sets MAX_ERROR to be the maximum absolute difference between the area of
%    any region of the partition, and the ideal area of a region as given by
%    AREA_OF_IDEAL_REGION(dim,N), which is 1/N times the area of S^dim.
%
% The argument dim must be a positive integer.
% The argument N must be a positive integer or an array of positive integers.
% The results TOTAL_ERROR and MAX_ERROR will be arrays of the same size as N.
%
%Examples
% > [total_error, max_error] = eq_area_error(2,10)
% total_error =
%    1.7764e-15
%
% max_error =
%    4.4409e-16
%
% > [total_error, max_error] = eq_area_error(3,1:6)
% total_error =
%    1.0e-12 *
%     0.0036    0.0036    0.1847    0.0142    0.0142    0.2132
%
% max_error =
%    1.0e-12 *
%     0.0036    0.0018    0.1954    0.0284    0.0440    0.0777
%
%See also
% EQ_REGIONS, AREA_OF_SPHERE, AREA_OF_IDEAL_REGION

% Copyright 2004-2005 Paul Leopardi for the University of New South Wales.
% $Revision 1.10 $ $Date 2005-06-01 $
% Documentation files renamed
% $Revision 1.00 $ $Date 2005-02-12 $
%
% For licensing, see COPYING.
% For references, see AUTHORS.
% For revision history, see CHANGELOG.

%
% Check number of arguments
%
error(nargchk(2,2,nargin));
error(nargoutchk(2,2,nargout));

%
% Flatten N into a row vector.
%
shape = size(N);
n_partitions = prod(shape);
N = reshape(N,1,n_partitions);

total_error = zeros(size(N));
max_error   = zeros(size(N));
sphere_area = area_of_sphere(dim);

for partition_n = 1:n_partitions
    n = N(partition_n);
    regions = eq_regions(dim,n);
    ideal_area = area_of_ideal_region(dim,n);
    total_area = 0;
    for region_n = 1:size(regions,3)
        area = area_of_region(regions(:,:,region_n));
        total_area = total_area + area;
        region_error = abs(area - ideal_area);
        if region_error > max_error(partition_n)
            max_error(partition_n) = region_error;
        end
    end
    total_error(partition_n) = abs(sphere_area - total_area);
end
%
% Reshape output to same array size as original N.
%
total_error = reshape(total_error,shape);
max_error = reshape(max_error,shape);
%
% end function

function area = area_of_region(region)
%AREA_OF_REGION Area of given region
%
% area = area_of_region(region);

dim = size(region,1);
s_top = region(dim,1);
s_bot = region(dim,2);
if dim > 1
    area = area_of_collar(dim, s_top, s_bot)*area_of_region(region(1:dim-1,:))/area_of_sphere(dim-1);
else
    if s_bot == 0
        s_bot = 2*pi;
    end
    if s_top == s_bot
        s_bot = s_top + 2*pi;
    end
    area = s_bot - s_top;
end
%
% end function


function result = fit_maxwell_pdf( x,y,W,hAx )
% fit_maxwell_pdf - Non Linear Least Squares fit of the maxwellian distribution.
%                   given the samples of the histogram of the samples, finds the
%                   distribution parameter that fits the histogram samples.
%
%    fits data to the probability of the form:
%        p(r) = sqrt(2/pi)*(a^(-3/2))*(r^2)*exp(-(r^2)/(2*a))
%    with parameter: a
%
% format:   result = fit_maxwell_pdf( x,y,W,hAx )
%
% input:    y   - vector, samples of the histogram to be fitted
%           x   - vector, position of the samples of the histogram (i.e. y = f(x,a))
%           W   - matrix or scalar, a square weighting matrix of the size NxN where
%                 N = length(y), or 0 to indicate no weighting is needed.
%           hAx - handle of an axis, on which the fitted distribution is plotted
%                 if h is given empty, a figure is created.
%
% output:   result  - structure with the fields
%                      a   - fitted parameter
%                      VAR - variance of the estimation
%                      type- weighted LS or not weighted LS
%                      iter- number of iteration for the solution
%

%
% Algorithm
% ===========
%
% We use the WLS algorithm to estimate the PDF from the samples.%
% The maxwell distribution is given by:
%
%    p(x,a) = sqrt(2/pi)*(a^(-3/2))*(x.^2).*exp(-(x.^2)/(2*a))
%           = Const * (a^(-3/2)) .* exp(-(x.^2)/(2*a))
%
%    note that X is known and therefore, considered a constant vector
%
% The non liner WLS estimator is given by:
%
%    a(n+1) = a(n) + inv(H'*W*H)*(H') * (y-h) = a(n) + G * err
%
%    where:   h = p(x,a)
%             H = diff( p(x,a) ) with respect to "a"
%             W = weighting matrix of size NxN  (N = length(y))
%             a = a single parameter to be estimated
%
% The error estimation is given by:
%
%    VAR( a ) = G * VAR( err ) * (G')
%
%       or when W=I and the noise is a gaussian noise
%
%    VAR( a ) = inv( H' * H )
%

if (nargin<3)
    error( 'fit_maxwell_pdf - insufficient input arguments' );
end

a       = x(find(y==max(y)))^2;         % initial guess
y       = y(:);                         % both should be column vectors !
x       = x(:);
x2      = x.^2;                         % save computation time
C       = sqrt(2/pi)*x2;                % a constant vector
thresh  = 0.995;                        % convergence threshold for the loop
last_cnt= inf;
iter    = 0;

% check weight matrix input
if (size(W,1)==length(y)) & (size(W,2)==length(y))
    weights_flag    = 1;
    type            = 'WLS';
else
    weights_flag    = 0;
    type            = 'LS';
end

% Estimation
% =============
if (weights_flag)
    % loop for convergence (with weighting matrix)
    % =============================================
    while (1)
        iter    = iter + 1;
        h       = C*(a^(-1.5)).*exp(-x2/(2*a));
        H       = h.*( x2/(2*a^2) - 3/(2*a) );
        HTW     = H'*W;
        e       = inv( HTW * H ) * HTW * (y-h);
        a       = a + e;
        control = e*e;
        if ( control > (last_cnt * thresh) )
            break;
        else
            last_cnt = control;
        end
    end

    % summarize results
    h           = C*(a^(-1.5)).*exp(-x2/(2*a));
    H           = h.*( x2/(2*a^2) - 3/(2*a) );
    HTW         = H'*W;
    G           = inv( HTW * H ) * HTW;
    err         = ( y - h );
    result.a    = a;
    result.VAR  = G * var( err ) * (G');
    result.RMS  = sqrt( (err')*err/ (x(2)-x(1))^2 / (length(err)-1) );
    result.iter = iter;
    result.type = type;
else

    % loop for convergence (without a weighting matrix) - assume white noise
    % ========================================================================
    while (1)
        iter    = iter + 1;
        h       = C*(a^(-1.5)).*exp(-x2/(2*a));
        H       = h.*( x2/(2*a^2) - 3/(2*a) );
        HT      = H';
        control = inv( HT * H );
        a       = a + control * HT * (y-h);
        if ( control>(last_cnt * thresh) )
            break;
        else
            last_cnt = control;
        end
    end

	% summarize results
    h           = C*(a^(-1.5)).*exp(-x2/(2*a));
	H           = h.*( x2/(2*a^2) - 3/(2*a) );
    err         = ( y - h );
    result.a    = a;
    result.VAR  = inv( (H') * H );
    result.RMS  = sqrt( (err')*err/ (x(2)-x(1))^2 / (length(err)-1) );
    result.iter = iter;
    result.type = type;
end

% plot distribution if asked for
% ===============================
if (nargin>3)
    if ishandle( hAx )
        plot_maxwell( x,result,hAx,2 );
    else
        figure;
        plot_maxwell( x,result,gca,2 );
    end
end


function [P] = JacobiP(x,alpha,beta,N);

% function [P] = JacobiP(x,alpha,beta,N)
% Purpose: Evaluate Jacobi Polynomial of type (alpha,beta) > -1
%          (alpha+beta <> -1) at points x for order N and returns P[1:length(xp))]
% Note   : They are normalized to be orthonormal.

% Turn points into row if needed.
xp = x; dims = size(xp);
if (dims(2)==1) xp = xp'; end;

PL = zeros(N+1,length(xp));

% Initial values P_0(x) and P_1(x)
gamma0 = 2^(alpha+beta+1)/(alpha+beta+1)*gamma(alpha+1)*...
    gamma(beta+1)/gamma(alpha+beta+1);
PL(1,:) = 1.0/sqrt(gamma0);
if (N==0) P=PL'; return; end;
gamma1 = (alpha+1)*(beta+1)/(alpha+beta+3)*gamma0;
PL(2,:) = ((alpha+beta+2)*xp/2 + (alpha-beta)/2)/sqrt(gamma1);
if (N==1) P=PL(N+1,:)'; return; end;

% Repeat value in recurrence.
aold = 2/(2+alpha+beta)*sqrt((alpha+1)*(beta+1)/(alpha+beta+3));

% Forward recurrence using the symmetry of the recurrence.
for i=1:N-1
  h1 = 2*i+alpha+beta;
  anew = 2/(h1+2)*sqrt( (i+1)*(i+1+alpha+beta)*(i+1+alpha)*...
      (i+1+beta)/(h1+1)/(h1+3));
  bnew = - (alpha^2-beta^2)/h1/(h1+2);
  PL(i+2,:) = 1/anew*( -aold*PL(i,:) + (xp-bnew).*PL(i+1,:));
  aold =anew;
end;

P = PL(N+1,:)';
return


function [varargout] = funname(varargin)

% SOLID_ANGLE of a planar triangle as seen from the origin
%
% The solid angle W subtended by a surface S is defined as the surface
% area W of a unit sphere covered by the surface's projection onto the
% sphere. Solid angle is measured in steradians, and the solid angle
% corresponding to all of space being subtended is 4*pi sterradians.
%
% Use:
%   [w] = solid_angle(v1, v2, v3)
% or
%   [w] = solid_angle(pnt, tri)
% where v1, v2 and v3 are the vertices of a single triangle in 3D or
% pnt and tri contain a description of a triangular mesh (this will
% compute the solid angle for each triangle)

% Copyright (C) 2003-2009, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.ru.nl/neuroimaging/fieldtrip
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id: solid_angle.m 2885 2011-02-16 09:41:58Z roboos $

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The first section contains the plain Matlab implementation. The mex file
% is many times faster and this function is called so frequently (for
% large meshes), that the mex file should be used in all practical cases.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [w] = solid_angle(r1, r2, r3);
%
% if nargin==2
%   % reassign the input arguments
%   pnt = r1;
%   tri = r2;
%   npnt = size(pnt,1);
%   ntri = size(tri,1);
%   w    = zeros(ntri,1);
%   % compute solid angle for each triangle
%   for i=1:ntri
%     r1 = pnt(tri(i,1),:);
%     r2 = pnt(tri(i,2),:);
%     r3 = pnt(tri(i,3),:);
%     w(i) = solid_angle(r1, r2, r3);
%   end
%   return
% elseif nargin==3
%   % compute the solid angle for this triangle
%   cp23_x = r2(2) * r3(3) - r2(3) * r3(2);
%   cp23_y = r2(3) * r3(1) - r2(1) * r3(3);
%   cp23_z = r2(1) * r3(2) - r2(2) * r3(1);
%   nom = cp23_x * r1(1) + cp23_y * r1(2) + cp23_z * r1(3);
%   n1 = sqrt (r1(1) * r1(1) + r1(2) * r1(2) + r1(3) * r1(3));
%   n2 = sqrt (r2(1) * r2(1) + r2(2) * r2(2) + r2(3) * r2(3));
%   n3 = sqrt (r3(1) * r3(1) + r3(2) * r3(2) + r3(3) * r3(3));
%   ip12 = r1(1) * r2(1) + r1(2) * r2(2) + r1(3) * r2(3);
%   ip23 = r2(1) * r3(1) + r2(2) * r3(2) + r2(3) * r3(3);
%   ip13 = r1(1) * r3(1) + r1(2) * r3(2) + r1(3) * r3(3);
%   den = n1 * n2 * n3 + ip12 * n3 + ip23 * n1 + ip13 * n2;
%   if (nom == 0)
%     if (den <= 0)
%       w = nan;
%       return
%     end
%   end
%   w = 2 * atan2 (nom, den);
%   return
% else
%   error('invalid input');
% end

% compile the missing mex file on the fly
% remember the original working directory
pwdir = pwd;

% determine the name and full path of this function
funname = mfilename('fullpath');
mexsrc  = [funname '.c'];
[mexdir, mexname] = fileparts(funname);

try
  % try to compile the mex file on the fly
  warning('trying to compile MEX file from %s', mexsrc);
  cd(mexdir);

  if ispc
    mex -I. -c geometry.c
    mex -I. -c solid_angle.c ; mex solid_angle.c solid_angle.obj geometry.obj
  else
    mex -I. -c geometry.c
    mex -I. -c solid_angle.c ; mex -o solid_angle solid_angle.o geometry.o
  end

  cd(pwdir);
  success = true;

catch
  % compilation failed
  disp(lasterr);
  error('could not locate MEX file for %s', mexname);
  cd(pwdir);
  success = false;
end

if success
  % execute the mex file that was juist created
  funname   = mfilename;
  funhandle = str2func(funname);
  [varargout{1:nargout}] = funhandle(varargin{:});
end


function poly_cof = r8poly_basis ( ntab, xtab )

%*****************************************************************************80
%
%% R8POLY_BASIS computes all Lagrange basis polynomial in standard form.
%
%  Discussion:
%
%    The I-th Lagrange basis polynomial for a set of NTAB X values XTAB,
%    L(I,NTAB,XTAB)(X) is a polynomial of order NTAB-1 which is zero at
%    XTAB(J) for J not equal to I, and 1 when J is equal to I.
%
%    The Lagrange basis polynomials have the property that the interpolating
%    polynomial through a set of NTAB data points (XTAB,YTAB) may be
%    represented as
%
%      P(X) = Sum ( 1 <= I <= N ) YTAB(I) * L(I,NTAB,XTAB)(X)
%
%    Higher order interpolation at selected points may be accomplished
%    using repeated X values, and scaled derivative values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NTAB, the number of data points XTAB.
%
%    Input, real XTAB(NTAB), the X values upon which the
%    Lagrange basis polynomial is to be based.
%
%    Output, real POLY_COF(NTAB,NTAB), the polynomial
%    coefficients for the I-th Lagrange basis polynomial are stored
%    in column I.  POLY_COF(1,I) is the constant term, and POLY_COF(1,NTAB)
%    is the coefficient of X**(NTAB-1).
%

%
%  Initialize POLY_COF to the identity matrix.
%
  poly_cof(1:ntab,1:ntab) = 0.0;
  for i = 1 : ntab
    poly_cof(i,i) = 1.0;
  end
%
%  Compute the divided difference table for the IVAL-th Lagrange basis
%  polynomial.
%
  for i = 1 : ntab
    poly_cof(1:ntab,i) = ( data_to_dif ( ntab, xtab, poly_cof(1:ntab,i) ) )';
  end
%
%  Convert the divided difference table coefficients to standard polynomial
%  coefficients.
%
  for i = 1 : ntab
    poly_cof(1:ntab,i) = ( dif_to_r8poly ( ntab, xtab, poly_cof(1:ntab,i) ) )';
  end

  return
end


% "Powel test function which has n decision variables, has no local optima.
%has one global optimum. all decision variables are bounded in [-4
% 5]. One global minimum at x=[3 -1 0, 1...,3 -1 0 1]  with f(x)=0

function sum=test10(x)
sum=0;

b=numel(x)/4;

for i=1:b
   sum=sum+(x(4*i-3)+10*x(4*i-2)).^2+5*(x(4*i-1)-x(4*i)).^2+(x(4*i-2)-x(4*i-1)).^4+10*(x(4*i-3)-x(4*i)).^4;
end


function H_MMSE = MMSE_CE(Y,Xp,pilot_loc,Nfft,Nps,h,SNR)
%function H_MMSE = MMSE_CE(Y,Xp,pilot_loc,Nfft,Nps,h,ts,SNR)
% MMSE channel estimation function
% Inputs:
%       Y         = Frequency-domain received signal
%       Xp        = Pilot signal
%       pilot_loc = Pilot location
%       Nfft      = FFT size
%       Nps       = Pilot spacing
%       h         = Channel impulse response
%       ts        = Sampling time
%       SNR       = Signal-to-Noise Ratio[dB]
% output:
%      H_MMSE     = MMSE channel estimate

%H = fft(h,N);
snr = 10^(SNR*0.1);
Np=Nfft/Nps; k=1:Np;
H_tilde = Y(1,pilot_loc(k))./Xp(k);  % LS estimate
k=0:length(h)-1; %k_ts = k*ts;
hh = h*h';
tmp = h.*conj(h).*k; %tmp = h.*conj(h).*k_ts;
r = sum(tmp)/hh;
r2 = tmp*k.'/hh; %r2 = tmp*k_ts.'/hh;
tau_rms = sqrt(r2-r^2);     % rms delay
df = 1/Nfft;  %1/(ts*Nfft);
j2pi_tau_df = j*2*pi*tau_rms*df;
K1 = repmat([0:Nfft-1].',1,Np);
K2 = repmat([0:Np-1],Nfft,1);
rf = 1./(1+j2pi_tau_df*(K1-K2*Nps));
K3 = repmat([0:Np-1].',1,Np);
K4 = repmat([0:Np-1],Np,1);
rf2 = 1./(1+j2pi_tau_df*Nps*(K3-K4));
Rhp = rf;
Rpp = rf2 + eye(length(H_tilde),length(H_tilde))/snr;
H_MMSE = transpose(Rhp*inv(Rpp)*H_tilde.');  % MMSE channel estimate


%   X: data matrix, each row is one observation, each column is one feature
%   d: reduced dimension
%   type: type of kernel, can be 'simple', 'poly', or 'gaussian'
%   para (input): parameter for computing the 'poly' kernel, for 'simple'
%       and 'gaussian' it will be ignored
%   Y: dimensionanlity-reduced data
%   eigVector: eigen-vector, will later be used for pre-image
%       reconstruction
%   para (output): automatically selected Gaussian kernel parameter

%   Copyright by Quan Wang, 2011/05/10
%   Please cite: Quan Wang. Kernel Principal Component Analysis and its
%   Applications in Face Recognition and Active Shape Models.
%   arXiv:1207.3538 [cs.CV], 2012.

function [Y, eigVector, para]=kPCA(X,d,type,para)

%% check input
if ( strcmp(type,'simple') || strcmp(type,'poly') || ...
        strcmp(type,'gaussian') ) == 0
    Y=[];
    eigVector=[];
    para=[];
    fprintf(['\nError: Kernel type ' type ' is not supported. \n']);
    return;
end

%% parameters
N=size(X,1);
if strcmp(type,'gaussian')
    DIST=zeros(N,N);
    for k=1:size(X,2)
        [a,b]=meshgrid(X(:,k));
        DIST=DIST+(a-b).^2;
    end
    DIST=DIST.^0.5;
    DIST=DIST+diag(ones(N,1)*inf);
    para=10*median(min(DIST));
end

%% kernel PCA
K0=kernel(X,type,para);
oneN=ones(N,N)/N;
K=K0-oneN*K0-K0*oneN+oneN*K0*oneN;

%% eigenvalue analysis
[V,D]=eig(K/N);
eigValue=diag(D);
% eigValue=eigValue(1:min(size(X)));
[eigValue,IX]=sort(eigValue,'descend');
eigVector=V(:,IX);

%% normailization
norm_eigVector=sqrt(sum(eigVector.^2));
eigVector=eigVector./repmat(norm_eigVector,size(eigVector,1),1);

%% dimensionality reduction
Y=K0*eigVector(:,1:d);


function n = cfn_e_size_total ( m, ell_max )

%*****************************************************************************80
%
%% CFN_E_SIZE_TOTAL: Closed Fully Nested, Exponential Growth.
%
%  Discussion:
%
%    This calculation assumes that an exponential growth rule is being used,
%    that is, that the 1D rules have orders 1, 3, 7, 15, 31, and so on.
%
%    It counts the number of points in all the product rules that compose
%    the sparse grid, and it does not reduce the count in any way to account
%    for repeated points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer ELL_MAX, the sparse grid level.
%
%    Output, integer N, the total number of points in the grids.
%
  n = 0;

  lvec = zeros ( m, 1 );

  ell_min = max ( ell_max + 1 - m, 0 );

  for ell = ell_min : ell_max

    more = 0;
    h = 0;
    t = 0;

    while ( 1 )

      [ lvec, more, h, t ] = comp_next ( ell, m, lvec, more, h, t );

      nvec = cfn_e_nvec_from_lvec ( lvec );
      ell_num = prod ( nvec(1:m) );
      n = n + ell_num;

      if ( ~ more )
        break
      end

    end
  end

  return
end


function varargout = boundedCentroidalVoronoi2d(germs, box, varargin)
%BOUNDEDCENTROIDALVORONOI2D Create a 2D Centroidal Voronoi Tesselation in a box.
%
%   [N, E, F] = boundedCentroidalVoronoi2d(GERMS, BOX)
%   GERMS are N-by-2 point array, BOX is given as [xmin xmax ymin ymax].
%   Algorithm is an iteration of voronoi diagram computations, using at
%   each steps the centroids of previous diagram as germs for the new
%   diagram.
%
%   [N, E, F] = boundedCentroidalVoronoi2d(GERMS, BOX, NITER)
%   Specifies the number of iterations. Default is 10.
%
%   [N, E, F, G] = boundedCentroidalVoronoi2d(...)
%   also returns the positions of germs/centroids for each face. If the
%   number of iteration was sufficient, location of germs should correspond
%   to centroids of faces 'fc' computed using:
%   fc(i,:) = polygonCentroid(n(f{i}, :));
%
%   Example
%   [n, e, f] = boundedCentroidalVoronoi2d(rand(20, 2)*100, [0 100 0 100]);
%   drawGraph(n, e, f);
%
%   See also
%     graphs, boundedVoronoi2d, centroidalVoronoi2d, clipGraph
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2007-01-12
% Copyright 2007-2022 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas

% number of iteration
nIter = 10;
if ~isempty(varargin)
    nIter = varargin{1};
end

% limits and size of the box
x0 = box(1); x1 = box(2);
y0 = box(3); y1 = box(4);
dx = x1 - x0;  dy = y1 - y0;

% far points to bound the voronoi diagram
farPoints = [...
    x1 + 10 * dx, y1 + 10 * dy;...
    x0 - 10 * dx, y1 + 10 * dy;...
    x0 - 10 * dx, y0 - 10 * dy;...
    x1 + 10 * dx, y0 - 10 * dy];

% iterate bounded voronoi tesselation
for i = 1:nIter
    % generate Voronoi diagram, and clip with the box
    [n, e, f] = voronoi2d([germs ; farPoints]);
    [n, e, f] = clipGraph(n, e, f, box);

    % centroid of each face will be used as germs for next iteration
    for j = 1:length(f)
        face = n(f{j}, :);
        germs(j, 1:2) = polygonCentroid(face);
    end
end

% result is given in n, e, and f, eventually germs
varargout{1} = n;
varargout{2} = e;
varargout{3} = f;
if nargout > 3
    varargout{4} = germs;
end


function z = distSqr_fast(x,y,x2,y2) %x2 = sum(x.^2,1)'; %y2 = sum(y.^2,1);
% function z = distSqr_fast(x,y,x2,y2)
%
% Return matrix of all-pairs squared distances between the vectors
% in the columns of x and y.
%
% INPUTS
% 	x 	dxn matrix of vectors
% 	y 	dxm matrix of vectors
%
% OUTPUTS
% 	z 	nxm matrix of squared distances
%
% This routine is faster when m<n than when m>n. In other words y should be
% smaller than x.
%
% David Martin <dmartin@eecs.berkeley.edu>
% March 2003

% Based on dist2.m code,
% Copyright (c) Christopher M Bishop, Ian T Nabney (1996, 1997)

if ~exist('y','var')
  y = x;
end

if ~exist('x2','var')
  x2 = sum(x.^2,1)';
end

if ~exist('y2','var')
  y2 = sum(y.^2,1);
end

if size(x,1) ~= size(y,1),
  error('size(x,1)~=size(y,1)');
end

[d,n] = size(x);
[d,m] = size(y);

%z = x'*y
%z = repmat(x2,1,m) ...
%    + repmat(y2,n,1) ...
%    - 2*x'*y;
%return

z = x'*y;

for i = 1:m,
  z(:,i) = x2 + y2(i) - 2*z(:,i);
end

%z = zeros(n,m);
%for i = 1:m,
%  z(:,i) = x2 + y2(i) - 2*x'*y(:,i);
%end


function rotatedData = rotatePoints(alignmentVector, originalData)

% rotatedData = rotatePoints(alignmentVector, originalData) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%     Rotate the 'originalData' in the form of Nx2 or Nx3 about the origin by aligning the x-axis with the alignment vector
%
%       Rdata = rotatePoints([1,2,-1], [Xpts(:), Ypts(:), Zpts(:)]) - rotate the (X,Y,Z)pts in 3D with respect to the vector [1,2,-1]
%
%       Rotating using spherical components can be done by first converting using [dX,dY,dZ] = cart2sph(theta, phi, rho);  alignmentVector = [dX,dY,dZ];
%
% Example:
%   %% Rotate the point [3,4,-7] with respect to the following:
%   %%%% Original associated vector is always [1,0,0]
%   %%%% Calculate the appropriate rotation requested with respect to the x-axis.  For example, if only a rotation about the z-axis is
%   %%%% sought, alignmentVector = [2,1,0] %% Note that the z-component is zero
%   rotData = rotatePoints(alignmentVector, [3,4,-7]);
%
%     Author: Shawn Arseneau
%     Created: Feb.2, 2006
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    alignmentDim = numel(alignmentVector);
    DOF = size(originalData,2); %---- DOF = Degrees of Freedom (i.e. 2 for two dimensional and 3 for three dimensional data)

    if alignmentDim~=DOF
        error('Alignment vector does not agree with originalData dimensions');
    end
    if DOF<2 || DOF>3
        error('rotatePoints only does rotation in two or three dimensions');
    end

    if DOF==2  % 2D rotation...
        [rad_theta, rho] = cart2pol(alignmentVector(1), alignmentVector(2));
        deg_theta = -1 * rad_theta * (180/pi);
        ctheta = cosd(deg_theta);  stheta = sind(deg_theta);

        Rmatrix = [ctheta, -1.*stheta;...
                   stheta,     ctheta];
        rotatedData = originalData*Rmatrix;

    else    % 3D rotation...
        [rad_theta, rad_phi, rho] = cart2sph(alignmentVector(1), alignmentVector(2), alignmentVector(3));
        rad_theta = rad_theta * -1;
        deg_theta = rad_theta * (180/pi);
        deg_phi = rad_phi * (180/pi);
        ctheta = cosd(deg_theta);  stheta = sind(deg_theta);
        Rz = [ctheta,   -1.*stheta,     0;...
              stheta,       ctheta,     0;...
              0,                 0,     1];                  %% First rotate as per theta around the Z axis
        rotatedData = originalData*Rz;

        [rotX, rotY, rotZ] = sph2cart(-1* (rad_theta+(pi/2)), 0, 1);          %% Second rotation corresponding to phi
        rotationAxis = [rotX, rotY, rotZ];
        u = rotationAxis(:)/norm(rotationAxis);        %% Code extract from rotate.m from MATLAB
        cosPhi = cosd(deg_phi);
        sinPhi = sind(deg_phi);
        invCosPhi = 1 - cosPhi;
        x = u(1);
        y = u(2);
        z = u(3);
        Rmatrix = [cosPhi+x^2*invCosPhi        x*y*invCosPhi-z*sinPhi     x*z*invCosPhi+y*sinPhi; ...
                   x*y*invCosPhi+z*sinPhi      cosPhi+y^2*invCosPhi       y*z*invCosPhi-x*sinPhi; ...
                   x*z*invCosPhi-y*sinPhi      y*z*invCosPhi+x*sinPhi     cosPhi+z^2*invCosPhi]';

        rotatedData = rotatedData*Rmatrix;
    end


function [phi] = evalPhi( r );
%
% [phi] = evalPhi( r );
%
%  Calculates phi( r ), phi the discrete delta function (without scaling)
%
%  Returns:
%     phi = value of unscaled discrete delta function at each point, r
%
%  Input:
%     r   = the points to evaluate the discrete delta function at
%
%  Note:
%     This code doesn't handle wrapping across periodic boundaries.
%
%
%  License: This code is free to use for any purposes, provided
%           any publications resulting from the use of this code
%           reference the original code/author.
%
%  Author:  Samuel Isaacson (isaacson@math.utah.edu)
%  Date:    11/2007
%
%  Please notify the author of any bugs, and contribute any
%  modifications or bug fixes back to the original author.
%
%  Disclaimer:
%   This code is provided as is. The author takes no responsibility
%   for its results or effects.

phi  = zeros(length(r), 1);
aR   = abs( r );
idx1 = find( aR < 1 );
idx2 = find( aR >= 1 );

rT        = aR(idx1);
phi(idx1) = 3 - 2*rT + sqrt( 1 + 4*rT - 4*rT.*rT );

rT        = aR(idx2);
phi(idx2) = 5 - 2*rT - sqrt( -7 + 12*rT - 4*rT.*rT );

phi = phi / 8;


function centroid = polyhedron_centroid_3d ( coord, maxorder, face_num, node, ...
  node_num, order )

%*****************************************************************************80
%
%% POLYHEDRON_CENTROID_3D computes the centroid of a polyhedron in 3D.
%
%  Discussion:
%
%    The centroid can be computed as the volume-weighted average of
%    the centroids of the tetrahedra defined by choosing a point in
%    the interior of the polyhedron, and using as a base every triangle
%    created by triangulating the faces of the polyhedron.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real COORD(3,NODE_NUM), the vertices.
%    The vertices may be listed in any order.
%
%    Input, integer MAXORDER, the maximum number of vertices that make
%    up a face of the polyhedron.
%
%    Input, integer FACE_NUM, the number of faces of the polyhedron.
%
%    Input, integer NODE(FACE_NUM,MAXORDER).  Face I is defined by
%    the vertices NODE(I,1) through NODE(I,ORDER(I)).  These vertices
%    are listed in neighboring order.
%
%    Input, integer NODE_NUM, the number of points stored in COORD.
%
%    Input, integer ORDER(FACE_NUM), the number of vertices making up
%    each face.
%
%    Output, real CENTROID(3), the centroid of the polyhedron.
%
  dim_num = 3;
%
%  Compute a point in the interior.
%  We take the area-weighted centroid of each face.
%
  point(1:dim_num) = 0.0;
  area = 0.0;

  for face = 1 : face_num

    vert_num = order(face);

    v(1:dim_num,1:vert_num) = coord(1:dim_num,node(face,1:vert_num));

    [ polygon_area, normal ] = polygon_area_3d ( vert_num, v );

    polygon_centroid(1:dim_num) = polygon_centroid_3d ( vert_num, v );

    point(1:dim_num) = point(1:dim_num) + polygon_area * polygon_centroid(1:dim_num);

    area = area + polygon_area;

  end

  point(1:dim_num) = point(1:dim_num) / area;
%
%  Now triangulate each face.
%  For each triangle, consider the tetrahedron created by including POINT.
%
  centroid(1:dim_num) = 0.0;
  volume = 0.0;

  for face = 1 : face_num

    n3 = node(face,order(face));

    for vert = 1 : order(face) - 2

      n1 = node(face,vert);
      n2 = node(face,vert+1);

      tetra(1:dim_num,1:4) = [ ...
        coord(1:dim_num,n1)'; coord(1:dim_num,n2)'; coord(1:dim_num,n3)'; point(1:dim_num) ]';

      tetra_volume = tetrahedron_volume_3d ( tetra );

      tetra_centroid(1:dim_num) = tetrahedron_centroid_3d ( tetra );

      centroid(1:dim_num) = centroid(1:dim_num) + tetra_volume * tetra_centroid(1:dim_num);

      volume = volume + tetra_volume;

    end
  end

  centroid(1:dim_num) = centroid(1:dim_num) / volume;

  return
end


function xyPoints=getEllipseHullPoints(z,A,gammaVal,numPoints,invertA)
%%GETELLIPSHULLPOINTS  Consider an ellipsoid in 3D (x,y,z) coordinates. For
%       every fixed z coordinate, if the x-y plane at the z coordinate
%       intersects the ellipsoid, it cuts an ellipse. For example, the
%       function projEllipse2ZPlane gets the parameters of the ellipse of
%       intersection. This function gets points marking the outer limits of
%       all of those ellipses in the x-y plane (the hull of the ellipses).
%
%INPUTS: z A 3XN vector corresponding to the centers of the N ellipses for
%          which points should be obtained.
%        A A 3X3XN set of N positive definite matrices that specify the
%          size and shape of the ellipsoids, where a point zp is on the ith
%          ellipsoid if
%          (zp-z(:,i))'*A(:,:,i)*(zp-z(:,i))=gammaVal (if invertA is true,
%          then replace A(:,:,i) with inv(A(:,:,i))).
% gammaVal An optional parameter specifying the size of the ellipse/
%          ellipsoid. If omitted or an empty matrix is passed, then
%          gammaVal=18.8049 is used. This is approximately the value for a
%          99.97% confidence region if A are inverse covariance matrices of
%          a Gaussian distribution. gammaVal must be positive gammaVal must
%          be positive.
% numPoints An optional parameter specifying how many points should be
%          generated. The default if omitted or an empty matrix is passed
%          is 500.
%  invertA If this is true, then A is inverted before use. The default if
%          omitted or an empty matrix is passed is false.
%
%OUTPUTS: xyPoints A 2XnumPointsXN set of the points for each of the
%                  ellipsoid projection hulls.
%
%The 3D ellipsoid (with the coordinate system shited to put the origin at
%the center) is defined as x'*R*x=gammaVal. We can rewrite this as
%[r*u,z]*R*[r*u;z]=gammaVal
%where r is a positive scalar and u is a 2X1 unit vector. The matrix R can
%be broken up accordingly as
%R=[Rxy, rz;
%   rz', rzz];
%We want the maximum extent of the ellipsoid in the x-y plane for each
%direction (as specified by u). This will provide the hull of the ellipsoid
%points. Thus, we perform the optimization:
% maximize (over r,z) r^2
% such that r^2*u'*Rxy*u+2*r*z*rz'*u+z^2*rzz=gammaVal
%where the constraint is just from rewriting the expression for the
%definition of an ellipsoid. Writing the Lagrangian, (with lambda as the
%Lagrangian parameter) taking derivatives and setting them equal to 0, we
%get two additional equations. Solving those two equations with the
%original constraint, we get expressions for r, z, and lambda. Choosing the
%solution such that r is positive and nonzero, one gets the expressions
%implemented in this function.
%
%December 2020 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(invertA))
    invertA=false;
end

if(nargin<4||isempty(numPoints))
    numPoints=500;
end

if(nargin<3||isempty(gammaVal))
    gammaVal=18.8049;
end

if(invertA)
    A=applyFunToEachMatrix(@inv,A);
end

numEllipse=size(z,2);

theta=linspace(-pi,pi,numPoints);
u=[cos(theta);
   sin(theta)];
xyPoints=zeros(2,numPoints,numEllipse);
for curEllipse=1:numEllipse
    Rxyrzz=A(1:2,1:2,curEllipse)*A(3,3,curEllipse);
    rz=A(1:2,3,curEllipse);
    rzzGamma=A(3,3,curEllipse)*gammaVal;
    zXYCur=z(1:2,curEllipse);

    for k=1:numPoints
        uCur=u(:,k);

        r=sqrt(rzzGamma/(uCur'*Rxyrzz*uCur-(rz'*uCur)^2));
        xyPoints(:,k,curEllipse)=zXYCur-r*uCur;
    end
end
end

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


function [C, sigma] = dataset3Params(X, y, Xval, yval)
%EX6PARAMS returns your choice of C and sigma for Part 3 of the exercise
%where you select the optimal (C, sigma) learning parameters to use for SVM
%with RBF kernel
%   [C, sigma] = EX6PARAMS(X, y, Xval, yval) returns your choice of C and
%   sigma. You should complete this function to return the optimal C and
%   sigma based on a cross-validation set.
%

% You need to return the following variables correctly.
C = 1;
sigma = 0.3;

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return the optimal C and sigma
%               learning parameters found using the cross validation set.
%               You can use svmPredict to predict the labels on the cross
%               validation set. For example,
%                   predictions = svmPredict(model, Xval);
%               will return the predictions on the cross validation set.
%
%  Note: You can compute the prediction error using
%        mean(double(predictions ~= yval))
%

results = eye(64,3);
errorRow = 0;

for C_test = [0.01 0.03 0.1 0.3 1, 3, 10 30]
    for sigma_test = [0.01 0.03 0.1 0.3 1, 3, 10 30]
        errorRow = errorRow + 1;
        model = svmTrain(X, y, C_test, @(x1, x2) gaussianKernel(x1, x2, sigma_test));
        predictions = svmPredict(model, Xval);
        prediction_error = mean(double(predictions ~= yval));

        results(errorRow,:) = [C_test, sigma_test, prediction_error];
    end
end

sorted_results = sortrows(results, 3); % sort matrix by column #3, the error, ascending

C = sorted_results(1,1);
sigma = sorted_results(1,2);

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

end


function [ wval, dwdx ] = r8poly_lagrange_factor ( npol, xpol, xval )

%*****************************************************************************80
%
%% R8POLY_LAGRANGE_FACTOR evaluates the polynomial Lagrange factor at a point.
%
%  Formula:
%
%    W(X) = Product ( 1 <= I <= NPOL ) ( X - XPOL(I) )
%
%  Discussion:
%
%    Suppose F(X) is at least N times continuously differentiable in the
%    interval [A,B].  Pick NPOL distinct points XPOL(I) in [A,B] and compute
%    the interpolating polynomial P(X) of order NPOL ( and degree NPOL-1)
%    which passes through all the points ( XPOL(I), F(XPOL(I)) ).
%    Then in the interval [A,B], the maximum error
%
%      abs ( F(X) - P(X) )
%
%    is bounded by:
%
%      C * FNMAX * W(X)
%
%    where
%
%      C is a constant,
%      FNMAX is the maximum value of the NPOL-th derivative of F in [A,B],
%      W(X) is the Lagrange factor.
%
%    Thus, the value of W(X) is useful as part of an estimated bound
%    for the interpolation error.
%
%    Note that the Chebyshev abscissas have the property that they minimize
%    the value of W(X) over the interval [A,B].  Hence, if the abscissas may
%    be chosen arbitrarily, the Chebyshev abscissas have this advantage over
%    other choices.
%
%    For a set of points XPOL(I), 1 <= I <= NPOL, the IPOL-th Lagrange basis
%    polynomial L(IPOL)(X), has the property:
%
%      L(IPOL)( XPOL(J) ) = delta ( IPOL, J )
%
%    and may be expressed as:
%
%      L(IPOL)(X) = W(X) / ( ( X - XPOL(IPOL) ) * W'(XPOL(IPOL)) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NPOL, the number of abscissas.
%    NPOL must be at least 1.
%
%    Input, real XPOL(NPOL), the abscissas, which should
%    be distinct.
%
%    Input, real XVAL, the point at which the Lagrange
%    factor is to be evaluated.
%
%    Output, real WVAL, the value of the Lagrange factor at XVAL.
%
%    Output, real DWDX, the derivative of W with respect to XVAL.
%
  wval = prod ( xval - xpol(1:npol) );

  dwdx = 0.0;

  for i = 1 : npol

    term = 1.0;

    for j = 1 : npol
      if ( i ~= j )
        term = term * ( xval - xpol(j) );
      end
    end

    dwdx = dwdx + term;

  end

  return
end


function ry = vecroty(angle)
%
% Function Name:
%
%   vecroty - Transformation matrix for a rotation around the y axis.
%
% Calling Sequence:
%
%   ry = vecroty(angle);
%
% Parameters:
%
%   angle		: rotation angle defined in radians
%
%   ry		: (4x4) Transformation matrix.
%
%
% Description:
%
%   Return the (4x4) Transformation matrix for a rotation about the y axis
%   by the defined angle.
%
%   The matrix is:
%
%         [  cos(angle)       0        sin(angle)       0]
%         [      0            1            0            0]
%         [ -sin(angle)       0        cos(angle)       0]
%         [      0            0            0            1]
%
% Examples:
%
%    Rotate the NURBS line (0.0 0.0 0.0) - (3.0 3.0 3.0) by 45 degrees
%    around the y-axis
%
%    line = nrbline([0.0 0.0 0.0],[3.0 3.0 3.0]);
%    trans = vecroty(%pi/4);
%    rline = nrbtform(line, trans);
%
% See:
%
%    nrbtform

%  Dr D.M. Spink
%  Copyright (c) 2000.

sn = sin(angle);
cn = cos(angle);
ry = [cn 0 sn 0; 0 1 0 0; -sn 0 cn 0; 0 0 0 1];


function a=randNestedParenth(n)
%%RANDNESTEDPARENTH Obtain a random character string of randomly nested
%                   parentheses. Such parantheses can represent random
%                   binary trees.
%
%INPUTS: n The number of open-closed parenthesis pairs.
%
%OUTPUTS: a A (2*n)X1 character string of n parenthesis pairs, with the
%           open parenthesis always coming before the close one.
%
%This function implements Algorithm W of Chapter 7.2.1.6 of [1].
%
%EXAMPLE:
% a=randNestedParenth(5).'
%We transpose it to make it easier to read.
%
%REFERENCES
%[1] D. E. Knuth, The Art of Computer Programming. Vol. 4A: Combinatorial
%    Algorithms, Part I, Boston: Addison-Wesley, 2011.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

p=n;
q=n;
m=1;
%Allocate space for the random set of nested parentheses.
a=repmat(' ',2*n,1);

while(q~=0)
    while(1)
        upperBound=(q+p)*(q-p+1);
        X=randi(upperBound-1);

        if(X<(q+1)*(q-p))
            q=q-1;
            a(m)=')';
            m=m+1;
            break;
        end
        p=p-1;
        a(m)='(';
        m=m+1;
    end
end
end

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


function rad=dms2rad(dms)
% DMS2RAD  Converts degrees-minutes-seconds to radians.
%   Vectorized.
% Version: 12 Mar 00
% Useage:  rad=dms2rad(dms)
% Input:   dms - [d m s] array of angles in deg-min-sec, where
%                d = vector of degrees
%                m = vector of minutes
%                s = vector of seconds
% Output: rad - vector of angles in radians

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

d=dms(:,1);
m=dms(:,2);
s=dms(:,3);
dec=abs(d)+abs(m)./60+abs(s)./3600;
rad=dec.*pi./180;
ind=(d<0 | m<0 | s<0);
rad(ind)=-rad(ind);


%................................................................

function stiffness=formStiffness2D(GDof,numberElements,...
    elementNodes,numberNodes,nodeCoordinates,C,thickness)

% compute stiffness matrix
% for plane stress Q8 elements

stiffness=zeros(GDof);

% 3 by 3 quadrature
[gaussWeights,gaussLocations]=gauss2d('3x3');

for e=1:numberElements
  numNodePerElement = length(elementNodes(e,:));
  numEDOF = 2*numNodePerElement;
  elementDof=zeros(1,numEDOF);
  for i = 1:numNodePerElement
      elementDof(2*i-1)=2*elementNodes(e,i)-1;
      elementDof(2*i)=2*elementNodes(e,i);
  end

  % cycle for Gauss point
  for q=1:size(gaussWeights,1)
    GaussPoint=gaussLocations(q,:);
    xi=GaussPoint(1);
    eta=GaussPoint(2);

% shape functions and derivatives
    [shapeFunction,naturalDerivatives]=shapeFunctionQ8(xi,eta);

% Jacobian matrix, inverse of Jacobian,
% derivatives w.r.t. x,y
    [Jacob,invJacobian,XYderivatives]=...
        Jacobian(nodeCoordinates(elementNodes(e,:),:),naturalDerivatives);

%  B matrix
    B=zeros(3,numEDOF);
    B(1,1:2:numEDOF)       = XYderivatives(:,1)';
    B(2,2:2:numEDOF)  = XYderivatives(:,2)';
    B(3,1:2:numEDOF)       = XYderivatives(:,2)';
    B(3,2:2:numEDOF)  = XYderivatives(:,1)';

% stiffness matrix
    stiffness(elementDof,elementDof)=...
        stiffness(elementDof,elementDof)+...
        B'*C*thickness*B*gaussWeights(q)*det(Jacob);
  end
end


function [qp]=zn2fr(input)
% [qp]=zn2fr(input)
% Ziegler-Nichols PID controller for processes of 2nd order.
% This function computes parameters of the controller (q0, q1, q2, p1, p2).
% Controller is based on forward rectangular method of discretization.
% Transfer function of the controller is as follows:
%
%            q0 + q1*z^-1 + q2*z^-2     q0 + q1*z^-1 + q2*z^-2
% G(z^-1) = ------------------------ = ------------------------
%                  1 - z^-1              1 + p1*z^-1 + p2*z^-2
%
% where p1=-1 and p2=0.
%
% Transfer function of the controlled system is:
%
%               b1*z^-1 + b2*z^-2
% Gs(z^-1) = -----------------------
%             1 + a1*z^-1 + a2*z^-2
%
% Input: input ... input parameters
%                  input(1) ... a1
%                  input(2) ... b1
%                  input(3) ... a2
%                  input(4) ... b2
%                  input(5) ... sample time T0
% Output: qp ... controller parameters
%                qp(1) ... q0
%                qp(2) ... q1
%                qp(3) ... q2
%                qp(4) ... -1   (p1 of the controller)
%                qp(5) ... 0    (p2 of the controller)

a1 = input(1);
b1 = input(2);
a2 = input(3);
b2 = input(4);
T0 = input(5);

% compute ultimate gain and frequency
[Kpu, Tu] =  ultim([b1 b2],[a1 a2],T0);

Kp = 0.6*Kpu;
Ti = Tu/2;
Td = Tu/8;

q0 = Kp*(1+Td/T0);
q1 = -Kp*(1-T0/Ti+2*Td/T0);
q2 = Kp*(Td/T0);
p1 = -1;
p2 = 0;

qp=[q0; q1; q2; p1; p2];


function ex2bvp
%EX2BVP  Example 2 of the BVP tutorial.
%   A standard linear problem with a boundary layer at the origin.
%   The differential equation y'' + 3*p*y/(p + t^2)^2 = 0 has the
%   analytical solution y(t) = t/sqrt(p + t^2).  The parameter p
%   is taken to be 1e-5, a common value in tests.  The solution is
%   to have specified values at t = -0.1 and +0.1, values taken from
%   this analytical solution.
%
%   The default RelTol of 1e-3 gives an acceptable solution, but
%   reducing RelTol to 1e-4 resolves better the boundary layer.  A
%   constant guess is used for RelTol = 1e-3.  The same guess could be
%   used for RelTol = 1e-4, but a very much better guess is provided
%   by the solution previously computed for RelTol = 1e-3.

% Copyright 2002, The MathWorks, Inc.

% Evaluate the analytical solution for comparison.
tt = -0.1:0.01:+0.1;
p = 1e-5;
yy = tt ./ sqrt(p + tt .^2);

options = bvpset('stats','on','Fjacobian',@ex2Jac);

% BVPINT is used to specify an initial guess for the mesh of 10
% equally spaced points.  A constant guess based on a straight line
% between the boundary values for y is 0 for y(t) and 10 for y'(t).
solinit = bvpinit(linspace(-0.1,0.1,10),[0 10]);

sol = bvp4c(@ex2ode,@ex2bc,solinit, options);
t = sol.x;
y = sol.y;

figure
plot(t,y(1,:),tt,yy,'*')
axis([-0.1 0.1 -1.1 1.1])
title(['Linear boundary layer problem with RelTol = 1e-3.'])
xlabel('t')
ylabel('y and analytical (*) solutions')

fprintf('\n');

% A smaller RelTol is used to resolve better the boundary layer.
% The previous solution provides an excellent guess.
options = bvpset(options,'RelTol',1e-4);
sol = bvp4c(@ex2ode,@ex2bc,sol,options);
t = sol.x;
y = sol.y;

figure
plot(t,y(1,:),tt,yy,'*')
axis([-0.1 0.1 -1.1 1.1])
title(['Linear boundary layer problem with RelTol = 1e-4.'])
xlabel('t')
ylabel('y and analytical (*) solutions')

% --------------------------------------------------------------------------

function dydt = ex2ode(t,y)
%EX2ODE  ODE function for Example 2 of the BVP tutorial.
%   The components of y correspond to the original variables
%   as  y(1) = y, y(2) = y'.
p = 1e-5;
dydt = [ y(2)
        -3*p*y(1)/(p+t^2)^2];

% --------------------------------------------------------------------------

function dfdy = ex2Jac(t,y)
%EX2JAC  The Jacobian of the ODE function for Example 2 of the BVP tutorial.
p = 1e-5;
dfdy = [          0        1
         -3*p/(p+t^2)^2    0 ];

% --------------------------------------------------------------------------

function res = ex2bc(ya,yb)
%EX2BC  Boundary conditions for Example 2 of the BVP tutorial.
%   The boundary conditions are that the solution should agree
%   with the values of an analytical solution at both a and b.
p = 1e-5;
yatb = 0.1/sqrt(p + 0.01);
yata = - yatb;
res = [ ya(1) - yata
        yb(1) - yatb ];