function [xUpdate,PUpdate,innov,Pzz,W]=EKFUpdate(xPred,PPred,z,R,h,HJacob,numIter,HHessian,innovTrans,measPredTrans,stateDiffTrans,stateTrans)
%%EKFUPDATE Perform the measurement update step in the first- or second-
%           order Extended Kalman Filter (EKF) or iterated Extended Kalman
%           filter (IEKF), which is what one gets with numIter>0.
%
%INPUTS: xPred The xDimX1 predicted target state.
%        PPred The xDimXxDim predicted state covariance matrix.
%            z The zDimX1 vector measurement.
%            R The zDimXzDim measurement covariance matrix.
%            h A function handle for the measurement function that takes
%              the state as its argument.
%       HJacob A function handle for the measurement Jacobian matrix that
%              takes the target state as a parameter or the zDimXxDim
%              measurement Jacobian matrix itself. Each column is the
%              derivative vector of h with respect to the corresponding
%              element of x. If not supplied or an empty matrix is passed,
%              then HJacob will be found using numerical differentiation
%              via the numDiff function with default parameters.
%      numIter The number of iterations to perform if an iterated EKF is
%              desired. If this parameter is omitted or an empty matrix is
%              passed, the default value of zero is used. That is, just
%              use the standard update without any additional iterations.
%     HHessian This parameter is only provided if a second-order EKF is
%              desired. This is either a function handle for the
%              measurement Hessian hypermatrix, or it is the measurement
%              Hessian hypermatrix itself. The matrix is
%              xDimXxDimXzDim. The matrix HHess=HHessian(x) is such that
%              HHess(i,j,k) is the second derivative of the kth element
%              of the vector returned by h with respect to the ith and jth
%              components of x. For each k, the Hessian matrix is
%              symmetric. If this parameter is omitted or an empty matrix
%              is passed, a first-order EKF update is used.
%   innovTrans An optional function handle that computes and optionally
%              transforms the value of the difference between the
%              observation and any predicted points. This is called as
%              innovTrans(a,b) and the default if omitted or an empty
%              matrix is passed is @(a,b)bsxfun(@minus,a,b). This must be
%              able to handle sets of values. For a zDimX1 measurement,
%              either of the inputs could be zDimXN in size while one of
%              the inputs could be zDimX1 in size.  This only needs to be
%              supplied when a measurement difference must be restricted
%              to a certain range. For example, the innovation between two
%              angles will be 2*pi if one angle is zero and the other
%              2*pi, even though they are the same direction. In such an
%              instance, a function handle to the
%              wrapRange(bsxfun(@minus,a,b),-pi,pi) function with the
%              appropriate parameters should be passed for innovTrans.
% measPredTrans An optional function handle that transforms the predicted
%              measurement into a particular range. The second order EKF
%              has a linear correction to the prediction of the
%              measurement, which might not be appropriate for all types of
%              measurements.
% stateDiffTrans An optional function handle that, like innovTrans does for
%              the measurements, a difference between states and transforms
%              it however might be necessary. For example, a state
%              containing angular components will generally need to be
%              transformed so that the difference between the angles is
%              wrapped to -pi/pi.
%   stateTrans An optional function handle that takes a state estimate and
%              transforms it. This is useful if one wishes the elements of
%              the state to be bound to a certain domain. For example, if
%              an element of the state is an angle, one should generally
%              want to bind it to the region +/-pi.
%
%OUTPUTS: xUpdate The xDim X 1 updated state vector.
%         PUpdate The updated xDim X xDim state covariance matrix.
%      innov, Pzz The zDimX1 innovation and the zDimXzDim innovation
%                 covariance matrix are returned in case one wishes to
%                 analyze the consistency of the estimator or use those
%                 values in gating or likelihood evaluation.
%               W The xDimXzDim gain used in the update. This can be
%                 useful when gating and using the function
%                 calcMissedGateCov.
%
%The first-order EKF is summarized in Figure 10.3.3-1 in Chapter 10.3.3 of
%[1]. The Joseph-form covariance update given in Chapter 5 of the same
%book is used for improved numerical stability. The expressions for the
%second-order EKF come from Chapter 10.3.2 of [1].
%
%The iteration of the measurement equation for the iterated EKF is given in
%Chapter 10.5.2 of the same book. In [2], it is noted that the iteration
%need only be done over each measurement update, not over an entire batch
%of measurements.
%
%The optional parameter innovTrans is not described in the above reference,
%but allow for possible modifications to the filter as described in [3].
%The parameters have been added to allow the filter to be used with
%angular quantities. For example, if the measurement consisted of
%range and angle, z=[r;theta], then
%innovTrans=@(a,b)[bsxfun(@minus,a(1,:),b(1,:));
%                  wrapRange(bsxfun(@minus,a(2,:),b(2,:)),-pi,pi)];
%should be used to approximately deal with the circular nature of the
%measurements.
%
%REFERENCES:
%[1] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
%    Applications to Tracking and Navigation. New York: John Wiley and
%    Sons, Inc, 2001.
%[2] T. H. Kerr, "Streamlining Measurement Iteration for EKF Target
%    Tracking," IEEE Transactions on Aerospace and Electronic Systems,
%    vol. 27, no. 2, pp. 408-421, Mar. 1991.
%[3] D. F. Crouse, "Cubature/ unscented/ sigma point Kalman filtering with
%    angular measurement models," in Proceedings of the 18th International
%    Conference on Information Fusion, Washington, D.C., 6-9 Jul. 2015.
%
%April 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

zDim=size(z,1);
if(nargin<6||isempty(HJacob))
    HJacob=@(x)numDiff(x,h,zDim);
end

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

if(nargin<8)
    HHessian=[];
end

if(nargin<9||isempty(innovTrans))
    %The function just returns the input.
    innovTrans=@(a,b)bsxfun(@minus,a,b);
end

if(nargin<10||isempty(measPredTrans))
    measPredTrans=@(x)x;
end

if(nargin<11||isempty(stateDiffTrans))
    stateDiffTrans=@(x)x;
end

if(nargin<12||isempty(stateTrans))
    stateTrans=@(x)x;
end

if(isa(HJacob,'function_handle'))
    H=HJacob(xPred);
else
    H=HJacob;%If the Jacobian matrix was directly given.
end

if(~isempty(HHessian))
    if(isa(HHessian,'function_handle'))
        HHess=HHessian(xPred);
    else
        HHess=HHessian;%If the Hessian matrix was directly given.
    end

    [zPredHessTerm,PzzHessTerm]=getHessianTerms(HHess,PPred,zDim);
else
    zPredHessTerm=0;
    PzzHessTerm=0;
end

zPred=measPredTrans(h(xPred)+zPredHessTerm);
innov=innovTrans(z,zPred);

Pzz=R+H*PPred*H'+PzzHessTerm;
%Ensure symmetry
Pzz=(Pzz+Pzz')/2;
W=PPred*H'/Pzz;

xUpdate=stateTrans(xPred+W*innov);

temp=W*H;
temp=eye(size(temp))-temp;
PUpdate=temp*PPred*temp'+W*R*W';
%Ensure symmetry
PUpdate=(PUpdate+PUpdate')/2;

for curIter=1:numIter
    if(isa(HJacob,'function_handle'))
        H=HJacob(xUpdate);
    else
        H=HJacob;%If the Jacobian matrix was directly given.
    end

    if(~isempty(HHessian))
        if(isa(HHessian,'function_handle'))
            HHess=HHessian(xUpdate);
        else
            HHess=HHessian;%If the hessian matrix was directly given.
        end

        [zPredHessTerm,PzzHessTerm]=getHessianTerms(HHess,PPred,zDim);
    else
        zPredHessTerm=0;
        PzzHessTerm=0;
    end

    %Update the PEstimate
    Pzz=R+H*PPred*H'+PzzHessTerm;
    %Ensure symmetry
    Pzz=(Pzz+Pzz')/2;
    W=PPred*H'/Pzz;
    temp=W*H;
    temp=eye(size(temp))-temp;
    PUpdate=temp*PPred*temp'+W*R*W';
    %Ensure symmetry
    PUpdate=(PUpdate+PUpdate')/2;
    zPred=measPredTrans(h(xUpdate)+zPredHessTerm);

    %Update the x estimate
    xUpdate=stateTrans(xUpdate+PUpdate*H'*lsqminnorm(R,innovTrans(z,zPred))-PUpdate*lsqminnorm(PPred,stateDiffTrans(xUpdate-xPred)));
end

end

function [zPredHessTerm,PzzHessTerm]=getHessianTerms(HHess,PPred,zDim)
%The function returns the terms associated with the second derivative in
%the second-order EKF.

PzzHessTerm=zeros(zDim,zDim);
zPredHessTerm=zeros(zDim,1);
for n=1:zDim
    en=zeros(zDim,1);
    en(n)=1;

    HPProdn=HHess(:,:,n)*PPred;
    zPredHessTerm=zPredHessTerm+en*trace(HPProdn);

    for m=1:zDim
        em=zeros(zDim,1);
        em(m)=1;
        PzzHessTerm=PzzHessTerm+en*em'*trace(HPProdn*HHess(:,:,m)'*PPred);
    end
end
PzzHessTerm=PzzHessTerm/2;
zPredHessTerm=zPredHessTerm/2;
end

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


function [dx, dy] = evalshift(p, pval, L1, L2, L3)

% function [dx, dy] = evalshift(p, pval, L1, L2, L3)
% Purpose: compute two-dimensional Warp & Blend transform

% 1) compute Gauss-Lobatto-Legendre node distribution
gaussX = -JacobiGL(0,0,p);

% 2) compute blending function at each node for each edge
blend1 = L2.*L3; blend2 = L1.*L3; blend3 = L1.*L2;

% 3) amount of warp for each node, for each edge
warpfactor1 = 4*evalwarp(p, gaussX, L3-L2);
warpfactor2 = 4*evalwarp(p, gaussX, L1-L3);
warpfactor3 = 4*evalwarp(p, gaussX, L2-L1);

% 4) combine blend & warp
warp1 = blend1.*warpfactor1.*(1 + (pval*L1).^2);
warp2 = blend2.*warpfactor2.*(1 + (pval*L2).^2);
warp3 = blend3.*warpfactor3.*(1 + (pval*L3).^2);

% 5) evaluate shift in equilateral triangle
dx = 1*warp1 + cos(2*pi/3)*warp2 + cos(4*pi/3)*warp3;
dy = 0*warp1 + sin(2*pi/3)*warp2 + sin(4*pi/3)*warp3;
return;


function dtscale = dtscale2D;

% function dtscale = dtscale2D;
% Purpose : Compute inscribed circle diameter as characteristic
%           for grid to choose timestep

Globals2D;

% Find vertex nodes
vmask1   = find( abs(s+r+2) < NODETOL)';
vmask2   = find( abs(r-1)   < NODETOL)';
vmask3   = find( abs(s-1)   < NODETOL)';
vmask  = [vmask1;vmask2;vmask3]';
vx = x(vmask(:), :); vy = y(vmask(:), :);

% Compute semi-perimeter and area
len1 = sqrt((vx(1,:)-vx(2,:)).^2+(vy(1,:)-vy(2,:)).^2);
len2 = sqrt((vx(2,:)-vx(3,:)).^2+(vy(2,:)-vy(3,:)).^2);
len3 = sqrt((vx(3,:)-vx(1,:)).^2+(vy(3,:)-vy(1,:)).^2);
sper = (len1 + len2 + len3)/2.0;
Area = sqrt(sper.*(sper-len1).*(sper-len2).*(sper-len3));

% Compute scale using radius of inscribed circle
dtscale = Area./sper;
return;


function [x,tspan] = w20scalar(f,L,tspan,x0,Q,gaussian)
%% W20scalar - Weak order 2.0 Ito-Taylor
%
% Syntax:
%   [x,tspan] = w20scalar(f,L,tspan,x0,Q,gaussian)
%
% In:
%   f        - Drift function and derivatives, {f, df, ddf}
%   L        - Diffusion function and derivatives, {L, dL, ddL}
%   tspan    - Time steps to simulate, [t0,...,tend]
%   x0       - Initial condition
%   Q        - Spectral density (default: standard Brownian motion)
%   gaussian - Use Gaussian increments (default: false)
%
% Out:
%   x      - Solved values
%   tspan  - Time steps
%
% Description:
%   Integrates the system of stochatic differential equations
%     dx = f(x,t) dt + L(x,t) dbeta,  for x(0) = x0
%   over the time interval defined in tspan.
%
% Copyright:
%   2018 - Simo Särkkä and Arno Solin
%
% License:
%   This software is provided under the MIT License. See the accompanying
%   LICENSE file for details.

%%

  % Check if Q given
  if nargin<5 || isempty(Q), Q = eye(size(L(x0,tspan(1)),2)); end

  % Check the sort of increments to use
  if nargin<6 || isempty(gaussian), gaussian = false; end

  % Extract derivatives
  ddf = f{3};
  df = f{2};
  f = f{1};
  ddL = L{3};
  dL = L{2};
  L = L{1};

  % Cholesky factor of Q
  cQ = chol(Q,'lower');

  % Number of steps
  steps = numel(tspan);

  % Allocate space
  x = zeros(size(x0,1),steps);

  % Initial state
  x(:,1) = x0;

  % Pre-calculate random numbers
  RR = rand(size(Q,1),steps);
  R = (RR<1/6)*sqrt(3) - (RR>5/6)*sqrt(3);

  % Iterate
  for k=2:steps

    % Time discretization
    dt = tspan(k)-tspan(k-1);

    % Increment
    if gaussian
      db = sqrt(dt)*cQ*randn;
      dz = 1/2*db*dt;
    else
      db = sqrt(dt)*cQ*R(:,k);
      dz = 1/2*dt^(3/2)*R(:,k);
    end

    % Evaluate only once
    fx = f(x(:,k-1),[]);
    dfx = df(x(:,k-1),[]);
    ddfx = ddf(x(:,k-1),[]);
    Lx = L(x(:,k-1),[]);
    dLx = dL(x(:,k-1),[]);
    ddLx = ddL(x(:,k-1),[]);

    % Step
    x(:,k) = x(:,k-1) + ...
        fx*dt + ...
        Lx*db + ...
        1/2*Lx*dLx*(db^2 - dt) + ...
        Lx*df(x(:,k-1),[])*dz + ...
        1/2*(fx*dfx + 1/2*Lx^2*ddfx)*dt^2 + ...
        (fx*dLx - 1/2*Lx^2*ddLx)*(db*dt - dz);

  end


function J=uGradient2D(xTar,lRx,M,includeV)
%%UGRADIENT2D Determine the gradient of a direction cosine measurement in
%          2D with respect to 2D position. Relativity and atmospheric
%          effects are not taken into account.
%
%INPUTS: xTar The 2XN set of target position vectors in the global
%          coordinate system with [x;y] components for which a gradient is
%          desired.
%      lRx The 2X1 position vector of the receiver. If omitted, the
%          receiver is placed at the origin.
%        M A 2X2 rotation matrix from the global Coordinate system to the
%          orientation of the coordinate system at the receiver. If
%          omitted, it is assumed to be the identity matrix.
% includeV An optional boolean value indicating whether a second direction
%          cosine component should be included. The u direction cosine is
%          one parts of a 2D unit vector. The default if this parameter is
%          omitted or an empty matrix is passed is false.
%
%OUTPUTS: J A 1X2XN set of N Jacobian matrices (or a 2X2XN set if includeV
%           is true) where the row represents u (and v) and the columns
%           take the partial derivatives of the rows with respect to [x,y]
%           in that order.
%
%A derivation of the components of the Jacobian of a 3D u-v measurement is
%given in [1]. The results here are similar.
%
%EXAMPLE:
%Here, we validate the results using numerical differentiation.
% x=[1000;4];
% lRx=[-40;2];
% M=eye(2,2);
% J=uGradient2D(x,lRx,M);
% f=@(x)getUDirection2D(x,lRx,M);
% N=3;
% epsVal=[1;0.01];
% numJAug=numDiff(x,f,2,N,epsVal);
% JNumDiff=numJAug(2,:);
% RelErr=max(abs((J-JNumDiff)./JNumDiff))
%One will see that J and JNumDiff agree to more than 7 digits, indicating
%good agreeement.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%April 2017 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

N=size(xTar,2);

if(nargin<4||isempty(includeV))
    includeV=false;
end

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

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

J=zeros(1+includeV,2,N);
for curPoint=1:N
    %Convert the state into the local coordinate system.
    xLocal=M*(xTar(1:2,curPoint)-lRx(1:2));

    r3=norm(xLocal)^3;
    x=xLocal(1);
    y=xLocal(2);

    %The gradient of u in local coordinates.
    du=zeros(1,2);
    du(1)=y^2/r3;
    du(2)=-x*y/r3;

    J(1,:,curPoint)=du*M;

    if(includeV)
        dv=zeros(1,2);
        dv(1)=-x*y/r3;
        dv(2)=x^2/r3;
        %Now, the gradient vectors for the angular components must be
        %rotated back into the global coordinate system.
        J(2,:,curPoint)=dv*M;
    end
end
end

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


function [wRed,xRed,PRed,exitCode]=kMeansGaussReduction(wOrig,xOrig,POrig,param4,xInit,PInit,maxIter)
%%KMEANSGAUSSREDUCTION Perform Gaussian mixture reduction by iterating a k-
%              means algorithm given an inital reduced estimator (or using
%              Runnals' algorithm for the initial estimate if none is
%              given.
%
%INPUTS: wOrig An NX1 or 1XN vector of weights of the components of the
%          original Gaussian mixture.
%   muOrig An xDimXN matrix of the means of the vector components of the
%          original Gaussian mixture.
%    POrig An xDimXxDim XN hypermatrix of the covariance matrices for the
%          components of the original Gaussian mixture.
%   param4 If not initial estimate of a reduced distribution is provided,
%          then this is k, the number of components to which the
%          distribution should be reduced (and xInit and PInit are either
%          not provided or are empty matrices) and RunnalsGaussMixRed will
%          be used to obtain an initial estimate. Otherwise, if an initial
%          reduced distribution is provided, this is wInit, the kX1 or 1Xk
%          set of weights for the initial estimate of the reduced
%          distribution.
%    xInit If an initial reduced distribution is provided, this is the
%          xDimXk set of Gaussian mean vectors. Otherwise, this parameter
%          can be omitted or an empty matrix passed.
%    PInit If an initial reduced distribution is provided, this is the
%          xDimXxDimXk set of covariance matrices. Otherwise, this
%          parameter can be omitted or an empty matrix passed.
%  maxIter The maximum number of iterations of the k means algorithm to
%          perform. The default if omitted or an empty matrix is passed is
%          50.
%
%OUTPUTS: wRed The KX1 weights of the mixture after reduction.
%        muRed The xDimXK means of the mixture after reduction.
%         PRed The xDimXxDimXK covariance matrices of the mixture after
%              reduction.
%     exitCode A parameter indicating how the algorithm terminated.
%              Possible values are:
%              0 The algorithm converged.
%              1 the maximum number of iterations elapsed.
%
%This implements the algorithm of [1] and [2], which is also described in
%[3]. TGlobal convergence is not guaranteed and the performance depends a
%lot on the quality of the initial estiamte. The Kullback-Leiblber
%divergence is used as a closeness criterion between components via the
%KLDivGauss function. Additional algorithmic optimizations precomputing
%various determinants are possible beyond what was done here.
%
%EXAMPLE:
%We reduce a 10-component mixture to five components. We first plot
%Runnals' algorithm. We then use a perturbed version of the Runnals
%solution as the initial estimate to the k-means estimator and we find that
%it converges back to the Runnals solution.
% w=[0.03,0.18,0.12,0.19,0.02,0.16,0.06,0.1,0.08,0.06];
% n=length(w);
% mu=[1.45,2.20,0.67,0.48,1.49,0.91,1.01,1.42,2.77,0.89];
% P=[0.0487,0.0305,0.1171,0.0174,0.0295,0.0102, 0.0323, 0.0380, 0.0115, 0.0679];
% P=reshape(P,[1,1,n]);
%
% k=6;%Number of reduced components.
%
% %Runnal's reduction
% [wRedR,muRedR,PRedR]=RunnalsGaussMixRed(w,mu,P,k);
%
% %Perturb Runnal's solution so that the kMeansReduction has a bad
% %initialization.
% wRedRP=wRedR;
% wRedRP(1)=wRedR(1)+0.1;
% wRedRP=wRedRP/sum(wRedRP);
% muRedRP=muRedR+0.1;
% [wRedkM,muRedkM,PRedkM]=kMeansGaussReduction(w,mu,P,wRedRP,muRedRP,PRedR);
%
% numPoints=500;
% xVals=linspace(0,3,numPoints);
% PDFVals1=GaussianMixtureD.PDF(xVals,w,mu,P);
% PDFVals2=GaussianMixtureD.PDF(xVals,wRedR,muRedR,PRedR);
% PDFVals3=GaussianMixtureD.PDF(xVals,wRedRP,muRedRP,PRedR);
% PDFVals4=GaussianMixtureD.PDF(xVals,wRedkM,muRedkM,PRedkM);
% figure(1)
% clf
% hold on
% plot(xVals,PDFVals1,'-k','linewidth',4)
% plot(xVals,PDFVals2,'-r','linewidth',4)
% plot(xVals,PDFVals3,'-.c','linewidth',2)
% plot(xVals,PDFVals4,'--g','linewidth',2)
% legend('Full Mixture','Algorithm of Runnals','Initialization of k-Means','k-Means Algorithm')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] D. Schieferdecker and M. F. Huber, "Gaussian mixture reduction via
%    clustering," in Proceedings of the 12th International Conference on
%    Information Fusion, Seattle, WA, 6-9 Jul. 2009, pp. 1536-1543.
%[2] A. Nikseresht and M. Gelgon, "Gossip-based computation of a Gaussian
%    mixture model for distributed multimedia indexing," IEEE Transactions
%    on Multimedia, vol. 10, no. 3, pp. 385-392, Apr. 2008.
%[3] D. F. Crouse, P. Willett, K. Pattipati, and L. Svensson, "A look at
%    Gaussian mixture reduction algorithms," in Proceedings of the 14th
%    International Conference on Information Fusion, Chicago, IL, 5-8 Jul.
%    2011.
%
%May 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<7||isempty(maxIter))
   maxIter=20;
end

if(nargin<5||isempty(xInit))
%If no initial estimate is given but the desired number of components is
%given, then get an initial estimate using Runnall's algorithm.
    [wInit,xInit,PInit]=RunnalsGaussMixRed(wOrig,xOrig,POrig,param4);
else
    wInit=param4;
end

%The original number of mixture parameters.
N=length(wOrig);
%The reduced number of mixture parameters.
k=length(wInit);

cAssign=zeros(N,1);
cAssignNew=zeros(N,1);

%We assign the Gaussians in the full mixture to cluster centers in the
%current reduced mixture. Then, we recompute the cluster centers. These two
%steps are looped until the assignments no longer change or a maximum
%number of iterations has passed.

wRed=wInit;
xRed=xInit;
PRed=PInit;

exitCode=1;
for curIter=1:maxIter
    %For each point, find the closest center.
    for curP=1:N
        minCost=Inf;
        for curC=1:k
            cost=KLDistGauss(xOrig(:,curP),POrig(:,:,curP),xRed(:,curC),PRed(:,:,curC));
            if(cost<minCost)
                minCost=cost;
                cAssignNew(curP)=curC;
            end
        end
    end

    %Terminate when the assignments no longer change.
    if(sum(cAssignNew~=cAssign)==0)
        exitCode=0;%Convergence attained.
        break;
    end
    cAssign=cAssignNew;

    %Now, merge all points having common centers.
    [cSorted,idx]=sort(cAssign,'ascend');
    [~,numReps]=runLenEncode(cSorted);
    startIdx=1;
    for curC=1:k
        selIdx=idx(startIdx:(startIdx+numReps(curC)-1));

        wSelNorm=wOrig(selIdx);
        wSum=sum(wSelNorm);
        wSelNorm=wSelNorm/wSum;

        [xMerged,PMerged]=calcMixtureMoments(xOrig(:,selIdx),wSelNorm,POrig(:,:,selIdx));

        wRed(curC)=wSum;
        xRed(:,curC)=xMerged;
        PRed(:,:,curC)=PMerged;

        startIdx=startIdx+numReps(curC);
    end

    wRed=wRed/sum(wRed);
end
end

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


% Descriptor for simple shapes (e.g letters).
% Computes the descriptor twice in order to describe horizontally and vertically with the same accuracy.
%
% Input: image (2-D, binary or gray)
%        depth of recursion (default 7),
%        plotFlag(if on - plot Illustration)
% Output:
%        vec - descriptor: Division locations, values around zero. Of length 2*(2^depth - 2)
%        (levels: the depth of the division locations.)
%
% Shahar Armon. 3/1/2012

function  [vec levels] = hierarchicalCentroid(im, depth, plotFlag)
    if nargin < 2
        depth = 7;
        plotFlag = 0;
    elseif nargin < 3
        plotFlag = 0;
    end

    im(im < 0) = 0;

    [vec1 levels1] = hierarchicalCentroid1(im  ,depth, plotFlag);
    [vec2 levels2] = hierarchicalCentroid1(im' ,depth, 0);  % transpose iamge

    vec = [vec1, vec2];
    levels = [levels1,-levels2];  % Minus is only to mark the transpose.
end

% The descriptor. Includes Normalization.
function  [vec levels] = hierarchicalCentroid1(im, d, plotFlag)

    p = hierarchicalCentroidRec(im, 1, d);

    if plotFlag  % Illustration
        showLines(im, p);
    end

    meany = ([1:size(im,1)]*sum(im,2))/sum(sum(im));
    indVer = (mod(p(:,2),2) == 1);
    % Normalization for location:
    p(indVer,1) = p(indVer,1) - p(1,1);
    p(~indVer,1) = p(~indVer,1) - meany;

    % Normalization for size (keeping aspect ratio):
    p(:,1) = p(:,1)/size(im,1);
    % Normalization for size:
    % p(indVer,1) = p(indVer,1)/size(im,2);
    % p(~indVer,1) = p(~indVer,1)/size(im,1);

    % Illustration of the lines after normalization without the image
    %     showLines([], p);

    vec = p(2:end,1)';
    levels = p(2:end,2)';
end

function p = hierarchicalCentroidRec(im, depth, maxDepth)
    if depth > maxDepth
        p = [];
    else
        area = sum(sum(im));
        [rows,cols] = size(im);
        % compute the centroid-x
        if cols == 1
            centroid = 0.5;
        elseif area == 0
            centroid = cols/2;
        elseif rows == 1
            centroid = (im*[1:cols]')/area;
        else
            centroid = sum(im)*[1:cols]'/area;
        end

        leftIm = im(:,1:floor(centroid));
        rightIm = im(:,ceil(centroid):end);

        pLeft  = hierarchicalCentroidRec(leftIm'  , depth+1, maxDepth);
        pRight = hierarchicalCentroidRec(rightIm' , depth+1, maxDepth);

        %  Updates the distances so they will be in relation to the complete image
        if size(pRight,1) > 1
            ind = find((1-mod(depth,2))-mod(pRight(2:end,2),2)) + 1;
            pRight(ind,1) = pRight(ind,1) + ceil(centroid) - 1;
            pRight(1,1) = pRight(1,1) - 1;
        end

        p = [centroid, depth; pLeft; pRight];
    end
end

function showLines(im, p)
    if max(p(:,1))<3
        hold on;
        axis([-1 1 -1 1])
        axis ij;
        temp = mod(p(:,2),2);
        indVer = find(temp);
        indHor = find(~temp);
        showLinesRec(min(p(indVer,1)),max(p(indVer,1)),min(p(indHor,1)),max(p(indHor,1)), p);
    else
        imshow(im);
        hold on;
        showLinesRec(0,size(im,2),0,size(im,1), p);
    end
    hold off
end

function p = showLinesRec(x1,x2,y1,y2, p)
    p1 = p(1,:);
    w = 1+max(p(:,2))-p1(1,2);
    t = p1(1);
    p = p(2:end,:);
    if mod(p1(1,2),2)
        plot([t t],[y1 y2],'-m','LineWidth',w);
    else
        plot([x1 x2],[t t],'-r','LineWidth',w);
    end
    if (p1(1,2) < max(p(:,2)))
        if mod(p1(1,2),2)
            p = showLinesRec(x1,t,y1,y2, p);
            p = showLinesRec(t,x2,y1,y2, p);
        else
            p = showLinesRec(x1,x2,y1,t, p);
            p = showLinesRec(x1,x2,t,y2, p);
        end
    end

end


function uDot=uDotEllipsoid(u,x,a,f)
%%UDOTELLIPSOID Return the derivative of the basis vectors for the local
%               coordinate system of a moving observer when using an
%               ellipsoidal approximation of the curved Earth. Such an
%               evolving coordinate system has been termed "wander
%               coordinate" or "naturally evolving coordinates". Targets
%               moving locally in non-maneuvering, level flight with
%               a coordinate system evolving as per this coordinate system
%               will follow geodesic curves, not rhumb lines.
%
%INPUTS: u The 3X3 orthonormal basis vectors for the local coordinate
%          system. u(:,3) is the local vertical.
%        x An nX1 vector whose first three elements are Cartesian position
%          in the global ECEF coordinate system and whose next three
%          elements are velocity in the local (flat-Earth) coordinate
%          system. Other elements of x do not matter.
%        a The semi-major axis of the reference ellipsoid. If this argument
%          is omitted, the value in Constants.WGS84SemiMajorAxis is used.
%        f The flattening factor of the reference ellipsoid. If this
%          argument is omitted, the value in Constants.WGS84Flattening is
%          used.
%
%OUTPUTS: uDot The derivative of the basis vector u with respect to time.
%
%The solution is detailed in [1], and has been simplified for an
%ellipsoidal Earth so that numerical differentiation is not necessary.
%
%REFERENCES:
%[1] D. F. Crouse, "Simulating aerial targets in 3D accounting for the
%    Earth's curvature," Journal of Advances in Information Fusion, vol.
%    10, no. 1, Jun. 2015.
%
%September 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(nargin<4||isempty(f))
        f=Constants.WGS84Flattening;
    end

    if(nargin<3||isempty(a))
        a=Constants.WGS84SemiMajorAxis;
    end

    %The first numerical eccentricity of the ellipsoid.
    e=sqrt(2*f-f^2);

    %The current position.
    r=x(1:3,1);
    %The local velocity.
    rDotLocal=x(4:6,1);

    %Get the velocity in global coordinates.
    rDot=getGlobalVectors(rDotLocal,u);

    %Convert to ellipsoidal coordinates.
    rEllipse=Cart2Ellipse(r,[],a,f);

    %The ellipsoidal latitude
    phi=rEllipse(1);

    %The height above the reference ellipsoid.
    h=rEllipse(3);

    %Obtain the local East-North-Up axes.
    ENUAxes=getENUAxes(rEllipse,false,a,f);
    EAxis=ENUAxes(:,1);
    NAxis=ENUAxes(:,2);

    %The local North and East components of the velocity vector.
    vE=dot(EAxis,rDot(1:3,1));
    vN=dot(NAxis,rDot(1:3,1));

    %The normal radius of curvature.
    Rp=a/sqrt(1-e^2*sin(phi)^2);

    %The radius of curvature of the meridian line.
    Rm=Rp*(1-e^2)/(1-e^2*sin(phi)^2);

    %The rotation rate North.
    omegaN=vE/(Rp+h);

    %The rotation rate East.
    omegaE=-vN/(Rm+h);

    %The 3D rotation vector.
    Omega=omegaN*NAxis+omegaE*EAxis;

    %Compute the derivatives of the coordinate system basis vectors with
    %respect to time.
    uDot(:,1)=cross(Omega,u(:,1));
    uDot(:,2)=cross(Omega,u(:,2));
    uDot(:,3)=cross(Omega,u(:,3));
end

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


function x = cg_ge ( n, a, b, x )

%*****************************************************************************80
%
%% CG_GE uses the conjugate gradient method for a general storage matrix.
%
%  Discussion:
%
%    The linear system has the form A*x=b, where A is a positive-definite
%    symmetric matrix, stored as a full storage matrix.
%
%    The method is designed to reach the solution to the linear system
%      A * x = b
%    after N computational steps.  However, roundoff may introduce
%    unacceptably large errors for some problems.  In such a case,
%    calling the routine a second time, using the current solution estimate
%    as the new starting guess, should result in improved results.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Frank Beckman,
%    The Solution of Linear Equations by the Conjugate Gradient Method,
%    in Mathematical Methods for Digital Computers,
%    edited by John Ralston, Herbert Wilf,
%    Wiley, 1967,
%    ISBN: 0471706892,
%    LC: QA76.5.R3.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A(N,N), the matrix.
%
%    Input, real B(N), the right hand side vector.
%
%    Input/output, real X(N).
%    On input, an estimate for the solution, which may be 0.
%    On output, the approximate solution vector.
%

%
%  Initialize
%    AP = A * x,
%    R  = b - A * x,
%    P  = b - A * x.
%
  ap(1:n,1) = a * x;

  r(1:n,1) = b(1:n,1) - ap(1:n,1);
  p(1:n,1) = b(1:n,1) - ap(1:n,1);
%
%  Do the N steps of the conjugate gradient method.
%
  for it = 1 : n
%
%  Compute the matrix*vector product AP = A*P.
%
    ap(1:n,1) = a * p;
%
%  Compute the dot products
%    PAP = P*AP,
%    PR  = P*R
%  Set
%    ALPHA = PR / PAP.
%
    pap = p(1:n,1)' * ap(1:n,1);
    pr =  p(1:n,1)' * r(1:n,1);

    if ( pap == 0.0 )
      return;
    end

    alpha = pr / pap;
%
%  Set
%    X = X + ALPHA * P
%    R = R - ALPHA * AP.
%
    x(1:n,1) = x(1:n,1) + alpha * p(1:n,1);
    r(1:n,1) = r(1:n,1) - alpha * ap(1:n,1);
%
%  Compute the vector dot product
%    RAP = R*AP
%  Set
%    BETA = - RAP / PAP.
%
    rap = r(1:n,1)' * ap(1:n,1);

    beta = - rap / pap;
%
%  Update the perturbation vector
%    P = R + BETA * P.
%
    p(1:n,1) = r(1:n,1) + beta * p(1:n,1);

  end

  return
end


function [ n_data, z, s, a, fx ] = lerch_values ( n_data )

%*****************************************************************************80
%
%% LERCH_VALUES returns some values of the Lerch transcendent function.
%
%  Discussion:
%
%    The Lerch function is defined as
%
%      Phi(z,s,a) = Sum ( 0 <= k < Infinity ) z^k / ( a + k )^s
%
%    omitting any terms with ( a + k ) = 0.
%
%    In Mathematica, the function can be evaluated by:
%
%      LerchPhi[z,s,a]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    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 Z, the parameters of the function.
%
%    Output, integer S, the parameters of the function.
%
%    Output, real A, the parameters of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 12;

  a_vec = [ ...
     0.0, ...
     0.0, ...
     0.0, ...
     1.0, ...
     1.0, ...
     1.0, ...
     2.0, ...
     2.0, ...
     2.0, ...
     3.0, ...
     3.0, ...
     3.0 ];

  fx_vec = [ ...
     0.1644934066848226E+01, ...
     0.1202056903159594E+01, ...
     0.1000994575127818E+01, ...
     0.1164481052930025E+01, ...
     0.1074426387216080E+01, ...
     0.1000492641212014E+01, ...
     0.2959190697935714E+00, ...
     0.1394507503935608E+00, ...
     0.9823175058446061E-03, ...
     0.1177910993911311E+00, ...
     0.3868447922298962E-01, ...
     0.1703149614186634E-04 ];

  s_vec = [ ...
     2, 3, 10, ...
     2, 3, 10, ...
     2, 3, 10, ...
     2, 3, 10 ];

  z_vec = [ ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.3333333333333333E+00, ...
     0.3333333333333333E+00, ...
     0.3333333333333333E+00, ...
     0.1000000000000000E+00, ...
     0.1000000000000000E+00, ...
     0.1000000000000000E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    z = 0.0;
    s = 0;
    a = 0.0;
    fx = 0.0;
  else
    z = z_vec(n_data);
    s = s_vec(n_data);
    a = a_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function x = r8lt_sl ( n, a, b, job )

%*****************************************************************************80
%
%% R8LT_SL solves a R8LT system.
%
%  Discussion:
%
%    The R8LT storage format is used for an M by N lower triangular matrix,
%    and sets aside storage even for the entries that must be zero.
%
%    No factorization of the lower triangular matrix is required.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A(N,N), the R8LT matrix.
%
%    Input, real B(N), the right hand side.
%    On output, the solution vector.
%
%    Input, integer JOB, is 0 to solve the untransposed system,
%    nonzero to solve the transposed system.
%
%    Output, real X(N), the solution vector.
%
  x(1:n) = b(1:n);

  if ( job == 0 )

    for j = 1 : n
      x(j) = x(j) / a(j,j);
      x(j+1:n) = x(j+1:n) - a(j+1:n,j)' * x(j);
    end

  else

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

  end

  return
end


function [Y, X] = minandmax2( f )
%MINANDMAX2   Find global minimum and maximum of a SPHEREFUN.
%   M = minandmax2(F) returns the minimum and maximum value of a SPHEREFUN
%   over its domain. M is a vector of length 2 such that
%   M(1) = min(f(lambda,theta)) and M(2) = max(f(lambda,theta)).
%
%   [M, LOC] = minandmax2(F) also returns the position of the minimum and
%   maximum. For example,
%
%       F(LOC(1,1),LOC(1,2)) = M(1)  and  F(LOC(2,1),LOC(2,2)) = M(2)
%
% See also SPHEREFUN/MAX2, SPHEREFUN/MIN2, SPHEREFUN/NORM.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% check for empty SPHEREFUN.
if ( isempty( f ) )
    Y = [];
    X = [];
    return
end

[C, D, R] = cdr(f);
f = C*D*R.';
% Maximum possible sample matrix size:
maxsize = 4e3;

% Is the function the zero function?
if ( iszero( f )  )
    dom = f.domain;
    X = [ (dom(2) + dom(1))/2 (dom(4) + dom(3))/2 ];
    X = [ X ; X ];
    Y = [0 ; 0];
    return;
end

% Extract low rank representation:
frows = f.rows;
fcols = f.cols;
piv = f.pivotValues;
dom = f.domain;

% Share out scaling:
sgn = sign( piv ).';
sq = 1 ./ sqrt( abs( piv ) );
frows = frows * diag( sq.'.*sgn );
fcols = fcols * diag( sq );

if ( length(f) == 1 ) % Rank-1 is easy:
    % We can find it from taking maximum and minimum in x and y direction.

    % Find minandmax of rows and columns:
    [yr, xr] = minandmax( frows );
    [yc, xc] = minandmax( fcols );
    % All possible combinations:
    vv = [yr(1)*yc(1), yr(1)*yc(2), yr(2)*yc(1), yr(2)*yc(2)];

    [Y(2), indmx] = max( vv );
    [Y(1), indmn] = min( vv );

    % Work out the location of the maximum.
    X = zeros(2);
    X(1,1) = xr(2);
    X(1,2) = xc(2);

    if ( indmn <= 2 )
        X(1,1) = xr(1);
    end
    if ( mod(indmn,2) == 1 )
        X(1,2) = xc(1);
    end
    X(2,1) = xr(2);
    X(2,2) = xc(2);

    if ( indmx <= 2 )
        X(2,1) = xr(1);
    end

    if ( mod(indmx,2) == 1 ),
        X(2,2) = xc(1);
    end

elseif ( length(f) <= maxsize )

    % We seek a fast initial guess. So we first truncate the SPHEREFUN.
    ypts = [trigpts(length(fcols), fcols.domain);pi];
    xpts = [trigpts(length(frows), frows.domain);pi];
    cvals = feval(fcols, ypts);
    rvals = feval(frows, xpts);

    A = cvals*rvals.';
    % Maximum entry in discretisation.
    [ignored, ind] = min( A(:) ); %#ok<ASGLU>
    [row, col] = ind2sub(size(A), ind);
    X(1,1) = xpts( col );
    X(1,2) = ypts( row );
    Y(1) = feval( f, X(1,1), X(1,2) );
    % Minimum entry in discretisation.
    [ignored, ind] = max(A(:)); %#ok<ASGLU>
    [row, col] = ind2sub(size(A), ind);
    X(2,1) = xpts(col);
    X(2,2) = ypts(row);
    Y(2) = feval(f, X(2,1), X(2,2));

    % Get more digits with optimisation algorithms.
    lb = [ dom(1) ; dom(3) ];
    ub = [ dom(2) ; dom(4) ];

    try

        % If the optimization toolbox is available then use it to get a better maximum.

        warnstate = warning;
        warning('off'); %#ok<WNOFF> % Disable verbose warnings from fmincon.
        options = optimset('Display', 'none', 'TolFun', eps, 'TolX', eps, ...
            'algorithm', 'active-set');
        [mn, Y(1)] = fmincon(@(x,y) feval(f, x(1), x(2)), X(1, :), ...
            [], [], [], [], lb, ub, [], options);
        [mx, Y(2)] = fmincon(@(x) -feval(f, x(1), x(2)), X(2,:), ...
            [], [], [], [], lb, ub, [], options);
        Y(2) = -Y(2);
        X(1,:) = mn;
        X(2,:) = mx;
        warning(warnstate);

    catch

        try
            % Try converting to an unconstrained problem and using built-in solver.

            % Maps from [-1, 1] to [dom(1:2)] and [dom(3:4)], respectively.
            map1 = bndfun.createMap(dom(1:2));
            map2 = bndfun.createMap(dom(3:4));
            % Unconstrained initial guesses:
            Z(:,1) = asin(map1.Inv(X(:,1)));
            Z(:,2) = asin(map2.Inv(X(:,2)));
            % Maps from R to [dom(1), dom(2)] and [dom(3), dom(4)], respectively.
            map1 = @(x) map1.For(sin(x));
            map2 = @(x) map2.For(sin(x));
            % Set options:
            options = optimset('Display', 'off', 'TolFun', eps, 'TolX', eps);
            warnstate = warning;
            warning('off'); %#ok<WNOFF> % Disable verbose warnings from fminsearch.
            f_mapped = @(x) feval(f, map1(x(1)), map2(x(2)));
            [mn, Y(1)] = fminsearch(@(x) f_mapped(x), Z(1, :), options);
            [mx, Y(2)] = fminsearch(@(x) -f_mapped(x), Z(2, :), options);
            Y(2) = -Y(2);
            X(1:2,1) = map1([mn(1) ; mx(1)]);
            X(1:2,2) = map2([mn(2) ; mx(2)]);
            warning(warnstate);

        catch

            % Nothing is going to work so initial guesses will have to do.

        end

    end

elseif ( length(f) >= maxsize )

    error('CHEBFUN:SPHEREFUN:minandmax2:length', 'Rank is too large.');

end

% If the location of the max/min is outside of [-pi,pi]^2, which can happen
% because Spherefun is based on the double Fourier sphere method, then
% translate back to [0,pi]x[-pi,pi] using BMC symmetry:

% check minimum:
if ( X(1,1) < 0 )
    X(1,1) = X(1,1)+pi;
    X(1,2) = -X(1,2);
end

% check maximum:
if ( X(2,1) < 0 )
    X(2,1) = X(2,1)+pi;
    X(2,2) = -X(2,2);
end

end

%%%
% Use the approach below when bivariate rootfinding is fully implemented.
%%%
% % Use bivariate rootfinding to find all the local extrema:
% F = gradient( f );
% r = roots( F );
% if ( ~isempty( r ) )
%     [inMax, idInMax] = max( feval(f, r(:,1), r(:,2) ) );
%     [inMin, idInMin] = min( feval(f, r(:,2), r(:,2) ) );
% else
%     inMax = inf;   % max and min must occur on boundary.
%     inMin = inf;
% end
%
% % Search along boundary:
% dom = f.domain;
% left = feval(f, dom(1), ':');
% right = feval(f, dom(2), ':');
% down = feval(f, ':', dom(3));
% up = feval(f, ':', dom(4));
% [Yleft, Xleft] = minandmax( left );
% [Yright, Xright] = minandmax( right );
% [Yup, Xup] = minandmax( up );
% [Ydown, Xdown] = minandmax( down );
%
% % Store Min/Max location for later:
% BcMinLocations = [ Xleft(1,:) ; Xright(1,:) ; Xup(1,:) ; Xdown(1,:) ];
% BcMaxLocations = [ Xleft(2,:) ; Xright(2,:) ; Xup(2,:) ; Xdown(2,:) ];
%
% [ BcMax, idBcMax ] = max( [ Yleft(2), Yright(2), Yup(2), Ydown(2) ].' );
% [ BcMin, idBcMin ] = min( [ Yleft(1), Yright(1), Yup(1), Ydown(1) ].' );
%
% % What is the global min and max?:
% [Ymax, inOrOutMax] = max( [inMax, BcMax].' );
% [Ymin, inOrOutMin] = min( [inMin, BcMin].' );
% Y = [Ymin, Ymax];
% X = zeros(2);
%
% % Unravel to find locations:
% if ( inOrOutMin == 1 )
%     X(1,:) = [ r(idInMin,1), r(idInMin,2) ];
% else
%     X(1,:) = BcMinLocations(idBcMin, :);
% end
% if ( inOrOutMax == 1 )
%     X(2,:) = [ r(idInMax,1), r(idInMax,2) ];
% else
%     X(2,:) = BcMaxLocations(idBcMax, :);
% end
%
% end


function haar_test03 ( )

%*****************************************************************************80
%
%% HAAR_TEST03 tests HAAR_2D and HAAR_2D_INVERSE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HAAR_TEST03\n' );
  fprintf ( 1, '  HAAR_2D computes the Haar transform of an array.\n' );
  fprintf ( 1, '  HAAR_2D_INVERSE inverts the transform.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Apply this to a 128x128 matrix of 0 and 1 values\n' );
  fprintf ( 1, '  which is actually a bit map of the Sierpinski triangle.\n' );
%
%  Load data from a file.
%
  u = load ( 'sierpinski.txt' );

  [ m, n ] = size ( u );
%
%  Demonstrate successful inversion.
%
  r8mat_print_some ( m, n, u, 1, 1, 10, 10, '  Input array U:' );

  v = haar_2d ( u );

  r8mat_print_some ( m, n, v, 1, 1, 10, 10, '  Transformed array V:' );

  w = haar_2d_inverse ( v );

  r8mat_print_some ( m, n, w, 1, 1, 10, 10, '  Recovered array W:' );

  return
end


%TEST 04: Algebraic Reconstruction Techniques
clear
clc
fprintf('TEST 04: Algebraic Reconstruction Techniques\rCompare ART and SART\r');
angles = 0:6:179;
%Phantom
im = imtest('s4',64);
siz = size(im);
figure('name','Original Image: Phantom')
imshow(im)
[W, p, ~, ~] = buildWeightMatrix(im,angles);
%Iteration 100
fprintf('ART method\niteration 100:\r')
im_rec1 = tomo_recon_myart(W,p,siz,100);
im_rec1 = uint8(imscale(im_rec1));
figure('name','ART')
imshow(im_rec1);
fprintf('SART method\niteration 100:\r')
im_rec2 = tomo_recon_sart(W,p,siz,100);
im_rec2 = uint8(imscale(im_rec2));
figure('name','SART')
imshow(im_rec2);
%Iteration 1000
fprintf('ART method\niteration 1000:\r')
im_rec1 = tomo_recon_myart(W,p,siz,1000);
im_rec1 = uint8(imscale(im_rec1));
figure('name','ART')
imshow(im_rec1);
fprintf('SART method\niteration 1000:\r')
%Code efficiency is too low!
%im_rec2 = tomo_recon_mysart(W,p,siz,1000);
%figure('name','ART')
%imshow(im_rec2);


% this function compute the spatial covariance matrix from complex Fourier
% transform of array signals.
% Inputs:
%   X: D x N x T matrix of Fourier transform coefficients. D is the number
%   of frequency bins, N is the number of microphone channels, and T is the
%   number of frames
%   context_size: number of context frames used to compute the spatial
%   covariance at each time-frequency bin.
% Outputs:
%   R: N x N x D x T matrix of spatial covariance matrixes
%
% Author: Xiong Xiao, Nanyang Technological University, Singapore
% Last Modified: 22 Feb 2016.
%
function R = ComplexSpectrum2SpatialCov(X, context_size, shift, useGPU)
if nargin<4
    useGPU = 1;
end

half_ctx = (context_size-1)/2;
[D, N, T] = size(X);    % D is number of freq bin, N is number of channel

if context_size == 0    % get global spatial covariance matrix
    X2 = permute(X, [2 1 3]);
    XX = outProdND(X2);
    R = mean(XX,4);
else    % get windowed spatial covariance
    if 0
        nBlock = length(1:shift:T);
        R = zeros(N,N,D,nBlock);
        for d = 1:D
            for t = 1:shift:T
                frame_selector = t-half_ctx:t+half_ctx;
                frame_selector = min(T,max(1,frame_selector));
                X2 = squeeze(X(d,:,frame_selector));
                R(:,:,d,(t-1)/shift+1) = X2*X2' / context_size;
            end
        end
    else
        X2 = permute(X, [2 1 3]);
        XX = outProdND(X2);
%         X2cell = num2cell(X2, [1]);
%         XXcell = cellfun(@(x) (reshape(x*x', N^2,1)), X2cell, 'UniformOutput', 0);
%         XX = cell2mat(XXcell);
        XX2 = reshape(XX, N^2*D, T);
        if 1
            if useGPU
                XX2 = gpuArray(XX2);
            end
            idx = [ones(1,half_ctx) 1:T ones(1,half_ctx)*T];
            SCM = conv2(XX2(:,idx), ones(1,context_size, class(gather(X)))/context_size, 'valid');
%             SCM = SCM(:,half_ctx+1:end-half_ctx);
        else
            fake_layer.a = XX2;
            XX3 = F_splice(fake_layer, context_size);
            XX4 = reshape(XX3,  N^2*D, context_size, T);
            SCM = mean(XX4,2);
        end
        SCM2 = reshape(SCM, N^2, D, T);
        SCM3 = reshape(SCM2, N, N, D, T);
        R = SCM3(:,:,:,1:shift:end);
    end
end


function varargout = plog(varargin)
%PLOG
%
% y = PLOG(x)
%
% Computes concave perspective log, x(1)*log(x(2)/x(1)) on x>0
%
% Implemented as evalutation based nonlinear operator. Hence, the concavity
% of this function is exploited to perform convexity analysis and rigorous
% modelling.

switch class(varargin{1})

    case 'double'

        if ~isequal(prod(size(varargin{1})),2)
            error('PLOG only defined for 2x1 arguments');
        end
        x = varargin{1};
        % Safe version with defined negative values (helps fmincon when
        % outside feasible region)

        if isequal(x(1),[0])
            varargout{1} = 0;
        else
            varargout{1} = x(1)*log(x(2)/x(1));
        end

    case 'sdpvar'

        if ~isequal(prod(size(varargin{1})),2)
            error('PLOG only defined for 2x1 arguments');
        else
            varargout{1} = yalmip('define',mfilename,varargin{1});
        end

    case 'char'

        operator = CreateBasicOperator('concave','callback');
        operator.range = [-inf inf];
        operator.domain = [0 inf];
        operator.bounds = @bounds;
        operator.convexhull = @convexhull;
        operator.derivative = @derivative;

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error([upper(mfilename) ' called with weird argument']);
end

function dp = derivative(x)
z = x(2)/x(1);
dp = [log(z)-1;1./z];

function [L,U] = bounds(xL,xU)
xU(isinf(xU)) = 1e12;
x1 = xL(1)*log(xL(2)/xL(1));
x2 = xU(1)*log(xU(2)/xU(1));
x3 = xL(1)*log(xU(2)/xL(1));
x4 = xU(1)*log(xL(2)/xU(1));
L = min([x1 x2 x3 x4]);

% Stationary in x1 when x1 = x2*exp(-1)
% Increasing in x2, so max at border
p1 = [exp(-1)*xU(2);xU(2)];
x5 = p1(1)*log(p1(2)/p1(1));
if x5(1) >= xL(1) && x5(1) <= xU(1)
    U = max([x1 x2 x3 x4 x5]);
else
    U = max([x1 x2 x3 x4]);
end

function [Ax,Ay,b,K] = convexhull(xL,xU)

x1 = [xL(1);xL(2)];
x2 = [xU(1);xL(2)];
x3 = [xL(1);xU(2)];
x4 = [xU(1);xU(2)];
x5 = (xL+xU)/2;

f1 = plog(x1);
f2 = plog(x2);
f3 = plog(x3);
f4 = plog(x4);
f5 = plog(x5);

df1 = derivative(x1);
df2 = derivative(x2);
df3 = derivative(x3);
df4 = derivative(x4);
df5 = derivative(x5);

[Ax,Ay,b,K] = convexhullConcave2D(x1,f1,df1,x2,f2,df2,x3,f3,df3,x4,f4,df4,x5,f5,df5);


function z = lambda_max( Y )

% LAMBDA_MAX    Maximum eigenvalue of a symmetric matrix.
%     For square matrix X, LAMBDA_MAX(X) is MAX(EIG(X)) if X is Hermitian
%     or symmetric and real; and +Inf otherwise.
%
%     An error results if X is not a square matrix.
%
%     Disciplined convex programming information:
%         LAMBDA_MAX is convex and nonmonotonic (at least with respect to
%         elementwise comparison), so its argument must be affine.

error( nargchk( 1, 1, nargin ) );
if ndims( Y ) > 2 || size( Y, 1 ) ~= size( Y, 2 ),
    error( 'Input must be a square matrix.' );
end
err = Y - Y';
Y   = 0.5 * ( Y + Y' );
if norm( err, 'fro' )  > 8 * eps * norm( Y, 'fro' ),
    z = Inf;
else
    z = max( eig( full( Y ) ) );
end

% 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 approx_display ( )

%*****************************************************************************80
%
%% APPROX_DISPLAY displays a sequence of Bernstein approximants to SIN(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 July 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'APPROX_DISPLAY\n' );
  fprintf ( 1, '  BPAB_APPROX evaluates the Bernstein polynomial\n' );
  fprintf ( 1, '  approximant to a function F(X).\n' );
  fprintf ( 1, '  This program displays the sequence of approximants\n' );
  fprintf ( 1, '  to y=sin(x) over the interval [1,3]\n' );

  a = 1.0;
  b = 3.0;

  for degree = 0 : 20
%
%  Generate data values.
%
    xdata = zeros ( degree + 1, 1 );
    ydata = zeros ( degree + 1, 1 );

    for i = 0 : degree

      if ( degree == 0 )
        xdata(i+1) = 0.5 * ( a + b );
      else
        xdata(i+1) = ( ( degree - i ) * a   ...
                     + (          i ) * b ) ...
                     / ( degree     );
      end

      ydata(i+1) = sin ( xdata(i+1) );

    end
%
%  Compare the true function and the approximant.
%
    nval = 501;

    xval = linspace ( a, b, nval );

    yval = bpab_approx ( degree, a, b, ydata, nval, xval );

    clf
    plot ( xval, yval, 'b-' );
    hold on
    plot ( xval, sin ( xval ), 'r-' );
    plot ( xdata, ydata, 'r.', 'Markersize', 30 );
    grid
    xlabel ( '<---X--->' );
    ylabel ( '<---Y--->' );
    title ( sprintf ( 'Bernstein Approximant Degree %d', degree ) );
    hold off

    fprintf ( 1, 'Bernstein approximant of degree %d\n', degree );

    pause

  end

  return
end


function stroud_test41 ( )

%*****************************************************************************80
%
%% TEST41 tests TRIANGLE_UNIT_SET and TRIANGLE_SUB.
%
%  Discussion:
%
%    Break up the triangle into NSUB**2 equal subtriangles.  Approximate
%    the integral over the triangle by the sum of the integrals over each
%    subtriangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  global FUNC_2D_INDEX;

  num = function_2d_num ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST41\n' );
  fprintf ( 1, '  TRIANGLE_UNIT_SET sets up a quadrature rule\n' );
  fprintf ( 1, '    on a triangle.\n' );
  fprintf ( 1, '  TRIANGLE_SUB applies it to subtriangles of an\n' );
  fprintf ( 1, '    arbitrary triangle.\n' );
  fprintf ( 1, '\n' );
%
%  Set the location of the triangle.
%
  xval(1) = 0.0;
  yval(1) = 0.0;

  xval(2) = 0.0;
  yval(2) = 1.0;

  xval(3) = 1.0;
  yval(3) = 0.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Triangle vertices:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %8f  %8f\n', xval(1), yval(1) );
  fprintf ( 1, '  %8f  %8f\n', xval(2), yval(2) );
  fprintf ( 1, '  %8f  %8f\n', xval(3), yval(3) );
%
%  Get the quadrature abscissas and weights for a unit triangle.
%
  rule = 3;
  order = triangle_unit_size ( rule );

  [ xtab, ytab, weight ] = triangle_unit_set ( rule, order );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Using unit triangle quadrature rule %d\n', rule );
  fprintf ( 1, '  Rule order = %d\n', order );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Function Nsub  Result\n' );
  fprintf ( 1, '\n' );
%
%  Set the function.
%
  for i = 1 : num

    FUNC_2D_INDEX = i;
%
%  Try an increasing number of subdivisions.
%
    for nsub = 1 : 5

      result = triangle_sub ( ...
        'function_2d', xval, yval, nsub, order, xtab, ytab, weight );

      fname = function_2d_name ( i );

      fprintf ( 1, '  %s  %2d  %12f\n', fname, nsub, result );

    end

  end

  return
end


function hidden_probability = visible_state_to_hidden_probabilities(rbm_w, visible_state)
% <rbm_w> is a matrix of size <number of hidden units> by <number of visible units>
% <visible_state> is a binary matrix of size <number of visible units> by <number of configurations that we're handling in parallel>.
% The returned value is a matrix of size <number of hidden units> by <number of configurations that we're handling in parallel>.
% This takes in the (binary) states of the visible units, and returns the activation probabilities of the hidden units conditional on those states.
    m_ = rbm_w * visible_state;
    hidden_probability = 1 ./ (1 + e .^ (-m_));
end


% Fig. 5.47  Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%  Figure 5.47 Root locus for the heat exchanger, with and without delay

clf
numG=1;
denG=conv([10 1],[60 1]);
sysG=tf(numG, denG);
rlocus(sysG);
axis([-.8 .8 -.6 .6]);
%pause;
hold on
gtext('locus without delay')
Td=5;
numD1 = 1;
denD1=[Td 1];
numGD1=conv(numG,numD1);
denGD1=conv(denG,denD1);
sysGD1= tf(numGD1, denGD1);
rlocus(sysGD1);

gtext('locus with (0,1) lag Pade')
title('Fig. 5.47 Root locus for heat exchanger with and without delay')
%pause;
numDe2=[Td^2/12 -Td/2 1];
denDe2=[Td^2/12 +Td/2 1];
numGD=conv(numG,numDe2);
denGD=conv(denG,denDe2);
sysGD= tf(numGD, denGD);
rlocus(sysGD);

gtext('locus with(2,2) Pade')
grid on
% title('Fig. 5.47 Root locus for heat exchanger with and without delay')
% z=0:.1:.9;
% wn= .1:.1:.6;
% sgrid(z, wn)
hold off


function som = SOMSimple(nfeatures, ndim, nepochs, ntrainingvectors, eta0, etadecay, sgm0, sgmdecay, showMode)
%SOMSimple Simple demonstration of a Self-Organizing Map that was proposed by Kohonen.
%   sommap = SOMSimple(nfeatures, ndim, nepochs, ntrainingvectors, eta0, neta, sgm0, nsgm, showMode)
%   trains a self-organizing map with the following parameters
%       nfeatures        - dimension size of the training feature vectors
%       ndim             - width of a square SOM map
%       nepochs          - number of epochs used for training
%       ntrainingvectors - number of training vectors that are randomly generated
%       eta0             - initial learning rate
%       etadecay         - exponential decay rate of the learning rate
%       sgm0             - initial variance of a Gaussian function that
%                          is used to determine the neighbours of the best
%                          matching unit (BMU)
%       sgmdecay         - exponential decay rate of the Gaussian variance
%       showMode         - 0: do not show output,
%                          1: show the initially randomly generated SOM map
%                             and the trained SOM map,
%                          2: show the trained SOM map after each update
%
%   For example: A demonstration of an SOM map that is trained by RGB values
%
%       som = SOMSimple(3,60,10,100,0.1,0.05,20,0.05,2);
%       % It uses:
%       %   3    : dimensions for training vectors, such as RGB values
%       %   60x60: neurons
%       %   10   : epochs
%       %   100  : training vectors
%       %   0.1  : initial learning rate
%       %   0.05 : exponential decay rate of the learning rate
%       %   20   : initial Gaussian variance
%       %   0.05 : exponential decay rate of the Gaussian variance
%       %   2    : Display the som map after every update

nrows = ndim;
ncols = ndim;

som = rand(nrows,ncols,nfeatures);

if showMode >= 1
    fig = figure;
    displaySOMmap(fig, 1, 'Randomly initialized SOM', som, nfeatures);
end

% Generate random training data
trainingData = rand(ntrainingvectors,nfeatures);

% Generate coordinate system
[x y] = meshgrid(1:ncols,1:nrows);

for t = 1:nepochs
    % Compute the learning rate for the current epoch
    eta = eta0 * exp(-t*etadecay);

    % Compute the variance of the Gaussian (Neighbourhood) function for the ucrrent epoch
    sgm = sgm0 * exp(-t*sgmdecay);

    % Consider the width of the Gaussian function as 3 sigma
    width = ceil(sgm*3);

    for ntraining = 1:ntrainingvectors
        % Get current training vector
        trainingVector = trainingData(ntraining,:);

        % Compute the Euclidean distance between the training vector and
        % each neuron in the SOM map
        dist = getEuclideanDistance(trainingVector, som, nrows, ncols, nfeatures);

        % Find the best matching unit (bmu)
        [~, bmuindex] = min(dist);

        % transform the bmu index into 2D
        [bmurow bmucol] = ind2sub([nrows ncols],bmuindex);

        % Generate a Gaussian function centered on the location of the bmu
        g = exp(-(((x - bmucol).^2) + ((y - bmurow).^2)) / (2*sgm*sgm));

        % Determine the boundary of the local neighbourhood
        fromrow = max(1,bmurow - width);
        torow   = min(bmurow + width,nrows);
        fromcol = max(1,bmucol - width);
        tocol   = min(bmucol + width,ncols);

        % Get the neighbouring neurons and determine the size of the neighbourhood
        neighbourNeurons = som(fromrow:torow,fromcol:tocol,:);
        sz = size(neighbourNeurons);

        % Transform the training vector and the Gaussian function into
        % multi-dimensional to facilitate the computation of the neuron weights update
        T = reshape(repmat(trainingVector,sz(1)*sz(2),1),sz(1),sz(2),nfeatures);
        G = repmat(g(fromrow:torow,fromcol:tocol),[1 1 nfeatures]);

        % Update the weights of the neurons that are in the neighbourhood of the bmu
        neighbourNeurons = neighbourNeurons + eta .* G .* (T - neighbourNeurons);

        % Put the new weights of the BMU neighbouring neurons back to the
        % entire SOM map
        som(fromrow:torow,fromcol:tocol,:) = neighbourNeurons;

        if showMode == 2
            displaySOMmap(fig, 2, ['Epoch: ',num2str(t),'/',num2str(nepochs),', Training Vector: ',num2str(ntraining),'/',num2str(ntrainingvectors)], som, nfeatures);
        end
    end
end

if showMode == 1
    displaySOMmap(fig, 2, 'Trained SOM', som, nfeatures);
end

function ed = getEuclideanDistance(trainingVector, sommap, nrows, ncols, nfeatures)

% Transform the 3D representation of neurons into 2D
neuronList = reshape(sommap,nrows*ncols,nfeatures);

% Initialize Euclidean Distance
ed = 0;
for n = 1:size(neuronList,2)
    ed = ed + (trainingVector(n)-neuronList(:,n)).^2;
end
ed = sqrt(ed);

function displaySOMmap(fig, nsubplot, description, sommap, nfeatures)
% Display given SOM map

figure(fig);
subplot(1,2,nsubplot);
if nfeatures >= 3
    imagesc(sommap(:,:,1:3));
else
    imagesc(sommap(:,:,1));
end
axis off;axis square;
title(description);


%% Rotation Matrices for Real Spherical Harmonics. Direct Determination by Recursion
%% This MATLAB code was written based on the C++ code at link:
%% http://mathlib.zfx.info/html_eng/SHRotate_8h-source.html
%% which I believe was again based on the implementation from Don Williamson.
%% http://www.donw.co.uk/home/downloads/SHRotation.zip
%% The original algorithm was given in the following references:
%% [1] Joseph Ivanic and Klaus Ruedenberg
%%     J. Phys. Chem. 1996, 100, 6342-5347
%%
%% [2] Additions and Corrections (to previous paper)
%%     Joseph Ivanic and Klaus Ruedenberg
%%     J. Phys. Chem. A, 1998, Vol. 102, No. 45, 9099
%% Todo:
%% http://www.cgg.cvut.cz/~xkrivanj/papers/2005-rapport1728/rr1728-irisa-kr
%% ivanek-shrotation.pdf

function [R] = SHRotate(r, degree)

%%=============================================================
%% Project:   Spherical Harmonics
%% Module:    $RCSfile: SHRotate.m,v $
%% Language:  MATLAB
%% Author:    $Author: bjian $
%% Date:      $Date: 2007/12/27 06:23:35 $
%% Version:   $Revision: 1.4 $
%%=============================================================

% The first 1x1 sub-matrix is always 1
R{1} = 1;

% The l=1 band is rotated by a permutation of the original matrix
R1(-1+2,-1+2) = r(2,2);
R1(-1+2, 0+2) = r(3,2);
R1(-1+2, 1+2) = r(1,2);

R1( 0+2,-1+2) = r(2,3);
R1( 0+2, 0+2) = r(3,3);
R1( 0+2, 1+2) = r(1,3);

R1( 1+2,-1+2) = r(2,1);
R1( 1+2, 0+2) = r(3,1);
R1( 1+2, 1+2) = r(1,1);

R{1+1} = R1;

% Calculate each block of the rotation matrix for each subsequent band
for band=2:degree
    for m=-band:band
        for n=-band:band
            R{band+1}(m+band+1,n+band+1) = M(band,m,n,R);
        end
    end
end

function [ret] = M(l, m, n, R)

d = (m==0);
if (abs(n)==l)
    denom = (2*l)*(2*l-1);
else
    denom = (l*l-n*n);
end

u = sqrt((l*l-m*m)/denom);

v = sqrt((1+d)*(l+abs(m)-1)*(l+abs(m))/denom)*(1-2*d)*0.5;
w = sqrt((l-abs(m)-1)*(l-abs(m))/denom)*(1-d)*(-0.5);

if (u~=0)
    u = u*U(l,m,n,R);
end

if (v~=0)
    v = v*V(l,m,n,R);
end

if (w~=0)
    w = w*W(l,m,n,R);
end

ret = u+v+w;

function [ret] = U(l,m,n,R)

ret = P(0,l,m,n,R);

function [ret] = P(i,l,a,b,R)

ri1 = R{2}(i+2,1+2);
rim1 = R{2}(i+2,-1+2);
ri0 = R{2}(i+2,0+2);

if (b==-l)
    ret = ri1*R{l}(a+l,1) + rim1*R{l}(a+l, 2*l-1);
else
    if (b==l)
        ret = ri1*R{l}(a+l,2*l-1) - rim1*R{l}(a+l, 1);
    else
        ret = ri0*R{l}(a+l,b+l);
    end
end

function [ret] = V(l,m,n,R)
if (m==0)
    p0 = P(1,l,1,n,R);
    p1 = P(-1,l,-1,n,R);
    ret = p0+p1;
else
    if (m>0)
        d = (m==1);
        p0 = P(1,l,m-1,n,R);
        p1 = P(-1,l,-m+1,n,R);
        ret = p0*sqrt(1+d) - p1*(1-d);
    else
        d = (m==-1);
        p0 = P(1,l,m+1,n,R);
        p1 = P(-1,l,-m-1,n,R);
        ret = p0*(1-d) + p1*sqrt(1+d);
    end
end

function [ret] = W(l,m,n,R)
if (m==0)
    error('never gets called')
else
    if (m>0)
        p0 = P(1,l,m+1,n,R);
        p1 = P(-1,l,-m-1,n,R);
        ret = p0 + p1;
    else
        p0 = P(1,l,m-1,n,R);
        p1 = P(-1,l,-m+1,n,R);
        ret = p0 - p1;
    end
end


function [rfrac,isonfly] = center_of_rotation2(trx,fly,debug)
if ~exist('debug','var')
  debug = false;
end
N = 100;

% note that if dtheta = 0, this will produce 0

cost = cos(trx(fly).theta_mm);
sint = sin(trx(fly).theta_mm);
dacost = 2*diff(trx(fly).a_mm.*cost);
dbcost = 2*diff(trx(fly).b_mm.*cost);
dasint = 2*diff(trx(fly).a_mm.*sint);
dbsint = 2*diff(trx(fly).b_mm.*sint);
Z = dacost .* dbcost + dbsint.*dasint;
Minv = zeros(2,2,trx(fly).nframes-1);
Minv(1,1,:) = dbcost ./ Z;
Minv(1,2,:) = dbsint ./ Z;
Minv(2,1,:) = -dasint ./ Z;
Minv(2,2,:) = dacost ./ Z;
Minv = reshape(Minv,[4,trx(fly).nframes-1]);
x = [ diff(trx(fly).x_mm) ; diff(trx(fly).y_mm) ];
rfrac = -[Minv(1,:) .* x(1,:) + Minv(3,:) .* x(2,:);...
  Minv(2,:) .* x(1,:) + Minv(4,:) .* x(2,:)];
% when no rotation, set center of rotation to middle of fly by default
rfrac(isnan(rfrac)) = 0;
rfrac0 = rfrac;
isoutofbounds = sum(rfrac.^2,1) > 1;
psi = linspace(0,2*pi,N)';
cospsi = cos(psi);
sinpsi = sin(psi);
idx = find(isoutofbounds);
nout = length(idx);
if nout > 0,
  x1 = repmat(trx(fly).x_mm(idx),[N,1]) + repmat(2*trx(fly).a_mm(idx).*cost(idx),[N,1]).*repmat(cospsi,[1,nout]) - ...
    repmat(2*trx(fly).b_mm(idx).*sint(idx),[N,1]).*repmat(sinpsi,[1,nout]);
  y1 = repmat(trx(fly).y_mm(idx),[N,1]) + repmat(2*trx(fly).a_mm(idx).*sint(idx),[N,1]).*repmat(cospsi,[1,nout]) + ...
    repmat(2*trx(fly).b_mm(idx).*cost(idx),[N,1]).*repmat(sinpsi,[1,nout]);
  x2 = repmat(trx(fly).x_mm(idx+1),[N,1]) + repmat(2*trx(fly).a_mm(idx+1).*cost(idx+1),[N,1]).*repmat(cospsi,[1,nout]) - ...
    repmat(2*trx(fly).b_mm(idx+1).*sint(idx+1),[N,1]).*repmat(sinpsi,[1,nout]);
  y2 = repmat(trx(fly).y_mm(idx+1),[N,1]) + repmat(2*trx(fly).a_mm(idx+1).*sint(idx+1),[N,1]).*repmat(cospsi,[1,nout]) + ...
    repmat(2*trx(fly).b_mm(idx+1).*cost(idx+1),[N,1]).*repmat(sinpsi,[1,nout]);
  d = (x1 - x2).^2 + (y1 - y2).^2;
  [~,j] = min(d,[],1);
  rfrac(1,idx) = cospsi(j);
  rfrac(2,idx) = sinpsi(j);
end
isonfly = ~isoutofbounds;

if debug,
  ntry = 50;
  psitry = linspace(0,2*pi,ntry+2);
  psitry = psitry(2:end-1);
  [rhotry,psitry] = meshgrid(linspace(0,1,ntry),psitry);
  rtry = [rhotry(:).*cos(psitry(:)),rhotry(:).*sin(psitry(:))]';
  rtry = [rtry,[0;0]];
  colorsplot = 'rg';
  colorsplot2 = 'mc';
  [~,order] = sort(-abs(modrange(diff(trx(fly).theta_mm),-pi,pi)));
  for i = order,
    u = [0,0]; v = [0,0];
    uout = [0,0]; vout = [0,0];
    utry = zeros(2,length(rtry)); vtry = zeros(2,length(rtry));
    clf; hold on;
    for j = [i,i+1],
      u(j-i+1) = trx(fly).x_mm(j) + rfrac(1,i)*trx(fly).a_mm(j)*2*cos(trx(fly).theta_mm(j)) - rfrac(2,i)*trx(fly).b_mm(j)*2*sin(trx(fly).theta_mm(j));
      v(j-i+1) = trx(fly).y_mm(j) + rfrac(1,i)*trx(fly).a_mm(j)*2*sin(trx(fly).theta_mm(j)) + rfrac(2,i)*trx(fly).b_mm(j)*2*cos(trx(fly).theta_mm(j));
      uout(j-i+1) = trx(fly).x_mm(j) + rfrac0(1,i)*trx(fly).a_mm(j)*2*cos(trx(fly).theta_mm(j)) - rfrac0(2,i)*trx(fly).b_mm(j)*2*sin(trx(fly).theta_mm(j));
      vout(j-i+1) = trx(fly).y_mm(j) + rfrac0(1,i)*trx(fly).a_mm(j)*2*sin(trx(fly).theta_mm(j)) + rfrac0(2,i)*trx(fly).b_mm(j)*2*cos(trx(fly).theta_mm(j));
      utry(j-i+1,:) = trx(fly).x_mm(j) + rtry(1,:)*trx(fly).a_mm(j)*2*cos(trx(fly).theta_mm(j)) - rtry(2,:)*trx(fly).b_mm(j)*2*sin(trx(fly).theta_mm(j));
      vtry(j-i+1,:) = trx(fly).y_mm(j) + rtry(1,:)*trx(fly).a_mm(j)*2*sin(trx(fly).theta_mm(j)) + rtry(2,:)*trx(fly).b_mm(j)*2*cos(trx(fly).theta_mm(j));
      ellipsedraw(trx(fly).a_mm(j)*2,trx(fly).b_mm(j)*2,trx(fly).x_mm(j),trx(fly).y_mm(j),trx(fly).theta_mm(j),colorsplot(j-i+1));
      plot(trx(fly).x_mm(j)+trx(fly).a_mm(j)*2*cos(trx(fly).theta_mm(j)),trx(fly).y_mm(j)+trx(fly).a_mm(j)*2*sin(trx(fly).theta_mm(j)),'x','color',colorsplot(j-i+1));
      htest(j-i+1) = plot([trx(fly).x_mm(j),u(j-i+1),uout(j-i+1)],[trx(fly).y_mm(j),v(j-i+1),vout(j-i+1)],'o-','color',colorsplot(j-i+1),'markerfacecolor',colorsplot(j-i+1)); %#ok<AGROW>
    end
    d = (utry(1,:)-utry(2,:)).^2 + (vtry(1,:)-vtry(2,:)).^2;
    [dexp,k] = min(d);
    uexp = utry(:,k);
    vexp = vtry(:,k);
    dexp = sqrt(dexp);
    dtest = sqrt(diff(u).^2 + diff(v).^2);
    dout = sqrt(diff(uout).^2 + diff(vout).^2);
    plot(uexp,vexp,'k.-');
    plot(u,v,'b.-');
    plot(uout,vout,'b.-');
    for j = [i,i+1],
      hexp(j-i+1) = plot([uexp(j-i+1),trx(fly).x_mm(j)],[vexp(j-i+1),trx(fly).y_mm(j)],'o-','color',colorsplot2(j-i+1),'markerfacecolor',colorsplot2(j-i+1)); %#ok<AGROW>
    end
    ax = nan(1,4);
    ax1 = nan(1,4);
    for j = [i,i+1],
      [ax1(1),ax1(2),ax1(3),ax1(4)] = ellipse_to_bounding_box(trx(fly).x_mm(j),trx(fly).y_mm(j),2*trx(fly).a_mm(j),2*trx(fly).b_mm(j),trx(fly).theta_mm(j));
      ax([1,3]) = min(ax([1,3]),ax1([1,3]));
      ax([2,4]) = max(ax([2,4]),ax1([2,4]));
    end
    ax = ax + [-3,3,-3,3];
    legend([htest,hexp],'analytic1','analytic2','empirical1','empirical2');
    title(sprintf('red = frame %d, green = frame %d, dtest = %f, dout = %f, dexp = %f',i,i+1,dtest,dout,dexp));
    axis equal
    axis(ax)
    input('');
  end
end


function [An Bn] = mieLayeredTerms(mu, epsilon, radius, isPEC, frequency, nmax)

% Compute the sphere coefficients An and Bn for a sphere having a number
% of homogeneous dielectric layers, with or without a perfectly
% electrically conducting (PEC) core. Follows the treatment in
% Chapter 3 of
%
% Ruck, et. al. "Radar Cross Section Handbook", Plenum Press, 1970.
%
% The incident electric field is in the -z direction (theta = 0) and is
% theta-polarized. The time-harmonic convention exp(jwt) is assumed, and
% the Green's function is of the form exp(-jkr)/r.
%
% Inputs:
%   mu: Relative complex permeability for each dielectric region
%   epsilon: Relative complex permittivity for each dielectric region
%       (the unbounded (free space) region should be the first entry)
%   radius: Radius of each spherical dielectric interface, from outermost
%           to innermost
%   isPEC: A flag for when the innermost sphere is conducting
%   frequency: Operating frequency (Hz)
%   nmax: Maximum mode for computing Bessel functions
% Outputs:
%   An: Array of Mie solution constants (used in mieScatteredField)
%   Bn: Array of Mie solution constants (used in mieScatteredField)
%
%   Author: Walton C. Gibson, email: kalla@tripoint.org

% speed of light
c = 299792458.0;

% radian frequency
w = 2.0*pi*frequency;

% wavenumber
k0 = w/c;

% total number of dielectric interfaces
numInterfaces = length(mu);

% free space impedance
eta0 = 376.7303134617707;

% impedance of each dielectric region
eta = eta0 * sqrt(mu./epsilon);

% scale factor
m = sqrt(mu.*epsilon);

% mode numbers
mode = 1:nmax;

Z = zeros(numInterfaces, length(mode));
Y = zeros(numInterfaces, length(mode));

numIFC = numInterfaces - 1;

for L = numIFC:-1:1

    if L == numIFC
        if isPEC == 0
            x = k0 * m(L + 1) * radius(L);
            [J JP] = SphericalBessel_JJP(mode, x);
            % Ruck, et. al. (3.4-23)
            P = (J ./ JP);
            % Ruck, et. al. (3.4-26), at innermost interface
            Z(L,:) = eta(L + 1) * P;
            % Ruck, et. al. (3.4-26), at innermost interface
            Y(L,:) = P / eta(L + 1);
        end
    else
        x1 = k0 * m(L + 1) * radius(L);
        x2 = k0 * m(L + 1) * radius(L + 1);
        [J1 JP1] = SphericalBessel_JJP(mode, x1);
        [H1 HP1] = SphericalHankel_HHP(mode, 2, x1);
        [J2 JP2] = SphericalBessel_JJP(mode, x2);
        [H2 HP2] = SphericalHankel_HHP(mode, 2, x2);
        % Ruck, et. al. (3.4-24)
        U = JP2 .* HP1 ./ (JP1 .* HP2);
        % Ruck, et. al. (3.4-25)
        V = (J2 .* H1) ./ (J1 .* H2);
        % Ruck, et. al. (3.4-23)
        P1 = J1 ./ JP1;
        % Ruck, et. al. (3.4-23)
        P2 = J2 ./ JP2;
        % Ruck, et. al. (3.4-23)
        Q2 = H2 ./ HP2;
        if (L == numIFC - 1) && (isPEC == 1)
            % Ruck, et. al. (3.4-27) for PEC boundary condition
            Z(L,:) = eta(L + 1) * P1 .* (1.0 - V) ./ (1.0 - U .*  P2 ./ Q2);
            % Ruck, et. al. (3.4-28) for PEC boundary condition
            Y(L,:) = (P1 / eta(L + 1)) .* (1.0 - V .*  Q2 ./ P2) ./ (1.0 - U);
        else
            % Ruck, et. al. (3.4-27)
            Z(L,:) = eta(L + 1) * P1 .* (1.0 - V .*((1.0 - Z(L+1,:)./(eta(L+1)*P2))./(1.0 - Z(L+1,:)./(eta(L+1)*Q2)))) ./ ...
                (1.0 - U .* ((1.0 - eta(L+1)*P2./Z(L+1,:))./(1.0 - eta(L+1)*Q2./Z(L+1,:))));
            % Ruck, et. al. (3.4-28)
            Y(L,:) = (P1 / eta(L + 1)) .* (1.0 - V .*((1.0 - eta(L+1)* Y(L+1,:)./ P2)./(1.0 - eta(L+1)*Y(L+1,:)./Q2))) ./ ...
                (1.0 - U .* ((1.0 - P2./(eta(L+1)*Y(L+1,:)))./(1.0 - Q2./(eta(L + 1)*Y(L+1,:)))));
        end
    end

end

% Ruck, et. al. (3.4-29)
Zn = i*Z(1,:)/eta0;
% Ruck, et. al. (3.4-29)
Yn = i*eta0*Y(1,:);

x = k0*radius(1);
[J JP] = SphericalBessel_JJP(mode, x);
[H HP] = SphericalHankel_HHP(mode, 2, x);

% Ruck, et. al. (3.4-1)
An = -((i).^(mode)) .* (2*mode + 1) ./ (mode.*(mode + 1)) .* (J + i*Zn .* JP) ./ (H + i*Zn .* HP);

% Ruck, et. al. (3.4-2) - there is an error in Ruck which is fixed here
Bn = ((i).^(mode+1)) .* (2*mode + 1) ./ (mode.*(mode + 1)) .* (J + i*Yn .* JP) ./ (H + i*Yn .* HP);

    function [J JP] = SphericalBessel_JJP(mode, x)

        % compute spherical bessel functions and their derivatives

        s = sqrt(0.5*pi/x);

        [J] = besselj(mode + 1/2, x); J = J*s;
        [J2] = besselj(mode - 1/2, x); J2 = J2*s;

        JP = (x * J2 - mode .* J);
        J = x*J;

    end

    function [H HP] = SphericalHankel_HHP(mode, arg, x)

        % compute spherical hankel functions and their derivatives

        s = sqrt(0.5*pi/x);

        [H] = besselh(mode + 1/2, arg, x); H = H*s;
        [H2] = besselh(mode - 1/2, arg, x); H2 = H2*s;

        HP = (x * H2 - mode .* H);
        H = x*H;
    end

end


function x = box_behnken ( dim_num, x_num, range )

%*****************************************************************************80
%
%% BOX_BEHNKEN returns a Box-Behnken design for the given number of factors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    George Box, Donald Behnken,
%    Some new three level designs for the study of quantitative variables,
%    Technometrics,
%    Volume 2, pages 455-475, 1960.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer X_NUM, the number of elements of the design.
%    X_NUM should be equal to DIM_NUM * 2**(DIM_NUM-1) + 1.
%
%    Input, real RANGE(DIM_NUM,2), the minimum and maximum
%    value for each component.
%
%    Output, real X(DIM_NUM,X_NUM), the elements of the design.
%

%
%  Ensure that the range is legal.
%
  if ( any ( range(1:dim_num,2) <= range(1:dim_num,1) ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BOX_BEHNKEN - Fatal error!\n' );
    fprintf ( 1, '  For some index I,\n' );
    fprintf ( 1, '  RANGE(I,2) <= RANGE(I,1).\n' );
    error ( 'BOX_BEHNKEN - Fatal error!' );
  end
%
%  The first point is the center.
%
  j = 1;

  x(1:dim_num,j) = ( range(1:dim_num,1) + range(1:dim_num,2) ) / 2.0;
%
%  For subsequent elements, one entry is fixed at the middle of the range.
%  The others are set to either extreme.
%
  for i = 1 : dim_num

    j = j + 1;

    x(1:dim_num,j) = range(1:dim_num,1);
    x(i,j) = ( range(i,1) + range(i,2) ) / 2.0;
%
%  The next element is made by finding the last low value, making it
%  high, and all subsequent high values low.
%
    while ( 1 )

      last_low = -1;

      for i2 = 1 : dim_num
        if ( x(i2,j) == range(i2,1) )
          last_low = i2;
        end
      end

      if ( last_low == -1 )
        break
      end

      j = j + 1;
      x(1:dim_num,j) = x(1:dim_num,j-1);
      x(last_low,j) = range(last_low,2);

      for i2 = last_low + 1 : dim_num
        if ( x(i2,j) == range(i2,2) )
          x(i2,j) = range(i2,1);
        end
      end

    end

  end

  return
end


function lm = gallager_ach(n, epsil, P)
% returns log M achievable by gallager's random coding.
%
% Note: In the case of uncostrained input gallager's bound can be written as
%
%  P_e <= M^s   V(s)^n
%
% Where V(s) = (   Sum_y [ Sum_x Q(x) W(y|x)^{1/1+s} ]^(1+s)   )
%
% Thus we could rewrite the bound as a lower bound on $M$ directly.
%
% For AWGN we can do the same in the case when we do not restrict codewords to a narrow shell
% around the power sphere. I did that and here is the result:
%
%   gallager_ach(3000, 1e-6, 1) = 1225
%   -betaq_low_v2(1-1e-6, 3000, 1) = 1287
%
%   new_gallager() = 1206 --- much worse than good gallager!
%
% Here's how to compute new_gallager:
% mu = chi2cdf(n,n);
% f = @(x) -( (1+x)./x * log2(mu) + n/2 .* log2( 1 + A^2 ./ (1+x)) + log2(epsil/2) ./ x )
% [x_best f_best] = fminbnd(f, 0, 1); new_gal = -f - 1;
%
% Note epsil/2 and also that we should subtract one bit.

R_up = cap_awgn(P);
R_down = 0;

% Check if for this n gallager works at all
Pe = gallager_pe(P, R_down, n);
if Pe > epsil;
    lm = 0;
    return;
end

precision = 1e-3 * R_up / n;

while 1;
	if ((R_up - R_down) < precision)
		break;
	end
	R = (R_up + R_down) / 2;

	Pe = gallager_pe(P, R, n);
	if( Pe < epsil)
		R_down = R;
	else
		R_up = R;
	end
end

lm = n * R_down;

function [pe Er] = gallager_pe(P, R, n, deltap_force)
%
% This function returns upper bound (achievability) on P_e using
% Gallager's random coding error exponents
%
% TODO: use expurgated exponent for low rates!
%

% conversion A->P. Old versions are all in terms of ``amplitude'' A.
A = sqrt(P);

if (nargin < 4)
	deltap_force = [];
end

if (R == 0)
	pe = 0;
	return;
end

Rcr = 1/2 * log2( 1/2 + A^2/4 + 1/2 * sqrt(1+A^4/4) );

% convert rate to nats
R = R / log2(exp(1));
Rcrn = Rcr / log2(exp(1));

if (R >= Rcrn)
	beta = exp(2*R);
	ro = A^2/(2*beta) * (1 + sqrt(1+ 4*beta/A^2/(beta-1) ) ) - 1;

	Er = A^2/(4*beta) * ...
		( (beta+1) - (beta-1)*sqrt(1 + 4*beta/A^2/(beta-1)) ) + ...
		1/2 * log(  beta - A^2*(beta-1)/2 * ( sqrt(1 + 4*beta/A^2/(beta-1)) - 1 ) );
else
	beta = 1/2 * (1 + A^2/2 + sqrt(1+A^4/4) );
	ro = 1;

	Er = 1 - beta + A^2/2 + 1/2 * log(beta - A^2/2) + 1/2*log(beta) - R;
end;

s = ro*A^2/(2 * (1+ro)^2 * beta);

%%
%% Now instead of maximizing over delta's we choose delta = 1/s  (see Gallager 7.4.39)
%% This is suboptimal, but let it be...
%%

if (max(size(deltap_force)) > 0)
	deltap = deltap_force * n;
else
	if (s == 0)
		deltap = n;
	else
		deltap = min(1/s, n);
	end;
end

%disp(sprintf('ro = %g, deltap = %g, s = %g', ro, deltap, s));

ccdfs = chi2cdf([n-deltap n], n);

mu = ccdfs(2) - ccdfs(1);
if (mu < 1e-10 * ccdfs(1))
	mu_new = quad(@(x)( chi2pdf(x, n) ), n - deltap, n);
	disp(sprintf( [	'--- gallager_pe: computing mu using quad()\n' ...
			'    `--> mu_new = %.3g, mu_old = %.3g (diff = %.2f%%)'], ...
			mu_new, mu, 200*abs(mu_new - mu)/(mu_new + mu)));
	mu = mu_new;
end;

multip = 2*exp(s*deltap)/mu;
multip_approx = ro*A^2*exp(1)*sqrt(4*pi*n)/(1+ro)^2 /beta;

%	disp(sprintf( [	'--- gallager_pe: multiplier comparsion\n' ...
%			'    `--> multip = %.3g, approx = %.3g (diff = %.2f%%)'], ...
%			multip, multip_approx, 100*abs(multip - multip_approx)/(multip)));

pe = multip*exp(-n*Er);

if(pe > 1) pe = 1; end;


function y=jinc(r)
% y=jinc(r) returns the jinc function [J1(2*pi*r)/(2*pi*r)] evaluated
% at r. Per this definition, the first zero of jinc function is at 0.61.

y=real(2*besselj(1,2*pi*r)./ (2*pi*r));
%Imaginary parts due to numerical error are of order 1E-16.

%Division by zero leads to NaN.
y(isnan(y))=1;

end


function c = mvnvbcost(x,Covx,mu,Covmu,pCholCov,pLogDetCov)
% c = mvnvbcost(x,Covx,mu,Covmu,pCholCov,pLogDetCov)
%
% Calculates c = -KL(q||p) = <log p(X)> - <log q(X)>
% where the expectation <.> is taken over q(X). This is used for
% calculating the lower bound of the marginal loglikelihood in VB
% models.
%
% p(X) = N(M,T)
% q(X) = N(x,Covx)
%
% Rest of the parameters are defined as:
% q(M) = N(mu,Covmu)
% <inv T> = inv(pCholCov * pCholCov')
% <log det T> = pLogDetCov
%
% If Covx==[], then the term <log q(X)> is not calculated.
% This is useful when X is, e.g., observations.

c = 0;
d = length(x);

% Cost from q-posterior
if ~isempty(Covx)
  % TODO:
  % if ~isempty(pCholCov), entropy = mvnentropy(pCholCov,true); else.... end
%  entropy = mvnentropy(pCholCov,true);
  entropy = mvnentropy(Covx);
  c = entropy;
else
  Covx = 0;
end

% Use Cholesky of the prior covariance
% $$$ opts.UT = true;
% $$$ opts.TRANSA = true;
z = solve_tril(pCholCov, x-mu);

% Cost from prior

% Below: (x-mu)' * Cov^(-1) * (x-mu) + trace(Cov^(-1)*(Covx+Covmu))
err2 = z'*z + trace(solve_triu(pCholCov',solve_tril(pCholCov,Covx+Covmu)));
%err2 = (x-mu)'*pInvCov*(x-mu) + (Covx(:)+Covmu(:))'*pInvCov(:);
%((x-mu).^2 + diag(Covx) + diag(Covmu));
c = c - 0.5*pLogDetCov - 0.5*err2 - 0.5*d*log(2*pi);


function d = det3x3(x)

% DET3X3 computes determinant of matrix x, using explicit analytic definition
% if size(x) = [3 3 K M]

% Copyright (C) 2017, Donders Centre for Cognitive Neuroimaging, Nijmegen, NL
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

siz = size(x);
if all(siz(1:2)==3)
  d = x(1,1,:,:).*x(2,2,:,:).*x(3,3,:,:) - ...
      x(1,1,:,:).*x(2,3,:,:).*x(3,2,:,:) - ...
      x(1,2,:,:).*x(2,1,:,:).*x(3,3,:,:) + ...
      x(1,2,:,:).*x(2,3,:,:).*x(3,1,:,:) + ...
      x(1,3,:,:).*x(2,1,:,:).*x(3,2,:,:) - ...
      x(1,3,:,:).*x(2,2,:,:).*x(3,1,:,:);
else
  ft_error('not implemented');
  % write for loop for the higher dimensions, using normal inv
end


% Author:
% - Mehrtash Harandi (mehrtash.harandi at gmail dot com)
%
% This file is provided without any warranty of
% fitness for any purpose. You can redistribute
% this file and/or modify it under the terms of
% the GNU General Public License (GPL) as published
% by the Free Software Foundation, either version 3
% of the License or (at your option) any later version.

function outS = Stein_Divergence(X1,X2)
l1 = size(X1,3);
l2 = size(X2,3);

outS = zeros(l1,l2);
for tmpC1 = 1:l1
    for tmpC2 = 1:l2
        X = X1(:,:,tmpC1);
        Y = X2(:,:,tmpC2);

        %outS(tmpC1,tmpC2) = log(det(0.5*(X+Y))) -   0.5*log(det(X*Y)); % Comment outed by HK

        S = log(det(0.5*(X+Y))) -   0.5*log(det(X*Y));

        real_flag = isreal(S);
        if real_flag
            outS(tmpC1,tmpC2) = S;
        else
            outS(tmpC1,tmpC2) = real(S);
        end

        if  (outS(tmpC1,tmpC2) < 1e-10)
            outS(tmpC1,tmpC2) = 0.0;
        end
    end
end


function zz=lpcss2zz(ss)
%LPCSS2ZZ Convert s-place poles to z-plane poles ZZ=(SS)
%the s-plane is in units of Normalized Hz and so the imaginary part
% of each ss() value is in the range +-0.5
%
% If you multiply ss by the sample frequency, a formant with
% frequency f and bandwidth b will give an s-plane pole-pair
% of approximately -b/2 +- j f

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

zz=exp(2*pi*ss);


function [Aeq, beq] = getAbeq(n_seg, n_order, ts, start_cond, end_cond)
    n_coeff = n_order+1
    n_all_poly = n_seg*(n_coeff);
    %#####################################################
    % STEP 2.1 p,v,a constraint in start
    Aeq_start = zeros(3, n_all_poly);
    beq_start = zeros(3, 1);

    d1 = n_order;
    d2 = n_order * (n_order - 1);
    Aeq_start(1:1:3, 1:1:n_coeff) = [  1,    0,  0, 0, 0, 0, 0, 0;
                                       -d1,   d1,  0, 0, 0, 0, 0, 0;
                                       d2, -2*d2, d2, 0, 0, 0, 0, 0];
    beq_start = start_cond';% p,v,a

    %#####################################################
    % STEP 2.2 p,v,a constraint in end
    Aeq_end = zeros(3, n_all_poly);
    Aeq_end = zeros(3, 1);
    Aeq_end(1:1:3, n_all_poly-n_order:1:n_all_poly) = [0, 0, 0, 0, 0,  0,     0,   1;
                                                       0, 0, 0, 0, 0,  0,   -d1,  d1;
                                                       0, 0, 0, 0, 0, d2, -2*d2,  d2];
    beq_end = end_cond';% p,v,a

    %#####################################################
    % Init array for continuity constrains
    Aeq_con_p = zeros(n_seg-1, n_all_poly);
    beq_con_p = zeros(n_seg-1, 1);
    Aeq_con_v = zeros(n_seg-1, n_all_poly);
    beq_con_v = zeros(n_seg-1, 1);
    Aeq_con_a = zeros(n_seg-1, n_all_poly);
    beq_con_a = zeros(n_seg-1, 1);

    for con_index = 1:n_seg-1
        index = n_coeff + n_coeff * (con_index - 1);

        % STEP 2.3 position continuity constrain between 2 segments
        Aeq_con_p(con_index,index) = 1;% the end of previous segment
        Aeq_con_p(con_index,index+1) = -1;% the begin of next segment

        % STEP 2.4 velocity continuity constrain between 2 segments
        Aeq_con_v(con_index,index-1:index) = [-d1, d1];% the end of previous segment
        Aeq_con_v(con_index,index+1:index+2) = -[-d1, d1];% the begin of next segment

        % STEP 2.5 acceleration continuity constrain between 2 segments
        Aeq_con_a(con_index,index-2:index) = [d2, -2*d2, d2];% the end of previous segment
        Aeq_con_a(con_index,index+1:index+3) = -[d2, -2*d2, d2];% the begin of next segment
    end

    %#####################################################
    % combine all components to form Aeq and beq
    Aeq_con = [Aeq_con_p; Aeq_con_v; Aeq_con_a];
    beq_con = [beq_con_p; beq_con_v; beq_con_a];
    Aeq = [Aeq_start; Aeq_end; Aeq_con];
    beq = [beq_start; beq_end; beq_con];
end


function [node,elem,interface] = interfacemeshdoc(box,phi,h)
%% INTERFACEMESHDOC doc for interfacemesh
%
% [node,elem,interfaceData] = INTERFACEMESH(box,phi,h) generates an
% interface-fitted mesh [node,elem]. The box is a rectangle containing the
% interface and the interface is defined as the zero level set of function
% phi. The parameter h is the size of the backgroud uniform mesh.
%
% Information on the discrete interface is stored in the output
% interface which includes:
%   - interface.vSign: sign of phi at vertices;
%        1: outside;  -1: inside;  0: on the interface
%   - interface.tElem: traingles near the interface;
%   - interface.sElem: traingles away from the interface;
%   - interface.edge: edges approximating the interface;
%   - interface.node: vertices on the interface;
%
%  Examples
%   % circle
%     box = [ -1, 1, -1, 1];
%     h = 0.1;
%     phi = @(p) sum(p.^2, 2) - 0.5.^2;
%     [node,elem,interfaceData] = interfacemeshdoc(box,phi,h);
%
%    % heart
%     box = [ -1, 1, -1, 1];
%    [node,elem,interface] = interfacemeshdoc(box,@phiheart,0.05);
%    showmesh(node,elem);
%    findedge(node,interface.edge,'all','noindex','draw');
%
%
% Author: Huayi Wei <weihuayi@xtu.edu.cn>, and Long Chen.
%
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Construct the initial structure mesh
[node,elem,T] = squarequadmesh(box,h);
edge = T.edge;
edge2elem = T.edge2elem;
clear T;
N = size(node,1);
NT = size(elem,1);
showmesh(node,elem);
pause;

%% Step 1: Find points near interface
% compute the phi value at each vertex
phiValue = phi(node);
phiValue(abs(phiValue) < eps*h) = 0;  % treat nearby nodes as on the interface
vSign = sign(phiValue);
findnode(node,vSign==0);

% Find the intersection points between edges and the interface
isCutEdge = (vSign(edge(:,1)).* vSign(edge(:,2))<0);
findedge(node,edge,isCutEdge,'noindex','draw');
A = node(edge(isCutEdge,1),:);
B = node(edge(isCutEdge,2),:);
cutNode = findintersectbisect(phi,A,B);
Ncut = size(cutNode, 1);
vSign(N+1:N+Ncut) = 0;
findnode(cutNode,'all','noindex','color','k','MarkerSize',12);
pause;

% find interface elem and nodes
isInterfaceElem = false(NT,1);
isInterfaceElem(edge2elem(isCutEdge,[1,2])) = true;
isInterfaceElem(sum(abs(vSign(elem)), 2) < 3) = true; % 2 vertices on interface
findelem(node,elem,isInterfaceElem,'noindex');

isInterfaceNode = false(N,1);
isInterfaceNode(elem(isInterfaceElem,:)) = true;
findnode(node,isInterfaceNode,'noindex');
pause;

% add centers of special elements (two vertices on the interface)
%  0 - -1    -1 - 0     0 -  0      1 - 0
%  1 -  0     0 - 1     1 - -1      1 - 0
isSpecialElem = (sum(abs(vSign(elem)),2) <= 2); % two or more vertices on interfaces
% isSpecialElem = (sum(vSign(elem),2) == 0) & (sum(abs(vSign(elem)),2) == 2);
specialElem = elem(isSpecialElem,:);
auxPoint = (node(specialElem(:,1),:) + node(specialElem(:, 3),:))/2.0;
Naux = size(auxPoint,1);
vSign(N+Ncut+1:N+Ncut+Naux) = sign(phi(auxPoint));
% find the first two cases
isDiagInterface = (vSign(specialElem(:,1)).*vSign(specialElem(:,3)) == -1) | ...
                  (vSign(specialElem(:,2)).*vSign(specialElem(:,4)) == -1);
vSign(N+Ncut+find(isDiagInterface)) = 0;
findnode(auxPoint,'all','noindex','color','m');
pause;

% add cut points and aux points
node = [node; cutNode; auxPoint];
nearInterfaceNode = [node(isInterfaceNode,:); cutNode; auxPoint];
interfaceNodeIdx = [find(isInterfaceNode); N+(1:Ncut)'; N+Ncut+(1:Naux)'];
% figure(2);
% showmesh(node,elem);
findnode(nearInterfaceNode,'all','noindex');
pause;

%% Step 2: generate a Delaunay triangulation of interface points
% construct the Delaunay triangulation of interfaceNode
% different versions of matlab using different delaunay triangulation
matlabversion = version();
if str2double(matlabversion(end-5:end-2)) <= 2013
    DT = DelaunayTri(nearInterfaceNode); %#ok<*DDELTRI>
    tElem = DT.Triangulation;
else
    DT = delaunayTriangulation(nearInterfaceNode);
    tElem = DT.ConnectivityList;
end
tElem = fixorder(nearInterfaceNode,tElem); % correct the orientation
% showmesh(node,tElem);

%% Step 3: Post-processing
% get rid of the unnecessary triangles
NI = sum(isInterfaceNode);  % number of pts of the background mesh near interface
haveNewPts = (sum(tElem > NI,2) > 0); % triangles containing new added vertices
% vSignNearInterface = zeros(size(nearInterfaceNode,1),1);
% vSignNearInterface(1:sum(isInterfaceNode)) = vSign(isInterfaceNode);
% haveInterfacePts = (sum(abs(vSignNearInterface(tElem)),2) < 3);
tElem = tElem(haveNewPts,:);
% tElem = tElem(haveInterfacePts,:);
tElem = interfaceNodeIdx(tElem);  % map interfaceNode index to node index
showmesh(node,tElem,'Facecolor','y');
findnode(node,vSign==0,'noindex');
pause;

% get the remainding quad elems
sElem = elem(~isInterfaceElem,:);
% merge into one triangulation
elem = [tElem; sElem(:,[2 3 1]); sElem(:,[4 1 3])];
T = auxstructure(tElem);
showmesh(node,elem); hold on;
showmesh(node,tElem,'Facecolor','y');
isInterfaceEdge = ((vSign(T.edge(:,1)) == 0) & vSign(T.edge(:,2)) == 0);
interfaceEdge = T.edge(isInterfaceEdge,:);
nearInterfaceNode = find(vSign == 0);
findedge(node,interfaceEdge,'all','noindex','draw');
pause;

%% Generate interfaceData
interface.vSign = vSign;
interface.tElem = tElem;
interface.sElem = sElem;
interface.edge = interfaceEdge;
interface.node = nearInterfaceNode;


function r8poly_print ( n, a, title )

%*****************************************************************************80
%
%% R8POLY_PRINT prints out a polynomial.
%
%  Discussion:
%
%    The power sum form is:
%
%      p(x) = a(0) + a(1) * x + ... + a(n-1) * x^(n-1) + a(n) * x^(n)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 July 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of A.
%
%    Input, real A(1:N+1), the polynomial coefficients.
%    A(1) is the constant term and
%    A(N+1) is the coefficient of X^N.
%
%    Input, character TITLE(*), an optional title.
%
  if ( 0 < s_len_trim ( title ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, '%s\n', title );
  end

  fprintf ( 1, '\n' );

  n = r8poly_degree ( n, a );

  if ( a(n+1) < 0.0 )
    plus_minus = '-';
  else
    plus_minus = ' ';
  end

  mag = abs ( a(n+1) );

  if ( 2 <= n )
    fprintf ( 1, '  p(x) = %c%14f * x^%d\n', plus_minus, mag, n );
  elseif ( n == 1 )
    fprintf ( 1, '  p(x) = %c%14f * x\n', plus_minus, mag );
  elseif ( n == 0 )
    fprintf ( 1, '  p(x) = %c%14f\n', plus_minus, mag );
  end

  for i = n-1 : -1 : 0

    if ( a(i+1) < 0.0 )
      plus_minus = '-';
    else
      plus_minus = '+';
    end

    mag = abs ( a(i+1) );

    if ( mag ~= 0.0E+00 )

      if ( 2 <= i )
        fprintf ( 1, '         %c%14f * x^%d\n', plus_minus, mag, i );
      elseif ( i == 1 )
        fprintf ( 1, '         %c%14f * x\n', plus_minus, mag );
      elseif ( i == 0 )
        fprintf ( 1, '         %c%14f\n', plus_minus, mag );
      end
    end

  end

  return
end


function [assignment, cost] = assignmentsuboptimal2(distMatrix)
%ASSIGNMENTSUBOPTIMAL2    Compute suboptimal assignment
%   ASSIGNMENTSUBOPTIMAL2(DISTMATRIX) computes a suboptimal assignment
%   (minimum overall costs) for the given rectangular distance or cost
%   matrix, for example the assignment of tracks (in rows) to observations
%   (in columns). The result is a column vector containing the assigned
%   column number in each row (or 0 if no assignment could be done).
%
%   [ASSIGNMENT, COST] = ASSIGNMENTSUBOPTIMAL2(DISTMATRIX) returns the
%   assignment vector and the overall cost.
%
%   The algorithm searches the matrix for the minimum element and makes the
%   corresponding row-column assignment. After setting all elements in the
%   given row and column to infinity (i.e. forbidden assignment), the
%   search procedure is repeated until all assignments are done or only
%   infinite values are found.
%
%   This function and the corresponding mex-function can further be
%   improved by first sorting all elements instead of searching for the
%   minimum of all elements many times.
%
%   assignment.html  File Exchange  Donate via PayPal
%
%   Markus Buehren
%   Last modified 05.07.2011

% initialize
nOfRows    = size(distMatrix, 1);
assignment = zeros(nOfRows,1);
cost       = 0;

for n=1:nOfRows

  % find minimum distance observation-to-track pair
  [minDist, index1] = min(distMatrix, [], 1);
  [minDist, index2] = min(minDist);
  row = index1(index2);
  col = index2;

  if isfinite(minDist)

    % make the assignment
    assignment(row) = col;
    cost = cost + minDist;

    % delete observation-to-track pair
    distMatrix(row, :) = inf;
    distMatrix(:, col) = inf;

  else
    return
  end

end