function Y=dd2(x)
%DD2 Single-level discrete 2-D wavelet transform.
%   DD2 performs a single-level 2-D wavelet decomposition
%   using Daubechies wavelet with four coefficients
%
%   Y = DD2(X) computes the approximation
%   coefficients matrix LL and details coefficients matrices
%   LH, HL, HH, obtained by a wavelet decomposition of the
%   input matrix X and puts the result in Y=[LL,LH;HL,HH].
%
%   The size of Y is the same as that of X which should be
%   a square matrix of size NxN where N is power of 2.
%   LL, LH, HL, and HH will have the size N/2xN/2
%   Minimum size of X is 4x4.
%   See also IDD2, DWTMODE, WAVEDEC2, WAVEINFO.

%   Auth: Dr. Bessam Z. Hassan
%   Last Revision: 27-Feb-2004.
%   Copyright 1995-2002 The MathWorks, Inc.
% $Revision: 1.0 $

%initialize coefficients

c0=(1+sqrt(3))/(4*sqrt(2));c1=(3+sqrt(3))/(4*sqrt(2));
c2=(3-sqrt(3))/(4*sqrt(2));c3=(1-sqrt(3))/(4*sqrt(2));

%check the inputs

[N,M]=size(x);
if N~=2^round(log(N)/log(2))
    error('size of the input should be power of 2');
end
if M~=N
    error('the input matrix must be square');
end

% construct the W matrix

w=[c0,c1,c2,c3];wi=[c3,-c2,c1,-c0];
W=[];X=x;p=zeros(N,N);
for i=1:N/2-1
    W(2*(i-1)+1:2*i,2*i-1:2*i+2)=[w;wi];
end
W=[W;[[w(3:4);wi(3:4)],zeros(2,N-4),[w(1:2);wi(1:2)]]];

% row transformation

z=W*X;

% row permutation
p(1:N,:)=[z(1:2:N,:);z(2:2:N,:)];

% column transformation

X=p';z=W*X;
p=zeros(N,N);

% column permutation

p(1:N,:)=[z(1:2:N,:);z(2:2:N,:)];
Y=p';


function ecef = llh2ecef(llh)
% llh2ecef: converts from navigation coordinates (latitude, longitude and
% altitude) to ECEF coordinates.
%
% INPUTS
%   llh: Nx3 LLH coordinates [lat, lon, h] (rad, rad, m).
%
% OUTPUTS
%   ned: Nx3 NED coordinates [X Y Z] (m, m, m).
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% Reference:
%
%   Guowei Cai et al. Unmanned Rotorcraft Systems. Springer. 2011.
% Eq. 2.22, p. 31.
%
% Version: 001
% Date:    2019/01/16
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

% Preallocate

ecef = zeros(size(llh));

lat = llh(:,1);
lon = llh(:,2);
h   = llh(:,3);

[~,RN] = radius(lat);

e = 0.0818191908426;    % Eccentricity

slat = sin(lat);
clat = cos(lat);
clon = cos(lon);
slon = sin(lon);

ecef(:,1) = (RN + h) .* clat .* clon;
ecef(:,2) = (RN + h) .* clat .* slon;
ecef(:,3) = (RN *(1-e^2) + h) .* slat;

end


 function [alphas, beta] = nufft_alpha_kb_fit(N, J, K, L, beta, chat)
%
% return the alpha and beta corresponding to LS fit of L components
% to optimized Kaiser-Bessel scaling factors (m=0, alpha=2.34J).
% This is the best method I know currently for choosing alpha!
%
% Copyright 2002-7-16, Jeff Fessler, The University of Michigan

if nargin < 3, help(mfilename), error(mfilename), end
if ~isvar('L') | isempty(L)
	if N > 40
		L = 13;		% empirically found to be reasonable
	else
		L = ceil(N/3);	% a kludge to avoid "rank deficient" complaints
	end
end
if ~isvar('beta') | isempty(beta),	beta = 1; end
if ~isvar('chat') | isempty(chat),	chat = 0; end

kb_alf = 2.34 * J;	% KB shape parameter
kb_m = 0;		% KB order

[tmp, sn_kaiser] = nufft1_error(0, N, J, K, 'kaiser', 'ft');
sn_kaiser = reale(sn_kaiser);

%
% use regression to match NUFFT with BEST kaiser scaling's
%
gam = 2*pi/K;
nlist = [0:(N-1)]' - (N-1)/2;
X = cos(beta*gam*nlist*[0:L]);	% [N,L]
coef = (X \ sn_kaiser)';	% regress(sn_kaiser, X)';
alphas = [reale(coef(1)) coef(2:end)/2];

if chat
	printf('cond # for LS fit to KB scale factors: %g', cond(X))
end


function [bcUx, bcUy, bcPR, bcdUndt] = KovasznayBC2D(x, y, nx, ny, mapI, mapO, mapW, mapC, time, nu)

% function [bcUx, bcUy, bcPR, bcdUndt] = KovasznayBC2D(x, y, nx, ny, mapI, mapO, mapW, mapC, time, nu)
% Purpose: evaluate boundary conditions for Kovasznay flow

zer = zeros(size(x)); bcUx = zer; bcUy = zer; bcPR = zer; bcdUndt = zer;

lam = (0.5/nu) - sqrt( (0.25/(nu^2)) + 4*pi^2 );

% inflow
xI = x(mapI); yI = y(mapI);
bcUx(mapI)= 1-exp(lam*xI).*cos(2*pi*yI);
bcUy(mapI)= (0.5*lam/pi)*exp(lam*xI).*sin(2*pi*yI);

% outflow
xO = x(mapO); yO = y(mapO);

if(0)
  bcPR(mapO) = .5*(1-exp(2*lam*xO));
  bcUx(mapO)= 1-exp(lam*xO).*cos(2*pi*yO);
  bcUy(mapO)= (0.5*lam/pi)*exp(lam*xO).*sin(2*pi*yO);
else
  bcPR(mapO) = .5*(1-exp(2*lam*xO));

  % Neumann data for each velocity
  bcUx(mapO) = -lam*exp(lam*xO).*cos(2*pi*yO);
  bcUy(mapO) = lam*(0.5*lam/pi)*exp(lam*xO).*sin(2*pi*yO);
end
return


function dist=KLDiv(P,Q)
%  dist = KLDiv(P,Q) Kullback-Leibler divergence of two discrete probability
%  distributions
%  P and Q  are automatically normalised to have the sum of one on rows
% have the length of one at each
% P =  n x nbins
% Q =  1 x nbins or n x nbins(one to one)
% dist = n x 1

if size(P,2)~=size(Q,2)
    error('the number of columns in P and Q should be the same');
end

if sum(~isfinite(P(:))) + sum(~isfinite(Q(:)))
   error('the inputs contain non-finite values!')
end

% normalizing the P and Q
if size(Q,1)==1
    Q = Q ./sum(Q);
    P = P ./repmat(sum(P,2),[1 size(P,2)]);
    dist =  sum(P.*log(P./repmat(Q,[size(P,1) 1])),2);

elseif size(Q,1)==size(P,1)

    Q = Q ./repmat(sum(Q,2),[1 size(Q,2)]);
    P = P ./repmat(sum(P,2),[1 size(P,2)]);
    dist =  sum(P.*log(P./Q),2);
end

% resolving the case when P(i)==0
dist(isnan(dist))=0;


% MORLET_2D_NODC computes the 2-D elliptic Morlet filter given a set of
%    parameters in Fourier domain
%
% Usage
%    gab = MORLET_2D_NODC(N, M, sigma, slant, xi, theta, offset)
%
% Input
%    N (numeric): Width of the filter.
%    M (numeric): Height of the filter.
%    sigma (numeric): Standard deviation of the envelope.
%    slant (numeric): Eccentricity of the elliptic envelope.
%       (the smaller slant, the larger angular resolution).
%    xi (numeric): The frequency peak.
%    theta (numeric): Orientation in radians of the filter.
%    offset (numeric, optional): 2-D vector reprensting the offset location
%       (default [0 0]).
%
% Output
%    gab (numeric): N-by-M matrix representing the gabor filter in spatial
%       domain.
%
% Description
%    Compute a Morlet wavelet in Fourier domain.
%
%    Morlet wavelets have a 0 DC component.
%
% See also
%    GABOR_2D, MORLET_2D_PYRAMID

function gab = morlet_2d_noDC(N, M, sigma, slant, xi, theta, offset)

	if ~exist('offset','var')
		offset = [0, 0];
	end
	[x , y] = meshgrid(1:M, 1:N);

	x = x - ceil(M/2) - 1 - offset(1);
	y = y - ceil(N/2) - 1 - offset(2);

	Rth = rotation_matrix_2d(theta);
	A = Rth\ [1/sigma^2, 0 ; 0 slant^2/sigma^2] * Rth ;
	s = x.* ( A(1,1)*x + A(1,2)*y) + y.*(A(2,1)*x + A(2,2)*y ) ;

	%normalize sucht that the maximum of fourier modulus is 1

	gaussian_envelope = exp( - s/2);
	oscilating_part = gaussian_envelope .* exp(1i*(x*xi*cos(theta) + y*xi*sin(theta)));
	K = sum(oscilating_part(:)) ./ sum(gaussian_envelope(:));
	gabc = oscilating_part - K.*gaussian_envelope;

	gab=1/(2*pi*sigma^2/slant^2)*fftshift(gabc);

end


function [La,dLa,lambda0] = lagrange(U,s,b,more)
%LAGRANGE Plot the Lagrange function for Tikhonov regularization.
%
% [La,dLa,lambda0] = lagrange(U,s,b,more)
% [La,dLa,lambda0] = lagrange(U,sm,b,more)  ,  sm = [sigma,mu]
%
% Plots the Lagrange function
%    La(lambda) = || A x - b ||^2 + lambda^2*|| L x ||^2
% and its first derivative dLa = dLa/dlambda versus lambda.
% Here, x is the Tikhonov regularized solution.
%
% If nargin = 4, || A x - b || and || L x || are also plotted.
%
% Returns La, dLa, and the value lambda0 of lambda for which
% dLa has its minimum.

% Per Christian Hansen, IMM, Feb. 21, 2001.

% Set default number of points.
npoints = 200;

% Initialization.
[m,n] = size(U); [p,ps] = size(s);
beta = U'*b; beta2 = norm(b)^2 - norm(beta)^2;
if (ps==2)
  s = s(p:-1:1,1)./s(p:-1:1,2); beta = beta(p:-1:1);
end
xi = beta(1:p)./s;

% Compute the L-curve.
eta = zeros(npoints,1); rho = eta;
lambda(npoints,1) = s(p);
ratio = (s(1)/s(p))^(1/(npoints-1));
for i=npoints-1:-1:1, lambda(i) = ratio*lambda(i+1); end
for i=1:npoints
  f = fil_fac(s,lambda(i));
  eta(i) = norm(f.*xi);
  rho(i) = norm((1-f).*beta(1:p));
end
if (m > n && beta2 > 0), rho = sqrt(rho.^2 + beta2); end

% Compute the Lagrange function and its derivative.
La = rho.^2 + (lambda.^2).*(eta.^2);
dLa = 2*lambda.*(eta.^2);
[mindLa,mindLi] = min(dLa); lambda0 = lambda(mindLi);

% Plot the functions.
if (nargin==3)
  loglog(lambda,La,'-',lambda,dLa,'--',lambda0,mindLa,'o')
  legend('La','dLa/d\lambda')
else
  loglog(lambda,La,'-',lambda,dLa,'--',lambda,eta,':',lambda,rho,'-.',...
         lambda0,mindLa,'o')
  legend('La','dLa/d\lambda','|| L x ||_2','|| A x - b ||_2')
end
xlabel('\lambda')
title('Lagrange function La and its derivative')


function probVal=trivarNormCDF(b,mu,R)
%%TRIVARNORMCDF Evaluate the cumulative density function of the trivariate
%              normal distribution with a specified mean and covariance
%              matrix. This evaluates Pr{x1<b(1), x2<b(2), x3<b(3)) where
%              the random vector is [x1;x2;x3].
%
%INPUTS: b A 2X1 vector [b1;b2] such that b1 is the upper bound of the
%           first variable and b2 is the upper bound on the second
%           variable.
%        mu The 2X1 mean of the distribution. If this is omitted or an
%           empty matrix is passed, then the default of [0;0] is used.
%         R The 2X2 covariance matrix of the distribution. If this is
%           omitted or an empty matrix is passed, then R=eye(2,2) is used.
%
%OUTPUTS: probVal The value of the trivariate normal CDF.
%
%This function implements the second Plankett method that is described in
%[1]. Specifically, Equation 14 is used.
%
%REFERENCES:
%[1] A. Genz, "Numerical computation of rectangular bivariate and
%    trivariate normal and t probabilites," Statistics and Computing, vol.
%    14, pp. 251-260, 2004.
%
%July 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public releas

if(nargin<3||isempty(R))
    R=eye(3,3);
end

if(nargin<2||isempty(mu))
    mu=[0;0;0];
end

%The exteme for one bound.
if(any(b)==-Inf)
    probVal=0;
    return;
end

%If any of the bounds is Inf, then then problem reduces to a 2D integral.
if(b(1)==Inf)
    R=R(2:3,2:3);
    mu=mu(2:3);
    b=b(2:3);
    probVal=bivarNormCDF(b,mu,R);
    return
elseif(b(2)==Inf)
     R=R([1,3],[1,3]);
     mu=mu([1;3]);
     b=b([1;3]);
     probVal=bivarNormCDF(b,mu,R);
     return;
elseif(b(3)==Inf)
    R=R(1:2,1:2);
    mu=mu(1:2);
    b=b(1:2);
    probVal=bivarNormCDF(b,mu,R);
    return
end

%Center the distribution.
b=b-mu;

%Scale the R matrix (and the associated b values) to make the diagonals of
%R all 1.
S=diag(1./sqrt(diag(R)));
b=S*b;
R=S*R*S';

%The correlation values.
pVec=[R(2,1);R(3,1);R(3,2)];

%We want the variables of integration to be permuted to minimize
%max(abs(p21),abs(p31)). This means that out of the above values, p21, p31,
%and p32, we want to change the ordering so that p21 and p31 are the values
%with the smallest magnitudes.
[~,idx]=sort(abs(pVec),'ascend');
pVec=pVec(idx);
b=b(idx);

b1=b(1);
b2=b(2);
b3=b(3);
p21=pVec(1);
p31=pVec(2);
p32=pVec(3);

R2D=[1,  p32;
     p32,1];
%The first term in Equation 14 in 1.
term1=GaussianD.CDF(b1)*bivarNormCDF([b2;b3],[],R2D);
f=@(t)costFun(t,b1,b2,b3,p21,p31,p32);

RelTol=1e-18;
AbsTol=1e-18;
term2=integral1DAdaptive(f,[0;1],[],[],[],RelTol,AbsTol);
probVal=term1+term2;

%In case finite precision limitations make the value invalid.
probVal=min(max(probVal,0),1);

end

function val=costFun(t,b1,b2,b3,p21,p31,p32)

t2=t.^2;
denomTerm2=1-p31^2*t2-p21^2*t2-p32^2+2*t2*p31*p21*p32;
denomTerm3=1-p21^2*t2-p31^2*t2-p32^2+2*t2*p31*p21*p32;
u2Hat=(b2*(1-p31^2*t2)-b1*t*(p21-p31*p32)-b3*(p32-p31*p21*t2))./sqrt((1-p31^2.*t2).*denomTerm2);
u3Hat=(b3*(1-p21^2*t2)-b1*t*(p31-p21*p32)-b2*(p32-p31*p21*t2))./sqrt((1-p21^2.*t2).*denomTerm3);

r=p31*t;
f2=(b1^2+b3^2-2*r*b1*b3)./(1-r.^2);
f2(isnan(f2))=Inf;
r=p21*t;
f3=(b1^2+b2^2-2*r*b1*b2)./(1-r.^2);
f3(isnan(f3))=Inf;
%The above NaN stuff should make it work properly if, for example, b1 and
%b2 are near realmax or realmin.

Phiu3Hat=GaussianD.CDF(u3Hat);
Phiu2Hat=GaussianD.CDF(u2Hat);

val=Phiu3Hat.*p21.*exp(-f3/2)./sqrt(1-p21^2.*t2)+Phiu2Hat.*p31.*exp(-f2/2)./sqrt(1-p31.^2.*t2);

end

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


function [a,f,B,W]=v_ldatrace(b,w,n,c)
%V_LDATRACE Calculates an LDA transform to maximize trace discriminant [a,f,B,W]=(b,w,n,c)
% If a feature vector X can come from one of several class and W and B are respectively
% the within-class and between-class covariance matrices, then the generalized Fisher discriminant
% F=trace(W\B) is a measure of how well the feature vector discriminates between the classes.
% If we choose a rectangular (tall, skinny) transformation matrix, we can define a smaller
% feature vector Y=A'*X. The aim of this routine is to choose A to maximize the Fisher
% discriminant. We assume that W is positive definite and B is positive semi-definite.
% The input argument C allows the uset to pre-specify some of the columns of A.
%
% Inputs:
%     w[m,m] = within class covariance matrix of x
%     b[m,m] = between class covariance matrix of x [default = I]
%     n is the number of columns in output matrix A [default = M]
%     c[m,r] specifies the first few columns of A to be predefined values [default = null)
%
% Outputs:
%     a[m,n] is the transformation matrix: y=a'*x
%     f[n,1] gives the incremental gain in f value for successive columns of A
%     B(n,n) gives the between-class covariance matrix of y
%     W[n,n] gives the within-class covariance matrix of y

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: v_ldatrace.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

m=size(b,1);    % dimension of data vectors
if nargin<4
    r=0;
    if nargin<3
        n=m;
        if nargin<2
            w=eye(m);
        end
    end
else
    r=size(c,2);    % number of columns that are pre-specified
end
if r
    if n>r          % need to find additional vectors
        g=chol(w);
        v=g\null(c'*g');
        [p,l,q]=svd(v'*b*v);
        a(:,r+1:n)=v*p(:,1:n-r);
        a(:,1:r)=c;
    else
        a=c;        % no new vectors to find
    end
    if nargout>1
        ari=a/triu(qr(chol(a'*w*a))); % matrix a must be of full rank
        f=diag(ari'*b*ari);
    end
else
    [g,d]=eig(b,w,'qz');
    [ds,is]=sort(-diag(d));
    a=g(:,is(1:n));
    if nargout>1
        f=-ds(1:n);
    end
end
if nargout > 2
    B=a'*b*a;
    if nargout > 3
        W=a'*w*a;
    end

end


function varargout = carromtable(X)
% Carrom table function
%
%   CARROMTABLE([x1, x2]) returns the value of the Carrom table
%   function at the specified points. [x1] and [x2] may be vectors.
%   The search domain is
%
%               -10 < x_i < 10
%
%   The four global minimum are found near the corners of the interval,
%   with
%
%               fmin = 24.1568155.

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

    % if no input is given, return dimensions, bounds and minimum
    if (nargin == 0)
        varargout{1} = 2;  % # dims
        varargout{2} = [-10, -10]; % LB
        varargout{3} = [+10, +10]; % UB
        varargout{4} = [+9.646157266348881e+000, +9.646134286497169e+000
                        -9.646157266348881e+000, +9.646134286497169e+000
                        +9.646157266348881e+000, -9.646134286497169e+000
                        -9.646157266348881e+000, -9.646134286497169e+000]; % solution
        varargout{5} = -2.415681551650653e+001; % function value at solution

    % otherwise, output function value
    else

        % keep values in the serach interval
        X(X < -10) = inf;     X(X > 10) = inf;

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

        % output function value
        varargout{1} = -((cos(x1).*cos(x2).*exp(abs(1 - sqrt(x1.^2 + x2.^2)/pi))).^2)/30;

    end

end


function [kl1,kl2] = mvnkl(Mu1,Sigma1,Mu2,Sigma2)
%MVNKL Kullback-Leibler divergence between two multivariate normal pdfs.

D = numel(Mu1);

Mu1 = Mu1(:);
Mu2 = Mu2(:);

dmu = Mu2 - Mu1;
detq1 = det(Sigma1);
detq2 = det(Sigma2);
lndet = log(detq2 / detq1);

kl1 = 0.5*(trace(Sigma2\Sigma1) + dmu'*(Sigma2\dmu) - D + lndet);
if nargout > 1
    kl2 = 0.5*(trace(Sigma1\Sigma2) + dmu'*(Sigma1\dmu) - D - lndet);
end


function yp = fpcube ( x )

%*****************************************************************************80
%
%% FPCUBE sets the derivative of the cubic function.
%
%  Discussion:
%
%    Y(X) = ( ( X + 2 ) * X + 3 ) * X + 4
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real YP, the value of the derivative of the cubic function.
%
  yp = ( 3.0E+00 * x + 4.0E+00 ) * x + 3.0E+00;

  return
end


function [theta, J_history] = gradientDescent(X, y, theta, alpha, num_iters)
%GRADIENTDESCENT Performs gradient descent to learn theta
%   theta = GRADIENTDESENT(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 (computeCost) and gradient here.
    %
    theta_temp = theta;
    for j = 1:size(X, 2)
        theta_temp(j) = theta(j)-alpha*(1/m)*(X*theta - y)' * X(:, j);
    end
    theta = theta_temp;

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

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

end

end


function x = tuple_next_fast ( m, n, rank )

%*****************************************************************************80
%
%% TUPLE_NEXT_FAST computes the next element of a tuple space, "fast".
%
%  Discussion:
%
%    The elements are N vectors.  Each entry is constrained to lie
%    between 1 and M.  The elements are produced one at a time.
%    The first element is
%      (1,1,...,1)
%    and the last element is
%      (M,M,...,M)
%    Intermediate elements are produced in lexicographic order.
%
%    This code was written as a possibly faster version of TUPLE_NEXT.
%
%  Example:
%
%    N = 2,
%    M = 3
%
%    INPUT        OUTPUT
%    -------      -------
%    Rank          X
%    ----          ----
%   -1            -1 -1
%
%    0             1  1
%    1             1  2
%    2             1  3
%    3             2  1
%    4             2  2
%    5             2  3
%    6             3  1
%    7             3  2
%    8             3  3
%    9             1  1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 August 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the maximum entry in each component.
%    M must be greater than 0.
%
%    Input, integer N, the number of components.
%    N must be greater than 0.
%
%    Input, integer RANK, indicates the rank of the tuples.
%    Typically, 0 <= RANK < N**M; values greater than this are
%    legal and meaningful, being equivalent to the corresponding
%    value mod N**M.  RANK < 0 indicates that this is the first
%    call for the given values of (M,N).  Initialization is done,
%    and X is set to a dummy value.
%
%    Output, integer X(N), the next tuple of the given rank,
%    or a dummy value if initialization is being done.
%
  global tuple_next_fast_BASE

  if ( rank < 0 )

    if ( m <= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'TUPLE_NEXT_FAST - Fatal error!\n' );
      fprintf ( 1, '  M <= 0 is illegal.\n' );
      fprintf ( 1, '  M = %d\n', m );
      error ( 'TUPLE_NEXT_FAST - Fatal error!' );
    end

    if ( n <= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'TUPLE_NEXT_FAST - Fatal error!\n' );
      fprintf ( 1, '  N <= 0 is illegal.\n' );
      fprintf ( 1, '  N = %d\n', n );
      error ( 'TUPLE_NEXT_FAST - Fatal error!' );
    end

    tuple_next_fast_BASE(n) = 1;
    for i = n-1 : -1 : 1
      tuple_next_fast_BASE(i) = tuple_next_fast_BASE(i+1) * m;
    end

    x(1:n) = -1;

  else

    x(1:n) = mod ( floor ( rank ./ tuple_next_fast_BASE(1:n) ), m ) + 1;

  end

  return
end


function orbEls=state2OrbEls(stateVec,elType,GM,epsVal)
%%STATE2ORBELS Convert a state vector consisting of at least position and
%              velocity into a set of orbital elements. Such elements are
%              for a simple two-body problem where the satellite (object)
%              in motion has negligible mass. The elements can be found
%              with respect to a given epoch time.
%
%INPUTS: stateVec A 6XnumVec matrix of numVec state vectors consisting of
%                 position and velocity in a Cartesian (quasi)-inertial
%                 coordinate system where the gravitating body is at the
%                 origin. Extra state elements are ignored. The units are
%                 assumed to be meters and meters per second.
%          elType A value indicating the type of orbital elements. Possible
%                 values are:
%                 0 (The default if omitted) The elements are Gooding's
%                   universal orbital elements.
%                 2 The elements are direct equinoctial orbital elements.
%                 3 The elements are retrograde equnoctial orbital
%                   elements.
%              GM Optionally, the universal gravitation constant times the
%                 mass of the body about which the object with the given
%                 state vector is orbiting. If this parameter is omitted,
%                 the value in Constants.WGS84GMWithAtmosphere is used. The
%                 units are m^3/sec^2.
%          epsVal If universal orbital elements are chosen, this is a
%                 precision bound used for equality comparisons when
%                 determining whether a trajectory is rectilinear,
%                 parabolic or circular. If omitted, a default value of
%                 eps is used.
%
%OUTPUTS: orbEls An 6XnumVec set of vectors of orbital elements, the format
%                of which depends on the elType parameter. The format of
%                the elements is discussed below.
%
%Gooding's universal orbital elements are consist of (in order)
%alpha=GM/a where a is the semi-major axis in meters
%q=a*(1-e)  where e is the eccentricity (unitless). This is the perifocal
%           distance.
%i          inclination in radians
%Omega      longitude (right ascension) of the ascending node in radians
%omega      argument of periapsis/perigee in radians
%tau        the time at pericenter (seconds)
%
%The equinoctial orbital elements (for both direct and retrograde elements)
%are
%a      semi-major axis in meters
%h      first eccentricity vector component (unitless)
%k      second eccentricity vector component (unitless)
%p      first ascending node vector component (unitless)
%q      second ascending node vector component (unitless)
%lambda mean longitude in radians.
%
%The algorithm using Gooding's universal orbital elements is very robust to
%numerical errors. It is implemented using the algorithm of [1], where the
%elements are also described.
%
%Equinoctial orbital elements are discussed in Section 2 of [2]. and in
%[3]. The diffference between direct and retrograde elements is essentially
%a matter of handedness.
%
%Orbital elements are discussed in general in Chapter 2.2 of [4].
%
%The inverse of this function is orbEls2State, which can also predict the
%state forward in time. Adding a time interval in seconds to the tau
%argument also predicts the trajectory forward in terms of universal
%orbital elements.
%
%REFERENCES:
%[1] R. H. Gooding, "On universal elements, and conversion procedures to
%    and from position and velocity," Celestial mechanics, vol. 44, no. 3,
%    pp. 283-298, 1988.
%[2] D. A. Danielson, C. P. Sagovac, B. Neta, and L. W. Early,
%    "Semianalytic satellite theory," Mathematics Department, Naval
%    Postgraduate School, Monterey, CA, Tech. Rep., 1995. [Online].
%    Available: http://oai.dtic.mil/oai/oai?verb=getRecord&metadataPrefix= html&identifier=ADA531136
%[3] R. A. Broucke and P. J. Cefola, "On the equinoctial orbit elements,"
%    Celestial Mechanics, vol. 5, no. 3, pp. 303-310, 1972.
%[4] O. Montenbruck and E. Gill, Satellite Orbits: Models, Methods
%    Applications. Berlin: Springer, 2000.
%
%January 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4)
    epsVal=eps;
end

if(nargin<3)
   GM=Constants.WGS84GMWithAtmosphere;
end

if(nargin<2)
    elType=0;
end

numVec=size(stateVec,2);
orbEls=zeros(6,numVec);

switch(elType)
    case 0
        for curVec=1:numVec
            orbEls(:,curVec)=state2OrbElsUniv(stateVec(1:6,curVec),GM,epsVal);
        end
    case 1
        for curVec=1:numVec
            orbEls(:,curVec)=state2OrbElsEquinoctial(stateVec(1:6,curVec),1,GM);
        end
    case 2
        for curVec=1:numVec
            orbEls(:,curVec)=state2OrbElsEquinoctial(stateVec(1:6,curVec),-1,GM);
        end
    otherwise
        error('Invalid element type provided.')
end

end

function elEqui=state2OrbElsEquinoctial(state,I,GM)
%%STATE2ORBELSEQUINOCTIAL An algorithm to convert from a target state to a
%                         set of equinoctial orbital elements.
%
%The input I is the retrograde factor ond is +1 for direct equinoctial
%elements and -1 for retrograde equinoctial elements.
%
%The algorithm is taken from Section 2.1.5 of
%D. A. Danielson, C. P. Sagovac, B. Neta, and L. W. Early, "Semianalytic
%satellite theory," Mathematics Department, Naval Postgraduate School,
%Monterey, CA, Tech. Rep., 1995. [Online]. Available:
%http://oai.dtic.mil/oai/oai?verb=getRecord&metadataPrefix= html&identifier=ADA531136
%
%January 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

%position
r=state(1:3);
%velocity
rDot=state(4:6);

%Equation 1
a=1/(2/norm(r)-norm(rDot)^2/GM);%Semi-major axis.
%Equation 2
w=cross(r,rDot)/norm(cross(r,rDot));

%Equation 3
p=w(1)/(1+I*w(3));%First ascending node vector component.
q=-w(2)/(1+I*w(3));%Second ascending node vector component.

%Equation 4
e=-r/norm(r)+cross(rDot,cross(r,rDot))/GM;

%Equation 2.1.4-1
coeff=(1/(1+p^2+q^2));
f=coeff*[1-p^2+q^2;
         2*p*q;
         -2*I*p];
g=coeff*[2*I*p*q;
         (1+p^2-q^2)*I;
         2*q];

%Equation 5
h=dot(e,g);%First eccentricity vector component.
k=dot(e,f);%Second eccentricity vector component.

%Equation 6
X=dot(r,f);
Y=dot(r,g);

%Equation 2.1.4-4
b=1/(1+sqrt(1-h^2-k^2));
%Equation 7
denom=a*sqrt(1-h^2-k^2);
sinF=h+((1-h^2*b)*Y-h*k*b*X)/denom;
cosF=k+((1-k^2*b)*X-h*k*b*Y)/denom;
F=atan2(sinF,cosF);

lambda=F+h*cosF-k*sinF;%Mean longitude

elEqui=[a;h;k;p;q;lambda];

end

function elUniv=state2OrbElsUniv(state,GM,epsVal)
%%STATE2ORBELSUNIV An implementation of Gooding's conversion from a target
%                  state to universal orbital elements. This implements the
%                  PV3ELS function.
%
%The implementation is taken from Appendix D of
%R. H. Gooding, "On universal elements, and conversion procedures to
%and from position and velocity," Celestial mechanics, vol. 44, no. 3,
%pp. 283-298, 1988.
%with minor changes so that equality comparisons are replaced with
%comparisons within a region epsVal.
%
%October 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.

rVec=state(1:3);
vVec=state(4:6);
x=rVec(1);
y=rVec(2);
z=rVec(3);

xyMag=norm(rVec(1:2));
r=norm(rVec);

%The projection of the velocity onto the radial component.
VR=dot(rVec,vVec)/r;%Radial velocity

%The angular momentum vector.
hVec=cross(rVec,vVec);

if(norm(hVec)^2<epsVal)%Rectilinear orbit
    %This works for an axial orbit as well as for a general rectilinear
    %orbit.
    i=pi/2;%inclination
    %For axial or general rectilinear orbits, atan2 will provide the
    %correct result. When axial and both are zero, it will return 0.
    Omega=atan2(y,x);%Longitude of the ascending node.
    %Angle from assumed reference direction (argument of latitude)
    u=atan2(z,xyMag);
    VT=0;%Transverse velocity
else%Non degenerate orbit
    b=cross(hVec,rVec);

    %r2h is r^2*h as explained before the beginning of Section 5.
    r2h=cross(rVec,b);
    W=dot(r2h(1:2),r2h(1:2));

    i=atan2(sqrt(W),r2h(3));%inclination

    if(W<epsVal)%If the orbit is in the reference plane
        Omega=0;%Longitude of the ascending node.
        u=atan2(y*sign(r2h(3)),x);%Angle from assumed reference direction
    else%General orbit
        Omega=atan2(r2h(1),-r2h(2));%Longitude of the ascending node.
        %Angle from assumed reference direction (argument of latitude)
        u=atan2(norm(r2h)*z,r^2*b(3));
    end
    VT=norm(r2h)/r^3;%Transverse velocity
end

[alpha,q,omega,tau]=PV2ELS(VR,VT,r,u,GM,epsVal);

elUniv=[alpha;
            q;
            i;
        Omega;
        omega;
          tau];
end

function [alpha,q,omega,tau]=PV2ELS(VR,VT,r,u,GM,epsVal)
%%PV2ELS  An implementation of the PV2ELS subroutine of Gooding's algorithm
%        for universal orbital element conversion. This is a
%        two-dimensional conversion suroutine.
%
%INPUTS VR, VT Radial and transerver velocity. Note VT>=0.
%       r      Radial distance.
%       u      Angle from assumed reference direction.
%       GM     Universal gravitational constant times the mass of the
%              gravitating body.
%       epsVal A small number for determining equality within finite
%              precision bounds.
%
%The algorithm is taken from Appendix C of
%R. H. Gooding, "On universal elements, and conversion procedures to
%and from position and velocity," Celestial mechanics, vol. 44, no. 3,
%pp. 283-298, 1988.
%with minor changes so that equality comparisons are not performed.
%
%October 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.

sw=0.25;

VMag2=VR^2+VT^2;
alpha=2*GM/r-VMag2;

d=r*VR;
h=r*VT;
p=h^2;

esq1=p*alpha;
es=d*sqrt(abs(alpha));
ec=r*VMag2-GM;
if(alpha>0)%One formula superior for the ellipse
    e=sqrt(ec^2+es^2);
else%Different formula superior for the hyperbola
    e=sqrt(GM^2-esq1);
end

q=p/(GM+e);
if(abs(alpha)<=epsVal)%Parabola
    tau=d*(2*q+r)/(3*GM);
    v=2*atan2(VR,VT);%The true anomaly
elseif(abs(e)<=epsVal)%Circle
    tau=0;
    v=0;%The true anomaly
else%Ellipse or hyperbola
    e1=alpha*q;

    if(alpha>0)%Ellipse
        eh=atan2(es,ec);
        if(GM*eh^2/6+e1>GM*sw)%General case
            em=GM*eh-es;
            ecesq=GM*ec-e^2;
        else%For e1 and eh both near zero
            em=GM*GoodingSinDiffFun(e1/GM,eh);
            ecesq=(esq1*ec^2-e^2*es^2)/(e^2+GM*ec);
        end
    else%Hyperbola
        eh=asinh(es/e);

        if(GM*eh^2/6-e1>GM*sw)%General case
            em=es-GM*eh;
            ecesq=e^2-GM*ec;
        else%For e1 and eh both near zero
            em=e*GoodingHyperSinDiffFun(-e1/e,es/e);
            ecesq=-(esq1*ec^2+e^2*es^2)/(e^2+GM*ec);
        end
    end
    %Still ellipse or hyperbola
    en=abs(alpha)^(3/2);
    tau=em/en;
    v=atan2(es*h*sqrt(abs(alpha)),ecesq);%The true anomaly
end
%All orbits
omega=u-v;%The argument of periapsis

%Adjust revolutions, if necessary, so that omega remains in the range
%-pi to pi. The second condition in the if-statement takes care of possible
%parabolic case; this is just for the elliptical case.
if(alpha>0&&abs(alpha)>epsVal)
    adj=2*pi*fix(abs(omega/(2*pi))+(1/2))*sign(omega);
    omega=omega-adj;
    tau=tau+adj/en;
end

end

function x=GoodingSinDiffFun(e,EE)
%%GOODINGSINDIFFFUN Evaluate the function EE-(1-e)*sin(EE) using Gooding's
%                   EMKEP procedure for when e and EE are close
%                   to (1,0). This is supposed to be more accurate than
%                   just directly evaluating the functions, unless EE is
%                   large as it is then supposed to worsen rounding errors.
%
%
%The algorithm is the EMKPL algorithm taken from Appendix C of
%A. W. Odell and R. H. Gooding, "Procedure for solving Kepler's
%equation," Celestial Mechanics, vol. 38, no. 4, pp. 307-334, Apr. 1986.
%modified to solve EE-(1-e)*sin(EE) instead of EE-e*sin(EE).
%
%October 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.

x=e*sin(EE);
EE2=-EE^2;
term=EE;
d=0;
while(1)
    d=d+2;
    term=term*EE2/(d*(d+1));
    x0=x;
    x=x-term;
    if(x==x0)
        break;
    end
end

end

function x=GoodingHyperSinDiffFun(g1,s)
%%GOODINGHYPERSINDIFFFUN Evaluate the function s-(1-g1)*asinh(s) when
%                        (g1,s) is close to (0,0) using Gooding's method.
%                        This is supposed to have a higher precision than
%                        just explicitly evaluating the function.
%
%The algorithm is the SHMKEP function taken from Appendix B of
%R. H. Gooding and A. W. Odell, "The hyperbolic Kepler's equation,
%and the elliptic equations revisited," Royal Aerospace Executive,
%Procurement Executive, Ministry of Defence, Farnborough, Hants, United
%Kingdom, Tech. Rep. 369, Jul. 1989.
%
%October 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.

g=1-g1;
t=s/(1+sqrt(1+s^2));
x=s*(g1+g*t^2);
term=2*g*t;
twoI1=1;
%Iterate until convergence or until a maximum number of iterations is
%reached.
maxIter=64;
for curIter=1:maxIter
    twoI1=twoI1+2;
    term=term*t^2;
    x0=x;
    x=x-term/twoI1;
    if(x==x0)
        break;
    end
end
end

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


function fx = p34_fun ( n, x )

%*****************************************************************************80
%
%% P34_FUN evaluates the integrand for problem 34.
%
%  Interval:
%
%    0 <= x <= 1
%
%  Integrand:
%
%    ( 10 * x - 1 ) * ( 10 * x - 1.1 ) * ( 10 * x - 1.2 ) * ( 10 * x - 1.3 )
%
%  Exact Integral:
%
%    1627879 / 1500
%
%  Approximate Integral (20 digits):
%
%    1085.2526666666666666...
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Hermann Engels,
%    Numerical Quadrature and Cubature,
%    Academic Press, 1980.
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), the evaluation points.
%
%    Output, real FX(N), the integrand values.
%
  fx = ( 10.0 * x - 1.0 ) .* ( 10.0 * x - 1.1 ) .* ( 10.0 * x - 1.2 ) ...
    .* ( 10.0 * x - 1.3 );

  return
end


function p = gaussian_prob(x, m, C, use_log)
% GAUSSIAN_PROB Evaluate a multivariate Gaussian density.
% p = gaussian_prob(X, m, C)
% p(i) = N(X(:,i), m, C) where C = covariance matrix and each COLUMN of x is a datavector

% p = gaussian_prob(X, m, C, 1) returns log N(X(:,i), m, C) (to prevents underflow).
%
% If X has size dxN, then p has size Nx1, where N = number of examples

if nargin < 4, use_log = 0; end

if length(m)==1 % scalar
  x = x(:)';
end
[d N] = size(x);
%assert(length(m)==d); % slow
m = m(:);
M = m*ones(1,N); % replicate the mean across columns
denom = (2*pi)^(d/2)*sqrt(abs(det(C)));
mahal = sum(((x-M)'*inv(C)).*(x-M)',2);   % Chris Bregler's trick
if any(mahal<0)
  warning('mahal < 0 => C is not psd')
end
if use_log
  p = -0.5*mahal - log(denom);
else
  p = exp(-0.5*mahal) / (denom+eps);
end


function [v,d,dref] = generate_doddbench(N)
% Generate DODDBENCH signal.
%
% Benchmark signal introduced in Dodd, T.J., Kadirkamanathan, V. and
% Harrison, R.F., "Function estimation in Hilbert space using sequential
% projections," Proc. of the IFAC Conf. on Intelligent Control Systems and
% Signal Processing, 113-118, 2003.
%
% Comment: copyright Cedric Richard, http://cedric-richard.fr/
%
% Input: N: number of data points
%
% Outputs: v: input sequence (2-dimensional sequence [v(:,1);v(:,2)])
%          d: noisy desired output (1-dimensional sequence)
%          dref: noise-free desired output
%
% This file is part of the Kernel Adaptive Filtering Toolbox for Matlab.
% https://github.com/steven2358/kafbox/

dref = zeros(1,N+2);
dref(1:2)=[0.1 0.1];

for t=3:N+2
    dref(t) = (0.8-0.5*exp(-dref(t-1)^2))*dref(t-1) - ...
        (0.3+0.9*exp(-dref(t-1)^2))*dref(t-2)+0.1*sin(pi*dref(t-1));
end
d = dref + 0.1*randn(1,N+2);
v = [d(1:N); d(2:N+1)]';

d(1:2)=[];
dref(1:2)=[];


function [ x, seed ] = simplex_unit_sample ( dim_num, n, seed )

%*****************************************************************************80
%
%% SIMPLEX_UNIT_SAMPLE samples the unit simplex.
%
%  Discussion:
%
%    The interior of the unit DIM_NUM-dimensional simplex is the set of
%    points X(1:DIM_NUM) such that each X(I) is nonnegative, and
%    sum(X(1:DIM_NUM)) <= 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 August 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Reuven Rubinstein,
%    Monte Carlo Optimization, Simulation, and Sensitivity
%    of Queueing Networks,
%    Wiley, 1986.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the space.
%
%    Input, integer N, the number of points.
%
%    Input/output, integer SEED, a seed for the random number generator.
%
%    Output, real X(DIM_NUM,N), the points.
%

%
%  The construction begins by sampling DIM_NUM+1 points from the
%  exponential distribution with parameter 1.
%
  for j = 1 : n

    [ e, seed ] = r8vec_uniform_01 ( dim_num+1, seed );

    e(1:dim_num+1) = -log ( e(1:dim_num+1) );

    x(1:dim_num,j) = e(1:dim_num)' / sum ( e(1:dim_num+1) );

  end

  return
end


function area = triangle_area_2d ( t )

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

  return
end


if (~exist('syms.m','file'))
    fprintf('The symbolic toolbox is not installed, skipping test\n');
    return;
end

%% Gradients
clear
clc

syms x1 x2
x = [0.1;0.1];

%OPTI Sym
fun = @(x) sin(x(1) + x(2)) + (x(1) - x(2))^2 - 1.5*x(1) + 2.5*x(2) + 1
[jac,jacp] = symJac(fun)
[hess,hessp] = symHess(fun)

%Symbolic
sfun = sin(x1+x2) + (x1-x2)^2 - 1.5*x1 + 2.5*x2 + 1
jac1 = jacobian(sfun)
hess1 = hessian(sfun)

%Eval
[jac(x);double(subs(jac1,{'x1','x2'},{x(1) x(2)}))]
full(jacp())
[hess(x);double(subs(hess1,{'x1','x2'},{x(1) x(2)}))]
full(hessp())

%OPTI Sym
fun = @(x) [x(1) + x(2) - 1;
            x(1)^2 + x(2)^2 - 1];
jac = symJac(fun)

%Symbolic
fun = [x1 + x2 - 1;
       x1^2 + x2^2 - 1];
jac1 = jacobian(fun)

%Eval
[jac(x);double(subs(jac1,{'x1','x2'},{x(1) x(2)}))]

%OPTI Sym
fun = @(x) 0.01*(x(1)-1)^2 + (x(2)-x(1)^2)^2;
[jac,jacp] = symJac(fun)
[hess,hessp] = symHess(fun)

%Symbolic
sfun = 0.01*(x1-1)^2 + (x2-x1^2)^2;
jac1 = jacobian(sfun)
hess1 = hessian(sfun)

%Eval
[jac(x);double(subs(jac1,{'x1','x2'},{x(1) x(2)}))]
full(jacp())
[hess(x);double(subs(hess1,{'x1','x2'},{x(1) x(2)}))]
full(hessp())

%% Symbolic Hessian of Lagrangian
clc
fun = @(x) x(1)*x(4)*(x(1) + x(2) + x(3)) + x(3);
% Nonlinear Constraints
 nlcon = @(x) [ x(1)*x(2)*x(3)*x(4);
                x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2 ];

jac = symJac(fun)
hessObj = symHess(fun)

[hessLag,pattern] = symHessLag(fun,nlcon,4,true)

% Hessian
 H = @(x,sigma,lambda) sparse(sigma*[ 2*x(4)             0      0   0;
                                      x(4)               0      0   0;
                                      x(4)               0      0   0;
                                      2*x(1)+x(2)+x(3)  x(1)  x(1)  0 ] + ...
                      lambda(1)*[   0          0         0         0;
                                 x(3)*x(4)     0         0         0;
                                 x(2)*x(4) x(1)*x(4)     0         0;
                                 x(2)*x(3) x(1)*x(3) x(1)*x(2)     0  ] + ...
                      lambda(2)*diag([2 2 2 2]));

x = 0.1:0.1:0.4;
sigma = 0.3;
lambda = 1:2;

hessLag(x,sigma,lambda)
full(H(x,sigma,lambda))
full(pattern())

%% Application to an NLP, first no derivs
clc
% Objective
 fun = @(x) x(1)*x(4)*(x(1) + x(2) + x(3)) + x(3);

% Nonlinear Constraints
 nlcon = @(x) [ x(1)*x(2)*x(3)*x(4);
                x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2 ];
 cl = [25;40];
 cu = [Inf;40];

% Bounds (lb <= x <= ub)
 lb = ones(4,1);
 ub = 5*ones(4,1);

% Initial Guess
 x0 = [1 5 5 1]';

opts = optiset('display','iter','solver','ipopt','derivCheck','on','solverOpts',ipoptset('derivative_test','first-order'));
Opt = opti('fun',fun,'nl',nlcon,cl,cu,'bounds',lb,ub,'x0',x0,'options',opts)

[x,f,e,i] = solve(Opt)

%% Application to an NLP, now with symJac, symHessLag
clc
% Objective
 fun = @(x) x(1)*x(4)*(x(1) + x(2) + x(3)) + x(3);

grad = symJac(fun);

% Nonlinear Constraints
 nlcon = @(x) [ x(1)*x(2)*x(3)*x(4);
                x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2 ];

[nljac,nljacstr] = symJac(nlcon);

 cl = [25;40];
 cu = [Inf;40];

 %Hessian
 [hess,hstr] = symHessLag(fun,nlcon);

% Bounds (lb <= x <= ub)
 lb = ones(4,1);
 ub = 5*ones(4,1);

% Initial Guess
 x0 = [1 5 5 1]';

opts = optiset('display','iter','solver','ipopt','derivCheck','on','solverOpts',ipoptset('derivative_test','second-order'));
Opt = opti('fun',fun,'grad',grad,'nl',nlcon,cl,cu,'nljac',nljac,'nljacstr',nljacstr,'H',hess,'HStr',hstr,'bounds',lb,ub,'x0',x0,'options',opts)

[x,f,e,i] = solve(Opt)

%% Application to an NLP, analytical derivs supplied
clc
% Objective
 fun = @(x) x(1)*x(4)*(x(1) + x(2) + x(3)) + x(3);

% Gradient
 grad = @(x) [x(1)*x(4) + x(4)*sum(x(1:3)), x(1)*x(4),...
              x(1)*x(4) + 1,  x(1)*sum(x(1:3))];

% Nonlinear Constraints
 nlcon = @(x) [ x(1)*x(2)*x(3)*x(4);
                x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2 ];

% Jacobian
 jac = @(x) sparse([prod(x')./x';
                    2.*x']);

% Jacobian Structure
 jacstr = @() sparse(ones(2,4));

 cl = [25;40];
 cu = [Inf;40];

 %Hessian
% Hessian
 H = @(x,sigma,lambda) sparse(sigma*[ 2*x(4)             0      0   0;
                                      x(4)               0      0   0;
                                      x(4)               0      0   0;
                                      2*x(1)+x(2)+x(3)  x(1)  x(1)  0 ] + ...
                      lambda(1)*[   0          0         0         0;
                                 x(3)*x(4)     0         0         0;
                                 x(2)*x(4) x(1)*x(4)     0         0;
                                 x(2)*x(3) x(1)*x(3) x(1)*x(2)     0  ] + ...
                      lambda(2)*diag([2 2 2 2]));

% Hessian Structure
 Hstr = @() sparse(tril(ones(4)));

% Bounds (lb <= x <= ub)
 lb = ones(4,1);
 ub = 5*ones(4,1);

% Initial Guess
 x0 = [1 5 5 1]';

opts = optiset('display','iter','solver','ipopt','derivCheck','on','solverOpts',ipoptset('derivative_test','second-order'));
Opt = opti('fun',fun,'grad',grad,'nl',nlcon,cl,cu,'nljac',nljac,'nljacstr',nljacstr,'H',H,'HStr',hstr,'bounds',lb,ub,'x0',x0,'options',opts)

[x,f,e,i] = solve(Opt)


function n = vectorNorm3d(v)
%VECTORNORM3D Norm of a 3D vector or of set of 3D vectors
%
%   N = vectorNorm3d(V);
%   Returns the norm of vector V.
%
%   When V is a N-by-3 array, compute norm for each vector of the array.
%   Vector are given as rows. Result is then a N-by-1 array.
%
%   NOTE: compute only euclidean norm.
%
%   See Also
%   vectors3d, normalizeVector3d, vectorAngle3d, hypot3
%

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

%   HISTORY
%   19/06/2009 rename as vectorNorm3d

n = sqrt(sum(v.*v, 2));


function result = polygon_x_2d ( n, v )

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

  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'POLYGON_X_2D - Warning!\n' );
    fprintf ( 1, '  The number of vertices must be at least 3.\n' );
    fprintf ( 1, '  The input value of N = %d\n', n );
    return
  end

  for i = 1 : n

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

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

  end

  result = result / 6.0;

  return
end


function pde = Darcydata4
%% Darcydata4 diagonal and anisotropic tensor
%
% p = x^2 + y^2
% K = [1 + 4*(x^2 + y^2)   0 ]
%   = [0   1 + 11*(x^2 + y^2)]

pde = struct('exactp',@exactp,'K',@K,'exactu',@exactu,'Dp',@gradp,...
             'f', @f,'g_D',@g_D,'g_N',@g_N);

    function s = K(pt)
       x = pt(:,1); y = pt(:,2);
%        s(:,3) = 3*x.*y;
       s(:,1) = 1 + 4*(x.^2 + y.^2);
       s(:,2) = 1 + 11*(x.^2 + y.^2);
    end
    function s = exactp(pt)
       x = pt(:,1); y = pt(:,2);
%        s = x.^3.*y + y.^4 + sin(pi*x).*cos(pi*y);
       s = x.^2 + y.^2;
    end
    function s = gradp(pt)
       x = pt(:,1); y = pt(:,2);
%        s(:,2) = x.^3 + 4*y.^3 - pi*sin(pi*x).*sin(pi*y);
%        s(:,1) = 3*x.^2.*y + pi*cos(pi*x).*cos(pi*y);
       s(:,2) = 2*y;
       s(:,1) = 2*x;
    end
    function s = exactu(pt)
       Dp = gradp(pt);  % u = K*grad(p)
       K = K(pt);
       s(:,2) = K(:,2).*Dp(:,2);
       s(:,1) = K(:,1).*Dp(:,1);
    end
    function s = f(pt)
        x = pt(:,1); y = pt(:,2);
%         s = 2+24*x.^2+14*y.^2+6*x.^2+2+22*x.^2+66*y.^2;
        s = 2+24*x.^2+16*y.^2++2+22*x.^2+66*y.^2;
        s = -s;
    end
    function s = g_D(pt)
       s = exactp(pt);
    end
    function f = g_N(p,vargin)
        if nargin > 1
            f = dot(exactu(p),vargin,2);
        else
            f = zeros(size(p,1),1);
            x = p(:,1);
            y = p(:,2);
            uprime = exactu(p);
            leftbd = (abs(x)<eps);  % n = (-1,0);
            f(leftbd) = - uprime(leftbd,1);
            rightbd = (abs(x-1)<eps); % n = (1,0);
            f(rightbd) = uprime(rightbd,1);
            topbd = (abs(y-1)<eps);   % n = (0,1)
            f(topbd) = uprime(topbd,2);
            bottombd = (abs(y)<eps);% n = (0,-1)
            f(bottombd) = - uprime(bottombd,2);
        end
    end
end


function [w] = hann_p(npt)
%
% Type: [w] = hann_p(npt);
%
% Inputs:
%
% npt   := number of points in the window vector
%
% Outputs:
%
% w     := periodic hanning window vector, npt x 1
%
% Compute the periodic hanning window.

% Scot McNeill, University of Houston, Fall 2007.
%
w=0.5*(1-cos(2*pi*[1:npt].'/(npt)));
%
%%


function b = isPerpendicular3d(v1, v2, varargin)
%ISPERPENDICULAR3D Check orthogonality of two 3D vectors.
%
%   B = isPerpendicular3d(V1, V2)
%   where V1 and V2 are 2 [1x3] arrays, returns 1 if the vectors are
%   orthogonal, and 0 otherwise.
%
%   Also works when V1 and V2 are two [Nx3] arrays with same number of
%   rows. In this case, return a [Nx1] array containing 1 at the positions
%   of parallel vectors.
%
%   Also works when one of V1 or V2 is scalar and the other one is [Nx3]
%   array, in this case return [Nx1] results.
%
%   B = isPerpendicular3d(V1, V2, TOL)
%   Specifies the absolute tolerance (default is 1e-14).
%
%
%   Example
%   isPerpendicular3d([1 0 0], [0 1 0])
%   ans =
%       1
%
%   isPerpendicular3d([1 0 1], [1 0 0])
%   ans =
%       0
%
%   See also
%   vectors3d, isParallel3d

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2006-04-25
% Copyright 2006-2022 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas)

% check if tolerance is specified
tol = 1e-14;
if ~isempty(varargin)
    tol = varargin{1};
end

% compute perpendicularity test
b = abs(sum(bsxfun(@times, v1, v2), 2)) < tol;


function [V] = radius2volume(r)
% radius2volume returns the volume of a sphere of radius r.
% Chad Greene 2012
V = (4*pi/3).*r.^3;


function a = householder ( n, x )

%*****************************************************************************80
%
%% HOUSEHOLDER constructs a HOUSEHOLDER matrix.
%
%  Discussion:
%
%    A Householder matrix is also called an elementary reflector.
%
%  Formula:
%
%     A = I - ( 2 * X * X' ) / ( X' * X )
%
%  Example:
%
%    N = 5, X = ( 1, 1, 1, 0, -1 )
%
%   1/2 -1/2 -1/2  0  1/2
%  -1/2  1/2 -1/2  0  1/2
%  -1/2 -1/2  1/2  0  1/2
%    0    0    0   1   0
%   1/2  1/2  1/2  0  1/2
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A is orthogonal: A' * A = A * A' = I.
%
%    inverse ( A ) = A.
%
%    det ( A ) = -1.
%
%    A is unimodular.
%
%    If Y and Z are nonzero vectors of equal length, and
%      X = ( Y - Z ) / NORM(Y-Z),
%    then
%      A * Y = Z.
%
%    A represents a reflection through the plane which
%    is perpendicular to the vector X.  In particular, A*X = -X.
%
%    LAMBDA(1) = -1;
%    LAMBDA(2:N) = +1.
%
%    If X is the vector used to define H, then X is the eigenvector
%    associated with the eigenvalue -1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gene Golub, Charles Van Loan,
%    Matrix Computations, second edition,
%    Johns Hopkins University Press, Baltimore, Maryland, 1989.
%
%    Pete Stewart,
%    Introduction to Matrix Computations,
%    Academic Press, 1973,
%
%    James Wilkinson,
%    The Algebraic Eigenvalue Problem,
%    Oxford University Press, 1965.
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real X(N), the vector that defines the
%    Householder matrix.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    a(i,i) = 1.0;
  end

  xdot = x(1:n) * x(1:n)';

  if ( 0.0 < xdot )

    for i = 1 : n
      for j = 1 : n
        a(i,j) = a(i,j) - 2.0 * x(i) * x(j) / xdot;
      end
    end

  end

  return
end


function d = bayesgauss(X, CA, MA, P)
%BAYESGAUSS Bayes classifier for Gaussian patterns.
%   D = BAYESGAUSS(X, CA, MA, P) computes the Bayes decision
%   functions of the n-dimensional patterns in the rows of X.
%   CA is an array of size n-by-n-by-W containing W covariance
%   matrices of size n-by-n, where W is the number of classes.
%   MA is an array of size n-by-W, whose columns are the corres-
%   ponding mean vectors. A cov. matrix and a mean vector must be
%   specified for each class, even is some are equal.  X is of size
%   K-by-n, where K is the number of patterns to be classified. P is
%   a 1-by-W array, containing the probabilities of occurrence of
%   each class.  If P is not included in the argument, the classes
%   are assumed to be equally likely.
%
%   D, is a column vector of length K. Its ith element is the class
%   number assigned to the ith vector in X during classification.

%   Copyright 2002-2004 R. C. Gonzalez, R. E. Woods, & S. L. Eddins
%   Digital Image Processing Using MATLAB, Prentice-Hall, 2004
%   $Revision: 1.10 $  $Date: 2004/12/15 20:15:38 $

d = [ ]; % Initialize d.
error(nargchk(3, 4, nargin)) % Verify correct no. of inputs.
n = size(CA, 1);             % Dimension of patterns.

% Protect against the possibility that the class number is
% included as an (n+1)th element of the vectors.
X = double(X(:, 1:n));
W = size(CA, 3); % Number of pattern classes.
K = size(X, 1);  % Number of patterns to classify.
if nargin == 3
   P(1:W) = 1/W; % Classes assumed equally likely.
else
   if sum(P) ~= 1
      error('Elements of P must sum to 1.');
   end
end
% Compute the determinants.
for J = 1:W
   DM(J) = det(CA(:, :, J));
end

% Evaluate the decision functions. Note the use of
% function mahalanobis discussed in Section 12.2.
MA = MA'; % Organize the mean vectors as rows.
for J = 1:W
   C = CA(:,:,J);
   M = MA(J,:);
   k1 = log(P(J));
   k2 = 0.5*log(DM(J));
   L(1:K,1) = k1;
   DET(1:K,1) = k2;
   if P(J) == 0;
      D(1:K, J) = -inf;
   else
      D(:, J) = L - DET - 0.5*mahalanobis(X, C, M);
   end
end

% Find the coordinates of the maximum value in each row. These maxima
% maxima give the class of each pattern:
[i, j] = find(D==repmat(max(D')',1,size(D,2)));
% Re-use X. It now contains the max value along each column.
X = [i j];
% Eliminate multiple classifications of the same patterns. Since
% the class assignment when two or more decision functions give
% the same value is arbitrary, we need to keep only one.
X = sortrows(X);
[b, m] = unique(X(:,1));
X = X(m, :);
% X is now sorted, with the 2nd column giving the class of the
% pattern no. in the 1st col.;  i.e., X(j, 1) refers to the jth
% input pattern, and X(j, 2) is its the class number.

% Output the result of classification. d is a col. vector with
% length equal to the total no. of inout patterns. The elements of
% d are the classes into which the patterns were classified.
d = X(:,2);


%%******************************************************************
%% dwd: distance weighted discrimination.
%%
%% [w,beta,residp,residn,totalviolation,X,y,Z] = ...
%%             dwd(Ap,An,penalty);
%%
%% You're given two matrices, A+ and A-, whose columns
%% give points in R^n you want to separate "nicely" to allow
%% classification of future points (e.g., to see if a patient
%% has breast cancer or not). The "classical" solution puts a
%% hyperplane between the points to maximize the minimum distance
%% from a point to a hyperplane. Steve is interested in smaller
%% instances, and is uncomfortable with the criterion, which is
%% too subject to noise (like l_\infty fitting). He wants to use
%% a criterion that depends on all the points. Initially he wanted
%% to minimize the sum of the inverse squares of the distances, but
%% he had no strong reason for the inverse square, so I suggested
%% just the inverse distance. This turns into a nice socp!
%% If there are mp points on the positive side and mn on the negative,
%% the dimensions are: 2*(mp+mn)+1 constraints,
%% (n+1) + (2) + (mp+mn)*3 variables in SOCs, with the dimensions as
%% given (one large, the rest small), and mp+mn nonneg. variables.
%% The latter are artificial variables to account for points that
%% are on the wrong side of the hyperplane: they can be moved a
%% distance d at a cost penalty*d to put them on the right side.
%%
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

   function [w,beta,residp,residn,totalviolation,X,y,Z] = ...
             dwd(Ap,An,penalty);

   [np,mp] = size(Ap);
   [nn,mn] = size(An);
   n = np;
   nv = 1 + n + 2 + 3*(mp + mn) + mp + mn;
   nc = 1 + 2*(mp + mn);
%%
   blk = cell(2,2);
   blk{1,1} = 'q';
   blk{1,2} = [n+1, 2, 3*ones(1, mp + mn)];
   blk{2,1} = 'l';
   blk{2,2} = mp + mn;
%%
   Avec = cell(2,1);
   A = sparse(nc,nv-mp-mn);
   A(1:mp,2:n+3) = [Ap',zeros(mp,1),ones(mp,1)];
   A(mp+1:mp+mn,2:n+3) = -[An',zeros(mn,1),ones(mn,1)];
   A(1:mp,n+4:3:n+3+3*mp) = - speye(mp,mp);
   A(1:mp,n+6:3:n+5+3*mp) = - speye(mp,mp);
   A(mp+1:mp+mn,3*mp+n+4:3:3*mp+n+3+3*mn) = - speye(mn,mn);
   A(mp+1:mp+mn,3*mp+n+6:3:3*mp+n+5+3*mn) = - speye(mn,mn);
   A(mp+mn+1,1) = 1;
   A(mp+mn+2:mp+mn+1+mp,n+5:3:n+4+3*mp) = speye(mp,mp);
   A(mp+mn+1+mp+1:mp+mn+1+mp+mn,3*mp+n+5:3:3*mp+n+4+3*mn) = speye(mn,mn);
%%
   Avec{1,1} = A;
   Avec{2,1} = [speye(mp+mn,mp+mn);zeros(1+mp+mn,mp+mn)];
   b = [zeros(mp+mn,1);ones(1+mp+mn,1)];
%%
   C = cell(2,1);
   c = zeros(nv-mp-mn,1);
   c(n+4:3:n+3+3*mp) = ones(mp,1);
   c(n+6:3:n+5+3*mp) = -ones(mp,1);
   c(3*mp+n+4:3:3*mp+n+3+3*mn) = ones(mn,1);
   c(3*mp+n+6:3:3*mp+n+5+3*mn) = -ones(mn,1);
   C{1,1} = c;
   C{2,1} = penalty*ones(mp+mn,1);
%%
   [obj,X,y,Z] = sqlp(blk,Avec,C,b);
   X1 = X{1};
   omega = X1(1);
   w = X1(2:n+1);
   alpha = X1(n+2);
   beta  = X1(n+3);
   residp = Ap'*w + beta*ones(mp,1);
   residn = An'*w + beta*ones(mn,1);
   X2 = X{2};
   totalviolation = sum(X2);
%%******************************************************************


function [ b ] = polyshift( a, shift )
%POLYSHIFT Compute coefficients of a shifted 1-D polynomial
%
% The input A is a polynomial of length n and represents the polynomial of
% order (n-1):
% a(1) + a(2)*x + a(3)*(x^2) + ... + a(n)*(x^(n-1))
%
% The polynomial shifted by the scalar input SHIFT is:
% a(1) + a(2)*(x+shift) + a(3)*((x+shift)^2) + ... + a(n)*((x+shift)^(n-1))
% =
% b(1) + b(2)*x + b(3)*(x^2) + ... + b(n)*(x^(n-1))
%
% This function is a direct computation of the coefficients B of the
% shifted polynomial.  "Synthetic division" would be more efficient, but it
% doesn't matter much when only computing a few shifts for small
% polynomials.
%
% Written by: Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

b=zeros(size(a));
for j = 1:numel(a)
    for k=j:numel(a)
        b(j) = b(j) + (a(k)*nchoosek(k-1,j-1)*shift^(k-j));
    end
end

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function [ w, xy ] = triangle_unit_o06 ( )

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

  xy(1:2,1:6) = [ ...
    0.10810301816807022736,  0.44594849091596488632; ...
    0.44594849091596488632,  0.10810301816807022736; ...
    0.44594849091596488632,  0.44594849091596488632; ...
    0.81684757298045851308,  0.091576213509770743460; ...
    0.091576213509770743460,  0.81684757298045851308; ...
    0.091576213509770743460,  0.091576213509770743460 ]';

  return
end


clear all, close all, clc

A = [-.75 1; -.3 -.75];
B = [2; 1];
C = [1 2];
D = 0;

sys = ss(A,B,C,D);

Wc = gram(sys,'c'); % Controllability Gramian
Wo = gram(sys,'o'); % Observability Gramian

[sysb,g,Ti,T] = balreal(sys); % Balance the system

BWc = gram(sysb,'c') % Balanced Gramians
BWo = gram(sysb,'o')

%% Plot Gramians
theta = 0:.01:2*pi;
xc = cos(theta);
yc = sin(theta);
CIRC = [xc; yc];

ELLIPb = Ti*sqrt(BWc)*T*CIRC;
ELLIPc = sqrt(Wc)*CIRC;
ELLIPo = sqrt(Wo)*CIRC;

plot(xc,yc,'k--','LineWidth',2)
hold on

% Draw controllability Gramian (unbalanced)
plot(ELLIPc(1,:),ELLIPc(2,:),'r','LineWidth',2)
patch(ELLIPc(1,:),ELLIPc(2,:),'r','FaceAlpha',.75)

% Draw observability Gramian (unbalanced)
plot(ELLIPo(1,:),ELLIPo(2,:),'b','LineWidth',2)
patch(ELLIPo(1,:),ELLIPo(2,:),'b','FaceAlpha',.75)

% Draw balanced Gramians
patch(ELLIPb(1,:),ELLIPb(2,:),'k','FaceColor',[.5 0 .5],'FaceAlpha',.25)
plot(ELLIPb(1,:),ELLIPb(2,:),'Color',[.35 0 .35],'LineWidth',2)

%% Formatting
axis equal, grid on
set(gcf,'Position',[1 1 550 400])
set(gcf,'PaperPositionMode','auto')
% print('-depsc2', '-loose', '../figures/FIG_BT_GRAM');

%% Manually compute scaled balancing transformation
[Tu,D] = eig(Wc*Wo); % Tu are unscaled e-vecs

Atu = inv(Tu)*A*Tu;
Btu = inv(Tu)*B;
Ctu = C*Tu;
Dtu = 0;
syst = ss(Atu,Btu,Ctu,Dtu);

Sigmac = gram(syst,'c') % Diagonal Gramians
Sigmao = gram(syst,'o') % (but not equal)
Sigmas = diag(Sigmac)./diag(Sigmao);

% Scaled balancing transformation
T = Tu*diag(Sigmas.^(1/4));

% Permute columns of T to order Sigma
Sigma = diag(Sigmac).^(1/2).*diag(Sigmao).^(1/2);
[sigsort,permind] = sort(Sigma,'descend');
T = T(:,permind); % Hierarchical

% Compute balanced system
At = inv(T)*A*T;
Bt = inv(T)*B;
Ct = C*T;
Dt = 0;
sysBal = ss(At,Bt,Ct,Dt);

BWc = gram(sysBal,'c') % Balanced Gramians
BWo = gram(sysBal,'o')

%%
% ELLIPb = sqrt(BWc)*CIRC;

% A = [-1 2; 0 -1];
% B = [1; 1];
% C = [1 1];
% D = 0;

% A = [-1 2; -.5 -1];
% B = [1; 1];
% C = [1 1];
% D = 0;


function [h,p,T] = BartlettTest(data,alpha)

%BartlettTest - Test if k groups of samples have equal variances (homogeneity of variances).
%
%  This test assumes that all populations follow a Gaussian distribution.
%
%  USAGE
%
%    [h,p,t] = BartlettTest(data,alpha)
%
%    data           Nx2 matrix of (observation,group) pairs
%    alpha          optional significance level (default = 0.05)
%
%    h              test result (1 = reject null hypothesis, 0 = accept)
%    p              p value
%    t              test statistics

% Copyright (C) 2004-2011 by Michaël Zugaro
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.

% Significance level
if nargin < 2,
	alpha = 0.05;
end

% Number of groups
k = max(data(:,2));
for i = 1:k,
	group = data(:,2) == i;
	% Number of samples in this group
	n(i) = sum(group);
	% Variance for this group
	s2(i) = var(data(group,1));
end
% Total number of samples
N = sum(n);
% Pooled variance
S2 = sum((n-1).*s2)/(N-k);

% Test statistics (biased)
t = (N-k)*log(S2)-sum((n-1).*log(s2));
% Bias correction
C = 1+(1/(3*(k-1)))*(sum(1./(n-1))-1/(N-k));
% Test statistics (unbiased)
T = t/C;

% Chi square at alpha with (k-1) degrees of freedom
p = 1 - chi2cdf(T,k-1);

h = p < alpha;

disp(['Bartlett test: Variances: ' num2str(s2)]);

if h,
	message = '+++ Two or more variances are significantly different';
else
	message = '--- Variances are not significantly different';
end
message = ['Bartlett test: ' message ' (p='  num2str(p) ', T=' num2str(T) ', N=' int2str(N) ')'];
disp(message);


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

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

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

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

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

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

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

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

end

github-repos/MATLAB/Borye-machine-learning-coursera-1/machine-learning-coursera-1-033fdc2e6da393eeb1179a09aafe92362021effb/Week 9 Assignments/Anomaly Detection and Recommender Systems/mlclass-ex8/cofiCostFunc.m


function result = polygon_integral_x ( n, v )

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

  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'POLYGON_INTEGRAL_X - Warning!\n' );
    fprintf ( 1, '  The number of vertices must be at least 3.\n' );
    fprintf ( 1, '  The input value of N = %d\n', n );
    return
  end

  for i = 1 : n

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

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

  end

  result = result / 6.0;

  return
end


function below = isBelowPlane(point, varargin)
%ISBELOWPLANE Test whether a point is below or above a plane.
%
%   BELOW = isBelowPlane(POINT, PLANE)
%   where POINT is given as coordinate row vector [XP YP ZP], and PLANE is
%   given as a row containing initial point and 2 direction vectors,
%   return TRUE if POINT lie below PLANE.
%
%   Example
%   isBelowPlane([1 1 1], createPlane([1 2 3], [1 1 1]))
%   ans =
%       1
%   isBelowPlane([3 3 3], createPlane([1 2 3], [1 1 1]))
%   ans =
%       0
%
%   See also
%   planes3d, points3d, linePosition3d, planePosition

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

if length(varargin)==1
    plane = varargin{1};
elseif length(varargin)==2
    plane = createPlane(varargin{1}, varargin{2});
end

% ensure same dimension for parameters
if size(point, 1)==1
    point = repmat(point, [size(plane, 1) 1]);
end
if size(plane, 1)==1
    plane = repmat(plane, [size(point, 1) 1]);
end

% compute position of point projected on 3D line corresponding to plane
% normal, and returns true for points locatd below the plane (pos<=0).
below = linePosition3d(point, [plane(:, 1:3) planeNormal(plane)]) <= 0;


function R2 = rsquare(y,yhat)
% PURPOSE:  calculate r square using data y and estimates yhat
% -------------------------------------------------------------------
% USAGE: R2 = rsquare(y,yhat)
% where:
%        y are the original values as vector or 2D matrix and
%        yhat are the estimates calculated from y using a regression, given in
%        the same form (vector or raster) as y
% -------------------------------------------------------------------------
% OUTPUTS:
%        R2 is the r square value calculated using 1-SS_E/SS_T
% -------------------------------------------------------------------
% Note: NaNs in either y or yhat are deleted from both sets.
%
% Felix Hebeler, Geography Dept., University Zurich, Feb 2007

if nargin ~= 2
    error('This function needs some exactly 2 input arguments!');
end

% reshape if 2d matrix
yhat=reshape(yhat,1,size(yhat,1)*size(yhat,2));
y=reshape(y,1,size(y,1)*size(y,2));

% delete NaNs
while sum(isnan(y))~=0 || sum(isnan(yhat))~=0
    if sum(isnan(y)) >= sum(isnan(yhat))
        yhat(isnan(y))=[];
        y(isnan(y))=[];
    else
        y(isnan(yhat))=[];
        yhat(isnan(yhat))=[];
    end
end

% 1 - SSe/SSt
R2 = 1 - ( sum( (y-yhat).^2 ) / sum( (y-mean(y)).^2 ) );

% SSr/SSt
% R2 = sum((yhat-mean(y)).^2) / sum( (y-mean(y)).^2 ) ;

if R2<0 || R2>1
    error(['R^2 of ',num2str(R2),' : yhat does not appear to be the estimate of y from a regression.'])
end


function dist = sphere_distance_xyz ( xyz1, xyz2 )

%*****************************************************************************80
%
%% SPHERE_DISTANCE_XYZ computes great circle distances on a sphere.
%
%  Discussion:
%
%    XYZ coordinates are used.
%
%    We assume the points XYZ1 and XYZ2 lie on the same sphere.
%
%    This computation is a special form of the Vincenty formula.
%    It should be less sensitive to errors associated with very small
%    or very large angular separations.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    "Great-circle distance",
%    Wikipedia.
%
%  Parameters:
%
%    Input, real XYZ1(3), the coordinates of the first point.
%
%    Input, real XYZ2(3), the coordinates of the second point.
%
%    Output, real DIST, the great circle distance between the points.
%
  r = norm ( xyz1 );

  lat1 = r8_asin ( xyz1(3) );
  lon1 = r8_atan ( xyz1(2), xyz1(1) );

  lat2 = r8_asin ( xyz2(3) );
  lon2 = r8_atan ( xyz2(2), xyz2(1) );

  top = ( cos ( lat2 ) * sin ( lon1 - lon2 ) ).^2 ...
      + ( cos ( lat1 ) * sin ( lat2 ) ...
      -   sin ( lat1 ) * cos ( lat2 ) * cos ( lon1 - lon2 ) ).^2;

  top = sqrt ( top );

  bot = sin ( lat1 ) * sin ( lat2 ) ...
      + cos ( lat1 ) * cos ( lat2 ) * cos ( lon1 - lon2 );

  dist = r * atan2 ( top, bot );

  return
end


%%% This program provides a numerical verification that equations (15.2)
%%% and (15.6) coincide in a 2x2 case. The program generates a random 2x2
%%% GOE matrix H and uses it to compute the numerical value of Z(x)
%%% according to the two formulas. You will be asked to choose the real and
%%% imaginary parts of the number x (imaginary part has to be positive).

clear all
close all

%%% Reads the real part from the Command Window
prompt = '\n Choose real part of argument x: ';
re = input(prompt);

%%% Reads the imaginary part from the Command Window
prompt = '\n Choose imaginary part of argument x: ';
im = input(prompt);

if im <= 0
    sprintf('ERROR: Imaginary part has to be positive')
    return;
end

x = re - i*im;

%%% Generating 2x2 GOE matrix
H = randn(2)/sqrt(2);
H = (H + H')/2;

%%% Eigenvalues of matrix H
E = eig(H);

%%% Definition of integrand function for Z
f = @(y1,y2) exp(-i*((x-H(1,1)).*y1.^2 + 2.*H(1,2).*y1.*y2 + (x-H(2,2)).*y2.^2)/2);

%%% Z function
Z = integral2(f,-Inf,Inf,-Inf,Inf);

%%% Explicit form of Z function
Z_exp = 2*pi*exp(-(log(E(1)-x) + log(E(2)-x))/2 + i*pi/2);

sprintf('Value of Z function computed as integral: %6.4f%+6.4fi',real(Z),imag(Z))
sprintf('Value of Z function computed explicitly via eigenvalues of H: %6.4f%+6.4fi',real(Z_exp),imag(Z_exp))


function AInv=invert2X2Matrix(A)
%%INVERT2X2MATRIX Inver the 2X2 matrix A. This function demonstrates the
%       algorithm given in [1], which is more efficient than Gaussian
%       elimination. This could be used as a template for implementation in
%       other languages. In Matlab, this function is not faster than the
%       built-in inv function.
%
%INPUTS: A A 2X2 real or complex invertible matrix.
%
%OUTPUTS: AInv The 2X2 matrix inverse of A.
%
%EXAMPLE:
%This just shows that this function produces the same result as Matlab
%within finite precision limitations.
% A=randn(2,2);
% C=invert2X2Matrix(A);
% C1=inv(A);%Matlab's way.
% RelErr=max(max(abs(abs((C-C1)./C1))))
%
%REFERENCES:
%[1] V. Strassen, "Gaussian elimination is not optimal," Numerische
%    Mathematik, vol. 13, pp. 354-356, 1965.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

I=1/A(1,1);
II=A(2,1)*I;
III=I*A(1,2);
IV=A(2,1)*III;
V=IV-A(2,2);
VI=1/V;

AInv=zeros(2,2);
AInv(1,2)=III*VI;
AInv(2,1)=VI*II;
VII=III*AInv(2,1);
AInv(1,1)=I-VII;
AInv(2,2)=-VI;

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.


% Demo script for software from the paper
% 'Generating spike-trains with specified correlations',
%  Macke et al., submitted for publication, 2008
%
% www.kyb.mpg.de/bethgegroup/code/efficientsampling
%
% Instructions:
%  - Change to the code directory and run demo.m
%  - The script will take you through the functions .
%  - After each step, the script will pause.
%  - To continue, hit any button.
%  - Read along in the demo.m file to follow what's happening.
%
% If you have questions, feel free to email us.

f = 1;
fprintf('\nDemo program for GENERATING SPIKE TRAINS WITH SPECIFIED CORRELATIONS, Macke et al.\n')

%% Step 1: Generating correlated binary variables (2D example)
mu = [.4,.3]';      % set the mean
v = mu.*(1-mu);     % calculate the variances
C = diag(v);        % build covariance matrix
C(1,2) = .1;
C(2,1) = .1;

[S,g,L] = sampleDichGauss01(mu,C,1e5);   % generate samples from the DG model

muHat = mean(S,2);  % estimate mean
CHat = cov(S');     % estimate covariance

fprintf('\nStep 1: Generating correlated binary variables (section 2.1)\n\n')
fprintf('Target mean X1:  %.2f     Estimated mean X1:  %.3f\n',mu(1),muHat(1))
fprintf('Target mean X2:  %.2f     Estimated mean X2:  %.3f\n',mu(2),muHat(2))
fprintf('Target cov X1X2: %.2f     Estimated cov X1X2: %.3f\n',C(1,2),CHat(1,2))

disp('To proceed to compare histograms, hit any key...')
pause

%% Step 2: How much do correlations change the distribution when means are
% assumed to be equal?

fprintf('\nStep 2: Effect of correlations\n\n')
fprintf('Computing...\n')

% First, we generate a mean vector in ten dimensions. We assume that all
% means are approximately .5, but we add some Gaussian noise to the network
% is not to uniform. We set the covariances so that the resulting
% correlations are sizable, but not to large (~0.1).

mu = ones(10,1) * .2 + randn(10,1)*0.05;
v = mu.*(1-mu);
C = ones(10,10) * .02;
C(eye(size(C))==1) = v;

% We find the histogram of the independent distribution with this mean
% vector P(x) = PROD_i P(X_i=1) and the DG distribution with the same mean
% and the covariances as defined above.

h1 = binHistIndep(mu);    % returns the histogram of an independet distribution
S = sampleDichGauss01(mu,C,1e5);
h2 = binHist(S);          % creates a histogram of the binary patterns in S
h2 = h2 / sum(h2);

% When we compare them, we see that some patterns occur much more often
% in the correlated then in the independent distribution. This a clear
% indication of the strength of the correlations. The "stripes" coincide
% with patterns with a different number of active neurons (starting at 0 in
% the upper left corner).

figure(f)
loglog(h1,h2,'k.')
xlabel('P(X) in independent model')
ylabel('P(X) in DG model')
title('Step 2: What effect do correlations have?')
axis square

f = f+1;

disp('To proceed to generate Poisson RVs, hit any key...')
pause

%% Step 3: Correlated Poisson from a binary process (3.1)
% First, we generate samples from a correlated Poisson distribution with
% specified mean and covariance matrix. The covariance between the two
% distributions is positive and quite strong. The construction follows the
% description in section 3.1, i.e. we first find a binary process with the
% correct parameters and sum over it. This means we create
% a joint Poisson distribution.

fprintf('\nStep 3: Correlated Poisson from a binary process (3.1)\n\n')
fprintf('Computing...\n')

mu = [7,9]';      % set the mean
C = [7 3;3 9];    % set an admissable covariance matrix

S = sampleCovPoisson(mu,C,10000);   % generate sample via discretized Gaussian

% We obtain a set of samples S. Next, we verify that the samples have
% desired mean and covariance structure. We see that this is indeed the case.

muHat = mean(S,2);
CHat = cov(S');

fprintf('Target mean X1:  %.2f     Estimated mean X1:  %.3f\n',mu(1),muHat(1))
fprintf('Target mean X2:  %.2f     Estimated mean X2:  %.3f\n',mu(2),muHat(2))
fprintf('Target cov X1X2: %.2f     Estimated cov X1X2: %.3f\n',C(1,2),CHat(1,2))

disp('To plot the marginals and joint histogram, hit any key...')
pause
fprintf('Computing...\n')

figure (f)

% Now, we look at the marginal distribution we obtain from our sampling
% procedure. For comparison, we also plot the true 1D Poisson distributions
% as specified by the mean of the marginal (dotted). We see that they are
% very close to one another.

h1 = histc(S(1,:),0:30); h1 = h1/sum(h1);
h2 = histc(S(2,:),0:30); h2 = h2/sum(h2);

subplot(1,2,1)
plot(0:length(h1)-1,h1,'-r','markersize',5), hold on
plot(0:length(h1),poisspdf(0:length(h1),mu(1)),'.r','markersize',5)
plot(0:length(h2)-1,h2,'-g','markersize',5)
plot(0:length(h1),poisspdf(0:length(h1),mu(2)),'.g','markersize',5)
t = legend('X_1','X_1 Poisson','X_2','X_2 Poisson');
set(t,'box','off')
axis square
xlabel('X'), ylabel('P(X)')
title(sprintf('Step 3: Correlated Poisson, Positive Correlation\nMarginal Distributions with Poisson\n distribution with correct mean'))

% Finally, we convince ourselves that the two variables are indeed
% positively correlated by looking at the 2D joint histogram. We can see
% that although the marginals are perfect Poissonian, most samples are
% concentrated along the main diagonal, indicating the positive
% correlation.

subplot(1,2,2)
hh = EstimateDiscreteJoint(S);
imagesc(hh)
axis square
title('')
xlabel('X_1'), ylabel('X_2')

disp('To proceed to positively correlated Poisson variables with another method, hit any key...')
pause

f = f+1;

%% Step 4: Generating correlated Poisson variables via the method
% in section 3.3 for positive correlations
%
% Again, we generate samples from a correlated Poisson distribution with
% specified mean and covariance matrix. The covariance between the two
% distributions is positive and quite strong. The construction follows now
% section 3.3, so we truncate a Gaussian to obtain a distribution with
% Poisson marginals and a given covariance.

fprintf('\nStep 4: Generating positively correlated Poisson variables (section 3.3)\n\n')
fprintf('Computing...\n')

mu = [7,9]';      % set the mean
C = [7 3;3 9];    % set an admissable covariance matrix

[S,gamma,Lambda,joints2D] = DGPoisson(mu,C,1e5);   % generate sample via discretized Gaussian

% We obtain a set of samples S, the parameters of the hidden Gaussian
% variable (gamma and Lambda) and a structure containing the marginal
% distributions as well as the 2D joint distribution

% Next, we verify that the samples have desired mean and covariance
% structure. We see that this is indeed the case.

muHat = mean(S,2);
CHat = cov(S');

fprintf('Target mean X1:  %.2f     Estimated mean X1:  %.3f\n',mu(1),muHat(1))
fprintf('Target mean X2:  %.2f     Estimated mean X2:  %.3f\n',mu(2),muHat(2))
fprintf('Target cov X1X2: %.2f     Estimated cov X1X2: %.3f\n',C(1,2),CHat(1,2))

disp('To plot the marginals and joint histogram, hit any key...')
pause

figure(f)

% Now, we look at the marginal distribution we obtain from our sampling
% procedure. For comparison, we also plot the true 1D Poisson distributions
% as specified by the mean of the marginal (dotted). We see that they are
% very close to one another.

subplot(2,2,1)
h1 = joints2D{1,1};   % marginal of X_1
h2 = joints2D{2,2};   % marginal of X_2
plot(0:length(h1)-1,h1,'-r','markersize',5), hold on
plot(0:length(h1),poisspdf(0:length(h1),mu(1)),'.r','markersize',5)
plot(0:length(h2)-1,h2,'-g','markersize',5)
plot(0:length(h1),poisspdf(0:length(h1),mu(2)),'.g','markersize',5)
t = legend('X_1','X_1 Poisson','X_2','X_2 Poisson');
set(t,'box','off')
axis square
xlabel('X'), ylabel('P(X)')
title(sprintf('Step 4: Correlated Poisson, Positive Correlation\nMarginal Distributions with Poisson\n distribution with correct mean'))

% Finally, we convince ourselves that the two variables are indeed
% positively correlated by looking at the 2D joint histogram. We can see
% that although the marginals are perfect Poissonian, most samples are
% concentrated along the main diagonal, indicating the positive
% correlation.

subplot(2,2,3)
hh = joints2D{2};   % joint histogram
imagesc(hh)
axis square
title('')
xlabel('X_1'), ylabel('X_2')

disp('To proceed to negatively correlated Poisson variables, hit any key...')
pause

%% Step 5: Generating correlated Poisson variables via the method
% in section 3.3 for negative correlations
%
% Again, we generate samples from a correlated Poisson distribution with
% specified mean and covariance matrix. This time the covariance is
% negative though.

fprintf('\nStep 5: Generating negatively correlated Poisson variables (section 3.3)\n\n')
fprintf('Computing...\n')

mu = [7,9]';      % set the mean
C = [7 -3;-3 9];    % set an admissable covariance matrix

[S,gamma,Lambda,joints2D] = DGPoisson(mu,C,1e5);   % generate sample via discretized Gaussian

% Next, we verify that the samples have desired mean and covariance
% structure. We see that this is indeed the case.

muHat = mean(S,2);
CHat = cov(S');

fprintf('Target mean X1:  %.2f     Estimated mean X1:  %.3f\n',mu(1),muHat(1))
fprintf('Target mean X2:  %.2f     Estimated mean X2:  %.3f\n',mu(2),muHat(2))
fprintf('Target cov X1X2: %.2f     Estimated cov X1X2: %.3f\n',C(1,2),CHat(1,2))

disp('To plot the marginals and joint histogram, hit any key...')
pause

figure(f)

% Again we compare the marginal distributions to the Poisson distribution
% with the same mean and we find them matching very well.

subplot(2,2,2)
h1 = joints2D{1,1};   % marginal of X_1
h2 = joints2D{2,2};   % marginal of X_2
plot(0:length(h1)-1,h1,'-r','markersize',5), hold on
plot(0:length(h1),poisspdf(0:length(h1),mu(1)),'.r','markersize',5)
plot(0:length(h2)-1,h2,'-g','markersize',5)
plot(0:length(h1),poisspdf(0:length(h1),mu(2)),'.g','markersize',5)
t = legend('X_1','X_1 Poisson','X_2','X_2 Poisson');
set(t,'box','off')
axis square
xlabel('X'), ylabel('P(X)')
title(sprintf('Step 5: Correlated Poisson, Negative Correlation\nMarginal Distributions with Poisson\n distribution with correct mean'))

% Nevertheless, the samples have a different structure as we can see from
% the 2D joint histogram. This time, the two variables are negatively
% correlated and when X_1 tends to take large values, X_2 tends to take low
% ones.

subplot(2,2,4)
hh = joints2D{2};   % joint histogram
imagesc(hh)
axis square
title('')
xlabel('X_1'), ylabel('X_2')


function mel = frq2mel(frq)
%FRQ2ERB  Convert Hertz to Mel frequency scale MEL=(FRQ)
%	mel = frq2mel(frq) converts a vector of frequencies (in Hz)
%	to the corresponding values on the Mel scale which corresponds
%	to the perceived pitch of a tone

%	The relationship between mel and frq is given by:
%
%	m = ln(1 + f/700) * 1000 / ln(1+1000/700)
%
%  	This means that m(1000) = 1000
%

mel = sign(frq).*log(1+abs(frq)/700)*1127.01048;
if ~nargout
    plot(frq,mel,'-x');
    xlabel(['Frequency (' xticksi 'Hz)']);
    ylabel(['Frequency (' yticksi 'Mel)']);
end


%{
Test the trace-constrained least-squares problem with positive
semi-definite matrix variables

     minimize (1/2)*norm( A * X - b )^2
     with the constraint that X is positive semi-definite ( X >= 0 )
and optionally,
    the constraint trace(X) <= lambda

%}

% Try to load the problem from disk
fileName = fullfile('reference_solutions','traceLS_problem2_noisy');
if exist([fileName,'.mat'],'file')
    load(fileName);
    fprintf('Loaded problem from %s\n', fileName );
else
    % Generate a new problem
    randn('state',sum('Trace')+5);
    rand('state',sum('Trace2')+5);

    N = 30; R = 2;

    df = R*N;
    oversample = 5;
    Left  = randn(M,R);
    Right = Left;
    k = round(oversample*df);
    k = min( k, round(.8*N*N) );
    omega = randperm(N*N);
    omega = sort(omega(1:k)).';

    X_original = Left*Right';       % the "original" signal -- may not be optimal value
    b_original = X_original(omega);
    EPS = .001;        % noise level
    noise = EPS * randn(k,1);  % this destroys symmetry...
    b = b_original + noise;
    lambda       = trace( X_original );
    objective    = @(X) sum_square( X(omega) - b )/2;
    obj_original = objective(X_original);

    % get references via CVX
    cvx_begin
        cvx_precision best
        cvx_quiet true
        variable Xcvx(N,N)
        minimize objective(Xcvx)
        subject to
            Xcvx == semidefinite(N)
    cvx_end
    X_reference_noTraceConstraint = Xcvx;
        fprintf('Difference between convex solution (no trace constraint) and original signal: %.2e\n', ...
        norm( Xcvx - X_original, 'fro' ) );

    cvx_begin
        cvx_precision best
        cvx_quiet true
        variable Xcvx(N,N)
        minimize objective(Xcvx)
        subject to
            Xcvx == semidefinite(N)
            trace(Xcvx) <= lambda
    cvx_end
    X_reference = Xcvx;         % the trace norm minimizer
    obj_reference = objective(X_reference);
    fprintf('Difference between convex solution (with trace constraint) and original signal: %.2e\n', ...
        norm( Xcvx - X_original, 'fro' ) );
    save(fileName,'X_original','X_reference','X_reference_noTraceConstraint',...
        'omega','b','obj_original',...
        'Left','EPS','b_original','R','obj_reference','lambda');
    fprintf('Saved data to file %s\n', fileName);

end

[M,N]            = size(X_reference);
norm_X_reference = norm(X_reference,'fro');
er_reference     = @(x) norm(x-X_reference,'fro')/norm_X_reference;
norm_X_reference2= norm(X_reference_noTraceConstraint,'fro');
er_reference2    = @(x) norm(x-X_reference_noTraceConstraint,'fro')/norm_X_reference2;
objective        = @(X) norm( X(omega) - b )^2/2;

[omegaI,omegaJ] = ind2sub([M,N],omega);
mat = @(x) reshape(x,M,N);
vec = @(x) x(:);

k  = length(omega);
p  = k/(M*N);
df = R*(M+N-R);
fprintf('%d x %d rank %d matrix, observe %d = %.1f x df = %.1f%% entries\n',...
    M,N,R,k,k/df,p*100);
fprintf(' Trace norm solution and original matrix differ by %.2e\n',...
    norm(X_reference-X_original,'fro')/norm_X_reference );
%% Solve unconstrained version. No smoothing is necessary!
opts = struct('maxIts',500);
opts.errFcn{1} = @(f,x) er_reference2(x); % no trace constraint
opts.errFcn{2} = @(f,x) sum( abs(eig(x)) > 1e-5 ); % numerical rank
% tell it to use eigs instead of eig
% opts.largescale = true; % ( but not recommended)

% opts.symmetrize = true; % another option

A = sparse( omegaI,omegaJ,b,N,N);
[x,out] = solver_psdComp( A, opts );
% Note: we do not expect to get zero error because there might be more
%   than one optimal solution for this problem (since there are not
%   so many constraints)
% Check that we have a feasible solution
fprintf('Objective is %.2e, min eigenvalue is %.2e\n', objective(x),...
    min(eig(x)) );
%% Solve trace constrained version. No smoothing necessary!
opts = struct('maxIts',1500,'tol',1e-9);
opts.errFcn{1} = @(f,x) er_reference(x); % no trace constraint
opts.errFcn{2} = @(f,x) trace(x)-lambda;
opts.errFcn{3} = @(f,x) sum( abs(eig(x)) > 1e-5 ); % numerical rank
% we can also symmetrize "omega", but it makes little difference,
%   and gives slightly differen value than CVX, since CVX symmetrizes
%   differently:
% opts.symmetrize = true;
A = sparse( omegaI,omegaJ,b,N,N);
[x,out] = solver_psdCompConstrainedTrace( A,lambda, opts );

h=figure();
semilogy(out.err(:,1));

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

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


function [theta, phi, r] = besa_transformCartesian2Spherical(X, Y, Z)
% BESA_TRANSFORMCARTESIAN2SPHERICAL takes the cartesian coordinates X, Y
% and Z of a 3D point and transforms them to the spherical coordinates
% theta, phi and r.
%
% Parameters:
%     [X]
%         The X-coordinate of the current 3D point. It should point to the
%         right.
%
%     [Y]
%         The Y-coordinate of the current 3D point. It should point
%         foreward.
%
%     [Z]
%         The Z-coordinate of the current 3D point. It should point up.
%
%
% Return:
%     [theta]
%         The azimuth angle with the vertical z-axis (0 degree) in the
%         x-z-plane, +90 degree lie in the positive x-axis.
%
%     [phi]
%         The latitude angle in the horizontal x-y-plane
%         (counter-clockwise).
%
%     [r]
%         The radius.

% Copyright (C) 2015, BESA GmbH
%
% File name: besa_transformCartesian2Spherical.m
%
% Author: Todor Jordanov
% Created: 2015-07-29

SquareOfRadiusXY = X*X + Y*Y;
RadiusXY = sqrt(SquareOfRadiusXY);

if(RadiusXY == 0.0 && Z == 0.0)

    theta = 0.0;

else

    theta = atan2d(RadiusXY, Z);

end

if(X==0.0 && Y==0.0)

    phi = 0.0;

else

    phi = atan2d(Y, X);

end

% square of total radius
SquareOfRadiusXY = SquareOfRadiusXY + Z*Z;

r = sqrt(SquareOfRadiusXY);

% set phi & theta to BESA ranges
if(phi > 90.)

    phi = phi - 180.0;
    theta = -theta;

elseif(phi < -90.)

    phi = phi+180.0;
    theta = -theta;

end


function volume = ellipsoid_volume ( m, a, v, r )

%*****************************************************************************80
%
%% ELLIPSOID_VOLUME returns the volume of an ellipsoid.
%
%  Discussion:
%
%    The points X in the ellipsoid are described by an M by M
%    positive definite symmetric matrix A, an M-dimensional point V,
%    and a "radius" R, such that
%      (X-V)' * A * (X-V) <= R * R
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 August 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, real A(M,M), the matrix that describes
%    the ellipsoid.  A must be symmetric and positive definite.
%
%    Input, real V(M), the "center" of the ellipse.
%    The value of V is not actually needed by this function.
%
%    Input, real R, the "radius" of the ellipse.
%
%    Output, real VOLUME, the volume of the ellipsoid.
%
  [ u, info ] = r8po_fa ( m, a );

  sqrt_det = 1.0;
  for i = 1 : m
    sqrt_det = sqrt_det * u(i,i);
  end

  volume = r ^ m * hypersphere_unit_volume ( m ) / sqrt_det;

  return
end


function geometry_test0493 ( )

%*****************************************************************************80
%
%% TEST0493 tests PARABOLA_EX, PARABOLA_EX2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0493\n' );
  fprintf ( 1, '  PARABOLA_EX finds the extreme value of a parabola\n' );
  fprintf ( 1, '    determined by three points.\n' );
  fprintf ( 1, '  PARABOLA_EX2 finds the extreme value of a parabola\n' );
  fprintf ( 1, '    determined by three points.\n' );

  a =  2.0;
  b = -4.0;
  c = 10.0;

  x1 = 1.0;
  y1 = a * x1 * x1 + b * x1 + c;
  x2 = 2.0;
  y2 = a * x2 * x2 + b * x2 + c;
  x3 = 3.0;
  y3 = a * x3 * x3 + b * x3 + c;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Parabolic coefficients (A,B,C) = %f  %f  %f\n', a, b, c );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  X, Y data\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  X1, Y1 = %f  %f\n', x1, y1 );
  fprintf ( 1, '  X2, Y2 = %f  %f\n', x2, y2 );
  fprintf ( 1, '  X3, Y3 = %f  %f\n', x3, y3 );

  a = 0.0;
  b = 0.0;
  c = 0.0;

  [ xmin, ymin, ierror ] = parabola_ex ( x1, y1, x2, y2, x3, y3 );

  if ( ierror == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  PARABOLA_EX returns (XMIN,YMIN) = %f  %f\n', xmin, ymin );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  PARABOLA_EX returns error code %d\n', ierror );
  end

  [ xmin, ymin, a, b, c, ierror ] = parabola_ex2 ( x1, y1, x2, y2, x3, y3 );

  if ( ierror == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  PARABOLA_EX2 returns (XMIN,YMIN) = %f  %f\n', xmin, ymin );
    fprintf ( 1, '  and (A,B,C) = %f  %f  %f\n', a, b, c );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  PARABOLA_EX2 returns error code %d\n', ierror );
  end

  return
end