% Showing the effect of the POCS partial Fourier reconstruction.
%
% Mark a cell (code beginning with "%%") and
% press <Ctrl> + <Enter> to evaluate the cell content only.

%% simple example (2D, single coil, no phase errors)
%
N = 256;
iter = 30;
pf = 9/16;      % a typical large partial fourier factor
p = single(phantom(N));
kspRef = fftshift(fftshift(  fft(fft(  fftshift(fftshift( p, 1),2), [],1),[],2), 1),2);
kspRef = kspRef + 3 * complex( randn(N), randn(N) );    % add noise
kspPF  = kspRef;
kspPF(:,1:floor((1-pf)*N)) = 0;

imReco = pocs( kspPF, iter, true );

imRef    = ifftshift(ifftshift(  ifft(ifft(  ifftshift(ifftshift( kspRef, 1),2), [],1),[],2), 1),2);
imDirect = ifftshift(ifftshift(  ifft(ifft(  ifftshift(ifftshift(  kspPF, 1),2), [],1),[],2), 1),2);

figure,
imagesc(abs([imRef  imReco  imDirect]), [0 2*mean(abs(imRef(:)))])
axis image
title('reference (full dataset)   |   POCS reco   |    zero-padded reco')
colormap(gray(256))

%% multicoil example with phase error
%
clear  p  kspRef  kspPF  imReco  imRef  imDirect
N = 256;
iter = 30;
pf = 9/16;
% Set up some pseudo coilmaps:
cmap(1,:,:) = repmat(        ( 3./(1:N)  .^0.10) .* exp(1i*(1:N)  *pi/(N/3)) , N, 1 );
cmap(2,:,:) = repmat(        ( 1./(1:N).'.^0.20) .* exp(1i*(1:N).'*pi/(N/4)) , 1, N );
cmap(3,:,:) = repmat( fliplr(( 2./(1:N)  .^0.04) .* exp(1i*(1:N)  *pi/(N/8))), N, 1 );
cmap(4,:,:) = repmat( flipud(( 2./(1:N).'.^0.15) .* exp(1i*(1:N).'*pi/(N/10))),1, N );
cmap = single(cmap);

Nc = size(cmap,1);
p = reshape( single(phantom(N)), 1, N, N );
subs = { 1, 164:181, 187:193 };
p(subs{:}) = 0;
subs = { 1, 166:179, 189:191 };
p(subs{:}) = 1 * exp(1i * pi/2);
p = bsxfun( @times, cmap, p );
kspRef = fftshift(fftshift(  fft(fft(  fftshift(fftshift( p, 2),3), [],2),[],3), 2),3);
kspRef = kspRef + 10 * complex( randn(Nc,N,N), randn(Nc,N,N) );    % add noise
kspPF  = kspRef;
kspPF(:,:,1:floor((1-pf)*N)) = 0;

imReco = pocs( kspPF, iter, true );

imReco   = squeeze(sqrt(sum( abs(imReco).^2, 1)));
imRef    = ifftshift(ifftshift(  ifft(ifft(  ifftshift(ifftshift( kspRef, 2),3), [],2),[],3), 2),3);
imRef    = squeeze(sqrt(sum( abs(imRef).^2, 1)));
imDirect = ifftshift(ifftshift(  ifft(ifft(  ifftshift(ifftshift(  kspPF, 2),3), [],2),[],3), 2),3);
imDirect = squeeze(sqrt(sum( abs(imDirect).^2, 1)));

figure,
imagesc(abs([imRef  imReco  imDirect]),[0 2*mean(abs(imRef(:)))])
axis image
title('reference (full dataset)   |   POCS reco   |    zero-padded reco')
colormap(gray(256))


function [ t, dtdr, dtds ] = shape_t10 ( r, s )

%*****************************************************************************80
%
%% SHAPE_T10 evaluates shape functions for a 10 node reference triangle.
%
%  Reference Element T10:
%
%    |
%    1  10
%    |  |\
%    |  | \
%    |  8  9
%    |  |   \
%    S  |    \
%    |  5  6  7
%    |  |      \
%    |  |       \
%    0  1--2--3--4
%    |
%    +--0----R---1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, S, the reference coordinates of a point.
%
%    Output, real T(10), the basis functions at the point.
%
%    Output, real DTDR(10), the R basis derivatives at the point.
%
%    Output, real DTDS(10), the S basis derivatives at the point.
%
  a = 1.0 / 3.0;
  b = 2.0 / 3.0;
  c = 1.0;

  t(1)  = 4.5 * ( a - r - s ) * ( b - r - s ) * ( c - r - s );
  t(2)  = 13.5 * r * ( b - r - s ) * ( c - r - s );
  t(3)  = - 13.5 * r * ( a - r ) * ( c - r - s );
  t(4)  = 4.5 * r * ( a - r ) * ( b - r );
  t(5)  = 13.5 * s * ( b - r - s ) * ( c - r - s );
  t(6)  = 27.0 * r * s * ( c - r - s );
  t(7)  = 13.5 * r * s * ( r - a );
  t(8)  = 13.5 * s * ( s - a ) * ( c - r - s );
  t(9)  = 13.5 * r * s * ( s - a );
  t(10) = 4.5 * s * ( a - s ) * ( b - s );

  dtdr(1) = 4.5 * ( ( a - s ) * ( 2.0 * r - c - b + 2.0 * s ) ...
    - ( s - b ) * ( s - c ) - 2.0 * ( 2.0 * s - b - c ) * r - 3.0 * r * r );
  dtdr(2) = 13.5 * ( ( s - b ) * ( s - c ) + 2.0 * ( 2.0 * s - b - c ) * r ...
    + 3.0 * r * r );
  dtdr(3) = - 13.5 * ( a * ( c - s ) + 2.0 * ( s - a - c ) * r + 3.0 * r * r );
  dtdr(4) = 4.5 * ( a * b - 2.0 * ( a + b ) * r + 3.0 * r * r );
  dtdr(5) = 13.5 * s * ( 2.0 * s - b - c + 2.0 * r );
  dtdr(6) = 27.0 * s * ( c - s - 2.0 * r );
  dtdr(7) = 13.5 * s * ( 2.0 * r - a );
  dtdr(8) = - 13.5 * s * ( s - a );
  dtdr(9) = 13.5 * s * ( s - a);
  dtdr(10) = 0.0;

  dtds(1) = 4.5 * ( ( a - r ) * ( 2.0 * s - c - b + 2.0 * r ) ...
    - ( r - b ) * ( r - c ) - 2.0 * ( 2.0 * r - b - c ) * s - 3.0 * s * s );
  dtds(2) = 13.5 * r * ( 2.0 * s + 2.0 * r - b - c );
  dtds(3) = 13.5 * r * ( a - r );
  dtds(4) = 0.0;
  dtds(5) = 13.5 * ( ( r - b ) * ( r - c ) + 2.0 * ( 2.0 * r - b - c ) * s ...
    + 3.0 * s * s );
  dtds(6) = 27.0 * r * ( c - r - 2.0 * s );
  dtds(7) = 13.5 * r * ( r - a );
  dtds(8) = - 13.5 * ( a * ( c - r ) + 2.0 * ( r - c - a ) * s + 3.0 * s * s );
  dtds(9) = 13.5 * r * ( 2.0 * s - a );
  dtds(10) = 4.5 * ( a * b - 2.0 * ( a + b ) * s + 3.0 * s * s );

  return
end


function kpn_test ( )

%*****************************************************************************80
%
%% KPN_TEST uses the KPN function for 1D quadrature over (-oo,+oo).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 May 2012
%
%  Author:
%
%    John Burkardt
%
  d = 1;
  exact = hermite_integral ( d );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'KPN_TEST:\n' );
  fprintf ( 1, '  Kronrod-Patterson-Hermite quadrature over (-oo,+oo):\n' );
  fprintf ( 1, '  Exact integral is %g\n', exact );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   Level   Nodes    Estimate  Error\n' );
  fprintf ( 1, '\n' );

  for l = 1 : 5

    n = kpn_order ( l );

    nh = floor ( ( n + 1 ) / 2 );

    [ xh, wh ] = kpn ( l );

    if ( mod ( n, 2 ) == 1 )
      x = [ -xh(nh:-1:2); xh ];
      w = [ wh(nh:-1:2); wh ];
    else
      x = [ -xh(nh:-1:1); xh ];
      w = [ wh(nh:-1:1); wh ];
    end

    fx = hermite_integrand ( d, n, x );
    q = w' * fx;
    e = sqrt ( ( q - exact ).^2 ) / exact;

    fprintf( '  %2d     %6d  %10.5g  %10.5g\n', l, n, q, e )

  end

  return
end


%
% [idx,netsim,dpsim,expref]=apclusterSparse(s,p)
%
% APCLUSTER uses affinity propagation (Frey and Dueck, Science,
% 2007) to identify data clusters, using a set of real-valued
% pair-wise data point similarities as input. Each cluster is
% represented by a data point called a cluster center, and the
% method searches for clusters so as to maximize a fitness
% function called net similarity. The method is iterative and
% stops after maxits iterations (default of 500 - see below for
% how to change this value) or when the cluster centers stay
% constant for convits iterations (default of 50). The command
% apcluster(s,p,'plot') can be used to plot the net similarity
% during operation of the algorithm.
%
% For N data points, there may be as many as N^2-N pair-wise
% similarities (note that the similarity of data point i to k
% need not be equal to the similarity of data point k to i).
% These may be passed to APCLUSTER in an NxN matrix s, where
% s(i,k) is the similarity of point i to point k. In fact, only
% a smaller number of relevant similarities are needed for
% APCLUSTER to work. If only M similarity values are known,
% where M < N^2-N, they can be passed to APCLUSTER in an Mx3
% matrix s, where each row of s contains a pair of data point
% indices and a corresponding similarity value: s(j,3) is the
% similarity of data point s(j,1) to data point s(j,2).
%
% APCLUSTER automatically determines the number of clusters,
% based on the input p, which is an Nx1 matrix of real numbers
% called preferences. p(i) indicates the preference that data
% point i be chosen as a cluster center. A good choice is to
% set all preference values to the median of the similarity
% values. The number of identified clusters can be increased or
% decreased  by changing this value accordingly. If p is a
% scalar, APCLUSTER assumes all preferences are equal to p.
%
% The fitness function (net similarity) used to search for
% solutions equals the sum of the preferences of the the data
% centers plus the sum of the similarities of the other data
% points to their data centers.
%
% The identified cluster centers and the assignments of other
% data points to these centers are returned in idx. idx(j) is
% the index of the data point that is the cluster center for
% data point j. If idx(j) equals j, then point j is itself a
% cluster center. The sum of the similarities of the data
% points to their cluster centers is returned in dpsim, the
% sum of the preferences of the identified cluster centers is
% returned in expref and the net similarity (sum of the data
% point similarities and preferences) is returned in netsim.
%
% EXAMPLE
%
% N=100; x=rand(N,2); % Create N, 2-D data points
% M=N*N-N; s=zeros(M,3); % Make ALL N^2-N similarities
% j=1;
% for i=1:N
%   for k=[1:i-1,i+1:N]
%     s(j,1)=i; s(j,2)=k; s(j,3)=-sum((x(i,:)-x(k,:)).^2);
%     j=j+1;
%   end;
% end;
% p=median(s(:,3)); % Set preference to median similarity
% [idx,netsim,dpsim,expref]=apclusterSparse(s,p,'plot');
% fprintf('Number of clusters: %d\n',length(unique(idx)));
% fprintf('Fitness (net similarity): %f\n',netsim);
% figure; % Make a figures showing the data and the clusters
% for i=unique(idx)'
%   ii=find(idx==i); h=plot(x(ii,1),x(ii,2),'o'); hold on;
%   col=rand(1,3); set(h,'Color',col,'MarkerFaceColor',col);
%   xi1=x(i,1)*ones(size(ii)); xi2=x(i,2)*ones(size(ii));
%   line([x(ii,1),xi1]',[x(ii,2),xi2]','Color',col);
% end;
% axis equal tight;
%
% PARAMETERS
%
% [idx,netsim,dpsim,expref]=apclusterSparse(s,p,'NAME',VALUE,...)
%
% The following parameters can be set by providing name-value
% pairs, eg, apcluster(s,p,'maxits',1000):
%
%   Parameter    Value
%   'maxits'     Any positive integer. This specifies the
%                maximum number of iterations performed by
%                affinity propagation. Default: 1000.
%   'convits'    Any positive integer. APCLUSTER decides that
%                the algorithm has converged if the estimated
%                cluster centers stay fixed for convits
%                iterations. Increase this value to apply a
%                more stringent convergence test. Default: 100.
%   'dampfact'   A real number that is less than 1 and
%                greater than or equal to 0.5. This sets the
%                damping level of the message-passing method,
%                where values close to 1 correspond to heavy
%                damping which may be needed if oscillations
%                occur. Default: .9
%   'plot'       No value needed. This creates a figure that
%                plots the net similarity after each iteration
%                of the method. If the net similarity fails to
%                converge, consider increasing the values of
%                dampfact and maxits.
%   'details'    No value needed. This causes idx, netsim,
%                dpsim and expref to be stored after each
%                iteration.
%   'nonoise'    No value needed. Degenerate input similarities
%                (eg, where the similarity of i to k equals the
%                similarity of k to i) can prevent convergence.
%                To avoid this, APCLUSTER adds a small amount
%                of noise to the input similarities. This flag
%                turns off the addition of noise.
%
% Copyright (c) Brendan J. Frey and Delbert Dueck (2006). This
% software may be freely used and distributed for
% non-commercial purposes.

function [idx,netsim,dpsim,expref]=apclusterSparse(s,p,varargin);

% Handle arguments to function
if nargin<2 error('Too few input arguments');
else
    maxits=1000; convits=100; lam=0.9; plt=0; details=0; nonoise=0;
    i=1;
    while i<=length(varargin)
        if strcmp(varargin{i},'plot')
            plt=1; i=i+1;
        elseif strcmp(varargin{i},'details')
            details=1; i=i+1;
        elseif strcmp(varargin{i},'nonoise')
            nonoise=1; i=i+1;
        elseif strcmp(varargin{i},'maxits')
            maxits=varargin{i+1};
            i=i+2;
            if maxits<=0 error('maxits must be a positive integer'); end;
        elseif strcmp(varargin{i},'convits')
            convits=varargin{i+1};
            i=i+2;
            if convits<=0 error('convits must be a positive integer'); end;
        elseif strcmp(varargin{i},'dampfact')
            lam=varargin{i+1};
            i=i+2;
            if (lam<0.5)||(lam>=1)
                error('dampfact must be >= 0.5 and < 1');
            end;
        else i=i+1;
        end;
    end;
end;
if lam>0.9
    fprintf('\n*** Warning: Large damping factor in use. Turn on plotting\n');
    fprintf('    to monitor the net similarity. The algorithm will\n');
    fprintf('    change decisions slowly, so consider using a larger value\n');
    fprintf('    of convits.\n\n');
end;

% Check that standard arguments are consistent in size
if length(size(s))~=2 error('s should be a 2D matrix');
elseif length(size(p))>2 error('p should be a vector or a scalar');
elseif size(s,2)==3
    tmp=max(max(s(:,1)),max(s(:,2)));
    if length(p)==1 N=tmp; else N=length(p); end;
    if tmp>N
        error('data point index exceeds number of data points');
    elseif min(min(s(:,1)),min(s(:,2)))<=0
        error('data point indices must be >= 1');
    end;
elseif size(s,1)==size(s,2)
    N=size(s,1);
    if (length(p)~=N)&&(length(p)~=1)
        error('p should be scalar or a vector of size N');
    end;
else error('s must have 3 columns or be square'); end;

% Make vector of preferences
if length(p)==1 p=p*ones(N,1); end;

% Append self-similarities (preferences) to s-matrix
tmps=[repmat([1:N]',[1,2]),p]; s=[s;tmps];
M=size(s,1);

% In case user did not remove degeneracies from the input similarities,
% avoid degenerate solutions by adding a small amount of noise to the
% input similarities
if ~nonoise
    rns=randn('state'); randn('state',0);
    s(:,3)=s(:,3)+(eps*s(:,3)+realmin*100).*rand(M,1);
    randn('state',rns);
end;

% Construct indices of neighbors
ind1e=zeros(N,1);
for j=1:M k=s(j,1); ind1e(k)=ind1e(k)+1; end;
ind1e=cumsum(ind1e); ind1s=[1;ind1e(1:end-1)+1]; ind1=zeros(M,1);
for j=1:M k=s(j,1); ind1(ind1s(k))=j; ind1s(k)=ind1s(k)+1; end;
ind1s=[1;ind1e(1:end-1)+1];
ind2e=zeros(N,1);
for j=1:M k=s(j,2); ind2e(k)=ind2e(k)+1; end;
ind2e=cumsum(ind2e); ind2s=[1;ind2e(1:end-1)+1]; ind2=zeros(M,1);
for j=1:M k=s(j,2); ind2(ind2s(k))=j; ind2s(k)=ind2s(k)+1; end;
ind2s=[1;ind2e(1:end-1)+1];

% Allocate space for messages, etc
A=zeros(M,1); R=zeros(M,1); t=1;
if plt netsim=zeros(1,maxits+1); end;
if details
    idx=zeros(N,maxits+1);
    netsim=zeros(1,maxits+1);
    dpsim=zeros(1,maxits+1);
    expref=zeros(1,maxits+1);
end;

% Execute parallel affinity propagation updates
e=zeros(N,convits); dn=0; i=0;
while ~dn
    i=i+1;

    % Compute responsibilities
    for j=1:N
        ss=s(ind1(ind1s(j):ind1e(j)),3);
        as=A(ind1(ind1s(j):ind1e(j)))+ss;
        [Y,I]=max(as); as(I)=-realmax; [Y2,I2]=max(as);
        r=ss-Y; r(I)=ss(I)-Y2;
        R(ind1(ind1s(j):ind1e(j)))=(1-lam)*r+ ...
            lam*R(ind1(ind1s(j):ind1e(j)));
    end;

    % Compute availabilities
    for j=1:N
        rp=R(ind2(ind2s(j):ind2e(j)));
        rp(1:end-1)=max(rp(1:end-1),0);
        a=sum(rp)-rp;
        a(1:end-1)=min(a(1:end-1),0);
        A(ind2(ind2s(j):ind2e(j)))=(1-lam)*a+ ...
            lam*A(ind2(ind2s(j):ind2e(j)));
    end;

    % Check for convergence
    E=((A(M-N+1:M)+R(M-N+1:M))>0); e(:,mod(i-1,convits)+1)=E; K=sum(E);
    if i>=convits || i>=maxits
        se=sum(e,2);
        unconverged=(sum((se==convits)+(se==0))~=N);
        if (~unconverged&&(K>0))||(i==maxits) dn=1; end;
    end;

    % Handle plotting and storage of details, if requested
    if plt||details
        if K==0
            tmpnetsim=nan; tmpdpsim=nan; tmpexpref=nan; tmpidx=nan;
        else
            tmpidx=zeros(N,1); tmpdpsim=0;
            tmpidx(find(E))=find(E); tmpexpref=sum(p(find(E)));
            discon=0;
            for j=find(E==0)'
                ss=s(ind1(ind1s(j):ind1e(j)),3);
                ii=s(ind1(ind1s(j):ind1e(j)),2);
                ee=find(E(ii));
                if length(ee)==0 discon=1;
                else
                    [smx imx]=max(ss(ee));
                    tmpidx(j)=ii(ee(imx));
                    tmpdpsim=tmpdpsim+smx;
                end;
            end;
            if discon
                tmpnetsim=nan; tmpdpsim=nan; tmpexpref=nan; tmpidx=nan;
            else tmpnetsim=tmpdpsim+tmpexpref;
            end;
        end;
    end;
    if details
        netsim(i)=tmpnetsim; dpsim(i)=tmpdpsim; expref(i)=tmpexpref;
        idx(:,i)=tmpidx;
    end;
    if plt
        netsim(i)=tmpnetsim;
        figure(234);
        tmp=1:i; tmpi=find(~isnan(netsim(1:i)));
        plot(tmp(tmpi),netsim(tmpi),'r-');
        xlabel('# Iterations');
        ylabel('Net similarity of quantized intermediate solution');
        drawnow;
    end;
end;
% Identify  exemplars
E=((A(M-N+1:M)+R(M-N+1:M))>0); K=sum(E);
if K>0
    tmpidx=zeros(N,1); tmpidx(find(E))=find(E); % Identify clusters
    for j=find(E==0)'
        ss=s(ind1(ind1s(j):ind1e(j)),3);
        ii=s(ind1(ind1s(j):ind1e(j)),2);
        ee=find(E(ii));
        [smx imx]=max(ss(ee));
        tmpidx(j)=ii(ee(imx));
    end;
    EE=zeros(N,1);
    for j=find(E)'
        jj=find(tmpidx==j); mx=-Inf;
        ns=zeros(N,1); msk=zeros(N,1);
        for m=jj'
            mm=s(ind1(ind1s(m):ind1e(m)),2);
            msk(mm)=msk(mm)+1;
            ns(mm)=ns(mm)+s(ind1(ind1s(m):ind1e(m)),3);
        end;
        ii=jj(find(msk(jj)==length(jj)));
        [smx imx]=max(ns(ii));
        EE(ii(imx))=1;
    end;
    E=EE;
    tmpidx=zeros(N,1); tmpdpsim=0;
    tmpidx(find(E))=find(E); tmpexpref=sum(p(find(E)));
    for j=find(E==0)'
        ss=s(ind1(ind1s(j):ind1e(j)),3);
        ii=s(ind1(ind1s(j):ind1e(j)),2);
        ee=find(E(ii));
        [smx imx]=max(ss(ee));
        tmpidx(j)=ii(ee(imx));
        tmpdpsim=tmpdpsim+smx;
    end;
    tmpnetsim=tmpdpsim+tmpexpref;
else
    tmpidx=nan*ones(N,1); tmpnetsim=nan; tmpexpref=nan;
end;
if details
    netsim(i+1)=tmpnetsim; netsim=netsim(1:i+1);
    dpsim(i+1)=tmpnetsim-tmpexpref; dpsim=dpsim(1:i+1);
    expref(i+1)=tmpexpref; expref=expref(1:i+1);
    idx(:,i+1)=tmpidx; idx=idx(:,1:i+1);
else
    netsim=tmpnetsim; dpsim=tmpnetsim-tmpexpref;
    expref=tmpexpref; idx=tmpidx;
end;
if plt||details
    fprintf('\nNumber of identified clusters: %d\n',K);
    fprintf('Fitness (net similarity): %f\n',tmpnetsim);
    fprintf('  Similarities of data points to exemplars: %f\n',dpsim(end));
    fprintf('  Preferences of selected exemplars: %f\n',tmpexpref);
    fprintf('Number of iterations: %d\n\n',i);
end;
if unconverged
    fprintf('\n*** Warning: Algorithm did not converge. The similarities\n');
    fprintf('    may contain degeneracies - add noise to the similarities\n');
    fprintf('    to remove degeneracies. To monitor the net similarity,\n');
    fprintf('    activate plotting. Also, consider increasing maxits and\n');
    fprintf('    if necessary dampfact.\n\n');
end;


function [D] = wishart_kl (arg1,arg2,arg3,arg4)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   [D] = wishart_kl (B_q,B_p,alpha_q,alpha_p)
%
%   computes the divergence
%                /
%      D(q||p) = | q(x)*log(q(x)/p(x)) dx
%               /
%   between two k-dimensional Wishart propability density for C  given
%   scale matrix B and shape parameter alpha
%
%            alpha
%         |B|              alpha-(k+1)/2
%   p(x)= ------------- |C|              exp (-tr(BC))
%         Gamma (alpha)
%              k
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin == 4
    B_q = arg1; B_p = arg2; alpha_q = arg3; alpha_p = arg4;
elseif nargin == 2
    B_q = arg1.Gam_rate; alpha_q = arg1.Gam_shape;
    B_p = arg2.Gam_rate; alpha_p = arg2.Gam_shape;
else
    error('Incorrect number of input arguments');
end

if size(B_q)~=size(B_p)
  error('Distributions must have equal dimensions');
end

K=size(B_p,1);

try
    Lq = -logdet(B_q,'chol');
    Lp = -logdet(B_p,'chol');
catch
    disp('One of the two covariance matrices (or both) is not positive definite')
    disp('Check for NaNs in the time series. Also, are there enough time points?')
    error('Error computing the inverse of the covariance matrix.')
end

lZq = log(2) * (alpha_q*K/2)  - Lq * (-alpha_q/2) + K*(K-1)/4 * log(pi);
lZp = log(2) * (alpha_p*K/2)  - Lp * (-alpha_p/2) + K*(K-1)/4 * log(pi);

Lq = Lq + K * log(2);
Lp = Lp + K * log(2);

for k = 1:K
    lZq = lZq + gammaln(alpha_q/2+0.5-0.5*k);
    lZp = lZp + gammaln(alpha_p/2+0.5-0.5*k);
    Lq = Lq + psi(alpha_q/2+0.5-0.5*k);
    Lp = Lp + psi(alpha_p/2+0.5-0.5*k);
end

D = (alpha_q/2-0.5-0.5*K)*Lq - (alpha_p/2-0.5-0.5*K)*Lp ...
    - alpha_q * K / 2 + alpha_q * trace(B_p / B_q) / 2 + lZp - lZq;

return;


% J. Shi and J. Malik,
% "Normalized Cuts and Image Segmentation",
% In Proc. IEEE Conf. Computer Vision and Pattern Recognition,
% pages 731-737, 1997.

% Asad Ali
% GIK Institute of Engineering Sciences & Technology, Pakistan
% Email: asad_82@yahoo.com

% CONCEPT: Introduced the use of 2nd generalized eigenvector for clustering
clear all;
close all;

% generate the data
data = GenerateData(2);
figure,plot(data(:,1), data(:,2),'r+'),title('Original Data Points'); grid on;shg

affinity = CalculateAffinity(data);
figure,imshow(affinity,[]), title('Affinity Matrix');

% compute the degree matrix
for i=1:size(affinity,1)
    D(i,i) = sum(affinity(i,:));
end

% compute the unnormalized graph laplacian matrix
L = D - affinity;

[eigVectors,eigValues] = eig(L);

% plot the eigen vector corresponding to the 2nd largest eigen value
figure,plot(eigVectors(:,2),'r*'),title('2nd Largest Eigenvector');

% threshold the eigen vectors
[xx1,yy1,val1] = find(eigVectors(:,2) > 0.15);
[xx2,yy2,val2] = find(eigVectors(:,2) > 0 & eigVectors(:,2) < 0.15);
[xx3,yy3,val3] = find(eigVectors(:,2) < 0);

figure,
hold on;
plot(data(xx1,1),data(xx1,2),'m*');
plot(data(xx2,1),data(xx2,2),'b*');
plot(data(xx3,1),data(xx3,2),'g*');
hold off;
title('Clustering Results using 2nd Generalized Eigen Vector');
grid on;shg


function [a1, b1, c1, a2, b2, c2] = aaa(A, B, C)
%AAA  gives both solutions to the angle-angle-angle problem, in radians.
%
%   AAA(A, B, C) will result in NaNs if the existence condition
%   |pi - |A|-|B|| <= |C| <= pi - ||A| - |B||
%   is not met.
%
%   See also AAAD.

% Rody P.S. Oldenhuis
% Delft University of Technology
% oldenhuis@gmail.com
%
% Created    : 23/Feb/2009
% Last edited: 30/Nov/2012

    % first solution
    a1 = acos2( (cos(A) + cos(B).*cos(C)) ./ (sin(B).*sin(C)),  A );
    b1 = acos2( (cos(B) + cos(A).*cos(C)) ./ (sin(A).*sin(C)),  B );
    c1 = acos2( (cos(C) + cos(A).*cos(B)) ./ (sin(A).*sin(B)),  C );

    % second solution
    a2 = 2*pi - a1;
    b2 = 2*pi - b1;
    c2 = 2*pi - c1;

    % check constraints
    indices = ( ...
        abs(pi - abs(A)-abs(B)) > abs(C) | ...
        abs(C) > pi - abs(abs(A)-abs(B)) );
    a1(indices) = NaN;    a2(indices) = NaN;
    b1(indices) = NaN;    b2(indices) = NaN;
    c1(indices) = NaN;    c2(indices) = NaN;

end


function p = matrix_T_pdf(A, M, V, K, n)
% MATRIX_T_PDF Evaluate the density of a matrix under a Matrix-T distribution
% p = matrix_T_pdf(A, M, V, K, n)

% See "Bayesian Linear Regression", T. Minka, MIT Tech Report, 2001

[d m] = size(K);
is = 1:d;
c1 = prod(gamma((n+1-is)/2)) / prod(gamma((n-m+1-is)/2));
c2 = det(K)^(d/2) / det(pi*V)^(m/2); %% pi or 2pi?
p = c1 * c2 * det((A-M)'*inv(V)*(A-M)*K + eye(m))^(-n/2);


function circle_segment_test11 ( )

%*****************************************************************************80
%
%% CIRCLE_SEGMENT_TEST11 demonstrates CIRCLE_SEGMENT_ROTATION_FROM_CHORD.
%
%  Discussion:
%
%    We make a table of all pairs of angles that are multiples of pi/12.
%
%    For each pair, we compute the rotation, that is, the angle of the
%    central radius of the circle segment.  We print out the result in
%    terms of multiples of pi/12.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 May 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_SEGMENT_TEST11:\n' );
  fprintf ( 1, '  CIRCLE_SEGMENT_ROTATION_FROM_CHORD is given the endpoints\n' );
  fprintf ( 1, '  of a chord, and is asked to determine the angle of the\n' );
  fprintf ( 1, '  central radius vector.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  We make a table of all pairs of angles that are multiples\n' );
  fprintf ( 1, '  of pi/12, determine the corresponding chord endpoints, and\n' );
  fprintf ( 1, '  compute the rotation angle, also printed as a multiple of pi/12.\n' );

  r = 2.0;
  c = [ 3.0; 5.0 ];
  fprintf ( 1, '\n' );
  fprintf ( 1, '            0.0   1.0   2.0   3.0   4.0   5.0   6.0   7.0   8.0   9.0  10.0  11.0  12.0\n' );
  fprintf ( 1, '\n' );
  for i = 0 : 12
    rho1 = i * pi / 6;
    p1 = c + r * [ cos(rho1); sin(rho1) ];
    fprintf ( 1, '  %2d.0:  ', i );
    for j = 0 : 12
      rho2 = j * pi / 6;
      p2 = c + r * [ cos(rho2); sin(rho2) ];
      alpha = circle_segment_rotation_from_chord ( r, c, p1, p2 );
      fprintf ( 1, '  %4.1f', round ( 10 * 6 * alpha / pi ) / 10 );
    end
    fprintf ( 1, '\n' );
  end

  return
end


% P2.7
clear;  clc;
n1 = 0:20;
x1 = 0.9.^n1;
subplot(3,2,1); stem(n1,x1,'ko');  title('First given sequence x1(n)');
axis([-20 20 0 1]);
[x1f,n1f] = sigfold(x1,n1);                 % obtain x1(-n)
n2 = -20:0;
x2 = 0.8.^(-n2);
subplot(3,2,2); stem(n2,x2,'ko');  title('Second given sequence x2(n)');
axis([-20 20 0 1]);
[x2f,n2f] = sigfold(x2,n2);                 % obtain x2(-n)
[r,nr] = conv_m(x1,n1,x1f,n1f);             % auto-correlation
subplot(3,2,3); stem(nr,r,'ko');  title('Auto-Correlation x1(n)*x1(-n)');
[r,nr] = conv_m(x2,n2,x2f,n2f);             % auto-correlation
subplot(3,2,4); stem(nr,r,'ko');  title('Auto-Correlation x2(n)*x2(-n)');
[r,nr] = conv_m(x1f,n1f,x2,n2);             % cross-correlation
subplot(3,1,3); stem(nr,r,'ko');  title('Cross-Correlation');


function [w, dwdx, dwdxx] = Weight(type, para, di, dmI)
% EVALUATE WEIGHT FUNCTION
%
% SYNTAX: [w, dwdr, dwdrr] = GaussWeight(type, para, di, dmi)
%
% INPUT PARAMETERS
%    type - Type of weight function
%    para - Weight function parameter
%    di   - Distance
%    dmi  - Support size
% OUTPUT PARAMETERS
%    w    - Value of weight function at r
%    dwdx - Value of first order derivative of weight function with respect to x at r
%    dwdxx- Value of Second order derivative of weight function with respect to x at r
%
r  = abs(di) / dmI;

if (di >= 0.0)
   drdx = 1.0/dmI;
else
   drdx = -1.0/dmI;
end

% EVALUATE WEIGHT FUNCTION AND ITS FIRST AND SECOND ORDER OF DERIVATIVES WITH RESPECT r AT r
if (type == 'GAUSS')
   [w,dwdr,dwdrr] = Gauss(para,r);
elseif (type == 'CUBIC')
   [w,dwdr,dwdrr] = Cubic(r);
elseif (type == 'SPLIN')
   [w,dwdr,dwdrr] = Spline(r);
elseif (type == 'power')
   [w,dwdr,dwdrr] = power_function(para,r);
elseif (type == 'CSRBF')
   [w,dwdr,dwdrr] = CSRBF2(r);
else
   error('Invalid type of weight function.');
end

dwdx  = dwdr * drdx;
dwdxx = dwdrr * drdx * drdx;

function [w,dwdr,dwdrr] = Gauss(beta,r)
if (r>1.0)
   w     = 0.0;
   dwdr  = 0.0;
   dwdrr = 0.0;
else
   b2 = beta*beta;
   r2 = r*r;
   eb2 = exp(-b2);

   w     = (exp(-b2*r2) - eb2) / (1.0 - eb2);
   dwdr  = -2*b2*r*exp(-b2*r2) / (1.0 - eb2);
   dwdrr = -2*b2*exp(-b2*r2)*(1-2*b2*r2) / (1.0 - eb2);
end

function [w,dwdr,dwdrr] = Cubic(r)
if (r>1.0)
   w     = 0.0;
   dwdr  = 0.0;
   dwdrr = 0.0;
else
   w     = 1-6*r^2+8*r^3-3*r^4;
   dwdr  = -12*r+24*r^2-12*r^3;
   dwdrr = -12+48*r-36*r^2;
end

function [w,dwdr,dwdrr] = power_function(arfa,r)
if (r>1.0)
   w     = 0.0;
   dwdr  = 0.0;
   dwdrr = 0.0;
else
    a2 = arfa*arfa;
   r2 = r*r;
    w     = exp(-r2/a2);
   dwdr  = (-2*r/a2)*exp(-r2/a2);
   dwdrr  = (-2/a2+(-2*r/a2).^2)*exp(-r2/a2);
end

function [w,dwdr,dwdrr] = Spline(r)
if (r>1.0)
   w     = 0.0;
   dwdr  = 0.0;
   dwdrr = 0.0;
elseif (r<=0.5)
   w     = 2/3 - 4*r^2 + 4*r^3;
   dwdr  = -8*r + 12*r^2;
   dwdrr = -8 + 24*r;
else
   w     = 4/3 - 4*r + 4*r^2 - 4*r^3/3;
   dwdr  = -4 + 8*r -4*r^2;
   dwdrr = 8 - 8*r;
end

function [w,dwdr,dwdrr] = CSRBF2(r)
if (r>1.0)
   w     = 0.0;
   dwdr  = 0.0;
   dwdrr = 0.0;
else
	w     = (1-r)^6*(6+36*r+82*r^2+72*r^3+30*r^4+5*r^5);
	dwdr  = 11*r*(r+2)*(5*r^3+15*r^2+18*r+4)*(r-1)^5;
   dwdrr = 22*(25*r^5+100*r^4+142*r^3+68*r^2-16*r-4)*(r-1)^4;
end


function [ dmap ] = dataDensity( x, y, width, height, limits, fudge )
%DATADENSITY Get a data density image of data
%   x, y - two vectors of equal length giving scatterplot x, y co-ords
%   width, height - dimensions of the data density plot, in pixels
%   limits - [xmin xmax ymin ymax] - defaults to data max/min
%   fudge - the amount of smear, defaults to size of pixel diagonal
%
% By Malcolm McLean
%
    if(nargin == 4)
        limits(1) = min(x);
        limits(2) = max(x);
        limits(3) = min(y);
        limits(4) = max(y);
    end
    deltax = (limits(2) - limits(1)) / width;
    deltay = (limits(4) - limits(3)) / height;
    if(nargin < 6)
        fudge = sqrt(deltax^2 + deltay^2);
    end
    dmap = zeros(height, width);
    for ii = 0: height - 1
        yi = limits(3) + ii * deltay + deltay/2;
        for jj = 0 : width - 1
            xi = limits(1) + jj * deltax + deltax/2;
            dd = 0;
            for kk = 1: length(x)
                dist2 = (x(kk) - xi)^2 + (y(kk) - yi)^2;
                dd = dd + 1 / ( dist2 + fudge);
            end
            dmap(ii+1,jj+1) = dd;
        end
    end

end


function variance = gamma_variance ( a, b, c )

%*****************************************************************************80
%
%% GAMMA_VARIANCE returns the variance of the Gamma PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, the parameters of the PDF.
%    0.0 < B,
%    0.0 < C.
%
%    Output, real VARIANCE, the variance of the PDF.
%
  variance = b * b * c;

  return
end


function [r,normr,nre,s] = reorth(Q,r,normr,index,alpha,method)

%REORTH   Reorthogonalize a vector using iterated Gram-Schmidt
%
%   [R_NEW,NORMR_NEW,NRE] = reorth(Q,R,NORMR,INDEX,ALPHA,METHOD)
%   reorthogonalizes R against the subset of columns of Q given by INDEX.
%   If INDEX==[] then R is reorthogonalized all columns of Q.
%   If the result R_NEW has a small norm, i.e. if norm(R_NEW) < ALPHA*NORMR,
%   then a second reorthogonalization is performed. If the norm of R_NEW
%   is once more decreased by  more than a factor of ALPHA then R is
%   numerically in span(Q(:,INDEX)) and a zero-vector is returned for R_NEW.
%
%   If method==0 then iterated modified Gram-Schmidt is used.
%   If method==1 then iterated classical Gram-Schmidt is used.
%
%   The default value for ALPHA is 0.5.
%   NRE is the number of reorthogonalizations performed (1 or 2).

% References:
%  Aake Bjorck, "Numerical Methods for Least Squares Problems",
%  SIAM, Philadelphia, 1996, pp. 68-69.
%
%  J.~W. Daniel, W.~B. Gragg, L. Kaufman and G.~W. Stewart,
%  ``Reorthogonalization and Stable Algorithms Updating the
%  Gram-Schmidt QR Factorization'', Math. Comp.,  30 (1976), no.
%  136, pp. 772-795.
%
%  B. N. Parlett, ``The Symmetric Eigenvalue Problem'',
%  Prentice-Hall, Englewood Cliffs, NJ, 1980. pp. 105-109

%  Rasmus Munk Larsen, DAIMI, 1998.

% Check input arguments.
% warning('PROPACK:NotUsingMex','Using slow matlab code for reorth.')
if nargin<2
  error('Not enough input arguments.')
end
[n k1] = size(Q);
if nargin<3 | isempty(normr)
%  normr = norm(r);
  normr = sqrt(r'*r);
end
if nargin<4 | isempty(index)
  k=k1;
  index = [1:k]';
  simple = 1;
else
  k = length(index);
  if k==k1 & index(:)==[1:k]'
    simple = 1;
  else
    simple = 0;
  end
end
if nargin<5 | isempty(alpha)
  alpha=0.5; % This choice garanties that
             % || Q^T*r_new - e_{k+1} ||_2 <= 2*eps*||r_new||_2,
             % cf. Kahans ``twice is enough'' statement proved in
             % Parletts book.
end
if nargin<6 | isempty(method)
   method = 0;
end
if k==0 | n==0
  return
end
if nargout>3
  s = zeros(k,1);
end

normr_old = 0;
nre = 0;
while normr < alpha*normr_old | nre==0
  if method==1
    if simple
      t = Q'*r;
      r = r - Q*t;
    else
      t = Q(:,index)'*r;
      r = r - Q(:,index)*t;
    end
  else
    for i=index,
      t = Q(:,i)'*r;
      r = r - Q(:,i)*t;
    end
  end
  if nargout>3
    s = s + t;
  end
  normr_old = normr;
%  normr = norm(r);
  normr = sqrt(r'*r);
  nre = nre + 1;
  if nre > 4
    % r is in span(Q) to full accuracy => accept r = 0 as the new vector.
    r = zeros(n,1);
    normr = 0;
    return
  end
end


function [all_theta] = oneVsAll(X, y, num_labels, lambda)
%ONEVSALL trains multiple logistic regression classifiers and returns all
%the classifiers in a matrix all_theta, where the i-th row of all_theta
%corresponds to the classifier for label i
%   [all_theta] = ONEVSALL(X, y, num_labels, lambda) trains num_labels
%   logisitc regression classifiers and returns each of these classifiers
%   in a matrix all_theta, where the i-th row of all_theta corresponds
%   to the classifier for label i

% Some useful variables
m = size(X, 1);
n = size(X, 2);

% You need to return the following variables correctly
all_theta = zeros(num_labels, n + 1);

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: You should complete the following code to train num_labels
%               logistic regression classifiers with regularization
%               parameter lambda.
%
% Hint: theta(:) will return a column vector.
%
% Hint: You can use y == c to obtain a vector of 1's and 0's that tell use
%       whether the ground truth is true/false for this class.
%
% Note: For this assignment, we recommend using fmincg to optimize the cost
%       function. It is okay to use a for-loop (for c = 1:num_labels) to
%       loop over the different classes.
%
%       fmincg works similarly to fminunc, but is more efficient when we
%       are dealing with large number of parameters.
%
% Example Code for fmincg:
%
%     % Set Initial theta
%     initial_theta = zeros(n + 1, 1);
%
%     % Set options for fminunc
%     options = optimset('GradObj', 'on', 'MaxIter', 50);
%
%     % Run fmincg to obtain the optimal theta
%     % This function will return theta and the cost
%     [theta] = ...
%         fmincg (@(t)(lrCostFunction(t, X, (y == c), lambda)), ...
%                 initial_theta, options);
%

for c = 1:num_labels
    options = optimset('GradObj', 'on', 'MaxIter', 50);

    all_theta(c, :) = fmincg (@(t)(lrCostFunction(t, X, (y == c), lambda)), ...
            zeros(n + 1, 1), options);

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

end


function varargout = createTetrakaidecahedron()
%CREATETETRAKAIDECAHEDRON Create a 3D mesh representing a tetrakaidecahedron.
%
%   [V, E, F] = createTetrakaidecahedron;
%   Create a mesh structure representing a tetrakaidecahedron, composed of
%   both square and hexagonal faces. Tetrakaidecahedron can be used to tile
%   the 3D Euclidean space.
%
%   V is a 24-by-3 array with vertex coordinates,
%   E is a 36-by-2 array containing indices of neighbour vertices,
%   F is a 14-by-1 cell array containing vertex indices array of each face.
%
%   [V, F] = createTetrakaidecahedron;
%   Returns only the vertices and the face vertex indices.
%
%   MESH = createTetrakaidecahedron;
%   Returns the data as a mesh structure, with fields 'vertices', 'edges'
%   and 'faces'.
%
%   Example
%   [n, e, f] = createTetrakaidecahedron;
%   drawMesh(n, f);
%
%   See also
%   meshes3d, drawMesh

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

nodes = [...
    1 0 2;0 1 2;-1 0 2;0 -1 2;...
    2 0 1;0 2 1;-2 0 1;0 -2 1;...
    2 1 0;1 2 0;-1 2 0;-2 1 0;-2 -1 0;-1 -2 0;1 -2 0;2 -1 0;...
    2 0 -1;0 2 -1;-2 0 -1;0 -2 -1;...
    1 0 -2;0 1 -2;-1 0 -2;0 -1 -2];

edges = [...
    1 2;1 4;1 5;2 3;2 6;3 4;3 7;4 8;...
    5 9;5 16;6 10;6 11;7 12;7 13;8 14;8 15;...
    9 10;9 17;10 18;11 12;11 18;12 19;13 14;13 19;14 20;15 16;15 20;16 17;....
    17 21;18 22;19 23;20 24;21 22;21 24;22 23;23 24];

faces = {...
    [1 2 3 4], ...
    [1 4 8 15 16 5], [1 5 9 10 6 2], [2 6 11 12 7 3], [3 7 13 14 8 4],...
    [5 16 17 9], [6 10 18 11], [7 12 19 13], [8 14 20 15],...
    [9 17 21 22 18 10], [11 18 22 23 19 12], [13 19 23 24 20 14], [15 20 24 21 17 16], ...
    [21 24 23 22]};
faces = faces';

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


function xyz = plane_normal_qr_to_xyz ( pp, normal, pq, pr, n, qr )

%*****************************************************************************80
%
%% PLANE_NORMAL_QR_TO_XYZ: QR_TO_XYZ coordinates for a normal form plane.
%
%  Discussion:
%
%    The normal form of a plane in 3D is:
%
%      PP is a point on the plane,
%      NORMAL is a normal vector to the plane.
%
%    Two vectors PQ and PR can be computed with the properties that
%    * NORMAL, PQ and PR are pairwise orthogonal;
%    * PQ and PR have unit length;
%    * every point P in the plane has a "QR" representation
%      as P = PP + q * PQ + r * PR.
%
%    This function is given the QR coordinates of a set of points on the
%    plane, and returns the XYZ coordinates.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real PP(3), a point on the plane.
%
%    Input, real NORMAL(3), a normal vector N to the plane.  The
%    vector must not have zero length, but it is not necessary for N
%    to have unit length.
%
%    Input, real PQ(3), a vector of unit length,
%    perpendicular to the vector N and the vector PR.
%
%    Input, real PR(3), a vector of unit length,
%    perpendicular to the vector N and the vector PQ.
%
%    Input, integer N, the number of points on the plane.
%
%    Input, real QR(2,N), the QR coordinates of the points.
%
%    Output, real XYZ(3,N), the XYZ coordinates of the points.
%
  xyz = repmat ( pp, 1, n ) + [ pq'; pr' ]' * qr(1:2,1:n);

  return
end


function [ a, seed ] = markov_random ( n, key )

%*****************************************************************************80
%
%% MARKOV_RANDOM returns a random Markov matrix.
%
%  Discussion:
%
%    A Markov matrix, also called a "stochastic" matrix, is distinguished
%    by two properties:
%
%    * All matrix entries are nonnegative;
%    * The sum of the entries in each row is 1.
%
%    A "transition matrix" is the transpose of a Markov matrix, and
%    has column sums equal to 1.
%
%  Example:
%
%    N = 4
%
%    1/10  2/10  3/10  4/10
%    1     0     0     0
%    5/10  2/10  3/10  0
%    2/10  2/10  2/10  4/10
%
%  Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    0 <= A(I,J) <= 1.0 for every I and J.
%
%    The sum of the entries in each row of A is 1.
%
%    Because it has a constant row sum of 1,
%    A has an eigenvalue of 1, and
%    a (right) eigenvector of ( 1, 1, 1, ..., 1 ).
%
%    All the eigenvalues of A have modulus no greater than 1.
%
%    The eigenvalue 1 lies on the boundary of all the Gerschgorin rowsum disks.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, integer KEY, a positive value that selects the data.
%
%    Output, real A(N,N), the matrix.
%
  seed = key;

  [ a, seed ] = r8mat_uniform_01 ( n, n, seed );

  for i = 1 : n

    row_sum = sum ( a(i,1:n) );

    a(i,1:n) = a(i,1:n) / row_sum;

  end

  return
end


function geometry_test007 ( )

%*****************************************************************************80
%
%% TEST007 tests BALL_UNIT_SAMPLE_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  dim_num = 3;
  n_sample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST007\n' );
  fprintf ( 1, '  For the unit ball in 3 dimensions:\n' );
  fprintf ( 1, '  BALL_UNIT_SAMPLE_3D samples;\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  A few sample values:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 5
    [ x, seed ] = ball_unit_sample_3d ( seed );
    fprintf ( 1, '  %10f  %10f  %10f\n', x(1:dim_num) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of sample points = %d\n', n_sample );

  average(1:dim_num) = 0.0;

  for i = 1 : n_sample
    [ x, seed ] = ball_unit_sample_3d ( seed );
    average(1:dim_num) = average(1:dim_num) + x(1:dim_num);
  end

  average(1:dim_num) = average(1:dim_num) / n_sample;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now average the points, which should get a value\n' );
  fprintf ( 1, '  close to zero, and closer as N_SAMPLE increases.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Average:        %10f  %10f  %10f\n', average(1:dim_num) );

  average_r = 0.0;

  for i = 1 : n_sample
    [ x, seed ] = ball_unit_sample_3d ( seed );
    r = sqrt ( sum ( x(1:dim_num).^2 ) );
    average_r = average_r + r;
  end

  average_r = average_r / n_sample;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now average the distance of the points from\n' );
  fprintf ( 1, '  the center, which should be the \n' );
  fprintf ( 1, '  1/2**(1/dim_num) = %f\n', 0.5^( 1.0 / dim_num ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Average:        %f\n', average_r );

  average_theta = 0.0;

  for i = 1 : n_sample
    [ x, seed ] = ball_unit_sample_3d ( seed );
    theta = r8_atan ( x(2), x(1) );
    average_theta = average_theta + theta;
  end

  average_theta = average_theta / n_sample;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now average the angle THETA,\n' );
  fprintf ( 1, '  which should be PI.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Average:        %f\n', average_theta );

  average_phi = 0.0;

  for i = 1 : n_sample
    [ x, seed ] = ball_unit_sample_3d ( seed );
    r = sqrt ( sum ( x(1:dim_num).^2 ) );
    phi = acos ( x(3) / r );
    average_phi = average_phi + phi;
  end

  average_phi = average_phi / n_sample;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now average the angle PHI,\n' );
  fprintf ( 1, '  which should be PI/2.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Average:        %f\n', average_phi );

  return
end


function [node,elem] = cuboidmesh(cuboid,h)
%% CUBEHEXMESH uniform mesh of cuboid
%
% [node,elem] = cuboidmesh([x0,x1,y0,y1,z0,z1],h) generates a uniform mesh of the
% cuboid [x0,x1]*[y0,y1]*[z0,z1] with mesh size h.
%
% Example
%
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

x0 = cuboid(1); x1 = cuboid(2);
y0 = cuboid(3); y1 = cuboid(4);
z0 = cuboid(5); z1 = cuboid(6);

[x,y,z] = meshgrid(x0:h:x1,y0:h:y1,z0:h:z1);

node = [x(:),y(:),z(:)];

%% Generate elements
nx = size(x,1) - 1; % number of cells in x-direction
ny = size(y,2) - 1; % number of cells in y-direction
nz = size(z,2) - 1; % number of cells in z-direction

elem = zeros(nx*ny*nz,8);
cellidx = 1:nx*ny*nz;

[i, j, k] = ind2sub([nx ny nz],cellidx);  % index of cells in subscript form

s =[ nx+1, ny+1, nz+1];

elem(cellidx,1) = sub2ind3(s,i,j,k);
elem(cellidx,2) = sub2ind3(s,i,j+1,k);
elem(cellidx,3) = sub2ind3(s,i+1,j+1,k);
elem(cellidx,4) = sub2ind3(s,i+1,j,k);
elem(cellidx,5) = sub2ind3(s,i,j,k+1);
elem(cellidx,6) = sub2ind3(s,i,j+1,k+1);
elem(cellidx,7) = sub2ind3(s,i+1,j+1,k+1);
elem(cellidx,8) = sub2ind3(s,i+1,j,k+1);

    function idx = sub2ind3(siz,i,j,k)
        nr = siz(1); nc = siz(2); nv = siz(3);
        if (max(j)>nc) || (max(i)>nr) || (max(k)>nv)
            error(message('MATLAB:mysub2ind:IndexOutOfRange'));
        end
        idx = (k-1)*nr*nc + (j-1)*nr+i;
    end

end


function f=ref_idwilt(c,g,a,M)
%REF_DWILT  Reference Inverse Discrete Wilson Transform
%   Usage:  f=ref_idwilt(c,g,a,M);
%

% Setup transformation matrix.

L=size(g,1);
W=size(c,2);
N=L/a;

F=zeros(L,M*N);

% Weight coefficients.

l=(0:L-1).';

pif=0;

if 1
  % This version uses sines and cosines to express the basis functions.

  for n=0:N/2-1
    % Do the unmodulated coefficient.
    F(:,2*M*n+1)=circshift(g,2*a*n);

    % Setting this to -n*a should produce a time-invariant transform.
    timeinv=0; %-n*a;

    % m odd case
    for m=1:2:M-1
      F(:,m+2*M*n+1)   = sqrt(2)*sin(pi*m/M*(l+timeinv)+pif).*circshift(g,2*n*a);
      F(:,m+2*M*n+M+1) = sqrt(2)*cos(pi*m/M*(l+timeinv)+pif).*circshift(g,(2*n+1)*a);
    end;

    % m even case
    for m=2:2:M-1
      F(:,m+2*M*n+1)     = sqrt(2)*cos(pi*m/M*(l+timeinv)+pif).*circshift(g,2*n*a);
      F(:,m+2*M*n+M+1)   = sqrt(2)*sin(pi*m/M*(l+timeinv)+pif).*circshift(g,(2*n+1)*a);
    end;

    % Most modulated coefficient, Nyquest frequency.
    if mod(M,2)==0
      F(:,M+2*M*n+1)=(-1).^(l+timeinv).*circshift(g,2*n*a);
    else
      F(:,M+2*M*n+1)=(-1).^(l+timeinv).*circshift(g,(2*n+1)*a);
    end;
  end;

else

  % This version uses a cosine,

  for n=0:N/2-1
    % Do the unmodulated coefficient.
    F(:,2*M*n+1)=circshift(g,2*a*n);

    timeinv=-n*a;

    % m odd case
    for m=1:2:M-1
      F(:,m+2*M*n+1)   = sqrt(2)*cos(pi*m/M*(l+timeinv-M/2)+pif).*circshift(g,2*n*a);
      F(:,m+2*M*n+M+1) = sqrt(2)*sin(pi*m/M*(l+timeinv-M/2-a)+pif).*circshift(g,(2*n+1)*a);
    end;

    % m even case
    for m=2:2:M-1
      F(:,m+2*M*n+1)     = sqrt(2)*cos(pi*m/M*(l+timeinv-M/2)+pif).*circshift(g,2*n*a);
      F(:,m+2*M*n+M+1)   = sqrt(2)*sin(pi*m/M*(l+timeinv-M/2-a)+pif).*circshift(g,(2*n+1)*a);
    end;

    % Most modulated coefficient, Nyquest frequency.
    if mod(M,2)==0
      F(:,M+2*M*n+1)=(-1).^(l+timeinv).*circshift(g,2*n*a);
    else
      F(:,M+2*M*n+1)=(-1).^(l+timeinv-a).*circshift(g,(2*n+1)*a);
    end;
  end;

end;

f=F*c;


function [ L, D, C, Cinv ] = get_transform_matrices_2d( R, method )
%UNTITLED4 Summary of this function goes here
%   Detailed explanation goes here
if method == 1
    % LDL Without the permutation
    [L, D] = ldl(R);
    C = inv(L);
    Cinv = L;

elseif method == 2
    % Eigen Decomp
    [V,D] = eig(R);
    C = V.';
    Cinv = V;

elseif method == 3
    % Cholesky Decomp (Special case of LDL, D=I)
    L = chol(R);  % NOTE: this is the UPPER triangular transform
    L = L.';
    C = inv(L);
    Cinv = L;
    D = eye(2,2);

elseif method == 4  %%% ANALYTICAL
    rho = R(1,2);
    L = [1 0; rho 1];
    D = diag([1, 1 - rho^2]);
    C = [1 0; -rho 1];
    % C = inv(L);
    Cinv = L;
end

end


function R = RiemannND(x)
% Riemann's non-differentiable function, R(x) for rational x = p/q

[p,q] = rat(x);
R = 0;
for k = 1:q-1
    R = R + sin(k^2*p*pi/q)/(sin(k*pi/2/q))^2;
end
R = R*pi/4/q/q;
end


 function y = jinc(x)
%function y = jinc(x)
% jinc(x) = J_1(pi x) / (2 x),
% where J_1 is Bessel function of the first kind of order 1.
% This is the 2D Fourier transform of a disk of diameter 1,
% so its DC value is the area of that disk which is pi/4.
% Equivalently it is the Hankel transform of rect = @(r) abs(r) < 1/2;
% Jeff Fessler, University of Michigan

x = abs(x); % kludge for bessel with negative arguments, perhaps not needed
y = pi/4 + 0 * x; % jinc(0) = pi/4
ig = x ~= 0;
y(ig) = besselj(1, pi * x(ig)) ./ (2 * x(ig));


function Y = symNormalRnd(M, S, sz)

% Generates independent symmetric matrices
% according to a matrix-variate symmetric normal distribution.
%
% Y = symNormalRnd(M, S, sz)
%
%
% Input:
%   M       pxp mean symmetric matrix
%   S       qxq covariance matrix
%   sz      size of the array (e.g. number of samples)
%
% Output:
%   Y       pxpx[sz] array of random symmetric matrices.
%
% Copyright: Armin Schwartzman, 2009
%

% HISTORY:
%   2008.12.30 ASH (armins@hsph.harvard.edu) wrote it.

% Check inputs
if (size(M,1) ~= size(M,2)),
    error('Wrong input format');
end
if (size(S,1) ~= size(S,2)),
    error('Wrong input format');
end
p = size(M,1);
q = p*(p+1)/2;
if (size(S,1) ~= q),
    error('Wrong input format');
end
if ~exist('sz'),
    sz = 1;
end

%-----------------------------------------------------------------
% Generate multivariate normal
N = prod(sz);       % number of samples
z = mvnrnd(zeros(1,q), S, N);   % size Nxq
z = reshape(z', [q sz]);        % size qxN
Y = repmat(M, [1 1 sz]) + vecdinv(z);

return


function res = bhp(im,thresh,n)

% inputs
% im is the fourier transform of the image
% thresh is the cutoff circle radius

%outputs
% res is the filtered image

[r,c]=size(im);
d0=thresh;

d=zeros(r,c);
h=zeros(r,c);

for i=1:r
    for j=1:c
     d(i,j)=  sqrt( (i-(r/2))^2 + (j-(c/2))^2);
    end
end

for i=1:r
    for j=1:c
      h(i,j)=  1 / (1+ (d0/d(i,j))^(2*n) ) ;
    end
end

for i=1:r
    for j=1:c
    res(i,j)=(h(i,j))*im(i,j);

    end
end


function value = p27_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P27_F evaluates the integrand for problem 27.
%
%  Dimension:
%
%    N arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integral Parameters:
%
%    The integral depends on a parameter R and vector C(1:N).
%
%    R defaults to 0.3.
%
%    The reference suggests choosing C at random in [0,1]
%    and then multiplying by the normalizing factor (25/N).
%    C(1:N) defaults to 1/N.
%
%    To get or set R, call P27_R8.
%    To get or set C, call P27_R8VEC.
%
%  Integrand:
%
%    cos ( 2 * pi * R + sum ( c(1:n) * x(1:n) ) )
%
%  Exact Integral:
%
%    2^N * cos ( 2 * pi * R + 0.5 * sum ( c(1:n) ) )
%      * product ( sin ( 0.5 * c(1:n) ) / c(1:n) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Alan Genz,
%    [Integral #1]
%    A Package for Testing Multiple Integration Subroutines,
%    in Numerical Integration: Recent Developments, Software
%    and Applications,
%    edited by Patrick Keast and Graeme Fairweather,
%    D Reidel, 1987, pages 337-340,
%    LC: QA299.3.N38.
%
%    Thomas Patterson,
%    [Integral #5],
%    On the Construction of a Practical Ermakov-Zolotukhin
%    Multiple Integrator,
%    in Numerical Integration: Recent Developments, Software
%    and Applications,
%    edited by Patrick Keast and Graeme Fairweather,
%    D. Reidel, 1987, pages 269-290,
%    LC: QA299.3.N38.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  r = 0.0;
  r = p27_r8 ( 'G', 'R', r );

  c = [];
  c = p27_r8vec ( 'G', 'C', dim_num, c );

  value(1:point_num) = 0.0;

  for point = 1 : point_num

    arg = 2.0 * pi * r + c(1:dim_num,1)' * x(1:dim_num,point);
    value(point) = cos ( arg );

  end

  p27_i4 ( 'I', '#', point_num );

  return
end


function y = sigmoidabTransform(x, transform, varargin)

% SIGMOIDABTRANSFORM Constrains a parameter to be between A and B
% by a scaled logistic sigmoid function.
%
% FORMAT
%
% DESC contains commands to constrain parameters to be between A
% and B via the sigmoid function, y=A+(B-A)/(1+exp(-x)).
%
% ARG x : input argument.
%
% ARG transform : type of transform, 'atox' maps a value into
% the transformed space (i.e. makes it between A and B). 'xtoa'
% maps the parameter back from transformed space to the original
% space. 'gradfact' gives the factor needed to correct gradients
% with respect to the transformed parameter, that is, it gives
% the gradient dx/da where x is the transformed parameter and a
% is the untransformed parameter.
%
% ARG transformsettings (first element of varargin): vector [A B]
% giving the minimum and maximum values A and B for the transformed
% parameter. If not given, assume A=0 and B=1 as in the function
% sigmoidTransform.
%
% OUTPUT y : return argument.
%
% SEEALSO : negLogLogitTransform, expTransform
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006, 2007
%
% COPYRIGHT : Jaakko Peltonen, 2011
%
% COPYRIGHT : Antti Honkela, 2012

% OPTIMI

% Get the transformation settings from the varargin structure
if ~isempty(varargin),
  transformsettings=varargin{1};
else
  transformsettings=[];
end;

identitymode=0;
if identitymode==1,
  % DEBUG ONLY, turns transformation to identity!
  fprintf(1,'Using identity mode for transform %s of parameter value %f\n',transform, x)
  switch transform
   case 'atox'
    y=x;
   case 'xtoa'
    y=x;
   case 'gradfact'
    y=0*x+1;
  end
else
  % normal mode, logistic sigmoid between A and B

  if isempty(transformsettings),
    warning(sprintf('sigmoidabTransform(%f,%s) called without transformation settings\n',x,transform));
    A=0; B=1;
  else
    A=transformsettings(1);
    B=transformsettings(2);
  end;

  limVal = 36;
  minval_sigmoid=A+(B-A)*eps;
  maxval_sigmoid=A+(B-A)*(1-eps);

  y = zeros(size(x));
  switch transform
   case 'atox'
    %x
    %A
    %B
    %fprintf(1,'transforming x %f, A %f, B %f\n',x,A,B);

    I1 = x < -limVal;
    y(I1) = A+(B-A)*eps;

    I2 = x > limVal;
    y(I2) = A+(B-A)*(1-eps);

    I3 = ~I1 & ~I2;
    y(I3) = A+(B-A)*sigmoid(x(I3));

   case 'xtoa'
    %  [x A B]
    %fprintf(1,'inverse-transforming x %f, A %f, B %f\n',x,A,B);
    I1 = x<=minval_sigmoid;
    y(I1) = -limVal;

    I2 = x>=maxval_sigmoid;
    y(I2) = limVal;

    I3 = ~I1 & ~I2;
    y(I3) = invSigmoid((x(I3)-A)/(B-A));

   case 'gradfact'
    %fprintf(1,'gradfact-transforming x %f, A %f, B %f\n',x,A,B);
    %y = (B-A)*x.*(1-x);
    y = (x-A).*(B-x)/(B-A);
    %y=0*x+1; % DEBUG ONLY, turns factors off!
  end
end;


function w = ymdf_to_weekday_julian2 ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_WEEKDAY_JULIAN2 returns the weekday of a Julian YMDF date.
%
%  Discussion:
%
%    This routine computes the day of the week from the date in
%    the Julian calendar, that is, the calendar in force before the
%    Gregorian calendar, in which every fourth year was a leap year.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Algorithm A,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999, pages 307-308.
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, integer W, the day of the week of the date.
%    The days are numbered from Sunday through Saturday, 1 through 7.
%
  m_prime = mod ( 9 + m, 12 );
  q = floor ( m_prime / 10 );
  z = floor ( ( 13 * m_prime + 2 ) / 5 );
  t = 28 * m_prime + z + d - 365 * q + 59;

  c = i4_wrap ( t, 1, 7 );

  y_prime = y - q;
  v = floor ( y / 4 ) - floor ( y_prime / 4 );
  p = y + floor ( y / 4 ) + 4 - v;
  n = 7 - mod ( p, 7 );

  w = i4_wrap ( c + 1 - n, 1, 7 );

  return
end


function value = c8_log ( z )

%*****************************************************************************80
%
%% C8_LOG evaluates the logarithm of a C8.
%
%  Discussion:
%
%    A C8 is a complex value.
%
%    Here we use the relationship:
%
%      C8_LOG ( Z ) = LOG ( MAG ( Z ) ) + i * ARG ( Z )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 February 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, complex Z, the argument.
%
%    Output, complex VALUE, the function value.
%
  arg = c8_arg ( z );
  mag = c8_mag ( z );

  value = log ( mag ) + arg * i;

  return
end


function KeplerUniversalVsSTK()
%% Constants
mu = 398600.4418;

%% Polar GEO Altitude Orbit Propagation Comparison
load('PolarGEO.mat');
r0 = repmat(PosVel(1,1:3)',[1 size(PosVel,1)]);
v0 = repmat(PosVel(1,4:6)',[1 size(PosVel,1)]);
[r,v] = keplerUniversal(r0,v0,0:60:(size(PosVel,1)-1)*60,mu);
PlotStateVectorErrors(r,v,PosVel,0:60:(size(PosVel,1)-1)*60,'GEO Altitude, Polar Inclination Propagation');

%% LEO Orbit Propagation Comparison
load('InclinedLEO.mat');
r0 = repmat(PosVel(1,1:3)',[1 size(PosVel,1)]);
v0 = repmat(PosVel(1,4:6)',[1 size(PosVel,1)]);
[r,v] = keplerUniversal(r0,v0,0:60:(size(PosVel,1)-1)*60,mu);
PlotStateVectorErrors(r,v,PosVel,0:60:(size(PosVel,1)-1)*60,'LEO Inclined Propagation');

%% Hyperbolic LEO Altitude Orbit Propagtion Comparison
load('HyperbolicLEO.mat');
r0 = repmat(PosVel(1,1:3)',[1 size(PosVel,1)]);
v0 = repmat(PosVel(1,4:6)',[1 size(PosVel,1)]);
[r,v] = keplerUniversal(r0,v0,0:60:(size(PosVel,1)-1)*60,mu);
PlotStateVectorErrors(r,v,PosVel,0:60:(size(PosVel,1)-1)*60,'Hyperbolic Orbit Propagation');

%% Parabolic LEO Altitude Orbit Propagation comparison
load('ParabolicLEO.mat');
r0 = repmat(PosVel(1,1:3)',[1 size(PosVel,1)]);
v0 = repmat(PosVel(1,4:6)',[1 size(PosVel,1)]);
[r,v] = keplerUniversal(r0,v0,0:60:(size(PosVel,1)-1)*60,mu);
PlotStateVectorErrors(r,v,PosVel,0:60:(size(PosVel,1)-1)*60,'Parabolic Orbit Propagation');

end

function PlotStateVectorErrors(r,v,PosVel,t,titleText)
MaxPositionError = abs(r-PosVel(:,1:3)');
MaxVelocityError = abs(v-PosVel(:,4:6)');
figure('color',[1 1 1]);
subplot(4,1,1); plot(t./60,MaxPositionError); title(titleText); ylabel('Pos Err (km)');
subplot(4,1,2); plot(t./60,MaxVelocityError); ylabel('Vel Err (km/s)');
subplot(4,1,3); plot(t./60,abs(sqrt(sum(r.^2,1)) - sqrt(sum(PosVel(:,1:3)'.^2,1)))); ylabel('Pos Mag Err (km)'); xlabel('Time (m)');
subplot(4,1,4); plot(t./60,abs(sqrt(sum(v.^2,1)) - sqrt(sum(PosVel(:,4:6)'.^2,1)))); ylabel('Vel Mag Err (km/s)'); xlabel('Time (m)');
end


function eg2OC_BVP
%EG2OC_BVP    Example 2 of optimal control tutorial.
%    This example is from D.E.Kirk's Optimal control theory: an introduction
%    example 6.2-2 on page 338-339
%    The problem is reformulated as a boundary value problem (BVP)
%    and solved with bvp4c solver

% Initial guess for the solution
solinit = bvpinit(linspace(0,0.78,50), [0 0 0.5 0.5]);
options = bvpset('Stats','on','RelTol',1e-1);
global R;
R = 0.1;
sol = bvp4c(@BVP_ode, @BVP_bc, solinit, options);
t = sol.x;
y = sol.y;

% Calculate u(t) from x1,x2,p1,p2
ut = (y(3,:).*(y(1,:) + 1/4))/(2*0.1);
n = length(t);
% Calculate the cost
J = 0.78*(y(1,:)*y(1,:)' + y(2,:)*y(2,:)' + ut*ut'*0.1)/n;

figure(1);
plot(t, y(1:2,:)','-');hold on;
plot(t,ut', 'r:')
text(.2,0.08,'x_1(t)');
text(.3,-.1,'x_2(t)');
text(.2,.4, 'u(t)');
s = strcat('Final cost is: J=',num2str(J));
text(.4,1,s);
xlabel('time');
ylabel('states');
hold off;
print -djpeg90 -r300 eg2_bvp.jpg

%------------------------------------------------
% ODE's for states and costates
function dydt = BVP_ode(t,y)
global R;
t1 = y(1)+.25;
t2 = y(2)+.5;
t3 = exp(25*y(1)/(y(2)+2));
t4 = 50/(y(1)+2)^2;
u =  y(3)*t1/(2*R);

dydt = [-2*t1+t2*t3-t2*u
        0.5-y(2)-t2*t3
        -2*y(1)+2*y(3)-y(3)*t2*t4*t3+y(3)*u+y(4)*t2*t4*t3
        -2*y(2)-y(3)*t3+y(4)*(1+t3)];

% -----------------------------------------------
% The boundary conditions:
% x1(0) = 0.05, x2(0) = 0, tf = 0.78, p1(tf) = 0, p2(tf) = 0;
function res = BVP_bc(ya,yb)
res = [ ya(1) - 0.05
        ya(2) - 0
        yb(3) - 0
        yb(4) - 0 ];


function f = tt_loglikelihood(X,M)
%TT_LOGLIKELIHOOD Compute log-likelihood of data X with model M.
%
%   F = TT_LOGLIKELIHOOD(X,M) computes the log-likelihood of model M given
%   data X, where M is a ktensor and X is a tensor or sptensor.
%   Specifically, F = - (sum_i m_i - x_i * log_i) where i is a multiindex
%   across all tensor dimensions.
%
%   See also cp_apr, tensor, sptensor, ktensor.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

N = ndims(X);

if ~isa(M, 'ktensor')
    error('M must be a ktensor');
end

M = normalize(M,1,1);

if isa(X, 'sptensor')
    xsubs = X.subs;
    A = M.U{1}(xsubs(:,1),:);
    for n = 2:N
       A = A .* M.U{n}(xsubs(:,n),:);
    end
    f = sum(X.vals .* log(sum(A,2))) - sum(sum(M.U{1}));
else
    f = sum(sum(double(tenmat(X,1)) .* log(double(tenmat(M,1))))) - sum(sum(M.U{1}));
end


function [ n_data, x, fx ] = elliptic_ka_values ( n_data )

%*****************************************************************************80
%
%% ELLIPTIC_KA_VALUES returns values of the complete elliptic integral K(ALPHA).
%
%  Discussion:
%
%    This is one form of what is sometimes called the complete elliptic integral
%    of the first kind.
%
%    The function is defined by the formula:
%
%      K(ALPHA) = integral ( 0 <= T <= PI/2 )
%        dT / sqrt ( 1 - sin ( ALPHA )^2 * sin ( T )^2 )
%
%    In Mathematica, the function can be evaluated by:
%
%      EllipticK[(Sin[alpha*Pi/180])^2]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function, measured
%    in degrees.
%
%    Output, real FX, the value of the function.
%
  n_max = 18;

  fx_vec = [ ...
     0.1570796326794897E+01, ...
     0.1573792130924768E+01, ...
     0.1582842804338351E+01, ...
     0.1598142002112540E+01, ...
     0.1620025899124204E+01, ...
     0.1648995218478530E+01, ...
     0.1685750354812596E+01, ...
     0.1731245175657058E+01, ...
     0.1786769134885021E+01, ...
     0.1854074677301372E+01, ...
     0.1935581096004722E+01, ...
     0.2034715312185791E+01, ...
     0.2156515647499643E+01, ...
     0.2308786798167196E+01, ...
     0.2504550079001634E+01, ...
     0.2768063145368768E+01, ...
     0.3153385251887839E+01, ...
     0.3831741999784146E+01 ];

  x_vec = [ ...
      0.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     15.0E+00, ...
     20.0E+00, ...
     25.0E+00, ...
     30.0E+00, ...
     35.0E+00, ...
     40.0E+00, ...
     45.0E+00, ...
     50.0E+00, ...
     55.0E+00, ...
     60.0E+00, ...
     65.0E+00, ...
     70.0E+00, ...
     75.0E+00, ...
     80.0E+00, ...
     85.0E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function y = entr( x )

%ENTR   Scalar entropy.
%   ENTR(X) returns an array of the same size as X with the unnormalized
%   entropy function applied to each element:
%                { -X.*LOG(X) if X > 0,
%      ENTR(X) = { 0          if X == 0,
%                { -Inf       otherwise.
%   If X is a vector representing a discrete probability distribution, then
%   SUM(ENTR(X)) returns its entropy.
%
%   Disciplined convex programming information:
%       ENTR(X) is concave and nonmonotonic in X. Thus when used in CVX
%       expressions, X must be real and affine. Its use will effectively
%       constrain X to be nonnegative: there is no need to add an
%       additional X >= 0 to your model in order to enforce this.

error(nargchk(1,1,nargin));
cvx_expert_check( 'entr', x );
y = -rel_entr( x, 1 );

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [x] = gels(a,b,param)
%
% GELS   Solves an overdetermined or underdetermined linear problem.
%
%    [X] = GELS(A,B) same as [X] = GELS(A,B,'default')
%
%    [X] = GELS(A,B,PARAM) if A is m-by-n and m > n then GELS computes the solution of the
%    linear least squares problem
%            min || A * X - B ||_F ,
%    if A is m-by-n and m < n then GELS computes the solution of the minimum norm problem
%            min || X ||_F s.t. A*X = B
%    PARAM controls the algorithm used:
%
%       'dc'
%             performs SVD factorization of A using the divide and conquer
%             method then solve the undetermined or overdetermined problem.
%             calls LAPACK routine _GELSD
%
%       'svd'
%             performs SVD factorization of A using QR iterations then solve the
%             undetermined or overdetermined problem.
%             calls LAPACK routine _GELSS
%
%       'y'
%             performs QR factorization of A then solve the undetermined or
%             overdetermined problem.
%             calls LAPACK routine _GELSY
%
%       'default'
%             (default)
%             performs QR factorization of A without pivoting, then solve the
%             undetermined or overdetermined problem.  The fact that there is no
%             pivoting in the QR factorization means that A(:,1:min(size(A))
%             needs to be full rank.
%             calls LAPACK routine _GELS
%
%    When A is m-by-n with m > n then [X] = GELS(A,B) is the same as X=A\B.
%
%    When A is m-by-n with m < n  then [X] = GELS(A,B,PARAM) is _not_ the same as
%    X=A\B. [X] = GELS(A,B,PARAM) solves the problem
%    (*)    min_X || X ||_F s.t. AX=B
%    while X=A\B solves AX=B by first performing a QR factorization of A
%          [Q,R,E] = QR(A)
%    then X is obtained via
%          X = E(:,1:m)*(R(:,1:m)\(Q'*b))]
%	The \ method is faster than GELS, X is such that AX=B but X is not (in the
%	general case) the solution of (*).
%
%    See also: \, MLDIVIDE.
%
	x=[];

	if (nargout~=1),
		disp('Wrong number of output parameters');
		return;
	end;

	if (nargin~=2 && nargin~=3),
		disp('Wrong number of input parameters');
		return;
	end;

	%Do all the boring checking
	if (~isnumeric(a)),
		disp('The matrix is not composed of numeric values.');
		return;
	elseif (isinteger(a)),
		a=double(a);
	end;

	if ((nargin == 2)||(strcmp(param,'default'))),
		trans = 'N';
		[x,info] = gels_2(trans,a,b);
	else
		if (strcmp(param,'dc')),
			rcond = -1; % machine precision
			[x,rank,info] = gelsd(a,b,rcond);
		elseif (strcmp(param,'svd')),
			rcond = -1; % machine precision
			[x,rank,info] = gelss(a,b,rcond);
		elseif (strcmp(param,'y')),
			rcond = eps; % machine precision
			[x,rank,info] = gelsy(a,b,rcond);
		else
			disp('Type of algorithm not correct. Please choose dc, svd, y, or default.');
			return;
		end;
	end;

	%Do post-call boring checking
	if (info~=0),
		disp('Resolution failed, no solution returned!');
		x=[];
		return;
	end;


function[I]=besselitilde(varargin)
%BESSELITILDE  I-type Bessel function after factoring off exponential growth.
%
%   ITILDE=BESSELITILDE(NU,Z) returns the modified Bessel function of the
%   first kind of order NU at argument Z, after factoring off the
%   asymptotic behavior of EXP(Z).
%
%   BESSELITILDE is useful for products of modified Bessel functions in
%   which the exponential behaviors cancel, but that cannot be evaluated
%   directly because of numerical overflow.
%
%   BESSELITILDE(NU,Z,N) use a summation truncated at N terms. The default
%   behavior uses N=30 and is highly accurate.
%
%   See 10.40.1 of https://dlmf.nist.gov/10.40.
%
%   This is low-level code used by WINDTRANS using an algorithm described
%   in Lilly and Elipot (2021).
%
%   See also BESSELKTILDE.
%
%   'besselitilde --t' runs a test.
%
%   Usage: I=besselitilde(nu,z);
%          I=besselitilde(nu,z,nterms);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2019--2021 J.M. Lilly --- type 'help jlab_license' for details

if strcmp(varargin{1}, '--t')
    besselitilde_test,return
end

nu=varargin{1};
z=varargin{2};
nterms=30;
if nargin==3
    nterms=varargin{3};
end

sizez=size(z);
z=z(:);
%let's sum over the third dimensions
zk=vrep(-z,nterms,2);  %this makes the alternating sign
zk(:,1)=1;
zk=cumprod(zk,2);

k=(0:nterms-1)';
ak=(4*nu.^2-(2*k-1).^2);
ak(1)=1;
ak=cumprod(ak,1);
ak=frac(ak,factorial(k).*8.^k);
if any(~isfinite(ak))
    ak=ak(1:find(~isfinite(ak),1,'first')-1);
end
zk=zk(:,1:length(ak));

I=frac(1,sqrt(2*z*pi)).*((1./zk)*ak);
I=reshape(I,sizez);
%nterms

% %let's sum over the third dimensions
% zk=vrep(-z,nterms,3);  %this makes the alternating sign
% zk(:,:,1)=1;
% zk=cumprod(zk,3);
%
% k=(0:nterms-1)';
% ak=(4*nu.^2-(2*k-1).^2);
% ak(1)=1;
% ak=cumprod(ak,1);
% ak=frac(ak,factorial(k).*8.^k);
% if any(~isfinite(ak))
%     ak=ak(1:find(~isfinite(ak),1,'first')-1);
% end
% zk=zk(:,:,1:length(ak));
%
% ak=vrep(permute(ak,[3 2 1]),size(z),[1 2]);
%
% I=frac(1,sqrt(2*z*pi)).*sum(frac(ak,zk),3);

function[]=besselitilde_test

for s=[1 -1]
    z=sqrt(s*1i)*[23:0.01:100]';
    %z=[20:0.01:100]';
    [bi0,bi]=vzeros(length(z),2);
    for i=1:2
        bi0(:,i)=exp(-z).*besseli(i-1,z);
        bi(:,i)=besselitilde(i-1,z);
    end

    if s==1
        reporttest('BESSELITILDE for z with phase of pi/4',allall(abs((bi0-bi)./bi)<1e-14))
    else
        reporttest('BESSELITILDE for z with phase of -pi/4',allall(abs((bi0-bi)./bi)<1e-14))
    end

	bi=vzeros(length(z),2);
    for i=1:2
        bi(:,i)=besselitilde(i-1,z,2);
    end

    if s==1
        reporttest('BESSELITILDE 2-term for z with phase of pi/4, order 0 and 1',allall(abs((bi0(:,1:2)-bi)./bi)<1e-3))
    else
        reporttest('BESSELITILDE 2-term for z with phase of -pi/4, order 0 and 1',allall(abs((bi0(:,1:2)-bi)./bi)<1e-3))
    end

    bi0=sqrt(frac(1,2*pi*z)).*(1+frac(1,8*z));
    bi1=sqrt(frac(1,2*pi*z)).*(1-frac(3,8*z));

    if s==1
        reporttest('BESSELITILDE 2-term vs. analytic for z with phase of pi/4',allall(abs(([bi0 bi1]-bi)./bi)<1e-15))
    else
        reporttest('BESSELITILDE 2-term  vs. analytic for z with phase of -pi/4',allall(abs(([bi0 bi1]-bi)./bi)<1e-15))
    end
end

% figure,plot(abs(z),abs(bi-bi0)),ylog

% nu=5;
% z=[0:0.001:100]';
% tic;bi(:,1)=besselitilde(nu,z);toc
% tic;bi(:,2)=sqrt(2*z*pi).*exp(-z).*besseli(nu,z);toc
% figure,plot(z,log10(abs((bi(:,1)-bi(:,2))./bi(:,2))))
% %wow, really excellent
%
%
% tic;bi0=sqrt(2*z*pi).*exp(-z).*besseli(nu,z);toc
% for i=1:50
%     bi(:,i)=besselitilde(nu,z,i);
% end
% bi0=vrep(bi0,size(bi,2),2);
% plot(z,log10(abs((bi-bi0)./bi0)))
%
%


function MS = createMesh3D(varargin)
% MeshStructure = createMesh3D(Nx, Ny, Nz, Width, Height, Depth)
% creates a uniform 3D mesh:
% Nx is the number of cells in x (horizontal) direction
% Ny is the number of cells in y (vertical) direction
% Nz is the number of cells in z (perpendicular) direction
% Lx is the domain length in x direction
% Ly is the domain length in y direction
% Lz is the domain length in z direction
%
% SYNOPSIS:
%   MeshStructure = createMesh3D(Nx, Ny, Nz, Lx, Ly, Lz)
%
% PARAMETERS:
%   Nx: number of cells in the x direction
%   Lx: domain length in x direction
%   Ny: number of cells in the y direction
%   Ly: domain length in y direction
%   Nz: number of cells in the z direction
%   Lz: domain length in z direction
%
% RETURNS:
%   MeshStructure.
%                 dimensions=3 (3D problem)
%                 numbering: shows the indexes of cellsn from left to right
%                 and top to bottom and back to front
%                 cellsize: x, y, and z elements of the cell size =[Lx/Nx,
%                 Ly/Ny, Lz/Nz]
%                 cellcenters.x: location of each cell in the x direction
%                 cellcenters.y: location of each cell in the y direction
%                 cellcenters.z: location of each cell in the z direction
%                 facecenters.x: location of interface between cells in the
%                 x direction
%                 facecenters.y: location of interface between cells in the
%                 y direction
%                 facecenters.z: location of interface between cells in the
%                 z direction
%                 numberofcells: [Nx, Ny, Nz]
%
%
% EXAMPLE:
%   Nx = 2;
%   Lx = 1.0;
%   Ny = 3;
%   Ly = 2.0;
%   Nz = 4;
%   Lz = 3.0;
%   m = createMesh3D(Nx, Ny, Nz, Lx, Ly, Lz);
%   [X, Y, Z] = ndgrid(m.cellcenters.x, m.cellcenters.y, m.cellcenters.z);
%   [Xf, Yf, Zf] = ndgrid(m.facecenters.x, m.facecenters.y, m.facecenters.z);
%   plot3(X(:), Y(:), Z(:), 'or')
%   hold on;
%   plot3(Xf(:), Yf(:), Zf(:), '+b')
%   legend('cell centers', 'cell corners');
%
% SEE ALSO:
%     createMesh1D, createMesh2D, createMeshCylindrical1D, ...
%     createMeshCylindrical2D, createCellVariable, createFaceVariable

% Written by Ali A. Eftekhari
% See the license file

if nargin==6
  % uniform 1D mesh
  Nx=varargin{1};
  Ny=varargin{2};
  Nz=varargin{3};
  Width=varargin{4};
  Height=varargin{5};
  Depth=varargin{6};
  % cell size is dx
  dx = Width/Nx;
  dy = Height/Ny;
  dz = Depth/Nz;
  G=reshape(1:(Nx+2)*(Ny+2)*(Nz+2), Nx+2, Ny+2, Nz+2);
  CellSize.x= dx*ones(Nx+2,1);
  CellSize.y= dy*ones(Ny+2,1);
  CellSize.z= dz*ones(Nz+2,1);
  CellLocation.x= [1:Nx]'*dx-dx/2;
  CellLocation.y= [1:Ny]'*dy-dy/2;
  CellLocation.z= [1:Nz]'*dz-dz/2;
  FaceLocation.x= [0:Nx]'*dx;
  FaceLocation.y= [0:Ny]'*dy;
  FaceLocation.z= [0:Nz]'*dz;
elseif nargin==3
  % nonuniform 1D mesh
  facelocationX=varargin{1};
  facelocationY=varargin{2};
  facelocationZ=varargin{3};
  facelocationX=facelocationX(:);
  facelocationY=facelocationY(:);
  facelocationZ=facelocationZ(:);
  Nx = length(facelocationX)-1;
  Ny = length(facelocationY)-1;
  Nz = length(facelocationZ)-1;
  G=reshape(1:(Nx+2)*(Ny+2)*(Nz+2), Nx+2, Ny+2, Nz+2);
  CellSize.x= [facelocationX(2)-facelocationX(1); ...
    facelocationX(2:end)-facelocationX(1:end-1); ...
    facelocationX(end)-facelocationX(end-1)];
  CellSize.y= [facelocationY(2)-facelocationY(1); ...
    facelocationY(2:end)-facelocationY(1:end-1); ...
    facelocationY(end)-facelocationY(end-1)];
  CellSize.z= [facelocationZ(2)-facelocationZ(1); ...
    facelocationZ(2:end)-facelocationZ(1:end-1); ...
    facelocationZ(end)-facelocationZ(end-1)];
  CellLocation.x= 0.5*(facelocationX(2:end)+facelocationX(1:end-1));
  CellLocation.y= 0.5*(facelocationY(2:end)+facelocationY(1:end-1));
  CellLocation.z= 0.5*(facelocationZ(2:end)+facelocationZ(1:end-1));
  FaceLocation.x= facelocationX;
  FaceLocation.y= facelocationY;
  FaceLocation.z= facelocationZ;
end
c=G([1,end], [1,end], [1, end]);
e1=G([1, end], [1, end], 2:Nz+1);
e2=G([1, end], 2:Ny+1, [1, end]);
e3=G(2:Nx+1, [1, end], [1, end]);
MS=MeshStructure(3, ...
  [Nx,Ny, Nz], ...
  CellSize, ...
  CellLocation, ...
  FaceLocation, ...
  c(:), ...
  [e1(:); e2(:); e3(:)]);


function t = traceProduct(A, B)

% TRACEPRODUCT Returns the trace of the product of two matrices.
% FORMAT
% DESC returns the trace of the product of two matrices, tr(A*B).
% ARG A : the first matrix in the product.
% ARG B : the second matrix in the product.
% RETURN t : the trace of the product.
%
% SEEALSO : trace
%
% COPYRIGHT : Neil D. Lawrence, 2004

% NDLUTIL

t = sum(sum(A.*B'));


function value = year_is_embolismic_hebrew ( y )

%*****************************************************************************80
%
%% YEAR_IS_EMBOLISMIC_HEBREW returns TRUE if the Hebrew year was embolismic.
%
%  Discussion:
%
%    In a 19 year cycle, there are 7 embolismic years.  During these years,
%    an extra month, "Adar II", (sometimes called "Veadar") is inserted after
%    the month of Adar.  Nonembolismic years are called "common" years.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 September 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year to be checked.
%
%    Output, logical VALUE, TRUE if the year was embolismic.
%
  if ( 12 <= i4_modp ( 7 * y + 13, 19 ) )
    value = 1;
  else
    value = 0;
  end

  return
end


function ellipse_t = fit_ellipse( x,y,axis_handle )
%
% fit_ellipse - finds the best fit to an ellipse for the given set of points.
%
% Format:   ellipse_t = fit_ellipse( x,y,axis_handle )
%
% Input:    x,y         - a set of points in 2 column vectors. AT LEAST 5 points are needed !
%           axis_handle - optional. a handle to an axis, at which the estimated ellipse
%                         will be drawn along with it's axes
%
% Output:   ellipse_t - structure that defines the best fit to an ellipse
%                       a           - sub axis (radius) of the X axis of the non-tilt ellipse
%                       b           - sub axis (radius) of the Y axis of the non-tilt ellipse
%                       phi         - orientation in radians of the ellipse (tilt)
%                       X0          - center at the X axis of the non-tilt ellipse
%                       Y0          - center at the Y axis of the non-tilt ellipse
%                       X0_in       - center at the X axis of the tilted ellipse
%                       Y0_in       - center at the Y axis of the tilted ellipse
%                       long_axis   - size of the long axis of the ellipse
%                       short_axis  - size of the short axis of the ellipse
%                       status      - status of detection of an ellipse
%
% Note:     if an ellipse was not detected (but a parabola or hyperbola), then
%           an empty structure is returned

% =====================================================================================
%                  Ellipse Fit using Least Squares criterion
% =====================================================================================
% We will try to fit the best ellipse to the given measurements. the mathematical
% representation of use will be the CONIC Equation of the Ellipse which is:
%
%    Ellipse = a*x^2 + b*x*y + c*y^2 + d*x + e*y + f = 0
%
% The fit-estimation method of use is the Least Squares method (without any weights)
% The estimator is extracted from the following equations:
%
%    g(x,y;A) := a*x^2 + b*x*y + c*y^2 + d*x + e*y = f
%
%    where:
%       A   - is the vector of parameters to be estimated (a,b,c,d,e)
%       x,y - is a single measurement
%
% We will define the cost function to be:
%
%   Cost(A) := (g_c(x_c,y_c;A)-f_c)'*(g_c(x_c,y_c;A)-f_c)
%            = (X*A+f_c)'*(X*A+f_c)
%            = A'*X'*X*A + 2*f_c'*X*A + N*f^2
%
%   where:
%       g_c(x_c,y_c;A) - vector function of ALL the measurements
%                        each element of g_c() is g(x,y;A)
%       X              - a matrix of the form: [x_c.^2, x_c.*y_c, y_c.^2, x_c, y_c ]
%       f_c            - is actually defined as ones(length(f),1)*f
%
% Derivation of the Cost function with respect to the vector of parameters "A" yields:
%
%   A'*X'*X = -f_c'*X = -f*ones(1,length(f_c))*X = -f*sum(X)
%
% Which yields the estimator:
%
%       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%       |  A_least_squares = -f*sum(X)/(X'*X) ->(normalize by -f) = sum(X)/(X'*X)  |
%       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%
% (We will normalize the variables by (-f) since "f" is unknown and can be accounted for later on)
%
% NOW, all that is left to do is to extract the parameters from the Conic Equation.
% We will deal the vector A into the variables: (A,B,C,D,E) and assume F = -1;
%
%    Recall the conic representation of an ellipse:
%
%       A*x^2 + B*x*y + C*y^2 + D*x + E*y + F = 0
%
% We will check if the ellipse has a tilt (=orientation). The orientation is present
% if the coefficient of the term "x*y" is not zero. If so, we first need to remove the
% tilt of the ellipse.
%
% If the parameter "B" is not equal to zero, then we have an orientation (tilt) to the ellipse.
% we will remove the tilt of the ellipse so as to remain with a conic representation of an
% ellipse without a tilt, for which the math is more simple:
%
% Non tilt conic rep.:  A`*x^2 + C`*y^2 + D`*x + E`*y + F` = 0
%
% We will remove the orientation using the following substitution:
%
%   Replace x with cx+sy and y with -sx+cy such that the conic representation is:
%
%   A(cx+sy)^2 + B(cx+sy)(-sx+cy) + C(-sx+cy)^2 + D(cx+sy) + E(-sx+cy) + F = 0
%
%   where:      c = cos(phi)    ,   s = sin(phi)
%
%   and simplify...
%
%       x^2(A*c^2 - Bcs + Cs^2) + xy(2A*cs +(c^2-s^2)B -2Ccs) + ...
%           y^2(As^2 + Bcs + Cc^2) + x(Dc-Es) + y(Ds+Ec) + F = 0
%
%   The orientation is easily found by the condition of (B_new=0) which results in:
%
%   2A*cs +(c^2-s^2)B -2Ccs = 0  ==> phi = 1/2 * atan( b/(c-a) )
%
%   Now the constants   c=cos(phi)  and  s=sin(phi)  can be found, and from them
%   all the other constants A`,C`,D`,E` can be found.
%
%   A` = A*c^2 - B*c*s + C*s^2                  D` = D*c-E*s
%   B` = 2*A*c*s +(c^2-s^2)*B -2*C*c*s = 0      E` = D*s+E*c
%   C` = A*s^2 + B*c*s + C*c^2
%
% Next, we want the representation of the non-tilted ellipse to be as:
%
%       Ellipse = ( (X-X0)/a )^2 + ( (Y-Y0)/b )^2 = 1
%
%       where:  (X0,Y0) is the center of the ellipse
%               a,b     are the ellipse "radiuses" (or sub-axis)
%
% Using a square completion method we will define:
%
%       F`` = -F` + (D`^2)/(4*A`) + (E`^2)/(4*C`)
%
%       Such that:    a`*(X-X0)^2 = A`(X^2 + X*D`/A` + (D`/(2*A`))^2 )
%                     c`*(Y-Y0)^2 = C`(Y^2 + Y*E`/C` + (E`/(2*C`))^2 )
%
%       which yields the transformations:
%
%           X0  =   -D`/(2*A`)
%           Y0  =   -E`/(2*C`)
%           a   =   sqrt( abs( F``/A` ) )
%           b   =   sqrt( abs( F``/C` ) )
%
% And finally we can define the remaining parameters:
%
%   long_axis   = 2 * max( a,b )
%   short_axis  = 2 * min( a,b )
%   Orientation = phi
%
%

% initialize
orientation_tolerance = 1e-3;

% empty warning stack
warning( '' );

% prepare vectors, must be column vectors
x = x(:);
y = y(:);

% remove bias of the ellipse - to make matrix inversion more accurate. (will be added later on).
mean_x = mean(x);
mean_y = mean(y);
x = x-mean_x;
y = y-mean_y;

% the estimation for the conic equation of the ellipse
X = [x.^2, x.*y, y.^2, x, y ];
a = sum(X)/(X'*X);

% check for warnings
if ~isempty( lastwarn )
    disp( 'stopped because of a warning regarding matrix inversion' );
    ellipse_t = [];
    return
end

% extract parameters from the conic equation
[a,b,c,d,e] = deal( a(1),a(2),a(3),a(4),a(5) );

% remove the orientation from the ellipse
if ( min(abs(b/a),abs(b/c)) > orientation_tolerance )

    orientation_rad = 1/2 * atan( b/(c-a) );
    cos_phi = cos( orientation_rad );
    sin_phi = sin( orientation_rad );
    [a,b,c,d,e] = deal(...
        a*cos_phi^2 - b*cos_phi*sin_phi + c*sin_phi^2,...
        0,...
        a*sin_phi^2 + b*cos_phi*sin_phi + c*cos_phi^2,...
        d*cos_phi - e*sin_phi,...
        d*sin_phi + e*cos_phi );
    [mean_x,mean_y] = deal( ...
        cos_phi*mean_x - sin_phi*mean_y,...
        sin_phi*mean_x + cos_phi*mean_y );
else
    orientation_rad = 0;
    cos_phi = cos( orientation_rad );
    sin_phi = sin( orientation_rad );
end

% check if conic equation represents an ellipse
test = a*c;
switch (1)
case (test>0),  status = '';
case (test==0), status = 'Parabola found';  warning( 'fit_ellipse: Did not locate an ellipse' );
case (test<0),  status = 'Hyperbola found'; warning( 'fit_ellipse: Did not locate an ellipse' );
end

% if we found an ellipse return it's data
if (test>0)

    % make sure coefficients are positive as required
    if (a<0), [a,c,d,e] = deal( -a,-c,-d,-e ); end

    % final ellipse parameters
    X0          = mean_x - d/2/a;
    Y0          = mean_y - e/2/c;
    F           = 1 + (d^2)/(4*a) + (e^2)/(4*c);
    [a,b]       = deal( sqrt( F/a ),sqrt( F/c ) );
    long_axis   = 2*max(a,b);
    short_axis  = 2*min(a,b);

    % rotate the axes backwards to find the center point of the original TILTED ellipse
    R           = [ cos_phi sin_phi; -sin_phi cos_phi ];
    P_in        = R * [X0;Y0];
    X0_in       = P_in(1);
    Y0_in       = P_in(2);

    % pack ellipse into a structure
    ellipse_t = struct( ...
        'a',a,...
        'b',b,...
        'phi',orientation_rad,...
        'X0',X0,...
        'Y0',Y0,...
        'X0_in',X0_in,...
        'Y0_in',Y0_in,...
        'long_axis',long_axis,...
        'short_axis',short_axis,...
        'status','' );
else
    % report an empty structure
    ellipse_t = struct( ...
        'a',[],...
        'b',[],...
        'phi',[],...
        'X0',[],...
        'Y0',[],...
        'X0_in',[],...
        'Y0_in',[],...
        'long_axis',[],...
        'short_axis',[],...
        'status',status );
end

% check if we need to plot an ellipse with it's axes.
if (nargin>2) & ~isempty( axis_handle ) & (test>0)

    % rotation matrix to rotate the axes with respect to an angle phi
    R = [ cos_phi sin_phi; -sin_phi cos_phi ];

    % the axes
    ver_line        = [ [X0 X0]; Y0+b*[-1 1] ];
    horz_line       = [ X0+a*[-1 1]; [Y0 Y0] ];
    new_ver_line    = R*ver_line;
    new_horz_line   = R*horz_line;

    % the ellipse
    theta_r         = linspace(0,2*pi);
    ellipse_x_r     = X0 + a*cos( theta_r );
    ellipse_y_r     = Y0 + b*sin( theta_r );
    rotated_ellipse = R * [ellipse_x_r;ellipse_y_r];

    % draw
    hold_state = get( axis_handle,'NextPlot' );
    set( axis_handle,'NextPlot','add' );
    plot( new_ver_line(1,:),new_ver_line(2,:),'r' );
    plot( new_horz_line(1,:),new_horz_line(2,:),'r' );
    plot( rotated_ellipse(1,:),rotated_ellipse(2,:),'r' );
    set( axis_handle,'NextPlot',hold_state );
end


function [m_database V_PCA V_Fisher ProjectedImages_Fisher] = FisherfaceCore(T)
% Use Principle Component Analysis (PCA) and Fisher Linear Discriminant (FLD) to determine the most
% discriminating features between images of faces.
%
% Description: This function gets a 2D matrix, containing all training image vectors
% and returns 4 outputs which are extracted from training database.
% Suppose Ti is a training image, which has been reshaped into a 1D vector.
% Also, P is the total number of MxN training images and C is the number of
% classes. At first, centered Ti is mapped onto a (P-C) linear subspace by V_PCA
% transfer matrix: Zi = V_PCA * (Ti - m_database).
% Then, Zi is converted to Yi by projecting onto a (C-1) linear subspace, so that
% images of the same class (or person) move closer together and images of difference
% classes move further apart: Yi = V_Fisher' * Zi = V_Fisher' * V_PCA' * (Ti - m_database)
%
% Argument:      T                      - (M*NxP) A 2D matrix, containing all 1D image vectors.
%                                         All of 1D column vectors have the same length of M*N
%                                         and 'T' will be a MNxP 2D matrix.
%
% Returns:       m_database             - (M*Nx1) Mean of the training database
%                V_PCA                  - (M*Nx(P-C)) Eigen vectors of the covariance matrix of the
%                                         training database
%                V_Fisher               - ((P-C)x(C-1)) Largest (C-1) eigen vectors of matrix J = inv(Sw) * Sb
%                ProjectedImages_Fisher - ((C-1)xP) Training images, which are projected onto Fisher linear space
%
% See also: EIG

% Original version by Amir Hossein Omidvarnia, October 2007
%                     Email: aomidvar@ece.ut.ac.ir

Class_number = ( size(T,2) )/2; % Number of classes (or persons)
Class_population = 2; % Number of images in each class
P = Class_population * Class_number; % Total number of training images

%%%%%%%%%%%%%%%%%%%%%%%% calculating the mean image
m_database = mean(T,2);

%%%%%%%%%%%%%%%%%%%%%%%% Calculating the deviation of each image from mean image
A = T - repmat(m_database,1,P);

%%%%%%%%%%%%%%%%%%%%%%%% Snapshot method of Eigenface algorithm
L = A'*A; % L is the surrogate of covariance matrix C=A*A'.
[V D] = eig(L); % Diagonal elements of D are the eigenvalues for both L=A'*A and C=A*A'.

%%%%%%%%%%%%%%%%%%%%%%%% Sorting and eliminating small eigenvalues
L_eig_vec = [];
for i = 1 : P-Class_number
    L_eig_vec = [L_eig_vec V(:,i)];
end

%%%%%%%%%%%%%%%%%%%%%%%% Calculating the eigenvectors of covariance matrix 'C'
V_PCA = A * L_eig_vec; % A: centered image vectors

%%%%%%%%%%%%%%%%%%%%%%%% Projecting centered image vectors onto eigenspace
% Zi = V_PCA' * (Ti-m_database)
ProjectedImages_PCA = [];
for i = 1 : P
    temp = V_PCA'*A(:,i);
    ProjectedImages_PCA = [ProjectedImages_PCA temp];
end

%%%%%%%%%%%%%%%%%%%%%%%% Calculating the mean of each class in eigenspace
m_PCA = mean(ProjectedImages_PCA,2); % Total mean in eigenspace
m = zeros(P-Class_number,Class_number);
Sw = zeros(P-Class_number,P-Class_number); % Initialization os Within Scatter Matrix
Sb = zeros(P-Class_number,P-Class_number); % Initialization of Between Scatter Matrix

for i = 1 : Class_number
    m(:,i) = mean( ( ProjectedImages_PCA(:,((i-1)*Class_population+1):i*Class_population) ), 2 )';

    S  = zeros(P-Class_number,P-Class_number);
    for j = ( (i-1)*Class_population+1 ) : ( i*Class_population )
        S = S + (ProjectedImages_PCA(:,j)-m(:,i))*(ProjectedImages_PCA(:,j)-m(:,i))';
    end

    Sw = Sw + S; % Within Scatter Matrix
    Sb = Sb + (m(:,i)-m_PCA) * (m(:,i)-m_PCA)'; % Between Scatter Matrix
end

%%%%%%%%%%%%%%%%%%%%%%%% Calculating Fisher discriminant basis's
% We want to maximise the Between Scatter Matrix, while minimising the
% Within Scatter Matrix. Thus, a cost function J is defined, so that this condition is satisfied.
[J_eig_vec, J_eig_val] = eig(Sb,Sw); % Cost function J = inv(Sw) * Sb
J_eig_vec = fliplr(J_eig_vec);

%%%%%%%%%%%%%%%%%%%%%%%% Eliminating zero eigens and sorting in descend order
for i = 1 : Class_number-1
    V_Fisher(:,i) = J_eig_vec(:,i); % Largest (C-1) eigen vectors of matrix J
end

%%%%%%%%%%%%%%%%%%%%%%%% Projecting images onto Fisher linear space
% Yi = V_Fisher' * V_PCA' * (Ti - m_database)
for i = 1 : Class_number*Class_population
    ProjectedImages_Fisher(:,i) = V_Fisher' * ProjectedImages_PCA(:,i);
end


function MISE=GaussKernelGaussMixMISE(n,H,w,mu,P)
%%GAUSSKERNELGAUSSMIXMISE Given a Gaussian mixture probability density
%            function (PDF) that one is attempting to approximate using a
%            kernel density estimator with Gaussian kernels all having
%            square root covariance matrix H, determine the mean integrated
%            squared error (MISE) of the estimate. The MISE is the expected
%            value of int_{R}(\hat{f}(x)-f(x))^2dx where \hat{f}(x) is the
%            kernel estimator based on a set of random samples of the true
%            density f(x) (since it is based on random samples, the mean
%            part is the expected value being taken over all possible
%            samples). This function is useful for assessing different
%            kernel bandwidth estimators (estimators of H) when using
%            Gaussian mixture sample problems.
%
%INPUTS: n The number of samples that the kernel-density estimator will
%          use.
%        H The dXd bandwidth matrix used in the kernel density estimator
%          (as one would use in the function kernelApprox).
%        w A numCompX1 or 1XnumComp vector of the weights associated with
%          each of the numComp components of the Gaussian mixture.
%       mu The dXnumComp matrix of the mean of each component of the
%          Gaussian mixture.
%        P The dXdXnumComp set of covariance matrices of the components of
%          the Gaussian mixture.
%
%OUTPUTS: MISE The MISE of thekernel estimator with the given number of
%              samples and bandwidth matrix.
%
%This function implements the formula for the MISE in an arbitrary number
%of dimensions in Section 4 of [1].
%
%REFERENCES:
%[1] M. P. Wand and M. C. Jones, "Comparison of smoothing parameterizations
%    in bivariate kernel density estimation," Journal of the American
%    Statistical Association, vol. 88, no. 422, pp. 520-528, Jun. 1993.
%
%December 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%The number of components in the
k=size(mu,2);
d=size(mu,1);

%The definition of the bandwidth used in the paper is the square of what is
%used here.
H=H*H';

Omega0=zeros(k,k);
Omega1=zeros(k,k);
Omega2=zeros(k,k);

for l=1:k
    for lp=l:k
       Sigma=P(:,:,l)+P(:,:,lp);

       Omega0(l,lp)=GaussianD.PDF(mu(:,l),mu(:,lp),Sigma);
       Omega1(l,lp)=GaussianD.PDF(mu(:,l),mu(:,lp),Sigma+H);
       Omega2(l,lp)=GaussianD.PDF(mu(:,l),mu(:,lp),Sigma+2*H);
    end
end

MISE=(1/n)*(4*pi)^(-d/2)*1/sqrt(det(H))+w(:)'*((1-1/n)*Omega2-2*Omega1+Omega0)*w(:);

end

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


function value = p14_exact ( dim_num )

%*****************************************************************************80
%
%% P14_EXACT returns the exact integral for problem 14.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Output, real VALUE, the exact value of the integral.
%
  value = ( - 1.0 / 3.0 ) * ( 1.0 - ( -1.0 / 2.0 )^dim_num );

  return
end


function [Q,err] = quadgr(fun,a,b,tol,trace,varargin)
%QUADGR Gauss-Legendre quadrature with Richardson extrapolation.
%   [Q,ERR] = QUADGR(FUN,A,B,TOL) approximates the integral of a function
%   FUN from A to B with an absolute error tolerance TOL. FUN is a function
%   handle and must accept vector arguments. TOL is 1e-6 by default. Q is
%   the integral approximation and ERR is an estimate of the absolute error.
%
%   QUADGR uses a 12-point Gauss-Legendre quadrature. The error estimate is
%   based on successive interval bisection. Richardson extrapolation
%   accelerates the convergence for some integrals, especially integrals
%   with endpoint singularities.
%
%   QUADGR(FUN,A,B,TOL,TRACE) with non-zero TRACE displays the
%   extrapolation table.
%
%   QUADGR can also be used as the quadrature in DBLQUAD and TRIPLEQUAD.
%
%   Examples:
%     Q = quadgr(@(x) log(x),0,1)
%     [Q,err] = quadgr(@(x) exp(x),0,9999i*pi)
%     [Q,err] = quadgr(@(x) sqrt(4-x.^2),0,2,1e-12)
%     [Q,err] = quadgr(@(x) x.^-0.75,0,1)
%     [Q,err] = quadgr(@(x) 1./sqrt(1-x.^2),-1,1)
%     [Q,err] = quadgr(@(x) exp(-x.^2),-inf,inf,1e-9) % sqrt(pi)
%     [Q,err] = quadgr(@(x) cos(x).*exp(-x),0,inf,1e-9)
%
%   See also QUAD, QUADGK, DBLQUAD, TRIPLEQUAD

%   Author: Jonas Lundgren <splinefit@gmail.com> 2009

%   2009-03-17  First published
%   2010-04-14  Adapted to DBLQUAD and TRIPLEQUAD (varargin added)

if nargin < 3, help quadgr, return, end
if nargin < 4 || isempty(tol), tol = 1.e-6; end;
if nargin < 5 || isempty(trace), trace = 0; end;

% Order limits (required if infinite limits)
if a == b
    Q = b - a;
    err = b - a;
    return
elseif a > b
    reverse = true;
    atmp = a;
    a = b;
    b = atmp;
else
    reverse = false;
end

% Infinite limits
if isinf(a) || isinf(b)
    % Check real limits
    if ~isreal(a) || ~isreal(b) || isnan(a) || isnan(b)
        error('quadgr:inflim','Infinite intervals must be real.')
    end
    % Change of variable
    if isfinite(a) && isinf(b)
        % a to inf
        fun1 = @(t,varargin) fun(a + t./(1-t), varargin{:})./(1-t).^2;
        [Q,err] = quadgr(fun1,0,1,tol,trace,varargin{:});
    elseif isinf(a) && isfinite(b)
        % -inf to b
        fun2 = @(t,varargin) fun(b + t./(1+t), varargin{:})./(1+t).^2;
        [Q,err] = quadgr(fun2,-1,0,tol,trace,varargin{:});
    else % -inf to inf
        fun1 = @(t,varargin) fun(t./(1-t), varargin{:})./(1-t).^2;
        fun2 = @(t,varargin) fun(t./(1+t), varargin{:})./(1+t).^2;
        [Q1,err1] = quadgr(fun1,0,1,tol/2,trace,varargin{:});
        [Q2,err2] = quadgr(fun2,-1,0,tol/2,trace,varargin{:});
        Q = Q1 + Q2;
        err = err1 + err2;
    end
    % Reverse direction
    if reverse
        Q = -Q;
    end
    return
end

% Gauss-Legendre quadrature (12-point)
xq = [0.12523340851146894; 0.36783149899818018; 0.58731795428661748; ...
      0.76990267419430469; 0.9041172563704748; 0.98156063424671924];
wq = [0.24914704581340288, 0.23349253653835478, 0.20316742672306584, ...
      0.16007832854334636, 0.10693932599531818, 0.047175336386511842];
xq = [xq; -xq];
wq = [wq, wq];
nq = length(xq);

% Initiate vectors
maxit = 17;                 % Max number of iterations
Q0 = zeros(maxit,1);       	% Quadrature
Q1 = zeros(maxit,1);       	% First Richardson extrapolation
Q2 = zeros(maxit,1);       	% Second Richardson extrapolation

% One interval
hh = (b - a)/2;             % Half interval length
x = (a + b)/2 + hh*xq;      % Nodes
% Quadrature
Q0(1) = hh*wq*fun(x,varargin{:});

% Successive bisection of intervals
for k = 2:maxit

    % Interval bisection
    hh = hh/2;
    x = [x + a; x + b]/2;
    % Quadrature
    Q0(k) = hh*wq*sum(reshape(fun(x,varargin{:}),nq,[]),2);

    % Richardson extrapolation
    if k >= 5
        Q1(k) = richardson(Q0,k);
        Q2(k) = richardson(Q1,k);
    elseif k >= 3
        Q1(k) = richardson(Q0,k);
    end

    % Estimate absolute error
    if k >= 6
        Qv = [Q0(k), Q1(k), Q2(k)];
        Qw = [Q0(k-1), Q1(k-1), Q2(k-1)];
    elseif k >= 4
        Qv = [Q0(k), Q1(k)];
        Qw = [Q0(k-1), Q1(k-1)];
    else
        Qv = Q0(k);
        Qw = Q0(k-1);
    end
    [err,j] = min(abs(Qv - Qw));
    Q = Qv(j);

    % Convergence
    if err < tol || ~isfinite(Q)
        break;
    end

end

% Convergence check
if ~isfinite(Q)
    warning('quadgr:infnan','Integral approximation is Infinite or NaN.')
elseif err > tol
    warning('quadgr:maxiter','Max number of iterations reached without convergence.')
end

% The error estimate should not be zero
err = err + 2*eps(Q);
% Reverse direction
if reverse
	Q = -Q;
end

% Display convergence
if trace
    disp(' ')
    disp([Q0(1:k) Q1(1:k) Q2(1:k)])
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function R = richardson(Q,k)
% Richardson extrapolation with parameter estimation
if Q(k) ~= Q(k-1)
    c = real((Q(k-1)-Q(k-2))/(Q(k)-Q(k-1))) - 1;
else
    c = 1;
end
% The lower bound 0.07 admits the singularity x.^-0.9
c = max(c,0.07);
R = Q(k) + (Q(k) - Q(k-1))/c;