% Copyright 2009 - 2010 The MathWorks, Inc.
% Loading the test data
% Note that we are tracking an object whose positions are recorded
% as x and y coordinates in a Cartesian space
load position.mat
hold; grid;
for idx = 1: numPts
z = position(:,idx);
plot(z(1), z(2), 'bx');
axis([-1 1 -1 1]);
end
hold;
%% This algorithm predicts the position of a moving object based on
% its past values. It uses a Kalman filter estimator.
% Kalman filter is a recursive adaptive filter that estimates
% the state of a dynamic system from a series of
% noisy measurements. It has a broad range of application in
% signal and imge processing, control design,
% and computational finance
% Let's run the script ObjTrack.m to see how Kalman filter
% gradually estimates and tracks  a moving object.
ObjTrack01(position)
%% In this example, by assuming a zero initial condition
% ( no knowledge of the position of the object in the past)
% Kalman filter iteratively and adaptively estimates and tracks
% the object. This is depicted by the green circular traces
% as outputs  of the Kalman estimator superimposed
% on the input positions vectors depicted by blue x traces.
% Note that there are 3 sudden shifts in position and every time
% Kalman filter re-adjusts and tracks the object after few iterations
edit ObjTrack01
%% Let's look at the Kalman filter function
% Note that the Kalman estimator computes the position vector
% by computing and updating the Kalman state vector.
% We define the state vector as a 6-by-1 column vector
% that includes position (x and y) velocity (Vx Vy) and
% acceleration (Ax and Ay) measurements in a 2-dimensional
% Cartesian space. Based on the classical laws of motion:
% X = X0 + Vx*t
% Y = Y0 + Vy*t
% Vx = Vx0 + Ax*t
% Vy = Yy0 + Ay*t
% Note how these laws of motion are reflected in the Kalman
% state transion matrix A:
% x=[X Y Vx Vy Ax Ay]' %% state vector
% x=A*x;               %% recursive estimation
% dt=1;
% A=[ 1 0 dt 0 0 0;... %% state transion matrix
%     0 1 0 dt 0 0;...
%     0 0 1 0 dt 0;...
%     0 0 0 1 0 dt;...
%     0 0 0 0 1 0 ;...
%     0 0 0 0 0 1 ];
% Note how by writing only 10 lines of MATLAB code,
% we implement the Kalman estimator as if you are writing
% the mathematical formula. This is power of MATLAB!
%% Version 1 Scalar Kalman filter
% input: one sample
% output: one sample
edit kalman01.m
%% Let us now genearte C code for the kalman filter
% First we run the emlmex command to see weather there is any
% function or operator in this function that is not compliant with the
% Embedded MATLAB subset
z = position(:,1);
emlmex -eg {z} kalman01.m
%% Now that the kalman filter is compliant, Is the
% calling function (ObjTrack01) compliant with Embedded MATLAB subset?
emlmex -eg {position} ObjTrack01.m
%% No,it contains unsupported visualization function
% How do we make it complaint with the embedded MATLAB subset?
% Notice eror messages when complaining due to the use of plotting
% functions. We now showcase use of eml.extrinsic that declare
% visualization functions to be extrinsic and instruct Embedded MATLAB
% not to compile them.
edit ObjTrack02.m
%% Notice how the non-supported plotting functions now
% do not take part in compilation and thus no more eror messages
emlmex -eg {position} ObjTrack02.m
%% Since this function kalman.m is consistent with
% Embedded MATLAB subset, let us now use the emlc command to
% automatically genearte C source code for this functioon
emlc -eg {z} kalman01.m -c -T RTW -report
%% Here we note and explain the following features:
% 1. MATLAB is an untyped language, C is strictly typed.
% So we use -eg option to set the (data type, size and complexity) of the
% input variable to the function based on an example variable
% availbale in MATLAB workspace (in this case varaiable name is z).
% Based on the properties of this variable,
% Embeded MATLAB sets and infers the data type, size and complexity of
% all internal variables to the function, declares them and generate C code.
% 2. -c option indicates we just want to see the C source code
% 3. -T RTW option says generate embeddable C-code and
% 4. -report option says generate a compilation report.
% Now browse and explain the organization of Compilation report
%% Now we want to showcase how in many cases we can
% accelerate the execution speed of a function running a large data set
% by using emlmex command that compiled the function into a MEX function.
% Use of emlmex not only verifies the compliance with the Embedded MATLAB
% subset but it may potentialy accelrate the execution speed of some algorithms
% specially if the algorithm is fixed-point
%% First run the kalman algorithm with 100,000 samples without
% using emlmex to compile it to a mex function
delete *.mexw32
which kalman01
%% Use tic; toc; to record how much time it takes for processing
% 100K samples of data in this case
SIZE=100000;
Z=rand(2,SIZE);
tic;
for i=1:SIZE
    z=Z(:,i);
    y=kalman01(z);
end;
toc;
%% Second run the kalman algorithm with 100,000 samples WITH
% using emlmex to compile it to a mex function
emlmex -eg {z} kalman01.m
which kalman01
%% Use tic; toc; to record how much time it takes for processing 100K
% samples
SIZE=100000;
Z=rand(2,SIZE);
tic;
for i=1:SIZE
    z=Z(:,i);
    y=kalman01(z);
end;
toc;
%% Notice a 3 times acceleration of the speed of the execution of this
% function just by using the emlmex function
%% Version 3 Variable size Vector (Packet based)
% Kalman filter
% input: a variable-sized vector
% output: sanme size as inoput
edit ObjTrack03.m
%% Variable-size data
% Let us now genearte C code for the packet-based
% kalman filter. First we run the emlmex command to see weather there is any
% function or operator in this function that is not compliant with the
% Embedded MATLAB subset
N=100;
z = position(1:2,1:N);
eg_z=emlcoder.egs(z,[2 N]);
emlmex -eg {eg_z} kalman03.m
%% Let us run the compiled version to make sure
% the generated C code can properly handle packets of diferent sizes.
% By examining ObjTrack03.m we see that we are calling kalman03.m with
% 7 different (x-y) pairs of varying sizes ranging from 2x10 to 2x100
which kalman03
ObjTrack03
%% Since this function kalman.m is consistent with
% Embedded MATLAB subset, let us now use the emlc command to
% automatically genearte C source code for this function
% We use the maximum size of 2x100 as the upper bound for compilation to C
% and use the emlcoder.egs to indicate the maximum size of the input
% variable z.
N=100;
z = position(1:2,1:N);
eg_z=emlcoder.egs(z,[2 N]);
emlc -eg {eg_z} kalman03.m -c -T RTW -report
%% Here we note and explain the following features:
% 1. The genearted C code has just enough extra code to make it flexible
% for any size input from 2x1 to 2x100.
% 2. We showcase emlcoder.egs and explain how it works
% 3. We explain how easy it is to make an algorithm be flexible relative to
% input size:
% We show that in the function at line 36, we had o explicitly index
% the input variable z (instead of z(:,i) -> z(1:2,i) to make explicit
% which portion of variable size input us being processed at that line
%% Fixed-point data: Look at the fixed-point Kalman estimator function
% It differs from the floating-point algorithm kalman.m
% by declaring the internal variables as fixed-point
% using the fi constructor. The variable declaration (where most changes are) are separated
% from the algorithm part, which remains mostly the same relative to the
% floating point algorithm.
edit kalman04.m
%% To convert algorithm to fixed-point, you need to set numeric type
% of the fixed-point input data and specify a set of global fimath
% properties.
% As of R2009b, you can specify accumulator ad product data types
% of all intermediate variables globally, which makes the task
% of fixed-point programming easier.
%% Look at the syntax of setting the global fimath properties
% Before setting the global fimath we have the default values
resetglobalfimath
fimath
%% Now note that I am using a helper functin myglobalfimath.m
% to globaly set the fixed-point math properties of all variables in MATLAB workspace.
edit myglobalfimath.m
%%
% I want to set a global fimath properties such that SpecifyPrecision Mode is used
% for setting the Sum and Product properties, accumulators and products
% internal variables have a wordlength of 32 bits, with 20 bits for their fractional part,
% 11 bits integer part and 1 bit for sign
myglobalfimath(20);
fimath
%% Notice how it can take a long time to run fixed-point function
% without using emlmex to accelerate execution speed
% In my machine about 7 seconds for 1000 samples
delete *.mexw32
which kalman04
SIZE=1000;
Z=fi(rand(2,SIZE));
tic;
for i=1:SIZE
    z=Z(:,i);
    y=kalman04(z);
end;
toc;
%% Now you can accelerate the execution speed of fixed-point
% and in most cases even floating-point functionsby using the
% emlmex function of Fixed-Point Toolbox. It generates C code automatically
% from the MATLAB function and compiles it into MEX code.
% It will run faster. Only catch is that the kalman_fixpt.m
% must use the subset of MATLAB language allowing automatic
% MATLAB to C translation. The Embedded MATLAB subset
z = Z(:,1);
emlmex -eg {z} kalman04.m
%% Run the same function and notice the acceleration
% In my machine it run around 0.3 seconds a 20x acceleration
myglobalfimath(20);
which kalman04
SIZE=1000;
Z=fi(rand(2,SIZE));
tic;
for i=1:SIZE
    z=Z(:,i);
    y=kalman04(z);
end;
toc;
%% Now generate C source code from your fixed-point Kalman filter
% by uising the emlc command of the Real Time Workshop
% When you use the -report option it creates a link to an HTML report
% showing the automatically generated C code as kalman_fixpt.c
% Note that all variables are statically defined as native integers and
% all scaling functions are defined before the main C function
emlc -eg {z} kalman04.m -c -T RTW -report
%% Clean up
delete *.mexw32
clear mex


function [b b0] = circ_skewness(alpha, w, dim)

% [b b0] = circ_skewness(alpha,w,dim)
%   Calculates a measure of angular skewness.
%
%   Input:
%     alpha     sample of angles
%     [w        weightings in case of binned angle data]
%     [dim      statistic computed along this dimension, 1]
%
%     If dim argument is specified, all other optional arguments can be
%     left empty: circ_skewness(alpha, [], dim)
%
%   Output:
%     b         skewness (from Pewsey)
%     b0        alternative skewness measure (from Fisher)
%
%   References:
%     Pewsey, Metrika, 2004
%     Statistical analysis of circular data, Fisher, p. 34
%
% Circular Statistics Toolbox for Matlab

% By Philipp Berens, 2009
% berens@tuebingen.mpg.de

if nargin < 3
  dim = 1;
end

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

% compute neccessary values
R = circ_r(alpha,w,[],dim);
theta = circ_mean(alpha,w,dim);
[~, rho2 mu2] = circ_moment(alpha,w,2,true,dim);

% compute skewness
theta2 = repmat(theta, size(alpha)./size(theta));
b = sum(w.*(sin(2*(circ_dist(alpha,theta2)))),dim)./sum(w,dim);
b0 = rho2.*sin(circ_dist(mu2,2*theta))./(1-R).^(3/2);    % (formula 2.29)


function q=axisAng2Quat(u,theta)
%%AXISANG2QUAT Get a unit quaternion representative of a rotation of a 3D
%              vector an angle of theta counterlockwise (right-handed) or
%              clockwise (left-handed) about an axis given by u. The
%              handedness of theta matches the handedness of the
%              quaternion produced.
%
%INPUTS: u A unit vector representing the axis about which a vector should
%          be rotated using the rotation matrix.
%    theta The angle in radians by which a vector is to be rotated about
%          the axis u when multiplied by the rotation matrix. The
%          handedness of the rotation angle matches the handedness of the
%          quaternion produced.
%
%OUTPUTS: q A 4X1 unit quaternion corresponding to the supplied axis and
%           angle. The handedness of the quaternion (which is important to
%           know when using the quaternion with other functions, such as
%           quatMult) is the same as that of the angle. The quaternion is
%           ordered in terms of hypercomplex numbers as
%           q(1)+i*q(2)+j*q(3)+k*q(4).
%
%Quaternions and rotations are discussed in [1].
%
%REFERENCES:
%[1] M. D. Shuster, "A survey of attitude representations," The Journal of
%    Astronautical Sciences, vol. 41, no. 4, pp. 439-517, Oct. -Dec. 1993.
%    wherein the convention for the quaternions is left-handed.
%
%August 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

q=[cos(theta/2);sin(theta/2)*u];

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 dX = dynamics(X,P)
% dX = dynamics(X,P)
%
% This function computes the first-order dynamics for the classical
% "three-body" problem in newtonian mechanics: three point masses with
% inverse-square gravity.
%
% The input state contains only information about the first two particles.
% The state of the third particle is computed from the assumption (without
% loss of generality) that the center of mass of the system is at the
% origin and not moving.
%
% INPUTS:
%   X = [12, N] = first order state vector
%       X(1:3,:) = X1 = [x1;y1;z1]
%       X(4:6,:) = X2 = [x2;y2;z2]
%       X(7:9,:) = V1 = [dx1;dy1;dz1]
%       X(10:12,:) = V2 = [dx2;dy2;dz2]
%   P = parameter struct:
%       .G = gravity constant
%       .m1 = mass one
%       .m2 = mass two
%       .m3 = mass three
%
% OUTPUTS:
%   dX = [V1;V2; dV1; dV2];
%
%

% Positions
X1 = X(1:3,:);
X2 = X(4:6,:);

% Velocity
V1 = X(7:9,:);
V2 = X(10:12,:);

% Compute X3 from definition that CoM is at the origin
%   x = (x1*m1 + x2*m2 + x3*m3)/(m1 + m2 + m3);
X3 = -(X1*P.m1 + X2*P.m2)/P.m3;
% V3 = -(V1*P.m1 + V2*P.m2)/P.m3;   %Unused, but good to know

% Force Vectors
F12 = getForce(X1, X2, P.m1, P.m2, P.G);  % Acting on 1 from 2
F23 = getForce(X2, X3, P.m2, P.m3, P.G);  % Acting on 2 from 3
F13 = getForce(X1, X3, P.m1, P.m3, P.G);  % Acting on 1 from 3

% Acceleration Vectors
A1 = (F12 + F13)/P.m1;
A2 = (-F12 + F23)/P.m2;
% A3 = (-F23 - F13)/P.m3;   %Unused, but good to know

% Pack up solution:
dX = [V1; V2; A1; A2];

end

function F = getForce(Xa, Xb, ma, mb, G)
%
% Computes the force acting on particle a due to particle b, given
% parameters in the struct P
%

r = Xa - Xb;    %Vector from b to a
r2 = ones(3,1)*sum(r.^2,1);   % Length squared

lFl = G*ma*mb./r2;  %Force magnitude

F = -lFl .* (r./sqrt(r2));   %Force vector

end


% Minimize thermal noise power of an array with arbitrary 2-D geometry
% "Convex optimization examples" lecture notes (EE364) by S. Boyd
% "Antenna array pattern synthesis via convex optimization"
% by H. Lebret and S. Boyd
% (figures are generated)
%
% Designs an antenna array such that:
% - it has unit a sensitivity at some target direction
% - obeys constraint for minimum sidelobe level outside the beamwidth
% - minimizes thermal noise power in y (sigma*||w||_2^2)
%
% This is a convex problem described as:
%
%   minimize   norm(w)
%       s.t.   y(theta_tar) = 1
%              |y(theta)| <= min_sidelobe   for theta outside the beam
%
% where y is the antenna array gain pattern (complex function) and
% variables are w (antenna array weights or shading coefficients).
% Gain pattern is a linear function of w: y(theta) = w'*a(theta)
% for some a(theta) describing antenna array configuration and specs.
%
% Written for CVX by Almir Mutapcic 02/02/06

% select array geometry
ARRAY_GEOMETRY = '2D_RANDOM';
% ARRAY_GEOMETRY = '1D_UNIFORM_LINE';
% ARRAY_GEOMETRY = '2D_UNIFORM_LATTICE';

%********************************************************************
% problem specs
%********************************************************************
lambda = 1;           % wavelength
theta_tar = 60;       % target direction
half_beamwidth = 10;  % half beamwidth around the target direction
min_sidelobe = -20;   % maximum sidelobe level in dB

%********************************************************************
% random array of n antenna elements
%********************************************************************
if strcmp( ARRAY_GEOMETRY, '2D_RANDOM' )
  % set random seed to repeat experiments
  rand('state',0);

  % (uniformly distributed on [0,L]-by-[0,L] square)
  n = 36;
  L = 5;
  loc = L*rand(n,2);

%********************************************************************
% uniform 1D array with n elements with inter-element spacing d
%********************************************************************
elseif strcmp( ARRAY_GEOMETRY, '1D_UNIFORM_LINE' )
  % (unifrom array on a line)
  n = 30;
  d = 0.45*lambda;
  loc = [d*[0:n-1]' zeros(n,1)];

%********************************************************************
% uniform 2D array with m-by-m element with d spacing
%********************************************************************
elseif strcmp( ARRAY_GEOMETRY, '2D_UNIFORM_LATTICE' )
  m = 6; n = m^2;
  d = 0.45*lambda;

  loc = zeros(n,2);
  for x = 0:m-1
    for y = 0:m-1
      loc(m*y+x+1,:) = [x y];
    end
  end
  loc = loc*d;

else
  error('Undefined array geometry')
end

%********************************************************************
% construct optimization data
%********************************************************************
% build matrix A that relates w and y(theta), ie, y = A*w
theta = [1:360]';
A = kron(cos(pi*theta/180), loc(:,1)') + kron(sin(pi*theta/180), loc(:,2)');
A = exp(2*pi*i/lambda*A);

% target constraint matrix
[diff_closest, ind_closest] = min( abs(theta - theta_tar) );
Atar = A(ind_closest,:);

% stopband constraint matrix
ind = find(theta <= (theta_tar-half_beamwidth) | ...
           theta >= (theta_tar+half_beamwidth) );
As = A(ind,:);

%********************************************************************
% optimization problem
%********************************************************************
cvx_begin
  variable w(n) complex
  minimize( norm( w ) )
  subject to
    Atar*w == 1;
    abs(As*w) <= 10^(min_sidelobe/20);
cvx_end

% check if problem was successfully solved
disp(['Problem is ' cvx_status])
if ~strfind(cvx_status,'Solved')
  return
end

fprintf(1,'The minimum norm of w is %3.2f.\n\n',norm(w));

%********************************************************************
% plots
%********************************************************************
figure(1), clf
plot(loc(:,1),loc(:,2),'o')
title('Antenna locations')

% plot array pattern
y = A*w;

figure(2), clf
ymin = -30; ymax = 0;
plot([1:360], 20*log10(abs(y)), ...
     [theta_tar theta_tar],[ymin ymax],'r--',...
     [theta_tar+half_beamwidth theta_tar+half_beamwidth],[ymin ymax],'g--',...
     [theta_tar-half_beamwidth theta_tar-half_beamwidth],[ymin ymax],'g--',...
     [0 theta_tar-half_beamwidth],[min_sidelobe min_sidelobe],'r--',...
     [theta_tar+half_beamwidth 360],[min_sidelobe min_sidelobe],'r--');
xlabel('look angle'), ylabel('mag y(theta) in dB');
axis([0 360 ymin ymax]);

% polar plot
figure(3), clf
zerodB = 50;
dBY = 20*log10(abs(y)) + zerodB;
plot(dBY.*cos(pi*theta/180), dBY.*sin(pi*theta/180), '-');
axis([-zerodB zerodB -zerodB zerodB]), axis('off'), axis('square')
hold on
plot(zerodB*cos(pi*theta/180),zerodB*sin(pi*theta/180),'k:') % 0 dB
plot( (min_sidelobe + zerodB)*cos(pi*theta/180), ...
      (min_sidelobe + zerodB)*sin(pi*theta/180),'k:')  % min level
text(-zerodB,0,'0 dB')
text(-(min_sidelobe + zerodB),0,sprintf('%0.1f dB',min_sidelobe));
theta_1 = theta_tar+half_beamwidth;
theta_2 = theta_tar-half_beamwidth;
plot([0 55*cos(theta_tar*pi/180)], [0 55*sin(theta_tar*pi/180)], 'k:')
plot([0 55*cos(theta_1*pi/180)], [0 55*sin(theta_1*pi/180)], 'k:')
plot([0 55*cos(theta_2*pi/180)], [0 55*sin(theta_2*pi/180)], 'k:')
hold off


function [ cur_group ] = kmeans_diy( input_data, k )
n = size(input_data, 1);
d = size(input_data, 2);
% initialise means from random samples
means = input_data(randsample(n, k), :);
% main loop
newmeans = zeros(k,d);
scores = zeros(n, k);
change = Inf;
while change > eps
    % assign labels to data points according to distances to the means
    for j = 1:k
        diff = bsxfun(@minus, input_data, means(j,:));
        scores(:,j) = sqrt(sum(diff.^2,2));
    end
    [~,cur_group] = min(scores, [], 2);
    % recompute means based on obtained labelings
    for j = 1:k
        newmeans(j,:) = mean(input_data(cur_group == j,:));
    end
    % stopping criteria
    change = sqrt(sum(sum((newmeans-means).^2, 1), 2))/k;
    means = newmeans;
end
end


function [vals,coeffs]=asinDerivVal(z,n,coeffs)
%%ASINDERIVVAL Return the value of the nth derivative of asin(x) with
%              respect to x  evaluated at x=z for real or imaginary z.
%
%INPUTS: z A matrix of real or complex values at which the nth derivative
%          of the arcsine function is desired. If an empty matrix is
%          passed, then just coeffs will be returned.
%        n The number of derivatives to take. n>=0.
%   coeffs The computation of the derivatives involves determining the
%          structure of a polynomial. If this function has been run before
%          for a given n value, then the polynomial can be passed back and
%          is not determined again. Otherwise, this can be omitted or an
%          empty matrix can be passed. This only makes a difference for
%          n>=12.
%
%OUTPUTS: vals The value of the nth derivative of the arcsine function
%              taken at all of the points in z. vals has the same
%              dimensions as z.
%       coeffs A vector of polynomial coefficients that can be passed back
%              to this function when evaluating with the same n to speed it
%              up.
%
%For n=0, vals=asin(z). Subsequent derivatives are computed essentially
%using the chain rule and combining terms to get the appropriate
%polynomial. All derivatives are of the form
%polynomial*sqrt(1-z(:).^2).^(-k). For n<12, the coefficients are tabulated
%to make the function faster.
%
%May 2018 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(n==0)
    vals=asin(z);
    coeffs=[];
    return;
end

if(nargin<3||isempty(coeffs))
    if(n<12)
        switch(n)
            case 1
                coeffs=1;
            case 2
                coeffs=[1;0];
            case 3
                coeffs=[2;0;1];
            case 4
                coeffs=[6;0;9;0];
            case 5
                coeffs=[24;0;72;0;9];
            case 6
                coeffs=[120;0;600;0;225;0];
            case 7
                coeffs=[720;0;5400;0;4050;0;225];
            case 8
                coeffs=[5040;0;52920;0;66150;0;11025;0];
            case 9
                coeffs=[40320;0;564480;0;1058400;0;352800;0;11025];
            case 10
                coeffs=[362880;0;6531840;0;17146080;0;9525600;0;893025;0];
            case 11
                coeffs=[3628800;0;81648000;0;285768000;0;238140000;0;44651250;0;893025];
            otherwise
                error('Invalid n')
        end
        k=1+2*(n-1);
    else
        %The derivative of (1-x^2)^(-(k/2)) is
        %kx(1-x^2)^(-1-k/2)
        %That is k*x*(1-x^2)^(-(k1/2)) where k1=k+2.
        xVal=[1,0];

        %This polynomial is 1-x^2.
        polyX=[-1,0,1];

        %The first derivative
        n=n-1;
        coeffs=1;
        k=1;
        while(n>0)
            %Take the derivative of the denominator.
            coeffsNum=conv(k*xVal,coeffs);
            k=k+2;

            %Now, differentiate the numerator and multiply to get a
            %common denominator.
            coeffs=polyder(coeffs);
            coeffs=conv(coeffs,polyX);
            coeffs=polySum(coeffs,coeffsNum);
            n=n-1;
        end
    end
else
    k=1+2*(n-1);
end

if(~isempty(z))
    vals=zeros(size(z));
    %Evaluate the result.
    vals(:)=polyval(coeffs,z(:)).*sqrt(1-z(:).^2).^(-k);
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 value = i4_choose ( n, k )

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

  if ( mn < 0 )

    value = 0;

  elseif ( mn == 0 )

    value = 1;

  else

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

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

  end

  return
end


function cdf = genlogistic_cdf ( x, a, b, c )

%*****************************************************************************80
%
%% GENLOGISTIC_CDF evaluates the Generalized Logistic CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument of the CDF.
%
%    Input, real A, B, C, the parameters of the PDF.
%    0.0 < B,
%    0.0 < C.
%
%    Output, real CDF, the value of the CDF.
%
  y = ( x - a ) / b;

  cdf = 1.0 / ( 1.0 + exp ( - y ) )^c;

  return
end


%%***********************************************************************
%% gencorrmat2: generate correlation matrix
%%
%%  B = gencorrmat(n,options);
%%
%%  options = 'Randcorr'
%%          = 'Randcorr2'
%%          = 'Diag'
%%          = 'AR1'
%%          = 'CompSym'
%%          = 'Rand'
%%***********************************************************************
%% SDPNAL+
%% Copyright (c) 2014 by
%% Liuqin Yang, Defeng Sun, and Kim-Chuan Toh
%%***********************************************************************

  function B = gencorrmat(n,options,alpha)

   rng('default')
   if nargin == 2; alpha = 0; end

   if strcmp(options,'Randcorr') || (options == 1)
      beta = 10^(-4/(n-1));
      xx = beta.^[0:n-1];
      xx = n*xx/sum(xx);
      B = gallery('randcorr',xx);
   elseif strcmp(options,'Randcorr2') || (options == 2)
      n2 = n/2;
      beta = 10^(-4/(n-1));
      xx = [beta.^[0:n2-1], zeros(1,n2)];
      xx = n*xx/sum(xx);
      B = gallery('randcorr',xx);
   elseif strcmp(options,'Diag') || (options == 3)
      n2 = n/2;
      tmp = [ones(n2), zeros(n2); zeros(n2), eye(n2)];
      B = diag(1e4*(2*rand(n,1)-1));
   elseif strcmp(options,'AR1') || (options == 4)
      tmp = -diag(ones(n,1)) - 0.8*diag(ones(n-1,1),-1);
      Dinv = (1/0.01)*speye(n,n);
      Binv = tmp'*Dinv*tmp;
      B = inv(Binv); B = 0.5*(B+B');
      diagB = diag(B);
      B = diag(1./sqrt(diagB))*B*diag(1./sqrt(diagB));
   elseif strcmp(options,'CompSym') || (options == 5)
      sigma = 1; rho = 0.5;
      dd  = sigma^2*(1-[0:n-1]*(rho^2)./(1+[0:n-1]*rho));
      tmp = zeros(n,n);
      for j=2:n
         tmp(1:j-1,j) = rho./(1+[1:j-1]'*rho);
      end
      tmp = -eye(n) -tmp;
      Dinv = diag(1./dd);
      Binv = tmp'*Dinv*tmp;
      B = inv(Binv); B = 0.5*(B+B');
      diagB = diag(B);
      B = diag(1./sqrt(diagB))*B*diag(1./sqrt(diagB));
   elseif strcmp(options,'Rand') || (options == 6)
      B = 2*rand(n)-1;
      B = 0.5*(B+B');
   end
%%
%% add noise
%%
    E = 2*(rand(n)-0.5); E = triu(E,1)+ triu(E,1)';
    B = (1-alpha)*B + alpha*E;
    B = min(B,1);
    B = max(-1,B);
    for i=1:n; B(i,i) = 1; end
%%****************************************************


function c = correlation_pentaspherical ( n, rho, rho0 )

%*****************************************************************************80
%
%% CORRELATION_PENTASPHERICAL evaluates the pentaspherical correlation function.
%
%  Discussion:
%
%    This correlation is based on the volume of overlap of two spheres
%    of radius RHO0 and separation RHO.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Petter Abrahamsen,
%    A Review of Gaussian Random Fields and Correlation Functions,
%    Norwegian Computing Center, 1997.
%
%  Parameters:
%
%    Input, integer N, the number of arguments.
%
%    Input, real RHO(N,1), the arguments.
%
%    Input, real RHO0, the correlation length.
%
%    Output, real C(N,1), the correlations.
%
  rho = rho ( : );

  rhohat = min ( abs ( rho ) / rho0, 1.0 );

  c = 1.0 - 1.875 * rhohat + 1.25 * rhohat.^3 - 0.375 * rhohat.^5;

  return
end


function [ux,uy,uz]=LK3D( image1, image2, r )
%This function estimates deformations between two subsequent 3-D images
%using Lucas-Kanade optical flow equation.
%
%   Description :
%
%   -image1, image2 :   two subsequent images or frames
%   -r : radius of the neighbourhood, default value is 2.
%
%   Reference :
%   Lucas, B. D., Kanade, T., 1981. An iterative image registration
%   technique with an application to stereo vision. In: Proceedings of the
%   7th international joint conference on Artificial intelligence - Volume 2.
%   Morgan Kaufmann Publishers Inc., San Francisco, CA, USA, pp. 674-679.
%
%   Author : Mohammad Mustafa
%   By courtesy of The University of Nottingham and Mirada Medical Limited,
%   Oxford, UK
%
%   Published under a Creative Commons Attribution-Non-Commercial-Share Alike
%   3.0 Unported Licence http://creativecommons.org/licenses/by-nc-sa/3.0/
%
%   June 2012

%  Default parameter
if nargin==2
    r=2;
end

[height,width,depth]=size(image1);
image1=im2double(image1);
image2=im2double(image2);

% Initializing flow vectors
ux=zeros(size(image1)); uy=ux; uz=ux;

% Computing image derivatives
[Ix,Iy,Iz,It]=imageDerivatives3D(image1,image2);

for i=(r+1):(height-r)
    for j=(r+1):(width-r)
        for k=(r+1):(depth-r)

        blockofIx=Ix(i-r:i+r,j-r:j+r,k-r:k+r);
        blockofIy=Iy(i-r:i+r,j-r:j+r,k-r:k+r);
        blockofIz=Iz(i-r:i+r,j-r:j+r,k-r:k+r);
        blockofIt=It(i-r:i+r,j-r:j+r,k-r:k+r);

        A=zeros(3,3);
        B=zeros(3,1);

        A(1,1)=sum(sum(sum(blockofIx.^2)));
        A(1,2)=sum(sum(sum(blockofIx.*blockofIy)));
        A(1,3)=sum(sum(sum(blockofIx.*blockofIz)));

        A(2,1)=sum(sum(sum(blockofIy.*blockofIx)));
        A(2,2)=sum(sum(sum(blockofIy.^2)));
        A(2,3)=sum(sum(sum(blockofIy.*blockofIz)));

        A(3,1)=sum(sum(sum(blockofIz.*blockofIx)));
        A(3,2)=sum(sum(sum(blockofIz.*blockofIy)));
        A(3,3)=sum(sum(sum(blockofIz.^2)));

        B(1,1)=sum(sum(sum(blockofIx.*blockofIt)));
        B(2,1)=sum(sum(sum(blockofIy.*blockofIt)));
        B(3,1)=sum(sum(sum(blockofIz.*blockofIt)));

        invofA=pinv(A);

        V=invofA*(-B);
        ux(i,j,k)=V(1,1);
        uy(i,j,k)=V(2,1);
        uz(i,j,k)=V(3,1);
        end
    end
end

end


function dist_signed = line_exp_point_dist_signed_2d ( p1, p2, p )

%*****************************************************************************80
%
%% LINE_EXP_POINT_DIST_SIGNED_2D: signed distance ( explicit line, point ) in 2D.
%
%  Discussion:
%
%    The explicit form of a line in 2D is:
%
%      ( P1, P2 ) = ( (X1,Y1), (X2,Y2) ).
%
%    The signed distance has two interesting properties:
%
%    *  The absolute value of the signed distance is the
%        usual (Euclidean) distance.
%
%    *  Points with signed distance 0 lie on the line,
%       points with a negative signed distance lie on one side
%         of the line,
%       points with a positive signed distance lie on the
%         other side of the line.
%
%    Assuming that C is nonnegative, then if a point is a positive
%    distance away from the line, it is on the same side of the
%    line as the point (0,0), and if it is a negative distance
%    from the line, it is on the opposite side from (0,0).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(2,1), P2(2,1), two points on the line.
%
%    Input, real P(2,1), the point whose signed distance is desired.
%
%    Output, real DIST_SIGNED, the signed distance from the
%    point to the line.
%

%
%  If the explicit line degenerates to a point, the computation is easy.
%
  if ( p1(1:2,1) == p2(1:2,1) )

    dist_signed = sqrt ( sum ( ( p1(1:2,1) - p(1:2,1) ).^2 ) );
%
%  Convert the explicit line to the implicit form A * X + B * Y + C = 0.
%  This makes the computation of the signed distance  to (X,Y) easy.
%
  else

    a = p2(2,1) - p1(2,1);
    b = p1(1,1) - p2(1,1);
    c = p2(1,1) * p1(2,1) - p1(1,1) * p2(2,1);

    dist_signed = ( a * p(1,1) + b * p(2,1) + c ) / sqrt ( a * a + b * b );

  end

  return
end


clear all, close all, clc
A = imread('../../CH01_SVD/DATA/dog.jpg');
B = rgb2gray(A);

for j=1:size(B,1);
    Cshift(j,:) = fftshift(fft(B(j,:)));
    C(j,:) = (fft(B(j,:)));
end

for j=1:size(C,2);
    D(:,j) = fft(C(:,j));
end

subplot(1,3,1)
imagesc(B);
subplot(1,3,2)
imagesc(log(abs(Cshift)))
subplot(1,3,3)
imagesc(fftshift(log(abs(D))))
colormap gray
%%
figure
imagesc(B)
colormap gray
axis off
axis tight
set(gcf,'Position',[100 100 600 800])
set(gcf,'PaperPositionMode','auto')
print('-depsc2', '-loose', '../figures/2DFFTa');

figure
imagesc(log(abs(Cshift)))
colormap gray

axis off
axis tight
set(gcf,'Position',[100 100 600 800])
set(gcf,'PaperPositionMode','auto')
print('-depsc2', '-loose', '../figures/2DFFTb');
figure
imagesc(fftshift(log(abs(D))))
colormap gray
axis off
axis tight
set(gcf,'Position',[100 100 600 800])
set(gcf,'PaperPositionMode','auto')
print('-depsc2', '-loose', '../figures/2DFFTc');

%% FFT Compression
figure
Bt=fft2(B);
F = log(abs(fftshift(Bt))+1);  % put FFT on log-scale

% Zero out all small coefficients and inverse transform
Btsort = sort(abs(Bt(:)));
keepvec = [.1 .05 .01 .005 .002 .001];
for k=4:6
    keep = keepvec(k);
    % keep = 0.05;
    thresh = Btsort(floor((1-keep)*length(Btsort)));
    ind = abs(Bt)>thresh;
    Atlow = Bt.*ind;
    Flow = log(abs(fftshift(Atlow))+1);  % put FFT on log-scale
    imshow(mat2gray(Flow),[]);

    % Plot Reconstruction
    Alow=uint8(ifft2(Atlow));
    imshow(Alow)

    axis off
    set(gcf,'PaperPositionMode','auto')
    %     print('-depsc2', '-loose', ['../figures/2DFFT_Compress',num2str(k)]);
    print('-dpng', '-loose', ['../figures/2DFFT_Compress',num2str(k)]);

end

%% Denoise
B = Bold(2:2:end,2:2:end);
ind = rand(size(B));
Bnoise = B + uint8(200*randn(size(B)));
% Bnoise = B + uint8(200*rand(size(B)));
% Bnoise = B.*uint8(ind<.2);
imagesc(Bnoise)
colormap gray
axis off
axis tight
set(gcf,'Position',[100 100 600 800])
set(gcf,'PaperPositionMode','auto')
% print('-dpng', '-loose', '../figures/2DFFT_Noise1');

Bt=fft2(Bnoise);
Btshift = fftshift(Bt);
[nx,ny] = size(B);

figure
F = log(abs(Btshift)+1);  % put FFT on log-scale
imagesc(F)
colormap gray
axis off
axis tight
set(gcf,'Position',[100 100 600 800])
set(gcf,'PaperPositionMode','auto')
% print('-dpng', '-loose', '../figures/2DFFT_Noise2');

[X,Y] = meshgrid(-ny/2+1:ny/2,-nx/2+1:nx/2);
R2 = X.^2+Y.^2;
ind = R2<150^2;
Btshiftfilt = Btshift.*ind;

figure
Ffilt = log(abs(Btshiftfilt)+1);  % put FFT on log-scale
imagesc(Ffilt)
colormap gray
axis off
axis tight
set(gcf,'Position',[100 100 600 800])
set(gcf,'PaperPositionMode','auto')
% print('-dpng', '-loose', '../figures/2DFFT_Noise3');

figure
Btfilt = ifftshift(Btshiftfilt);
Bfilt = ifft2(Btfilt);
imagesc(uint8(Bfilt))
colormap gray
axis off
axis tight
set(gcf,'Position',[100 100 600 800])
set(gcf,'PaperPositionMode','auto')
% print('-dpng', '-loose', '../figures/2DFFT_Noise4');

%% Wavelet Compression
figure
[C,S] = wavedec2(B,4,'db1');

% Zero out all small coefficients and inverse transform
Csort = sort(abs(C(:)));
keepvec = [.1 .05 .01 .005 .002 .001];
for k=1:6
    figure
    keep = keepvec(k);
    % keep = 0.05;
    thresh = Csort(floor((1-keep)*length(Csort)));
    ind = abs(C)>thresh;
    Cfilt = C.*ind;

    % Plot Reconstruction
    Alow=uint8(waverec2(Cfilt,S,'db1'));
    imagesc(uint8(Alow))
    colormap gray

    colormap gray
    axis off
    axis tight
    set(gcf,'Position',[100 100 600 800])
    set(gcf,'PaperPositionMode','auto')
    %     print('-depsc2', '-loose', ['../figures/2DFFT_Compress',num2str(k)]);
        print('-dpng', '-loose', ['../figures/2DWavelet_Compress',num2str(k)]);

end


function a = circulant ( m, n, x )

%*****************************************************************************80
%
%% CIRCULANT returns the CIRCULANT matrix.
%
%  Formula:
%
%    K = 1 + mod ( J-I, N )
%    A(I,J) = X(K)
%
%  Example:
%
%    M = 4, N = 4, X = ( 1, 2, 3, 4 )
%
%    1  2  3  4
%    4  1  2  3
%    3  4  1  2
%    2  3  4  1
%
%    M = 4, N = 5, X = ( 1, 2, 3, 4, 5 )
%
%    1  2  3  4  5
%    5  1  2  3  4
%    4  5  1  2  3
%    3  4  5  1  2
%
%    M = 5, N = 4, X = ( 1, 2, 3, 4 )
%
%    1  2  3  4
%    5  1  2  3
%    4  5  1  2
%    3  4  5  1
%    1  2  3  4
%
%  Discussion:
%
%    Westlake lists the following "special" circulants:
%
%      B2, X = ( T^2, 1, 2, ..., T, T+1, T, T-1, ..., 1 ),
%      with T = ( N - 2 ) / 2;
%
%      B3, X = ( N+1, 1, 1, ..., 1 );
%
%      B5, X = ( 1, 2, 3, ..., N ).
%
%  Rectangular Properties:
%
%    The product of two circulant matrices is a circulant matrix.
%
%    The transpose of a circulant matrix is a circulant matrix.
%
%    A circulant matrix C, whose first row is (c1, c2, ..., cn), can be
%    written as a polynomial in the upshift matrix U:
%
%      C = c1 * I + c2 * U + c3 * U**2 + ... + cn * U**n-1.
%
%    A is a circulant: each row is shifted once to get the next row.
%
%  Square Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    A is Toeplitz: constant along diagonals.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    A commutes with any other circulant matrix.
%
%    A is normal.
%
%    The transpose of A is also a circulant matrix.
%
%    The inverse of A is also a circulant matrix.
%
%    The Fourier matrix is the eigenvector matrix for every circulant matrix.
%
%    Because the Fourier matrix F diagonalizes A, the inverse (or
%    pseudoinverse, if any LAMBDA is zero) can be written
%
%      inverse ( A ) = (F*) * 1/LAMBDA * F
%
%    A is symmetric if, for all I, X(I+1) = X(N-I+1).
%
%    If R is an N-th root of unity, that is, R is a complex number such
%    that R**N = 1, then
%
%      Y = X(1) + X(2)*R + X(3)*R**2 + ... + X(N)*R**(N-1)
%
%    is an eigenvalue of A, with eigenvector
%
%     ( 1, R, R**2, ..., R**(N-1) )
%
%    and left eigenvector
%
%      ( R**(N-1), R**(N-2), ..., R**2, R, 1 ).
%
%    Although there are exactly N distinct roots of unity, the circulant
%    may have repeated eigenvalues, because of the behavior of the polynomial.
%    However, the matrix is guaranteed to have N linearly independent
%    eigenvectors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis,
%    Circulant Matrices,
%    John Wiley, 1979, QA188.D37.
%
%    Robert Gregory, David Karney,
%    A Collection of Matrices for Testing Computational Algorithms,
%    Wiley, New York, 1969, page 22,
%    LC: QA263.G68.
%
%    Joan Westlake,
%    Test Matrix A24,
%    A Handbook of Numerical Matrix Inversion and Solution of Linear Equations,
%    John Wiley, 1968.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of A.
%
%    Input, real X(N), the values in the first row of A.
%
%    Output, real A(M,N), the matrix.
%
  for i = 1 : m
    for j = 1 : n

      k = 1 + i4_modp ( j-i, n );
      a(i,j) = x(k);

    end
  end

  return
end


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.
    %

    delta = (1 / m * (X * theta - y)' * X)';

    theta = theta - alpha * delta;

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

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

end

end


function [ res, jac ] = opt01_rj ( x, flag )

%*****************************************************************************80
%
%% OPT01_RJ evaluates RES and JAC for test case #1.
%
%  Discussion:
%
%    This example is discussed in Dennis and Schnabel, pages 100, 104 and 202.
%
%    Suggested starting value for X is ( 1, 1 ).
%
%    The optimizing value is
%
%      X* = ( 2, -1 ),
%
%    for which
%
%      RES(X*) = (0,0,0).
%
%  Modified:
%
%    05 January 2008
%
%  Author:
%
%    Jeff Borggaard,
%    Gene Cliff,
%    Virginia Tech.
%
%  Reference:
%
%    John Dennis, Robert Schnabel,
%    Numerical Methods for Unconstrained Optimization
%    and Nonlinear Equations,
%    SIAM, 1996,
%    ISBN13: 978-0-898713-64-0,
%    LC: QA402.5.D44.
%
%  Parameters:
%
%    Input, real X(2), the evaluation point.
%
%    Input, string FLAG, indicates what must be computed.
%    'f' means only the value of RES is needed,
%    'g' means only the value of JAC is needed,
%    'all' means RES and JAC are needed.
%    It is acceptable to behave as though FLAG was 'all'
%    on every call.
%
%    Output, real RES(3), the residual column vector.
%
%    Output, real JAC(3,2), the Jacobian matrix.
%
  n = length ( x );

  if ( n ~= 2 )
    fprintf ( '\n' );
    fprintf ( 'OPT01_RJ - Fatal error!\n' );
    fprintf ( '  The input vector X should have length 2.\n'),
    fprintf ( '  Instead, it has length = %d.\n', n );
    keyboard
  end
%
%  Formulated as a nonlinear system:
%
  res(1,1) = ( x(1) - 2 )^2;
  res(2,1) = ( x(1) - 2 ) * x(2);
  res(3,1) = ( x(2) + 1);

  jac(1,1) = 2 * ( x(1) - 2 );
  jac(1,2) = 0;

  jac(2,1) = x(2);
  jac(2,2) = x(1) - 2;

  jac(3,1) = 0;
  jac(3,2) = 1;

return


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.
    %

h=theta'*X';

delta=1/m*(X'*X*theta-X'*y);

theta=theta-alpha.*delta;

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

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

end

end


function [ytrend,ycycle]=one_sided_hpfilter_serial(y,lambda,discard)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% I am grateful to Martin Kliem for pointing out this alternative to the
% Kalman variant to me and for sharing his code with me.
%
% Copyright: Alexander Meyer-Gohde
%
% You are free to use/modify/redistribute this program so long as original
% authorship credit is given and you in no way impinge on its free
% distribution
%
%
% This program executes a one-sided HP filter by running the
% standard two-sided HP filter successively with each new observation
%
%  Input: y - a Txn data matrix, where T is the number of observations
%            on n variables (i.e.,data is assumed to be in column format).
%         lambda - a smoothing scalar. If not entered, default is 1600.
%         discard  -  a scalar. The first discard periods will be
%            discarded resulting in output matrices of size
%            (T-discard)xn. Optional: if not entered, default value is 0.
%
% Output: ytrend - a (T-discard)xn matrix of extracted trends for
%                  each of the n variables.
%         ycycle a  (T-discard)xn matrix of deviations from the trends
%                     for  each of the n variables. Optional.
%
%       Usage example:
%       [ytrend]=one_sided_hp_filter_kalman(y)
%                   will yield the Txn matrix of trends using the data in
%                   y with lambda set to 1600
%
%
% This one-side HP filter finds a series {ytrend_t}_{t=1}^T for
% each n by calculating for each t the standard HP filtered trend using
% all data upto that t and setting ytrend_t equal to the resulting trend
% value for period t.
%
% See: Mehra, Y.P. (2004). "The Output Gap, Expected Future Inflation and
%                        Inflation Dynamics: Another Look,"
%           The B.E. Journal of Macroeconomics, Berkeley Electronic Press.
%
% The standard HP filter finds a series {ytrend_t}_{t=1}^T that solves
% the following minimization probelm
%
% min sum_{t=1}^T(y_t-ytrend_t)
% +lambda*sum_{t=2}^{T-1}[(ytrend_{t+1}-ytrend_{t})-(ytrend_{t}-ytrend_{t-1})]
%
% Rearanging the first order conditions yields:  A*ytrend=y   where
% A=[   1+lambda    -2*lambda       lambda        0               0           0 ...
%     [    -2*lambda   1+5*lambda   -4*lambda       lambda       0           0 ...
%     [    lambda       -4*lambda        1+6*lambda   -4*lambda  lambda  0 ...
%     [                                         ...
%     [    0      ...   0   lambda       -4*lambda        1+6*lambda   -4*lambda  lambda 0  ... 0]
%     [                                         ...      ]
%     [  0   ...    lambda       -4*lambda        1+6*lambda  -4*lambda  lambda]
%     [  0   ...           0     lambda  -4*lambda       1+5*lambda      -2*lambda ]
%     [  0   ...           0       0        lambda              -2*lambda    1+lambda]
%
%
%   Hodrick, R.J. and E.C.Prescott (1997), "Postwar U.S. Business Cycles:
%   An Empirical Investigation." Jounal of Money, Credit and Banking.
%   29(1), Feb. pp. 1--16.
%
%  Note that filter will be calculated over and over again with an
%  expanding matrix A. This program uses sparse matrices and exploits the
%  patter in A as t progresses.
%

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

if nargin < 2,  lambda = 1600; end%If the user does not provide lambda, set it to 1600
[T,n] = size (y); % Calculate the number of periods and the number of variables in the series

%%Preliminary calculations
x1=[1+lambda, -2*lambda, lambda]; %The non-zero elements of the first row of A
x2=[ -2*lambda, 1+5*lambda, -4*lambda, lambda];%The non-zero elements of the second row of A
x3=[lambda, -4*lambda, 1+6*lambda, -4*lambda, lambda];%The non-zero elements of thej'th row of A, 2<j<T-2
x2rev=x2(end:-1:1);%The non-zero elements of the second-to-last row of A (just x2 in reverse)
x1rev=x1(end:-1:1); %The non-zero elements of the last row of A (just x1 in reverse)

%The minimization problem for the HP filter is either trivially given by
%the following or is not defined when there are less than three
%observations
for t=1:2
    ytrend(t,:)=y(t,:); %Thus, the trend is set to the observation
end
t=t+1;
%For three observations, the minimization problem yields the following
junk=[x1;-2*lambda, 1+4*lambda, -2*lambda;x1rev]\y(1:t,:);%Build the matrix A and solve the system A*y_standard_hp_trend=y
ytrend(t,:)=junk(t,:);%select the t'th element of the resulting trend
%Starting with four observations, the pattern using the preliminary
%calculations begins
t=t+1;
Ibegin=[1; 1; 1; 2; 2; 2; 2];%make a list (a column vector) containing at position i the row of the i'th non-zero element in the first two rows of A
Jbegin=[1;2;3;1;2;3;4];%make a list (a column vector) containing at position i the column of the i'th non-zero element in the first two rows of A
Xbegin=[x1';x2'];%make a list (a column vector) containing at position i the i'th non-zero element in the first two rows of A
Iend=[3; 3; 3; 3; 4; 4; 4];%make a list (a column vector) containing at position i the row of the i'th non-zero element in the last two rows of A
Jend=[1;2;3;4;2;3;4];%make a list (a column vector) containing at position i the column of the i'th non-zero element in the last two rows of A
Xend=[x2rev';x1rev'];%make a list (a column vector) containing at position i the i'th non-zero element in the last two rows of A
junk=sparse([Ibegin; Iend],[Jbegin; Jend],[Xbegin; Xend])\y(1:t,:);
ytrend(t,:)=junk(t,:);
%For the fifth observation on up, the simple pattern can be exploited by
%adding the new row and column indicies of the new non-zero elements to the Ibegin and Jbegin as well as
%the values of the elements themselves. Note that this requires the
%row and column indicies of the non-zeor elements in the last two rows to
%be advanced
while t<T
    t=t+1;
    Ibegin=[Ibegin;t-2;t-2;t-2;t-2;t-2];%Add to the list the rows of the non-zero elements of the t-2'th row
    Jbegin=[Jbegin;t-4;t-3;t-2;t-1;t];%Add to the list the col.umns of the non-zero elements of the t-2'th row
    Xbegin=[Xbegin;x3'];%Add to the list the non-zero elements of the t-2'th row
    Iend=Iend+1;%Advance the rows of the non-zero elements in the last two rows
    Jend=Jend+1;%Advance the columns of the non-zero elements in the last two rows
    junk=sparse([Ibegin; Iend],[Jbegin; Jend],[Xbegin; Xend],t,t,(t-4)*5+14)\y(1:t,:);%Build the matrix A and solve the system A*y_standard_hp_trend=y
    ytrend(t,:)=junk(t,:);%select the t'th element of the resulting trend
end

if nargout==2%Should the user have requested a second output
    ycycle=y-ytrend;%The second output will be the deviations from the HP trend
end

if nargin==3%If the user provided a discard parameter
    ytrend=ytrend(discard+1:end,:);%Remove the first "discard" periods from the trend series
    if nargout==2%Should the user have requested a second output
        ycycle=ycycle(discard+1:end,:);%The second output will be the deviations from the HP trend, likewise with  first "discard" periods removed
    end
end
end


%%  Generate a set of data points from a plane
%Copyright (c) 2011, The MathWorks, Inc.

clear all
clc

[CleanX, CleanY] = meshgrid(1:10);
CleanX = reshape(CleanX,100,1);
CleanY = reshape(CleanY,100,1);
CleanZ = 3*CleanX + 4*CleanY;

scatter3(CleanX, CleanY, CleanZ, 'filled')
hold on
foo = fit([CleanX, CleanY], CleanZ, 'poly11')
plot(foo)

%% Add noise vectors to all three dimensions

% Note:  Normally, if we were going to show a regression example, we'd
% create Noisy_Z by adding a noise vector to CleanZ but leave CleanX and
% Clean Y as is.  Here, we're using a fitting technique that is designed to
% create a model where there is noise associated with both the dependent
% and the independent variables.

NoisyX = CleanX + randn(100,1);
NoisyY = CleanY + randn(100,1);
NoisyZ = CleanZ + randn(100,1);

% create a scatter plot

figure1 = figure;
scatter3(NoisyX,NoisyY,NoisyZ, '.k')

%%  Fit a plane to the data using OLS
[foo2, GoF] = fit([NoisyX NoisyY],NoisyZ, 'poly11');

% superimpose the plane on the scatter plot
hold on
h1 = plot(foo2)
set( h1, 'FaceColor', 'r' )

%% Calculate the sum of the Squared Errors

% Calculate the difference between the observed (NoisyX and NoisyY) and
% the "known" value for CleanZ

resid1 = CleanZ - foo(NoisyX, NoisyY);

% Square residuals
resid1_sqrd = resid1.^2;

% Take the sum of the squared residuals
SSE_OLS = sum(resid1_sqrd)

% Create textbox
annotation(figure1,'textbox',...
    [0.147443514644351 0.802739726027397 0.305903765690377 0.0931506849315069],...
    'String',{['SSE OLS = ' num2str(SSE_OLS)]},...
    'FitBoxToText','off',...
    'BackgroundColor',[1 1 1]);

%% Use Principal Component Analysis to perform an Orthogonal Regression

% PCA is based on centering and rotation.
% PCA rotates the data such that dimension with the greatest amount of
% variance is parallel to the X axis.  The dimension with the second
% largest amount of variance will be parallel to the Y axis.  This operation
% defines a plane.  The direction with the third largest variance will be
% parallel to the Z axis.  This dimension defines a set of residuals which
% are at right angles to the XY plane.

[coeff,score,roots] = princomp([NoisyX NoisyY NoisyZ]);

basis = coeff(:,1:2);
normal = coeff(:,3);
pctExplained = roots' ./ sum(roots)

% Translate the output from PCA back to the original coordinate system

[n,p] = size([NoisyX NoisyY NoisyZ]);
meanNoisy = mean([NoisyX NoisyY NoisyZ],1);
Predicted = repmat(meanNoisy,n,1) + score(:,1:2)*coeff(:,1:2)';

% Generate a fit object that represents the output from the Orthogonal Regression

[foo3, Gof2] = fit([Predicted(:,1) Predicted(:,2)], Predicted(:,3), 'poly11')

h2  = plot(foo3)
set( h2, 'FaceColor', 'b' )

%% Calculate the Sum of the Squared Errors for the Orthogonal Regression
% Calculate residuals
resid2 = CleanZ - Predicted(:,3);

% Square residuals
resid2_sqrd = resid2.^2;

% Take the sum of the squared residuals
SSE_TLS = sum(resid2_sqrd)

annotation(figure1,'textbox',...
    [0.147443514644351 0.802739726027397 0.305903765690377 0.0931506849315069],...
    'String',{['SSE OLS = ' num2str(SSE_OLS)], ['SSE TLS = ' num2str(SSE_TLS)]},...
    'FitBoxToText','off',...
    'BackgroundColor',[1 1 1]);


function [Bx,By,Bz] = magnetic_field_current_loop(x,y,z,x_p,y_p,z_p,a,I0)

%Kilian O'Donoghue

%30th July 2013

%This function calculates the magnetic field resulting from a single
%circular coil of radius a, carrying a currnet I0. The coil points in the z
%direction. The centre of the coil is located at {x_p,y_p,z_p} and the
%magnetic field is calculated at the point or points stored in the arrays
%x, y and z. This code accepts meshgrid inputs or single values along with
%vectors.

%These equations are based on those given in "General relation for the vector
%magnetic field of a circular current loop: a closer look" by Robert
%Schill, this is available from IEEE.

global u0 %permeability of free space is a global variable

rc=((x-x_p).^2+(y-y_p).^2).^.5; %Radial component is required for cylindrical coordinate system.
m=(4.*a.*rc).*(((rc+a).^2)+((z-z_p).^2)).^(-1); %This is a parameter for calculating the Elliptical integrals
kofkc=(pi/2)+(pi/8).*m+(9*pi/128).*m.^2; %K(k) elliptical function, this is a taylor expansion of the K elliptical integral.
eofkc=(pi/2)+(-pi/8).*m+(-3*pi/128).*m.^2;%E(k) elliptical function this is a taylor expansion of the E elliptical integral.

%Note for improved accuracy, Matlab has built in elliptical integral
%calculation but these expressions here are still very accurate when rc < a

Brc=(u0.*I0./(2.*pi.*rc)).*(z-z_p).*((((rc+a).^2)+((z-z_p).^2)).^(-.5)).*(-kofkc+eofkc.*((rc.^2+a.^2+(z-z_p).^2)./(((rc-a).^2)+((z-z_p).^2)))); %radial component of B%
Bz=(u0.*I0./(2.*pi)).*((((rc+a).^2)+((z-z_p).^2)).^(-.5)).*(kofkc-eofkc.*((rc.^2-a.^2+(z-z_p).^2)./(((rc-a).^2)+((z-z_p).^2)))); %axial component of B
Bx=Brc.*(x-x_p)./rc; %This converts the polar component into cartesian form.
By=Brc.*(y-y_p)./rc;

%The following sets any terms that result in Inf to zero, this occurs at
%the points near the coil itself.
Bx(isnan(Bx)) = 0 ;
By(isnan(By)) = 0 ;
Bz(isnan(Bz)) = 0 ;

github-repos/MATLAB/Sable-mcbench-benchmarks/mcbench-benchmarks-ba13b2f0296ef49491b95e3f984c7c41fccdb6d8/43394-magnetic-field-of-circular-coils-using-elliptical-integrals/Magnetic field of circular coil using elliptical integrals/magnetic_field_current_loop.m


function differ_test04 ( )

%*****************************************************************************80
%
%% DIFFER_TEST04 tests DIFFER_FORWARD, DIFFER_BACKWARD, DIFFER_CENTRAL.
%
%  Discussion:
%
%    Evaluate the coefficients for uniformly spaced finite difference
%    approximations of derivatives.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 November 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'DIFFER_TEST04\n' );
  fprintf ( 1, '  DIFFER_FORWARD,\n' );
  fprintf ( 1, '  DIFFER_BACKWARD, and\n' );
  fprintf ( 1, '  DIFFER_CENTRAL produce coefficients for difference\n' );
  fprintf ( 1, '  approximations of the O-th derivative, \n' );
  fprintf ( 1, '  with error of order H^P, for a uniform spacing of H.\n' );

  h = 1.0;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use a spacing of H = %g for all examples.\n', h );
%
%  Forward difference approximation to the third derivative with error of O(h).
%
  o = 3;
  p = 1;
  n = o + p;
  [ c, x ] = differ_forward ( h, o, p );
  label = sprintf ( '  Forward difference coefficients, O = %d, P = %d', o, p );
  r8vec2_print ( n, x, c, label );
%
%  Backward difference approximation to the third derivative with error of O(h).
%
  o = 3;
  p = 1;
  n = o + p;
  [ c, x ] = differ_backward ( h, o, p );
  label = sprintf ( '  Backward difference coefficients, O = %d, P = %d', o, p );
  r8vec2_print ( n, x, c, label );
%
%  Central difference approximation to the third derivative with error of O(h^2).
%
  o = 3;
  p = 2;
  n = o + p;
  [ c, x ] = differ_central ( h, o, p );
  label = sprintf ( '  Central difference coefficients, O = %d, P = %d', o, p );
  r8vec2_print ( n, x, c, label );
%
%  Central difference approximation to the third derivative with error of O(h^4).
%
  o = 3;
  p = 4;
  n = o + p;
  [ c, x ] = differ_central ( h, o, p );
  label = sprintf ( '  Central difference coefficients, O = %d, P = %d', o, p );
  r8vec2_print ( n, x, c, label );
%
%  Forward difference approximation to the fourth derivative with error of O(h).
%
  o = 4;
  p = 1;
  n = o + p;
  [ c, x ] = differ_forward ( h, o, p );
  label = sprintf ( '  Forward difference coefficients, O = %d, P = %d', o, p );
  r8vec2_print ( n, x, c, label );
%
%  Backward difference approximation to the fourth derivative with error of O(h).
%
  o = 4;
  p = 1;
  n = o + p;
  [ c, x ] = differ_backward ( h, o, p );
  label = sprintf ( '  Backward difference coefficients, O = %d, P = %d', o, p );
  r8vec2_print ( n, x, c, label );
%
%  Central difference approximation to the fourth derivative with error of O(h^3).
%
  o = 4;
  p = 3;
  n = o + p;
  [ c, x ] = differ_central ( h, o, p );
  label = sprintf ( '  Central difference coefficients, O = %d, P = %d', o, p );
  r8vec2_print ( n, x, c, label );

  return
end


function [Zica A T mu] = myICA(Z,r,dispFlag)
%--------------------------------------------------------------------------
% Syntax:       Zica = myICA(Z,r);
%               Zica = myICA(Z,r,dispFlag);
%               [Zica A T mu] = myICA(Z,r);
%               [Zica A T mu] = myICA(Z,r,dispFlag);
%
% Inputs:       Z is an (d x n) matrix containing n samples of an
%               d-dimensional random vector
%
%               r is the desired number of independent components
%
%               [OPTIONAL] dispFlag = {true false} sets the stdout print
%               state. The default value is dispFlag = true
%
% Outputs:      Zica is an (r x n) matrix containing the r independent
%               components - scaled to variance 1 - of the input samples
%
%               A and T are the ICA transformation matrices such that
%               Zr = T \ pinv(A) * Zica + repmat(mu,1,n);
%               is the r-dimensional ICA approximation of Z
%
%               mu is the (d x 1) sample mean of Z
%
% Description:  This function performs independent component analysis (ICA)
%               on the input samples using the FastICA algorithm with
%               Gaussian negentropy
%
% Author:       Brian Moore
%               brimoor@umich.edu
%
% Date:         April 26, 2015
%--------------------------------------------------------------------------

% Knobs
eps = 1e-6;         % Convergence criteria
maxSamples = 1000;  % Max # data points in sample mean calculation
maxIters = 100;     % Maximum # iterations

% Parse display flag
dispFlag = (nargin < 3) || dispFlag;

% Center and whiten input data
[Zcw T mu] = myCenterAndWhiten(Z);

% Parse whitened data
[d n] = size(Zcw);
if (n > maxSamples)
    % Truncate data for sample mean calculations
    Zcwt = Zcw(:,randperm(n,maxSamples));
else
    % Full data
    Zcwt = Zcw;
end

% Random initial weights
normRows = @(X) bsxfun(@times,X,1 ./ sqrt(sum(X.^2,2)));
W = normRows(rand(r,d));

% FastICA w/ Gaussian negentropy
k = 0;
err = inf;
while (err > eps) && (k < maxIters)
    % Increment counter
    k = k + 1;

    % Update weights
    Wlast = W; % Save last weights
    Sk = permute(Wlast * Zcwt,[1 3 2]);
    G = Sk .* exp(-0.5 * Sk.^2);
    Gp = Sk .* G;
    W = mean(bsxfun(@times,G,permute(Zcwt,[3 1 2])),3) + bsxfun(@times,mean(Gp,3),Wlast);
    W = normRows(W);

    % Decorrelate weights
    [U,S,~] = svd(W,'econ');
    W = U * diag(1 ./ diag(S)) * U' * W;

    % Update error
    err = max(1 - dot(W,Wlast,2));

    % Display progress
    if dispFlag == true
        sprintf('Iteration %i: max(1 - <w%i,w%i>) = %.4g\n',k,k,k - 1,err);
    end
end

% Transformation matrix
A = W;

% Independent components
Zica = A * Zcw;


function f = f12_f0 ( n, x, y )

%*****************************************************************************80
%
%% F12_F0 returns the value of function 12.
%
%  Discussion:
%
%    This is an example from Vicente Romero.
%
%      R = sqrt ( X^2 + Y^2 )
%      T = atan ( Y / X )
%      F(X,Y) = ( 0.8 * R + 0.35 * sin ( 2.4 * pi * R / sqrt ( 2 )  ) )
%               * 1.5 * sin ( 1.3 * T )
%
%    The mean and standard deviation of the function over the interval
%    are approximately:
%
%      mu    = 0.581608
%      sigma = 0.343208
%
%    Since the interpolation interval is the unit square, this means the
%    integral of the function over the interval can also be estimated as
%
%      I = 0.581608
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Vicente Romero, John Burkardt, Max Gunzburger, Janet Peterson,
%    Initial Evaluation of Centroidal Voronoi Tessellation for
%    Statistical Sampling and Function Integration,
%    Fourth International Symposium on Uncertainty Modeling and Analysis,
%    (ISUMA) 2003.
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N,1), Y(N,1), the evalution points.
%
%    Output, real F(N,1), the function values.
%
  r(1:n,1) = sqrt ( x(1:n,1).^2 + y(1:n,1).^2 );
  t(1:n,1) = atan2 ( y(1:n,1), x(1:n,1) );

  f(1:n,1) = 1.5 * ( 0.8 * r(1:n,1) ...
    + 0.35 * sin ( 2.4 * pi * r(1:n,1) / sqrt ( 2.0 ) ) ) ...
    .* sin ( 1.3 * t(1:n,1) );

  return
end


function w = moments_normal_01 ( m )

%*****************************************************************************80
%
%% MOMENTS_NORMAL_01 returns moments of the standard Normal distribution.
%
%  Discussion:
%
%    pdf(x) = exp ( -x^2/2 ) / sqrt ( pi * 2 )
%    mu(k) = integral ( -oo < x < +oo ) x^k pdf(x) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of moments desired.
%
%    Output, real W(M), the weighted integrals of X^0 through X^(M-1).
%
  w = zeros ( m, 1 );

  w(1) = 1.0;

  for k = 3 : 2: m
    w(k) = r8_factorial2 ( k - 2 );
  end

  for k = 2 : 2 : m
    w(k) = 0.0;
  end

  return
end


function pdf = dirichlet_pdf ( x, n, a )

%*****************************************************************************80
%
%% DIRICHLET_PDF evaluates the Dirichlet PDF.
%
%  Discussion:
%
%    PDF(X)(N,A) = Product ( 1 <= I <= N ) X(I)**( A(I) - 1 )
%      * Gamma ( A_SUM ) / A_PROD
%
%    where
%
%      0 < A(I) for all I;
%      0 <= X(I) for all I;
%      Sum ( 1 <= I <= N ) X(I) = 1;
%      A_SUM = Sum ( 1 <= I <= N ) A(I).
%      A_PROD = Product ( 1 <= I <= N ) Gamma ( A(I) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(N), the argument of the PDF.  Each X(I) should
%    be greater than 0.0, and the X(I)'s must add up to 1.0.
%
%    Input, integer N, the number of components.
%
%    Input, real A(N), the probabilities for each component.
%    Each A(I) should be positive.
%
%    Output, real PDF, the value of the PDF.
%
  tol = 0.0001;

  for i = 1 : n
    if ( x(i) <= 0.0 )
      fprintf ( 1, '\n' );
      fprintf ( 1,  'DIRICHLET_PDF - Fatal error!\n' );
      fprintf ( 1,  '  X(I) <= 0.\n' );
    end
  end

  x_sum = sum ( x(1:n) );

  if ( tol < abs ( x_sum - 1.0 ) )
    fprintf ( 1, '\n' );
    fprintf ( 1,  'DIRICHLET_PDF - Fatal error!\n' );
    fprintf ( 1,  '  SUM X(I) =/= 1.\n' );
  end

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

  a_prod = 1.0;
  for i = 1 : n
    a_prod = a_prod * gamma ( a(i) );
  end

  pdf = gamma ( a_sum ) / a_prod;
  for i = 1 : n
    pdf = pdf * x(i)^( a(i) - 1.0 );
  end

  return
end


function dist = circle_imp_point_dist_2d ( r, center, p )

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

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

  dist = abs ( r2 - r );

  return
end


% Calculate the number of star motifs of given (subgraph) size.
% Note 1: Easily extendible to return the actual stars as k-tuples of nodes.
% Note 2: Star of size 1 is the trivial case of a single node.
%
% INPUTs: adjacency list {} (1xn), k - star motif size
% OUTPUTs: number of stars with k nodes (k-1 spokes)
%
% GB: last updated, Oct 5, 2012

function num = numStarMotifs(adjL,k)

num = 0;

for i=1:length(adjL)
  if length(adjL{i})>=(k-1); num = num + nchoosek(length(adjL{i}),k-1); end
end

% ALTERNATIVE
% function num = numStarMotifs(adj,k)
%
% % INPUTs: adjacency matrix, k - star motif size
% % OUTPUTs: number of stars with k nodes (k-1 spokes)
%
% [deg,~,~]=degrees(adj);
%
% num=0;
%
% for i=1:length(deg)
%     if deg(i)>=(k-1); num=num+nchoosek(deg(i),k-1); end
% end


function a = parter ( m, n )

%*****************************************************************************80
%
%% PARTER returns the PARTER matrix.
%
%  Formula:
%
%    A(I,J) = 1 / ( i - j + 0.5 )
%
%  Example:
%
%    N = 5
%
%     2   -2  -2/3 -2/5 -2/7
%    2/3   2   -2  -2/3 -2/5
%    2/5  2/3   2   -2  -2/3
%    2/7  2/5  2/3   2   -2
%    2/9  2/7  2/5  2/3   2
%
%  Properties:
%
%    The diagonal entries are all 2, the first superdiagonals all -2.
%
%    A is Toeplitz: constant along diagonals.
%
%    A is generally not symmetric: A' ~= A.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    A is a special case of the Cauchy matrix.
%
%    Most of the singular values are very close to Pi.
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Seymour Parter,
%    On the distribution of the singular values of Toeplitz matrices,
%    Linear Algebra and Applications,
%    Volume 80, August 1986, pages 115-130.
%
%    Evgeny Tyrtyshnikov,
%    Cauchy-Toeplitz matrices and some applications,
%    Linear Algebra and Applications,
%    Volume 149, 15 April 1991, pages 1-18.
%
%  Parameters:
%
%    Input, integer M, N, the order of A.
%
%    Output, real A(M,N), the matrix.
%
  a = zeros ( m, n );

  for i = 1 : m
    for j = 1 : n
      a(i,j) = 1.0 / ( ( i - j ) + 0.5 );
    end
  end

  return
end


function [u,s,v] = svdsim(a,tol)
%SVDSIM  simple SVD program
%
% A simple program that demonstrates how to use the
% QR decomposition to perform the SVD of a matrix.
% A may be rectangular and complex.
%
% usage: [U,S,V]= SVDSIM(A)
%     or      S = SVDSIM(A)
%
% with A = U*S*V' , S>=0 , U'*U = Iu  , and V'*V = Iv
%
% The idea is to use the QR decomposition on A to gradually "pull" U out from
% the left and then use QR on A transposed to "pull" V out from the right.
% This process makes A lower triangular and then upper triangular alternately.
% Eventually, A becomes both upper and lower triangular at the same time,
% (i.e. Diagonal) with the singular values on the diagonal.
%
% Matlab's own SVD routine should always be the first choice to use,
% but this routine provides a simple "algorithmic alternative"
% depending on the users' needs.
%
%see also: SVD, EIG, QR, BIDIAG, HESS
%

% Paul Godfrey
% October 23, 2006

if ~exist('tol','var')
   tol=eps*1024;
end

%reserve space in advance
sizea=size(a);
loopmax=100*max(sizea);
loopcount=0;

% or use Bidiag(A) to initialize U, S, and V
u=eye(sizea(1));
s=a';
v=eye(sizea(2));

Err=realmax;
while Err>tol & loopcount<loopmax ;
%   log10([Err tol loopcount loopmax]); pause
    [q,s]=qr(s'); u=u*q;
    [q,s]=qr(s'); v=v*q;

% exit when we get "close"
    e=triu(s,1);
    E=norm(e(:));
    F=norm(diag(s));
    if F==0, F=1;end
    Err=E/F;
    loopcount=loopcount+1;
end
% [Err/tol loopcount/loopmax]

%fix the signs in S
ss=diag(s);
s=zeros(sizea);
for n=1:length(ss)
    ssn=ss(n);
    s(n,n)=abs(ssn);
    if ssn<0
       u(:,n)=-u(:,n);
    end
end

if nargout<=1
   u=diag(s);
end

return


classdef PCA < handle
    %PCA  Principal Component Analysis class
    %
    % The class is used to compute a special basis for a set of vectors.
    % The basis will consist of eigenvectors of the covariance matrix
    % computed from the input set of vectors. The class PCA can also
    % transform vectors to/from the new coordinate space defined by the
    % basis. Usually, in this new coordinate system, each vector from the
    % original set (and any linear combination of such vectors) can be
    % quite accurately approximated by taking its first few components,
    % corresponding to the eigenvectors of the largest eigenvalues of the
    % covariance matrix. Geometrically it means that you compute a
    % projection of the vector to a subspace formed by a few eigenvectors
    % corresponding to the dominant eigenvalues of the covariance matrix.
    % And usually such a projection is very close to the original vector.
    % So, you can represent the original vector from a high-dimensional
    % space with a much shorter vector consisting of the projected vector's
    % coordinates in the subspace. Such a transformation is also known as
    % Karhunen-Loeve Transform, or KLT. See
    % [PCA](https://en.wikipedia.org/wiki/Principal_component_analysis).
    %
    % ## Example
    % The following shows a quick example of how to reduce dimensionality
    % of samples from 10 to 3.
    %
    %     Xtrain = randn(100,10);
    %     Xtest  = randn(100,10);
    %     pca = cv.PCA(Xtrain, 'MaxComponents',3);
    %     Y = pca.project(Xtest);
    %     Xapprox = pca.backProject(Y);
    %
    % The class also implements the save/load pattern to regular MAT-files,
    % so we can do the following:
    %
    %     pca = cv.PCA(randn(100,5));
    %     save out.mat pca
    %     clear pca
    %
    %     load out.mat
    %     disp(pca)
    %
    % See also: cv.PCA.PCA, cv.SVD
    %

    properties (SetAccess = private)
        % Object ID
        id
    end

    % cached properties of their C++ couterpart
    properties (Access = private, Hidden)
        p_eigenvectors
        p_eigenvalues
        p_mean
    end

    % publicly exposed properties (backed by the cached ones)
    properties (Dependent)
        % Eigenvectors of the covariation matrix.
        eigenvectors
        % Eigenvalues of the covariation matrix.
        eigenvalues
        % Mean value subtracted before the projection and added after
        % the back projection.
        mean
    end

    methods
        function this = PCA(varargin)
            %PCA  PCA constructors
            %
            %     pca = cv.PCA()
            %     pca = cv.PCA(data, 'OptionName', optionValue, ...)
            %     pca = cv.PCA(S)
            %
            % ## Input
            % * __data__ input samples stored as matrix rows or matrix columns
            % * __S__ A scalar struct to be imported
            %
            % ## Options
            % Same set of options as the cv.PCA.compute method.
            %
            % The constructor creates an empty PCA object without an argument.
            % Use cv.PCA.compute to analyze data samples. The second form
            % accepts data samples with the same arguments as cv.PCA.compute.
            % The third form create a new PCA object from a scalar struct with
            % 'eigenvectors', 'eigenvalues', and 'mean' fields.
            %
            % See also: cv.PCA, cv.PCA.compute
            %
            this.id = PCA_(0,'new');
            if nargin > 0
                if nargin==1 && isstruct(varargin{1}) && isscalar(varargin{1})
                    S = varargin{1};
                    this.eigenvectors = S.eigenvectors;
                    this.eigenvalues = S.eigenvalues;
                    this.mean = S.mean;
                else
                    this.compute(varargin{:});
                end
            end
        end

        function delete(this)
            %DELETE  Destructor
            %
            %     obj.delete()
            %
            % See also: cv.PCA
            %
            if isempty(this.id), return; end
            PCA_(this.id, 'delete');
        end

        function read(this, fname_or_str, varargin)
            %READ  Read PCA objects from file
            %
            %     obj.read(filename)
            %     obj.read(str, 'FromString',true)
            %     obj.read(..., 'OptionName', optionValue, ...)
            %
            % ## Input
            % * __filename__ Name of the file to read.
            % * __str__ String containing serialized object you want to load.
            %
            % ## Options
            % * __FromString__ Logical flag to indicate whether the input is a
            %   filename or a string containing the serialized object.
            %   default false
            %
            % Loads `eigenvalues`, `eigenvectors` and `mean` from specified
            % storage.
            %
            % See also: cv.PCA.write
            %
            PCA_(this.id, 'read', fname_or_str, varargin{:});
        end

        function varargout = write(this, filename)
            %WRITE  Write PCA objects to file
            %
            %     obj.write(filename)
            %     str = obj.write(filename)
            %
            % ## Input
            % * __filename__ Name of the file to write to.
            %
            % ## Output
            % * __str__ optional output. If requested, the object is persisted
            %   to a string in memory instead of writing to disk.
            %
            % Writes `eigenvalues`, `eigenvectors` and `mean` to the specified
            % file.
            %
            % See also: cv.PCA.read
            %
            [varargout{1:nargout}] = PCA_(this.id, 'write', filename);
        end

        function compute(this, data, varargin)
            %COMPUTE  Performs Principal Component Analysis on the supplied dataset
            %
            %     pca.compute(data)
            %     pca.compute(..., 'OptionName', optionValue, ...)
            %
            % ## Input
            % * __data__ input samples stored as the matrix rows or as the
            %   matrix columns
            %
            % ## Options
            % * __DataAs__ Data layout option. Default 'Row'. One of:
            %   * __Row__ indicates that the input samples are stored as
            %     matrix rows.
            %   * __Col__ indicates that the input samples are stored as
            %     matrix columns.
            % * __MaxComponents__ Maximum number of components that PCA should
            %   retain. default 0 (all the components are retained).
            % * __RetainedVariance__ Percentage of variance that PCA should
            %   retain. Using this parameter will let the PCA decided how many
            %   components to retain but it will always keep at least 2.
            %   default 1.0 (all the components are retained).
            % * __Mean__ Optional mean value. By default, the mean is computed
            %   from the data. Not set by default.
            %
            % **Note**: `RetainedVariance` and `MaxComponents` are mutually
            % exclusive options, and shoudn't be used together.
            %
            % The method performs PCA of the supplied dataset. It is safe
            % to reuse the same PCA structure for multiple datasets. That
            % is, if the structure has been previously used with another
            % dataset, the existing internal data is reclaimed and the new
            % `eigenvalues`, `eigenvectors` and `mean` are allocated and
            % computed.
            %
            % The computed `eigenvalues` are sorted from the largest to the
            % smallest and the corresponding `eigenvectors` are stored as
            % cv.PCA.eigenvectors rows.
            %
            % See also: cv.PCA.PCA, cv.PCA.project, cv.PCA.backProject
            %
            PCA_(this.id, 'compute', data, varargin{:});

            % invalidate cached properties
            this.p_eigenvectors = [];
            this.p_eigenvalues = [];
            this.p_mean = [];
        end

        function Y = project(this, X)
            %PROJECT  Projects vector(s) to the principal component subspace
            %
            %     Y = pca.project(X)
            %
            % ## Input
            % * __X__ input vector(s); must have the same dimensionality and
            %   the same layout as the input data used at PCA phase, that is,
            %   if 'Row' was specified, then `size(X,2)==size(data,2)` (vector
            %   dimensionality) and `size(X,1)` is the number of vectors to
            %   project, and the same is true for the 'Col' case.
            %
            % ## Output
            % * __Y__ output vectors (PC coefficients); in case of 'Col', the
            %   output matrix has as many columns as the number of input
            %   vectors, this means that `size(Y,2)==size(X,2)` and the number
            %   of rows match the number of principal components (for example,
            %   `MaxComponents` parameter passed to the constructor).
            %
            % The method project one or more vectors to the principal
            % component subspace, where each vector projection is
            % represented by coefficients in the principal component basis.
            %
            % See also: cv.PCA, cv.PCA.backProject
            %
            Y = PCA_(this.id, 'project', X);
        end

        function X = backProject(this, Y)
            %BACKPROJECT  Reconstructs vectors from their PC projections
            %
            %     X = pca.backProject(Y)
            %
            % ## Input
            % * __Y__ coordinates of the vectors in the principal component
            %   subspace, the layout and size are the same as of
            %   cv.PCA.project output vectors.
            %
            % ## Output
            % * __X__ reconstructed vectors; the layout and size are the same
            %   as of cv.PCA.project input vectors.
            %
            % The method is the inverse operation to cv.PCA.project. It
            % takes PC coordinates of projected vectors and reconstruct the
            % original vectors. Unless all the principal components have
            % been retained, the reconstructed vectors are different from
            % the originals. But typically, the difference is small if the
            % number of components is large enough (but still much smaller
            % than the original vector dimensionality). As a result, PCA is
            % used.
            %
            % See also: cv.PCA.compute, cv.PCA.project
            %
            X = PCA_(this.id, 'backProject', Y);
        end
    end

    %% Getters/Setters for dependent properties
    methods
        function value = get.eigenvectors(this)
            if isempty(this.p_eigenvectors)
                this.p_eigenvectors = PCA_(this.id, 'get', 'eigenvectors');
            end
            value = this.p_eigenvectors;
        end
        function set.eigenvectors(this, value)
            PCA_(this.id, 'set', 'eigenvectors', value);
            this.p_eigenvectors = value;
        end

        function value = get.eigenvalues(this)
            if isempty(this.p_eigenvalues)
                this.p_eigenvalues = PCA_(this.id, 'get', 'eigenvalues');
            end
            value = this.p_eigenvalues;
        end
        function set.eigenvalues(this, value)
            PCA_(this.id, 'set', 'eigenvalues', value);
            this.p_eigenvalues = value;
        end

        function value = get.mean(this)
            if isempty(this.p_mean)
                this.p_mean = PCA_(this.id, 'get', 'mean');
            end
            value = this.p_mean;
        end
        function set.mean(this, value)
            PCA_(this.id, 'set', 'mean', value);
            this.p_mean = value;
        end
    end

    methods (Hidden)
        function S = struct(this)
            %STRUCT  Converts to a struct array
            %
            %     S = struct(obj)
            %
            % ## Output
            % * __S__ output struct array
            %
            % See also: cv.PCA
            %
            S = struct('eigenvectors',{this.eigenvectors},...
                       'eigenvalues', {this.eigenvalues},...
                       'mean',        {this.mean});
        end

        function S = saveobj(this)
            %SAVEOBJ  Serialization before save
            %
            %     S = obj.saveobj()
            %
            % ## Output
            % * __S__ output struct.
            %
            % Called by the `save` function, when saving object to a MAT-file.
            %
            % See also: cv.PCA.loadobj, saveobj, loadobj
            %
            S = struct(this);
        end
    end

    methods (Static, Hidden)
        function this = loadobj(S)
            %LOADOBJ  Deserialization after load
            %
            %     obj = loadobj(S)
            %
            % ## Input
            % * __S__ input struct.
            %
            % Called by the `load` function, when loading object from a
            % MAT-file.
            %
            % See also: cv.PCA.PCA, cv.PCA.saveobj, saveobj, loadobj
            %
            this = cv.PCA(S);
        end
    end

end


function weight = subset_weight ( n, t )

%*****************************************************************************80
%
%% SUBSET_WEIGHT computes the Hamming weight of a set.
%
%  Discussion:
%
%    The Hamming weight is simply the number of elements in the set.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the order of the master set, of which T
%    is a subset.  N must be positive.
%
%    Input, integer T(N), defines the subset T.
%    T(I) is 1 if I is an element of T, and 0 otherwise.
%
%    Output, integer WEIGHT, the Hamming weight of the subset T.
%

%
%  Check.
%
  subset_check ( n, t );

  weight = sum ( t(1:n) );

  return
end


function a = smoke ( n )

%*****************************************************************************80
%
%% SMOKE returns the SMOKE matrix.
%
%  Formula:
%
%    W = exp ( 2 * PI * sqrt ( -1 ) / N )
%
%    If ( J = I + 1 )
%      A(I,J) = 1
%    If ( I = N and J = 1 )
%      A(I,J) = 1
%    If ( I = J )
%      A(I,J) = W**I
%    Else
%      A(I,J) = 0
%
%  Example:
%
%    N = 5
%
%    w  1    0    0    0
%    0  w**2 1    0    0
%    0  0    w**3 1    0
%    0  0    0    w**4 1
%    1  0    0    0    w**5
%
%  Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    The matrix has an interesting spectrum.  The eigenvalues are
%    the N-th roots of unity times 2**(1/N).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Lothar Reichel, Lloyd Trefethen,
%    Eigenvalues and pseudo-eigenvalues of Toeplitz matrices,
%    Linear Algebra and Applications,
%    Volume 162-164, 1992, pages 153-185.
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, complex A(N,N), the smoke matrix.
%
  angle = 2.0 * pi / n;
  w = complex ( cos ( angle ), sin ( angle ) );

  for i = 1 : n
    for j = 1 : n
      if ( j + 1 == i )
        a(i,j) = 1.0;
      elseif ( i == n & j == 1 )
        a(i,j) = 1.0;
      elseif ( i == j )
        a(i,j) = w^i;
      else
        a(i,j) = 0.0;
      end
    end
  end

  return
end


function genotypeFactor = genotypeGivenAlleleFreqsFactor(alleleFreqs, genotypeVar)
% This function computes the probability of each genotype given the allele
% frequencies in the population.

% Note that we assume that the copies of the gene are independent.  Thus,
% knowing the allele for one copy of the gene does not affect the
% probability of having each allele for the other copy of the gene.  As a
% result, the probability of a genotype is the product of the frequencies
% of its constituent alleles (or twice that product for heterozygous
% genotypes).

% Input:
%   alleleFreqs: An n x 1 vector of the frequencies of the alleles in the
%   population, where n is the number of alleles
%   genotypeVar: The variable number for the genotype (goes in the .var
%   part of the factor)
%
% Output:
%   genotypeFactor: Factor in which the val has the probability of having
%   each genotype (note that this is the FULL CPD with no evidence
%   observed)

% The number of genotypes is (number of alleles choose 2) + number of
% alleles -- need to add number of alleles at the end to account for
% homozygotes

genotypeFactor = struct('var', [], 'card', [], 'val', []);
numAlleles = length(alleleFreqs);

% Each allele has an ID that is the index of its allele frequency in the
% allele frequency list.  Each genotype also has an ID.  We need allele and
% genotype IDs so that we know what genotype and alleles correspond to each
% probability in the .val part of the factor.  For example, the first entry
% in .val corresponds to the probability of having the genotype with
% genotype ID 1, which consists of having two copies of the allele with
% allele ID 1.  There is a mapping from a pair of allele IDs to genotype
% IDs and from genotype IDs to a pair of allele IDs below; we compute this
% mapping using generateAlleleGenotypeMappers(numAlleles). (A genotype
% consists of 2 alleles.)

[allelesToGenotypes, genotypesToAlleles] = generateAlleleGenotypeMappers(numAlleles);

% One or both of these matrices might be useful.
%
%   1.  allelesToGenotypes: n x n matrix that maps pairs of allele IDs to
%   genotype IDs, where n is the number of alleles -- if
%   allelesToGenotypes(i, j) = k, then the genotype with ID k comprises of
%   the alleles with IDs i and j
%
%   2.  genotypesToAlleles: m x 2 matrix of allele IDs, where m is the
%   number of genotypes -- if genotypesToAlleles(k, :) = [i, j], then the
%   genotype with ID k is comprised of the allele with ID i and the allele
%   with ID j

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%INSERT YOUR CODE HERE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Fill in genotypeFactor.var.  This should be a 1-D row vector.
% Fill in genotypeFactor.card.  This should be a 1-D row vector.
m=size(genotypesToAlleles,1);
genotypeFactor.var = [genotypeVar] ;
genotypeFactor.card = [m];
genotypeFactor.val = zeros(1, prod(genotypeFactor.card));
% Replace the zeros in genotypeFactor.val with the correct values.

for i=1:m
	k = genotypesToAlleles(i,:);
	genotypeFactor.val(i) = alleleFreqs(k(1))*alleleFreqs(k(2));
	if k(1)!=k(2)
		genotypeFactor.val(i)*=2;
	end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function area = sphere01_area ( )

%*****************************************************************************80
%
%% SPHERE01_AREA returns the area of the unit sphere.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real AREA, the area of the unit sphere.
%
  r = 1.0;
  area = 4.0 * pi * r * r;

  return
end


function alpha = sphericalAngle(p1, p2, p3)
%SPHERICALANGLE Compute angle between points on the sphere.
%
%   ALPHA = sphericalAngle(P1, P2, P3)
%   Computes angle (P1, P2, P3), i.e. the angle, measured at point P2,
%   between the direction (P2, P1) and the direction (P2, P3).
%   The result is given in radians, between 0 and 2*PI.
%
%   Points are given either as [x y z] (there will be normalized to lie on
%   the unit sphere), or as [phi theta], with phi being the longitude in [0
%   2*PI] and theta being the elevation on horizontal [-pi/2 pi/2].
%
%
%   NOTE:
%   this is an 'oriented' version of the angle computation, that is, the
%   result of sphericalAngle(P1, P2, P3) equals
%   2*pi-sphericalAngle(P3,P2,P1). To have the more classical relation
%   (with results given betwen 0 and PI), it suffices to take the minimum
%   of angle and 2*pi-angle.
%
%   Examples
%     % Use inputs as cartesian coordinates
%     p1 = [0 1 0];
%     p2 = [1 0 0];
%     p3 = [0 0 1];
%     alpha = sphericalAngle(p1, p2, p3)
%     alpha =
%         1.5708
%
%     % Use inputs as spherical coordinates
%     sph1 = [.1 0];
%     sph2 = [0 0];
%     sph3 = [0 .1];
%     alphas = sphericalAngle(sph1, sph2, sph3)
%     alphas =
%         1.5708
%
%
%   See also:
%   geom3d, angles3d, spheres, sph2cart

%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 21/02/2005.
%

%   HISTORY
%   23-05-2006 fix bug for points with angle from center > pi/2
%   05-06-2013 fix bug for points given as spherical coordinates, better
%       support for multiple inputs

% test if points are given as matlab spherical coordinates
if size(p1, 2) == 2
    [x, y, z] = sph2cart(p1(:,1), p1(:,2), ones(size(p1,1), 1));
    p1 = [x y z];
    [x, y, z] = sph2cart(p2(:,1), p2(:,2), ones(size(p2,1), 1));
    p2 = [x y z];
    [x, y, z] = sph2cart(p3(:,1), p3(:,2), ones(size(p3,1), 1));
    p3 = [x y z];
end

% normalize points
p1  = normalizeVector3d(p1);
p2  = normalizeVector3d(p2);
p3  = normalizeVector3d(p3);

% create the plane tangent to the unit sphere and containing central point
plane = createPlane(p2, p2);

% project the two other points on the plane
pp1 = planePosition(projPointOnPlane(p1, plane), plane);
pp3 = planePosition(projPointOnPlane(p3, plane), plane);

% compute angle on the tangent plane
pp2 = zeros(max(size(pp1, 1), size(pp3,1)), 2);
alpha = angle3Points(pp1, pp2, pp3);


function value = r8vec_norm_l0 ( n, a )

%*****************************************************************************80
%
%% R8VEC_NORM_L0 returns the l0 "norm" of an R8VEC.
%
%  Discussion:
%
%    An R8VEC is a vector of R8's.
%
%    The l0 "norm" simply counts the number of nonzero entries in the vector.
%    It is not a true norm, but has some similarities to one.  It is useful
%    in the study of compressive sensing.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, real A(N), the vector.
%
%    Output, real R8VEC_NORM_L0, the value of the norm.
%
  value = sum ( a(1:n) ~= 0.0 );

  return
end


function matrixOut = smooth2a(matrixIn,Nr,Nc)
% Smooths 2D array data.  Ignores NaN's.
%
%function matrixOut = smooth2a(matrixIn,Nr,Nc)
%
% This function smooths the data in matrixIn using a mean filter over a
% rectangle of size (2*Nr+1)-by-(2*Nc+1).  Basically, you end up replacing
% element "i" by the mean of the rectange centered on "i".  Any NaN
% elements are ignored in the averaging.  If element "i" is a NaN, then it
% will be preserved as NaN in the output.  At the edges of the matrix,
% where you cannot build a full rectangle, as much of the rectangle that
% fits on your matrix is used (similar to the default on Matlab's builtin
% function "smooth").
%
% "matrixIn": original matrix
% "Nr": number of points used to smooth rows
% "Nc": number of points to smooth columns.  If not specified, Nc = Nr.
%
% "matrixOut": smoothed version of original matrix
%
%
% 	Written by Greg Reeves, March 2009.
% 	Division of Biology
% 	Caltech
%
% 	Inspired by "smooth2", written by Kelly Hilands, October 2004
% 	Applied Research Laboratory
% 	Penn State University
%
% 	Developed from code written by Olof Liungman, 1997
% 	Dept. of Oceanography, Earth Sciences Centre
% 	Gďż˝teborg University, Sweden
% 	E-mail: olof.liungman@oce.gu.se

%
% Initial error statements and definitions
%
if nargin < 2, error('Not enough input arguments!'), end

N(1) = Nr;
if nargin < 3, N(2) = N(1); else N(2) = Nc; end

if length(N(1)) ~= 1, error('Nr must be a scalar!'), end
if length(N(2)) ~= 1, error('Nc must be a scalar!'), end

%
% Building matrices that will compute running sums.  The left-matrix, eL,
% smooths along the rows.  The right-matrix, eR, smooths along the
% columns.  You end up replacing element "i" by the mean of a (2*Nr+1)-by-
% (2*Nc+1) rectangle centered on element "i".
%
[row,col] = size(matrixIn);
eL = spdiags(ones(row,2*N(1)+1),(-N(1):N(1)),row,row);
eR = spdiags(ones(col,2*N(2)+1),(-N(2):N(2)),col,col);

%
% Setting all "NaN" elements of "matrixIn" to zero so that these will not
% affect the summation.  (If this isn't done, any sum that includes a NaN
% will also become NaN.)
%
A = isnan(matrixIn);
matrixIn(A) = 0;

%
% For each element, we have to count how many non-NaN elements went into
% the sums.  This is so we can divide by that number to get a mean.  We use
% the same matrices to do this (ie, "eL" and "eR").
%
nrmlize = eL*(~A)*eR;
nrmlize(A) = NaN;

%
% Actually taking the mean.
%
matrixOut = eL*matrixIn*eR;
matrixOut = matrixOut./nrmlize;


function y = Evaluate_FT(x,shift,boxlen,center,w)
% Evaluate_FT -- 1d unequispaced Fourier transform followed by windowing
% Usage:
%   y = Evaluate_FT(x,shift,boxlen,center,w)
% Inputs:
%   x	      vector of length n
%   shift     vector of shifts
%   boxlen    half length of interval around each shift
%   center    boolean variable: 0/1 = unbiased/biased FT
%   w         tapering window of size 2*boxlen
% Outputs:
%   y         vector of length: (# shifts) * (2*boxlen)
% Description:
%  Evaluates the FT at the frequency points
%                   omega(j,k) = shift(j) + k, -boxlen <= k <boxlen
%  If center = 0,
%
%  y = sum_{-n/2 <= t < n/2} exp(-i 2pi omega t/n) x(t), -n/2 <= t < n/2
%
%  If center = 1,
%
%  y = sum_{0 <= t < n} exp(-i 2pi omega t/n) x(t), 0 <= t < n
%
%  For each shift parameter (shift(j)), the outcome y_j is then
%  tapered with the window w
%
%  Evaluate_FT uses two different methods for computing the
%  unesquispaced Fourier transform. If the number of shifts is less
%  than 8, it uses an exact method, otherwise it uses the
%  accelerated USFFT.
%  See Also
%    Adj_Evaluate_FT, USFFT, USFT_simple
%
% By Emmanuel candes, 2003-2004

  if nargin < 5,
    w = ones(1,2*boxlen);
  end

  if nargin < 4,
    center = 0;
  end

  n = length(x);
  boxcnt = length(shift);

  if (boxcnt <= 16)
	  y = USFT_simple(x,shift,boxlen,center,w); % nonequispaced FT
  else
    % Make frequency grid
    k = (-boxlen):(boxlen -1);
    omega = meshgrid(shift,k) + meshgrid(k,shift).';
    omega = 2*pi*(omega(:))./n;

    y = USFFT(x,omega,16,6,center)./sqrt(n); % Take USFFT
    y = repmat(w(:),boxcnt,1) .* y; % Tapering
  end