function demoMultipleModelFiltering()
%%DEMOMULTIPLEMODELFILTERING This demonstrates how multiple model filters
%           can be used. The use of multiple dynamic models can improve the
%           accuracy of tracking targets than can maneuver. The scenario
%           considered is the simple 2D tracking of an aircraft. The
%           separate problems of data association, scheduling, track
%           initiation and termination are not addressed here.
%
%The example scenario is the air traffic control (ATC) scenario discussed
%in Chapter 11.7.4 of [1]. However, rather than using a fixed mode
%transition probability matrix, the use of the function
%getMarkovPTransProbMat to get the mode transition probability matrix given
%mean sojorn times and the time between samples is demonstrated. This
%allows the algorithms to be used with variable sampling rates, though in
%this instance, the sampling rate is held constant.
%
%Six scenarios are considered. One is a baseline Kalman filter with a
%polynomial dynamic model and a large process noise. The second is a Kalman
%filter with a first-order Gauss-Markov model (the integrated Ornstein-
%Uhlenbeck model), which, unlike the standard polynomial model, includes a
%correlation constant to try to better cover turns. The third is the
%reduced state filter. The fourth is the separated covariance filter. The
%other two scenarios utilize the interacting multiple model (IMM) filter.
%The first IMM scenario is as in [1], where two nearly constant velocity
%dynamic models with differing process noises are used in a standard Kalman
%filter (with the IMM). The second IMM scenario is where a nearly constant
%velocity model in a Kalman filter is used as well as a coordinated turn
%model in an extended Kalman filter (EKF). The functions multipleModelPred
%and multipleModelUpdate are used to handle the prediction and mixing in
%the IMM, though a change of the algorithm selection would allow one to
%just as easily use a generalized pseudo-Bayesian 2 filter. The IMM
%estimators can be more difficult to use as there are more parameters to
%tune.
%
%The measurements are generated in Cartesian space as measurement
%conversion and tracking using non-Cartesian measurements is not the focus
%of this example.
%
%REFERENCES:
%[1] Y. Bar-Shalom, X. R. Li, and T. Kiruabarajan, Estimation with
%    Applications to Tracking and Navigation. New York: Wiley Interscience,
%    2001.
%
%June 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%This can be changed to try other multiple model algorithms. If GPB1 is
%selected, the multiple model filter using the coordinate dturn model is
%not used, because the GPB1 requires that all models have the same
%dimensionality.
AlgSel='GPB2';

%Interval between samples in seconds.
T=5;

%%STEP 1: Generate the simulation scenario.
disp('Generating the "true" track for the simulation scenario.')
TTotal=125+90+125+30+125;
%The extra sample is the very first step in the trajectory.
numSamples=TTotal/T+1;
xTrue=zeros(4,numSamples);

%An indicator of the maneuvering mode 1 for not maneuvering, and 2 for
%maneuvering.
mode=zeros(numSamples,1);

%The plane first flies westward at 120 m/s for 125 seconds.
xInit=[25e3;10e3; -120; 0];
xTrue(:,1)=xInit;
mode(1)=1;
numStepSeg=125/T;%The number of steps in this segment.

%The state transition matrix for constant velocity motion.
F=FPolyKal(T,4,1);

%Propagate the constant velocity model for 125 seconds.
baseStep=1;
for curStep=(baseStep+1):(baseStep+numStepSeg)
    xTrue(:,curStep)=F*xTrue(:,curStep-1);
    mode(curStep)=1;
end
baseStep=baseStep+numStepSeg;

%Next, the plane performs a 1 degree per second coordinated turn for 90
%seconds.
turnRate=1*(pi/180);%Convert from degrees per second to radians per second.

numStepSeg=90/T;
for curStep=(baseStep+1):(baseStep+numStepSeg)
    %Get the discrete-time propagation matrix for the state. The
    %FCoordTurn2D function assumes that the turn rate is part of the state.
    %In this instance, we just add it to the state, and then remove the
    %extra elements from the returned matrix.
    F=FCoordTurn2D(T,[xTrue(:,curStep-1);turnRate]);
    F=F(1:4,1:4);

    xTrue(:,curStep)=F*xTrue(:,curStep-1);
    mode(curStep)=2;
end
baseStep=baseStep+numStepSeg;

%Next, go straight (South after the previous turn) for another 125 seconds.
numStepSeg=125/T;%The number of steps in this segment.

%The state transition matrix for constant velocity motion.
F=FPolyKal(T,4,1);

for curStep=(baseStep+1):(baseStep+numStepSeg)
    xTrue(:,curStep)=F*xTrue(:,curStep-1);
    mode(curStep)=1;
end
baseStep=baseStep+numStepSeg;

%Make another turn. This time, it is at a rate of -three degrees per second
%for 30 seconds.
turnRate=-3*(pi/180);%Degrees per second to radians per second.

numStepSeg=30/T;%The number of steps in this segment.
for curStep=(baseStep+1):(baseStep+numStepSeg)
    F=FCoordTurn2D(T,[xTrue(:,curStep-1);turnRate]);
    F=F(1:4,1:4);

    xTrue(:,curStep)=F*xTrue(:,curStep-1);
    mode(curStep)=2;
end
baseStep=baseStep+numStepSeg;

%Finally, go straight again for another 125 seconds.
numStepSeg=125/T;%The number of steps in this segment.

%The state transition matrix for constant velocity motion.
F=FPolyKal(T,4,1);

for curStep=(baseStep+1):(baseStep+numStepSeg)
    xTrue(:,curStep)=F*xTrue(:,curStep-1);
    mode(curStep)=1;
end

%%STEP 2: Generate measurements.
disp('Generating the measurements for the simulation; A simple Cartesian model is used.')
%The measurements are just Cartesian with 100 meters of noise added per
%dimension, with no correlation between them.
H=[1,0,0,0;
   0,1,0,0];%Measurement matrix

SR=[100, 0;
    0,  100];%Square root covariance matrix.
R=SR*SR';%The covariance matrix.

z=zeros(2,numSamples);
for curSamp=1:numSamples
    z(:,curSamp)=H*xTrue(:,curSamp)+SR*randn(2,1);
end

%Plot the trajectory and the measurements
disp('Plotting the trajectory the measurements, and the true maneuver mode.')
figure(1)
clf
hold on
plot(xTrue(1,:),xTrue(2,:),'-b','linewidth',2)
scatter(z(1,:),z(2,:),'.k')

h1=xlabel('Meters West->East');
h2=ylabel('Meters South->North');
h3=title('The True Trajectory and the Detections');
set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h3,'FontSize',14,'FontWeight','bold','FontName','Times')

%Plot when it is in Mode 2, 1 for being in it and 2 for not.
figure(2)
clf
hold on
plot(mode-1,'-k')
h1=xlabel('Discrete Step');
h2=ylabel('Maneuver mode');
h3=title('Indicating when the target is performing maneuvers.');
set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h3,'FontSize',14,'FontWeight','bold','FontName','Times')

%STEP 3: Get the baseline scenario: Run a Kalman filter with a standard
%first-order white-noise process model, a Kalman filter with a first-order
%Gauss-Markov model, a reduced state estimator, and the
%separated covariance filter.
disp('Computing the baseline scenarios with just a Kalman filter and with the')
disp('reduced state estimator.')

%A direct-discrete model with a 1m/2s^2 process noise standard deviation is
%used (normally, a discretized model is preferred, because it is
%more consistent when using a variable sampling rate, and the covariance
%matrix is not singular).
Q=QPolyKalDirectDisc(T,4,1,1^2);%1m/s^2 process noise

%The state transition matrix for constant velocity motion.
F=FPolyKal(T,4,1);

%Parameters for the Gauss-Markov dynamic model. We are using a first-order
%Gauss-Markov model.
tau=20;%20 seconds; the assumed maneuver decorrelation time.
maxAccel=9.8*3;%Assume max 3G turn
%Rule of thumb- process noise suggestion.
q=processNoiseSuggest('PolyKal-ROT',maxAccel,T);
QGM=QGaussMarkov(T,4,q,tau,1);%Process noise matrix.
FGM=FGaussMarkov(T,4,tau,1);%State Transition matrix

%Allocate space for the polynomial Kalman filter estimates
xKalman=zeros(4,numSamples);%The states
PKalman=zeros(4,4,numSamples);%The covariance matrices

%Allocate space for the Gauss-Markov Kalman filter estimates
xKalmanGM=zeros(4,numSamples);%The states
PKalmanGM=zeros(4,4,numSamples);%The covariance matrices

%Allocate space for the reduced state estimator.
xRedState=zeros(4,numSamples);%The states

%Allocate space for the separated covariance filter.
xSCFState=zeros(4,numSamples);%The states

%We are using the version of the reduced state estimator that needs a
%maximum linear acceleration (directed along the direction of motion of the
%target) for the target along with a maximum turn rate acceleration.
%A 0.5m/s^2 maximum linear acceleration with 1 deg/s^2 maximum turn rate
%acceleration is used.
ARedState=0.5;
OmegaRedState=1*(pi/180);

%The separated covariance filter needs a matrix introducing the effects of
%the maximum acceleration onto the state (for covariance computation
%results. For a state consisting of position and velocity in 2D, this can
%be
aMax=9.8/2;
Ba=[T^2/2,   0;
    0,       T^2/2;
	T,       0;
	0,       T]*aMax;
%where we chose the maximum acceleration in each dimension to be 0.5G
%(1G=9.8m/s^2).

%Track initiation is by one-point differencing using just assumed maximum
%values as the standard deviations for the velocity covariance matrix. The
%assumed maximum value is 300m/s.
[xKalman(:,1),PKalman(:,:,1)]=onePointCartInit(z(:,1),SR,300);
xKalmanGM(:,1)=xKalman(:,1);
PKalmanGM(:,1)=PKalman(:,1);

%The reduced state estimator keeps the covariance matrix broken into parts
%M and D, which will be propagated and updated.
xRedState(1:2)=z(:,1);
M=PKalman(:,:,1);
D=zeros(4,2);

%The separated covariance filter. It has been chosen to start the filter
%with zero lag and all uncertainty in the covariance matrix.
xSCFState(1:2)=z(:,1);
PSCF=PKalman(:,:,1);
LSCF=zeros(4,2);

absErrKalman=zeros(numSamples,1);%Allocate space
absErrKalman(1)=norm(xKalman(1:2,1)-xTrue(1:2,1));

absErrKalmanGM=zeros(numSamples,1);%Allocate space
absErrKalmanGM(1)=norm(xKalman(1:2,1)-xTrue(1:2,1));

absErrRedState=zeros(numSamples,1);%Allocate space
absErrRedState(1)=norm(xRedState(1:2,1)-xTrue(1:2,1));

absErrSCF=zeros(numSamples,1);%Allocate space
absErrSCF(1)=norm(xSCFState(1:2,1)-xTrue(1:2,1));

%Run the filter
for curSamp=2:numSamples
    %The Kalman filter with the polynomial model
    %Predict the state forward
    [xPred, PPred]=discKalPred(xKalman(:,curSamp-1),PKalman(:,:,curSamp-1),F,Q);
    %Update the state with a measurement
    [xKalman(:,curSamp),PKalman(:,:,curSamp)]=KalmanUpdate(xPred,PPred,z(:,curSamp),R,H);

    absErrKalman(curSamp)=norm(xKalman(1:2,curSamp)-xTrue(1:2,curSamp));

    %The Kalman filter with the Gauss-Markov model
    [xPred, PPred]=discKalPred(xKalmanGM(:,curSamp-1),PKalmanGM(:,:,curSamp-1),FGM,QGM);
    %Update the state with a measurement
    [xKalmanGM(:,curSamp),PKalmanGM(:,:,curSamp)]=KalmanUpdate(xPred,PPred,z(:,curSamp),R,H);

    absErrKalmanGM(curSamp)=norm(xKalmanGM(1:2,curSamp)-xTrue(1:2,curSamp));

    %The reduced state estimator.
    %Predict the state forward
    modParams=[];
    modParams.T=T;
    modParams.A=ARedState;
    modParams.Omega=OmegaRedState;
    [xPred,M,D,PPred]=reducedStateDiscPred(xRedState(:,curSamp-1),M,D,[],[],'GenTurn',modParams);
    %Update the state with a measurement
    [xRedState(:,curSamp),M,D]=reducedStateUpdate(xPred,PPred,M,D,z(:,curSamp),R);

    absErrRedState(curSamp)=norm(xRedState(1:2,curSamp)-xTrue(1:2,curSamp));

    %The separated covariance filter.
    %Predict the state forward
    [xPred,LSCF,TSCF]=separatedCovDiscPred(xSCFState(:,curSamp-1),PSCF,LSCF,F,Ba);
    %Update the state with a measurement
    [xSCFState(:,curSamp),LSCF,PSCF]=separatedCovUpdate(xPred,LSCF,TSCF,z(:,curSamp),R,H);
    absErrSCF(curSamp)=norm(xSCFState(1:2,curSamp)-xTrue(1:2,curSamp));
end

figure(1)
hold on
plot(xKalman(1,:),xKalman(2,:),'-g','linewidth',2)
plot(xKalmanGM(1,:),xKalmanGM(2,:),'-c','linewidth',2)
plot(xRedState(1,:),xRedState(2,:),'--b','linewidth',2)
plot(xSCFState(1,:),xSCFState(2,:),'-.m','linewidth',2)

figure(3)
clf
hold on
plot(absErrKalman,'-g','linewidth',2)
plot(absErrKalmanGM,'-c','linewidth',2)
plot(absErrRedState,'--b','linewidth',2)
plot(absErrSCF,'-.m','linewidth',2)

h1=xlabel('Discrete Step');
h2=ylabel('Absolute distance error');
set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h3,'FontSize',14,'FontWeight','bold','FontName','Times')

%%STEP 4: Run the algorithms with the two linear models.
%The linear model pair consists of trackers using direct discrete linear
%dynamic models.
disp('Computing the IMM scenario with two linear Kalman filters with different process noises.')

%The covariance matrix for the low-noise model.
QLow=QPolyKalDirectDisc(T,4,1,0.1^2);%0.1m/s^2 process noise
%The covariance matrix for the high-noice model.
QHigh=QPolyKalDirectDisc(T,4,1,2^2);%2m/s^2 process noise.

%The propagation routines for each model
transFuns{1}=@(xPrev,PPrev)discKalPred(xPrev,PPrev,F,QLow);
transFuns{2}=@(xPrev,PPrev)discKalPred(xPrev,PPrev,F,QHigh);

%The measurement update function is the same for both models.
measUpdateFuns=@(x,P,z,R)KalmanUpdate(x,P,z,R,H);

%Allocate space for the IMM model states. This is the size of the
%largest state by the number of models, except when using the GPB1
%estimator, in which case it is just the size of the state (all states have
%to be the same size).
if(~strcmp(AlgSel,'GPB1'))
    xIMML=zeros(4,2);
    PIMML=zeros(4,4,2);

    %Initialize the states using one-point differencing. The covariance matrix
    %is set using the assumed velocity for each of the models, which is
    %300m/s^2.
    %The first model.
    [xIMML(:,1),PIMML(:,:,1)]=onePointCartInit(z(:,1),SR,300);
    %The second model has the same initialization.
    xIMML(:,2)=xIMML(:,1);
    PIMML(:,:,2)=PIMML(:,:,1);
else
    [xIMML,PIMML]=onePointCartInit(z(:,1),SR,300);
end

%Allocate space for the states to display. These will be the merged values
%from the IMM model states.
xIMMLDisp=zeros(4,numSamples);
PIMMLDisp=zeros(4,4,numSamples);

%Initially, both models have the same initialization, which will thus be
%the one to display at the first step.
xIMMLDisp(:,1)=xIMML(:,1);
PIMMLDisp(:,:,1)=PIMML(:,:,1);

%Allocate space for the mode probabilities. These are saved over all scans
%so that they can be plotted.
muMode=zeros(2,numSamples);%There is one for each model at each step.
%Initially, the mode probabilities are uniform: we do not know which mode
%it is in.
muMode(:,1)=1/2;

%The mode transition probability matrix for the two linear models must be
%determined. This is based on a matrix relating the mean sojourn times of
%the models and the time between samples.
%The mean sojourn time matrix is a design parameter. The negative inverse
%of the diagonal terms is the mean time spent in that state. Each row must
%sum to one. If the matrix were larger, the relation between the
%non-diagonal values in the rows would affect to which state it
%transitions. This is a design parameter.
A=[-0.01/2, 0.01/2;
    0.02*2, -0.02*2];

%Compute the mode transition probability matrix for the given time between
%samples. Because the sample interval is constant, this does not change
%over time.
LambdaL=getMarkovPTransProbMat(A,T);

absErrIMML=zeros(numSamples,1);%Allocate space
absErrIMML(1)=norm(xIMMLDisp(1:2,1)-xTrue(1:2,1));
%Run the filter
for curSamp=2:numSamples
   %Predict the states forward
   [xIMML,PIMML]=multipleModelPred(AlgSel,xIMML,PIMML,transFuns);

   %Update the state with a measurement
   [xIMML,PIMML,muMode(:,curSamp),xIMMLDisp(:,curSamp),PIMMLDisp(:,:,curSamp)]=multipleModelUpdate(AlgSel,xIMML,PIMML,z(:,curSamp),R,measUpdateFuns,muMode(:,curSamp-1),LambdaL);
   absErrIMML(curSamp)=norm(xIMMLDisp(1:2,curSamp)-xTrue(1:2,curSamp));
end

figure(1)
plot(xIMMLDisp(1,:),xIMMLDisp(2,:),'-r','linewidth',2)

figure(2)
plot(muMode(2,:),'-r','linewidth',2)

figure(3)
plot(absErrIMML,'-r','linewidth',2)

if(strcmp(AlgSel,'GPB1'))
    return
end

%STEP 5: Run the algorithms with the linear and the coordinated turn model.
disp('Computing the IMM scenario with a linear Kalman filter and an EKF.')

%The covariance matrix for the linear model.
QLinear=QPolyKalDirectDisc(T,4,1,0.01^2);%0.01m/s^2 process noise.

%0.025m/s^2 linear acceleration (in 3D, not just directed along the
%direction of motion of the target) with 0.5 deg/s^2 turning acceleration
%for the coordinated turn model as the standard deviations going into the
%covariance matrix. Note that these assumptions differ from those used in
%the reduced state filter. The filters should be tuned separately for a
%fair comparison. Often, improving the IMM's ability to recognize a turn
%worsens its overall track performance. Thus, a tradeoff must be present.
QCT=QCoordTurn(T,zeros(5,1),0.025^2,(0.5*(pi/180))^2);

%The propagation routines for each model
transFuns=[];
transFuns{1}=@(xPrev,PPrev)discKalPred(xPrev,PPrev,F,QLinear);
f=@(x)(FCoordTurn2D(T,x)*x);
transFuns{2}=@(xPrev,PPrev)discEKFPred(xPrev,PPrev,f,@(x)JacobCoordTurn2D(T,x),QCT);

%The measurement update function differs for the different models, because
%H is different for the coordinated turn model, due to the extra element
%in the state.
HCT=[1,0,0,0,0;
     0,1,0,0,0];
measUpdateFuns=[];
measUpdateFuns{1}=@(x,P,z,R)KalmanUpdate(x,P,z,R,H);
measUpdateFuns{2}=@(x,P,z,R)KalmanUpdate(x,P,z,R,HCT);

%The number of elements in each state and the number of dimensions of each
%state that play a role in mixing the states.
numStateDims=[4;5];
numMixDims=[4;4];

%Allocate space for the IMM model states. The number of rows is the size of
%the longest state.
xLCT=zeros(5,2);
PLCT=zeros(5,2);

%Initialize the states using one-point differencing. The covariance matrix
%is set using the assumed velocity for each of the models, which is
%300m/s^2 and the maximum turn rate, which is assumed to be 20 degrees per
%second.
%The first model.
xLCT(1:2,1)=z(:,1);
PLCT(1:2,1:2,1)=R;
PLCT(3,3,1)=300^2;
PLCT(4,4,1)=300^2;
%The second model has the same initialization.
xLCT(:,2)=xLCT(:,1);
PLCT(:,:,2)=PLCT(:,:,1);
PLCT(5,5,2)=(20*(pi/180))^2;

%Allocate space for the states to display. These will be the merged values
%from the IMM model states. The size is the size of the state with the
%least number of mixing components.
xLCTDisp=zeros(4,numSamples);
PLCTDisp=zeros(4,4,numSamples);

%Initially, both models have the same initialization, which will thus be
%the one to display at the first step.
xLCTDisp(:,1)=xLCT(1:4,1);
PLCTDisp(:,:,1)=PLCT(1:4,1:4,1);

%Allocate space for the mode probabilities. These are saved over all scans
%so that they can be plotted.
muModeLCT=zeros(2,numSamples);%There is one for each model at each step.
%Initially, the mode probabilities are uniform: we do not know which mode
%it is in.
muModeLCT(:,1)=1/2;

%Compute the mode transition probability matrix.
A=[-0.01/2, 0.01/2;
    0.02*2, -0.02*2];
LambdaCT=getMarkovPTransProbMat(A,T);

absErrLCT=zeros(numSamples,1);%Allocate space
absErrLCT(1)=norm(xLCTDisp(1:2,1)-xTrue(1:2,1));
%Run the filter
for curSamp=2:numSamples
   %Predict the states forward.
   [xLCT,PLCT]=multipleModelPred(AlgSel,xLCT,PLCT,transFuns,numStateDims,numMixDims);

   %Update the state with a measurement.
   [xLCT,PLCT,muModeLCT(:,curSamp),xLCTDisp(:,curSamp),PLCTDisp(:,:,curSamp)]=multipleModelUpdate(AlgSel,xLCT,PLCT,z(:,curSamp),R,measUpdateFuns,muModeLCT(:,curSamp-1),LambdaCT,numStateDims,numMixDims);
   absErrLCT(curSamp)=norm(xLCTDisp(1:2,curSamp)-xTrue(1:2,curSamp));
end

figure(1)
plot(xLCTDisp(1,:),xLCTDisp(2,:),'--c','linewidth',2)
legend('True Trajectory','Raw Detections','Basic Kalman Filter', 'Gauss-Markov Kalman Filter','Reduced State Filter', 'Separated Covariance Filter', 'Two Linear Models','Linear and Maneuvering Models','Location','SouthEast')

figure(2)
plot(muModeLCT(2,:),'--c','linewidth',2)
legend('True Mode', 'Two Linear Models','Linear and Maneuvering Models','Location','NorthWest')

figure(3)
plot(absErrLCT,'--c','linewidth',2)
legend('Polynomial Kalman Filter','Gauss-Markov Kalman Filter','Reduced State Filter', 'Separated Covariance Filter', 'Two Linear Models','Linear and Maneuvering Models','Location','NorthWest')
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=usasi(n,fs)
%USASI generates N samples of USASI noise at sample frequency FS X=(N,FS)

% This routine is based on the USASI noise defined in [1] which was later
% reissued as [2]. USASI noise is intended to simulate the long-term average
% of typical audio program material. The routine does not currently implement
% the pulsation at 2.5Hz 12.5% duty cycle that is recommended by the standard.
% Also it should probably be scaled to a well-defined power.
%
%  [1] NRSC AM Reemphasis, Deemphasize, and Broadcast Audio Transmission Bandwidth Specifications,
%      EIA-549 Standard, Electronics Industries Association , July 1988.
%  [2] NRSC AM Reemphasis, Deemphasize, and Broadcast Audio Transmission Bandwidth Specifications,
%      NRSC-1-A Standard, Sept 2007, Online: http://www.nrscstandards.org/SG/NRSC-1-A.pdf

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: usasi.m,v 1.6 2008/05/27 08:38:05 dmb Exp $
%
%   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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<2 fs=8000; end
b=[1 0 -1];
a=poly(exp(-[100 320]*2*pi/fs));

x=randfilt(b,a,n);


function show_curve(b)
% function show_curve(b)

if ~strcmpi(class(b),'bezier')
  error('Expecting a bezier object');
end

t=0:0.01:1;
t2=t.^2;
t3=t2.*t;
x=b.ax*t3+b.bx*t2+b.cx*t+b.x0;
y=b.ay*t3+b.by*t2+b.cy*t+b.y0;

plot(x,y,'b',[b.x0],[b.y0],'r*',...
     [b.x1],[b.y1],'ro',...
     [b.x0,b.x1],[b.y0,b.y1],'r',...
     [b.x2],[b.y2],'ro',...
     [b.x3],[b.y3],'r*',...
     [b.x2,b.x3],[b.y2,b.y3],'r');

text([b.x0],[b.y0],'(x_0,y_0)');
text([b.x1],[b.y1],'(x_1,y_1)');
text([b.x2],[b.y2],'(x_2,y_2)');
text([b.x3],[b.y3],'(x_3,y_3)');


function PlotFieldonMesh(coordinates,nodes,component)
%--------------------------------------------------------------------------
% Code written by : Siva Srinivas Kolukula                                |
%                   Senior Research Fellow                                |
%                   Structural Mechanics Laboratory                       |
%                   Indira Gandhi Center for Atomic Research              |
%                   India                                                 |
% E-mail : allwayzitzme@gmail.com                                         |
%          http://sites.google.com/site/kolukulasivasrinivas/             |
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% Purpose:
%         To plot the profile of a component on mesh
% Synopsis :
%           ProfileonMesh(coordinates,nodes,component)
% Variable Description:
%           coordinates - The nodal coordinates of the mesh
%           -----> coordinates = [X Y ]
%           nodes - The nodal connectivity of the elements
%           -----> nodes = [node1 node2......]
%           component - The components whose profile to be plotted
%           -----> components = a column vector in the order of node
%                               numbers
%--------------------------------------------------------------------------

nel = length(nodes) ;                  % number of elements
nnode = length(coordinates) ;          % total number of nodes in system
nnel = size(nodes,2);                % number of nodes per element
%
% Initialization of the required matrices
X = zeros(nnel,nel) ;
Y = zeros(nnel,nel) ;
Z = zeros(nnel,nel) ;
profile = zeros(nnel,nel) ;
%
for iel=1:nel
     for i=1:nnel
     nd(i)=nodes(iel,i);         % extract connected node for (iel)-th element
     X(i,iel)=coordinates(nd(i),1);    % extract x value of the node
     Y(i,iel)=coordinates(nd(i),2);    % extract y value of the node
     end
     profile(:,iel) = component(nd') ;         % extract component value of the node
end

% Plotting the FEM mesh and profile of the given component
     fh = figure ;
     set(fh,'name','Postprocessing','numbertitle','off') ;
     fill(X,Y,profile)
     axis off ;
     % Colorbar Setting
     SetColorbar
 end


%This Matlab script can be used to reproduce Figure 7.8 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Maximum number of pilots
pilotMax = 40;

%Define range of average number of UEs
Kaverages = [1 10 20 40];

%Define range of effective number of UEs
Krange = 0:pilotMax;

%Prepare to store simulation results
Kdistributions = zeros(length(Krange),length(Kaverages));

%% Go through all average numbers of UEs
for n = 1:length(Kaverages)

    %Compute the probabilities of having different numbers of active UEs,
    %based on a Poission distribution
    Kdistributions(:,n) = poisspdf(Krange,Kaverages(n));

    %Enforce scheduling to make sure that there cannot be more than
    %pilotMax UEs active per cell
    Kdistributions(end,n) = 1 - sum(Kdistributions(1:end-1,n));

end

%% Plot simulation results
figure;
hold on; box on;

plot(Krange,Kdistributions(:,1),'r*-','LineWidth',1);
plot(Krange,Kdistributions(:,2),'ko-','LineWidth',1);
plot(Krange,Kdistributions(:,3),'bs-','LineWidth',1);
plot(Krange,Kdistributions(:,4),'rd--','LineWidth',1);

xlabel('Number of active UEs');
ylabel('Probability');
legend('K=1  ','K=10  ','K=20  ','K=40  ','Location','NorthWest');
ylim([0 0.6]);


function [eta, Heta, inner_it, stop_tCG] ...
                 = tCG(problem, x, grad, eta, Delta, options, storedb, key)
% tCG - Truncated (Steihaug-Toint) Conjugate-Gradient method
% minimize <eta,grad> + .5*<eta,Hess(eta)>
% subject to <eta,eta>_[inverse precon] <= Delta^2
%
% See also: trustregions

% This file is part of Manopt: www.manopt.org.
% This code is an adaptation to Manopt of the original GenRTR code:
% RTR - Riemannian Trust-Region
% (c) 2004-2007, P.-A. Absil, C. G. Baker, K. A. Gallivan
% Florida State University
% School of Computational Science
% (http://www.math.fsu.edu/~cbaker/GenRTR/?page=download)
% See accompanying license file.
% The adaptation was executed by Nicolas Boumal.
%
% Change log:
%
%   NB Feb. 12, 2013:
%       We do not project r back to the tangent space anymore: it was not
%       necessary, and as of Manopt 1.0.1, the proj operator does not
%       coincide with this notion anymore.
%
%   NB April 3, 2013:
%       tCG now also returns Heta, the Hessian at x along eta. Additional
%       esthetic modifications.
%
%   NB Dec. 2, 2013:
%       If options.useRand is activated, we now make sure the preconditio-
%       ner is not used, as was originally intended in GenRTR. In time, we
%       may want to investigate whether useRand can be modifed to work well
%       with preconditioning too.
%
%   NB Jan. 9, 2014:
%       Now checking explicitly for model decrease at each iteration. The
%       first iteration is a Cauchy point, which necessarily realizes a
%       decrease of the model cost. If a model increase is witnessed
%       (which is theoretically impossible if a linear operator is used for
%       the Hessian approximation), then we return the previous eta. This
%       ensures we always achieve at least the Cauchy decrease, which
%       should be sufficient for convergence.
%
%   NB Feb. 17, 2015:
%       The previous update was in effect verifying that the current eta
%       performed at least as well as the first eta (the Cauchy step) with
%       respect to the model cost. While this is an acceptable strategy,
%       the documentation (and the original intent) was to ensure a
%       monotonic decrease of the model cost at each new eta. This is now
%       the case, with the added line: "model_value = new_model_value;".
%
%   NB April 3, 2015:
%       Works with the new StoreDB class system.

% All terms involving the trust-region radius will use an inner product
% w.r.t. the preconditioner; this is because the iterates grow in
% length w.r.t. the preconditioner, guaranteeing that we will not
% re-enter the trust-region.
%
% The following recurrences for Prec-based norms and inner
% products come from [CGT2000], pg. 205, first edition.
% Below, P is the preconditioner.
%
% <eta_k,P*delta_k> =
%          beta_k-1 * ( <eta_k-1,P*delta_k-1> + alpha_k-1 |delta_k-1|^2_P )
% |delta_k|^2_P = <r_k,z_k> + beta_k-1^2 |delta_k-1|^2_P
%
% therefore, we need to keep track of
% 1)   |delta_k|^2_P
% 2)   <eta_k,P*delta_k> = <eta_k,delta_k>_P
% 3)   |eta_k  |^2_P
%
% initial values are given by:
%    |delta_0|_P = <r,z>
%    |eta_0|_P   = 0
%    <eta_0,delta_0>_P = 0
% because we take eta_0 = 0 (if useRand = false).
%
% [CGT2000] Conn, Gould and Toint: Trust-region methods, 2000.

inner = problem.M.inner;
lincomb = problem.M.lincomb;

theta = options.theta;
kappa = options.kappa;

if ~options.useRand % and therefore, eta == 0
    Heta = problem.M.zerovec(x);
    r = grad;
    e_Pe = 0;
else % and therefore, no preconditioner
    % eta (presumably) ~= 0 was provided by the caller.
    Heta = getHessian(problem, x, eta, storedb, key);
    r = lincomb(x, 1, grad, 1, Heta);
    e_Pe = inner(x, eta, eta);
end
r_r = inner(x, r, r);
norm_r = sqrt(r_r);
norm_r0 = norm_r;

% Precondition the residual.
if ~options.useRand
    z = getPrecon(problem, x, r, storedb, key);
else
    z = r;
end

% Compute z'*r.
z_r = inner(x, z, r);
d_Pd = z_r;

% Initial search direction.
delta  = lincomb(x, -1, z);
if ~options.useRand % and therefore, eta == 0
    e_Pd = 0;
else % and therefore, no preconditioner
    e_Pd = inner(x, eta, delta);
end

% If the Hessian or a linear Hessian approximation is in use, it is
% theoretically guaranteed that the model value decreases strictly
% with each iteration of tCG. Hence, there is no need to monitor the model
% value. But, when a nonlinear Hessian approximation is used (such as the
% built-in finite-difference approximation for example), the model may
% increase. It is then important to terminate the tCG iterations and return
% the previous (the best-so-far) iterate. The variable below will hold the
% model value.
model_fun = @(eta, Heta) inner(x, eta, grad) + .5*inner(x, eta, Heta);
if ~options.useRand
    model_value = 0;
else
    model_value = model_fun(eta, Heta);
end

% Pre-assume termination because j == end.
stop_tCG = 5;

% Begin inner/tCG loop.
j = 0;
for j = 1 : options.maxinner

    % This call is the computationally expensive step.
    Hdelta = getHessian(problem, x, delta, storedb, key);

    % Compute curvature (often called kappa).
    d_Hd = inner(x, delta, Hdelta);

    % Note that if d_Hd == 0, we will exit at the next "if" anyway.
    alpha = z_r/d_Hd;
    % <neweta,neweta>_P =
    % <eta,eta>_P + 2*alpha*<eta,delta>_P + alpha*alpha*<delta,delta>_P
    e_Pe_new = e_Pe + 2.0*alpha*e_Pd + alpha*alpha*d_Pd;

    if options.debug > 2,
        fprintf('DBG:   (r,r)  : %e\n', r_r);
        fprintf('DBG:   (d,Hd) : %e\n', d_Hd);
        fprintf('DBG:   alpha  : %e\n', alpha);
    end

    % Check against negative curvature and trust-region radius violation.
    % If either condition triggers, we bail out.
    if d_Hd <= 0 || e_Pe_new >= Delta^2,
        % want
        %  ee = <eta,eta>_prec,x
        %  ed = <eta,delta>_prec,x
        %  dd = <delta,delta>_prec,x
        tau = (-e_Pd + sqrt(e_Pd*e_Pd + d_Pd*(Delta^2-e_Pe))) / d_Pd;
        if options.debug > 2,
            fprintf('DBG:     tau  : %e\n', tau);
        end
        eta  = lincomb(x, 1,  eta, tau,  delta);

        % If only a nonlinear Hessian approximation is available, this is
        % only approximately correct, but saves an additional Hessian call.
        Heta = lincomb(x, 1, Heta, tau, Hdelta);

        % Technically, we may want to verify that this new eta is indeed
        % better than the previous eta before returning it (this is always
        % the case if the Hessian approximation is linear, but I am unsure
        % whether it is the case or not for nonlinear approximations.)
        % At any rate, the impact should be limited, so in the interest of
        % code conciseness (if we can still hope for that), we omit this.

        if d_Hd <= 0,
            stop_tCG = 1;     % negative curvature
        else
            stop_tCG = 2;     % exceeded trust region
        end
        break;
    end

    % No negative curvature and eta_prop inside TR: accept it.
    e_Pe = e_Pe_new;
    new_eta  = lincomb(x, 1,  eta, alpha,  delta);

    % If only a nonlinear Hessian approximation is available, this is
    % only approximately correct, but saves an additional Hessian call.
    new_Heta = lincomb(x, 1, Heta, alpha, Hdelta);

    % Verify that the model cost decreased in going from eta to new_eta. If
    % it did not (which can only occur if the Hessian approximation is
    % nonlinear or because of numerical errors), then we return the
    % previous eta (which necessarily is the best reached so far, according
    % to the model cost). Otherwise, we accept the new eta and go on.
    new_model_value = model_fun(new_eta, new_Heta);
    if new_model_value >= model_value
        stop_tCG = 6;
        break;
    end

    eta = new_eta;
    Heta = new_Heta;
    model_value = new_model_value; %% added Feb. 17, 2015

    % Update the residual.
    r = lincomb(x, 1, r, alpha, Hdelta);

    % Compute new norm of r.
    r_r = inner(x, r, r);
    norm_r = sqrt(r_r);

    % Check kappa/theta stopping criterion.
    % Note that it is somewhat arbitrary whether to check this stopping
    % criterion on the r's (the gradients) or on the z's (the
    % preconditioned gradients). [CGT2000], page 206, mentions both as
    % acceptable criteria.
    if j >= options.mininner && norm_r <= norm_r0*min(norm_r0^theta, kappa)
        % Residual is small enough to quit
        if kappa < norm_r0^theta,
            stop_tCG = 3;  % linear convergence
        else
            stop_tCG = 4;  % superlinear convergence
        end
        break;
    end

    % Precondition the residual.
    if ~options.useRand
        z = getPrecon(problem, x, r, storedb, key);
    else
        z = r;
    end

    % Save the old z'*r.
    zold_rold = z_r;
    % Compute new z'*r.
    z_r = inner(x, z, r);

    % Compute new search direction.
    beta = z_r/zold_rold;
    delta = lincomb(x, -1, z, beta, delta);

    % Update new P-norms and P-dots [CGT2000, eq. 7.5.6 & 7.5.7].
    e_Pd = beta*(e_Pd + alpha*d_Pd);
    d_Pd = z_r + beta*beta*d_Pd;

end  % of tCG loop
inner_it = j;

end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = RegCoeff_M(S, M, Ns, g, df, B, Nb, Nr)
% Calculates the regression coefficiients

v = g(:,end);   % start for backward induction

f = zeros(Nb,Nr-1);

% backward induction and regression from t_{Nr-1} up to t_1
for i = Nr-1:-1:1
        index = find(g(:,i+1) > 0); % all ITM paths
        s = S(index,i+1);           % values of S at given time points
        m = M(index,i+1);           % values of M at given time points
        v = v * df(i+1);            % option value at t_i

        Acell = B(s,m);             % evaluate basis function in cell array B
        A = cell2mat(Acell{:,:});   % convert to matrix

        f(:,i) = (A'*A)\(A'*v(index));  % determine coefficients
        c = A*f(:,i);                   % continuation value

        exercise = g(index,i+1) >= c;    % early exercise
        v(index(exercise)) = g(index(exercise),i+1);
end

y = f;

end


%% Colour Segmentation - VIBGYOR Colour Segmentation
%% This function can be used for VIBGYOR Colour segmentation from the RGB
%% Color images.
%% Function C = VIBGYORsegmentation(img)
%% input    img = Color image (The input image should be a color image)
%%  Example: C = VIBGYORsegmentation(img);
%%      Posted date : 14 - 07 - 2008
%%
%% Developed By : K.Kannan & Jeny Rajan
%%                  Medical Imaging Research Group (MIRG), NeST, Trivandrum.
%%
function C = VIBGYORsegmentation(img)
%% Electro-Magnetic wavelengths ranges in Nano Meter
% red 760-720
% orange 620-590
% yellow 590-545
% green 525-490
% blue 490-450
% indigo 450-420
% violet 420-380
%%
[row col plane] = size(img);
img = double(img);
C = zeros(row,col,plane);
if plane ~= 3
    disp('Input should be a color image');
    return;
end
GL = 255;
%% Color Choice
clc;
disp('   ');
disp('           r / R - for Red Color');
disp('           o / O - for Orange Color');
disp('           y / Y - for Yellow Color');
disp('           g / G - for Green Color');
disp('           b / B - for Blue Color');
disp('           i / I - for Indigo Color');
disp('           v / V - for Violet Color');
color = input('\nYour Color Choice = ','s');
%%
switch color
    case {'R','r'}
        f1 = (GL * 1);f2 = (GL * 0.4);f3 = (GL * 0.5);
        f4 = (GL * 0);f5 = (GL * 0.5);f6 = (GL * 0);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,1,0);
    case {'O','o'}
        f1 = (GL * 1);f2 = (GL * 0.6);f3 = (GL * 0.68);
        f4 = (GL * 0.3);f5 = (GL * 0.3);f6 = (GL * 0);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,1,0);
    case {'Y','y'}
        f1 = (GL * 1);f2 = (GL * 0.78);f3 = (GL * 1);
        f4 = (GL * 0.72);f5 = (GL * 0.52);f6 = (GL * 0);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,1,1);
    case {'G','g'}
        f1 = (GL * 0.68);f2 = (GL * 0);f3 = (GL * 1);
        f4 = (GL * 0.4);f5 = (GL * 0.68);f6 = (GL * 0);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,2,0);
    case {'B','b'}
        f1 = (GL * 0.5);f2 = (GL * 0);f3 = (GL * 0.68);
        f4 = (GL * 0);f5 = (GL * 1);f6 = (GL * 0.4);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,3,0);
    case {'I','i'}
        f1 = (GL * 0.68);f2 = (GL * 0);f3 = (GL * 1);
        f4 = (GL * 0.6);f5 = (GL * 1);f6 = (GL * 0.6);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,1,1);
    case {'V','v'}
        f1 = (GL * 1);f2 = (GL * 0.6);f3 = (GL * 0.68);
        f4 = (GL * 0);f5 = (GL * 1);f6 = (GL * 0.6);
        C = cfilter(img,f1,f2,f3,f4,f5,f6,3,1);
    otherwise
        disp('unknown method');
end
%% Display
C = uint8(C);
figure, subplot(1,2,1),imshow(uint8(img),[]);title('Original Image');
subplot(1,2,2),imshow(uint8(C),[]);title('Color Segmented Image');

%% Function for Color Filter
function C = cfilter(img,f1,f2,f3,f4,f5,f6,m,flg)
[row col plane] = size(img);
C = zeros(row,col,plane);
for i = 1:row
    for j = 1:col
        if flg == 0
            if (img(i,j,1) <= f1 && img(i,j,1) >= f2 && ...
                img(i,j,2) <= f3 && img(i,j,2) >= f4 && ...
                img(i,j,3) <= f5 && img(i,j,3) >= f6 ...
                && img(i,j,m) == max([img(i,j,1) img(i,j,2) img(i,j,3)]))
                C(i,j,1:3) = img(i,j,1:3);
            else
                C(i,j,1:3) = (img(i,j,1) * 0.3) + (img(i,j,2) * 0.59) + (img(i,j,3) * 0.11);
            end
        else
            if (img(i,j,1) <= f1 && img(i,j,1) >= f2 && ...
                img(i,j,2) <= f3 && img(i,j,2) >= f4 && ...
                img(i,j,3) <= f5 && img(i,j,3) >= f6)
                C(i,j,1:3) = img(i,j,1:3);
            else
                C(i,j,1:3) = (img(i,j,1) * 0.3) + (img(i,j,2) * 0.59) + (img(i,j,3) * 0.11);
            end
        end
    end
end
%%


%==============================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% function [Sc,dS,d2S] = curvatureST(uc,omega,m,varargin)
%
% Matrix-free spatio-temporal curvature regularization energy for vc
% where vc is cell-centered
%
% Sv) = 0.5 * \int_{\omega}\int_0^1
%               alpha(1)*v(x,t)'*A*v(x,t)+ alpha(2)*v(x,t)'*B*v(x,t) dx dt,
%
% where A is the curvature operator and B the first-order time derivative
% operator.
%
% Input:
%
%   vc          instationary velocity field (cell-centered)
%   omega       spatial domain
%   m           number of discretization points in space
%   varargin    optional parameters (see below)
%
% Optional Input:
%   tspan       time span (default: [0 1])
%   nt          number of time points for velocity
%
%
% Output:
%
%   Sc          current value  (0.5 * hd * uc'* A *uc)
%   dS          derivative     (hd * uc'*A )
%   d2S         Hessian        A
%  if ~matrixFree,  d2S is sparse matrix; else, d2S is struct; endif
%
% see also curvature.m
% ==================================================================================

function [Sc,dS,d2S] = mfCurvatureST(vc,omega,m,varargin)
if nargin == 0
    help(mfilename);
    return;
end

if strcmp(vc,'para')
    Sc = 'cell-centered';       % grid
    dS = 1;                     % matrixFree
    d2S = @spectralPrecondPCG;  % solver
    return;
end

persistent A omegaOld mOld alphaOld ntOld tspanOld

if ~exist('mOld','var'),     mOld = [];     end;
if ~exist('omegaOld','var'), omegaOld = []; end;
if ~exist('alphaOld','var'), alphaOld = []; end;

matrixFree  = 0;
alpha       = [1 1e-3];
tspan       = [0 1];
nt          = [];
for k=1:2:length(varargin) % overwrites default parameter
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

dim = numel(omega)/2;

if isempty(nt) % roughly estimate nt
    nt = round(numel(vc)/(prod(m)*dim))-1;
end

d2S.regularizer = regularizer;
d2S.alpha  = alpha;
d2S.B      = @(omega,m)     getCurvatureMatrixST( omega,tspan,m,nt,alpha);
d2S.d2S    = @(u,omega,m)   curvatureOperatorST(u,omega,tspan,m,nt,alpha);
d2S.diag   = @(omega,m)     getCurvatureDiag(omega,tspan,m,nt,alpha); %getDiag(omega,tspan,m,nt,alpha);
d2S.solver = @spectralPrecondPCG;
d2S.res    = vc;
dS         = d2S.d2S(vc,omega,m)';
Sc         = .5*dS*vc;

function C = getCurvatureMatrixST(omega,tspan,m,nt,alpha)

h   = (omega(2:2:end)-omega(1:2:end))./m;
hd  = prod(h);
% compute time-stepsize
dt = abs(tspan(2)-tspan(1))/nt;

% build gradient matrix for one transformation
C =  getCurvatureMatrix(omega,m);
a = sqrt(alpha(1).*hd.*dt*[1/2;ones(nt-1,1);1/2]);
% apply spatial regularization to all transformations and sum up in
% time
C =  kron(sdiag(a(:)),C);

% get time regularization matrix
if alpha(2)>0
    b = sqrt(alpha(2)*hd.*dt);
    Bt = b * getTimeDiffusionMatrix(nt,dt,prod(m),length(omega)/2);
    C  = [C;Bt];
end

function D = sdiag(v)
	D = diag(sparse(v(:)));

% matrix free implementation of spatio-temporal curvature operator
function Ay = curvatureOperatorST(uc,omega,tspan,m,nt,alpha)
dim = numel(omega)/2;
h   = (omega(2:2:end)-omega(1:2:end))./m;
hd  = prod(h(1:dim));
dt  = abs(tspan(1)-tspan(2))/nt;
w   = dt*[1/2;ones(nt-1,1);1/2];
n   = prod(m);

uc   = reshape(uc,dim*n,[]);
ntp1 = size(uc,2);
Ay = 0*uc;

switch dim
    case 2
        D2x =  D2(1,omega,m);
        D2y =  D2(2,omega,m);

        % space: curvature
        for k=1:ntp1
            uct = reshape(uc(:,k),[m dim]);
            t1  = D2x*uct(:,:,1) + uct(:,:,1)*D2y;
            t1  = D2x*t1 + t1*D2y;
            t2  = D2x*uct(:,:,2) + uct(:,:,2)*D2y;
            t2  = D2x*t2 + t2*D2y;

            Ay(:,k) = (w(k)*hd*alpha(1))*[t1(:);t2(:)];
        end
        Ay = Ay(:);
    case 3
        % space: curvature
        d2  = @(i) D2(i,omega,m); % this is a shortcut to the discrete second derivative

        % the following line is a shortcut for
        %  - permuting the 3d-array using the permutation J
        %  - reshape it to a 2D-array of size q-by-prod(m)/q, where q=m(J(1))
        %  - multiply by A (which is q-by-q)
        %  - undo the reshape, i.e. make the result to m(J(1))-by-m(J(2))-by-m(J(3))
        %  - undo the permutation
        operate = @(A,z,J) ipermute(reshape(A*reshape(permute(z,J),m(J(1)),[]),m(J)),J);
        for k=1:ntp1 % loop over all time steps
            uct = reshape(uc(:,k),[m,dim]);
            for ell=1:3 % run over all components y^ell of Y=(Y^1,Y^2,Y^3)
                % compute
                % (I_3\otimes I_2\otimes d2(1) + I_3\otimes d2(2)\otimes I_1 ...
                % + d2(3)\otimes I_2\otimes I_1) y^ell
                %                 utt = 0;
                for rep=1:2 % go twice to have the bi-Laplacian
                    for j=1:3
                        z = operate(d2(j),uct(:,:,:,ell),[j,setdiff(1:3,j)]);
                        Ay((ell-1)*n+(1:n),k) = Ay((ell-1)*n+(1:n),k) + reshape(z,[],1);
                    end;
                    if rep==1 % overwrite uct and reset Ay. Compare to t1,t2 in dim=2 implementation
                        uct(:,:,:,ell) = reshape(Ay((ell-1)*n+(1:n),k),m);
                        Ay((ell-1)*n+(1:n),k) = 0;
                    end
                end;
            end
            Ay(:,k) = w(k)*hd*alpha(1)*Ay(:,k);
        end
        Ay = Ay(:);
    otherwise
        error('%s - dimension %d not supported.',mfilename,dim);
end
% time: diffusion
if alpha(2)>0
    d3  = @(Y) (Y(:,2:end)-Y(:,1:end-1))/dt;
    d3T = @(Y) ([-Y(:,1),Y(:,1:end-1)-Y(:,2:end),Y(:,end)])/dt;
    At = d3T(d3(uc));
    Ay = Ay(:) + (alpha(2)* hd*dt)*At(:);
end

function D = getCurvatureDiag(omega,tspan,m,nt,alpha)
% just for testing - not very efficient
dim = numel(omega)/2;
h   = (omega(2:2:end)-omega(1:2:end))./m;
hd  = prod(h(1:dim));
dt  = abs(tspan(1)-tspan(2))/nt;

C = getCurvatureMatrix(omega,m);
D = full(diag(C'*C'));

D = hd*dt*alpha(1)*repmat(D(:),1,1);
if alpha(2)>0
    errory('nyi')
end

function D = D2(i,omega,m)
h = (omega(2:2:end)-omega(1:2:end))./m;
D = spdiags(ones(m(i),1)*[1,-2,1],-1:1,m(i),m(i))/h(i)^2;
D([1,end]) = -D([2,end-1]);

function A = getTimeDiffusionMatrix(nt,dt,n,dim)
Dt=spdiags(ones(nt+1,1)*[-1/dt 1/dt],0:1,nt,nt+1);
A = kron(Dt, speye(n*dim)); % 2nd deriveative over time for two components


function value = r8vec_amin ( n, a )

%*****************************************************************************80
%
%% R8VEC_AMIN returns the minimum absolute value in an R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real A(N), the array.
%
%    Output, real VALUE, the value of the entry
%    of smallest magnitude.
%
  value = min ( abs ( a(1:n) ) );

  return
end


d = 500;
n = 20;
C = 100;
k = 10;
q = 50;
q2 = 50;

ODLSI = C*k*(k*d+d*n + q*k*n) + C*q*k*d^3
EDLSI = C*k*(k*d+d*n + q*k*n) + C*d^3 + C*q*d*k*(q*k + d)
OFDDL = C^2*d*k*(n+C*k+C*n)+C*k^2*q*(d + C^2*n)
EFDDL = C^2*k*((q+1)*k*(d+C*n) + 2*d*n)

OCOPAR = C^3*k^2*(2*d + C*k + q*n) + C*q*k*d^3
ECOPAR = C^3*k^2*(2*d + C*k + q*n) + C*d^3 + C*q*d*k*(q*k + d)

LRSDL = C^2*k*((q+1)*k*(d + C*n) + 2*d*n) + C^2*d*k*n + (q + q2)*d*k^2

%%
ODLSID = C*q*k*d^3
EDLSID = C*d^3 + C*q*d*k*(q*k + k)
OFDDLX = C^2*k*(d*n + q*C*k*n + C*d*k)
EFDDLX = C^2*k*(d*n + q*C *n*k + d*k)
OFDDLD = C*d*k*(q*k + C^2*n)
EFDDLD = C*d*k*(C*n + C*q*k) + C^3*k^2 *n


function [filteredPrice,filteredTime,actualTime] = realized_price_filter(price,time,timeType,samplingType,samplingInterval)
% Price filtering for computing realized variances and other
% high-frequency price based returns
%
% USAGE:
%   [FILTEREDPRICE,FILTEREDTIME,ACTUALTIME] =  realized_price_filter(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL)
%
% INPUTS:
%   PRICE             - m by 1 column vector of prices
%   TIME              - m by 1 column vector of times corresponding to PRICE.
%                         Must be sorted and increasing.
%   TIMETYPE          - String describing the way times are measured
%                        'wall'    24-hour clock of the form HHMMSS, e.g. 101543 or 153217
%                        'seconds' Time measures in seconds past midnight.
%                          TIME must satisfy 0<=TIME<86400
%                        'unit'  Unit normalized date format, e.g. .1, .234, .9
%                          Unit normalized times are more general than the
%                          other types and can be applied to data from more
%                          than one calendar day
%   SAMPLINGTYPE      - String describing the type of sampling to use when
%                         filtering PRICE
%                         'CalendarTime' - Sample in calendar time using
%                           observations separated by SAMPLINGINTERVAL
%                           seconds. If TIMETYPE is 'unit',
%                           SAMPLINGINTERVAL must be between 0 and 1 and
%                           represents the fraction of the sample to skip
%                           when sampling.
%                         'CalendarUniform' - Sample in calendar time using
%                           SAMPLINGINTERVAL observations spread uniformly
%                           between TIME(1) and TIME(m)
%                         'BusinessTime' - Sample in business (tick) time
%                           using observation separated by SAMPLINGINTERVAL
%                           ticks
%                         'BusinessUniform' - Sample in business (tick)
%                           time using observations uniformly spaced in
%                           business time.
%                         'Fixed' - Sample at specific points in time. When
%                           using fixed, SAMPLINGINTERVAL must be a n by 1 vector
%                           of times with the same TIMETYPE as TIME (i.e.
%                           seconds if TIME is in seconds)
%   SAMPLINGINTERVAL   - Scalar integer or n by 1 vector whose meaning depends on the
%                          selected SAMPLINGTYPE
%
% OUTPUTS:
%   FILTEREDPRICE      - n by 1 vector of filtered prices.  n depends on
%                          SAMPLINGTYPE as well as the data
%   FILTEREDTIME       - n by 1 vector of sampling times corresponding to
%                          the chosen sampling scheme
%   ACTUALTIME         - n by 1 vector. ACTUALTIME(i) provides the actual
%                          observation time that was used when sampling at FILTEREDTIME(i)
%
%
% COMMENTS:
%   This is a helper function for REALIZED_KERNEL.  Last price
%   interpolation is always used.  If SAMPLINGTYPE is 'Fixed' then any
%   requested samples before the first observation will be back filled
%   with PRICE(1). An indicator for back filled values can be constructed
%   using ACTUALTIME>FILTEREDTIME
%
% EXAMPLES:
%   % 5-minute prices
%   FILTEREDPRICE =  realized_price_filter(PRICE,TIME,'wall','CalendarTime',300)
%
%   % 79 prices spread uniformly in calendar time
%   FILTEREDPRICE =  realized_price_filter(PRICE,TIME,'wall','CalendarUniform',79)
%
%   % 20-tick returns
%   FILTEREDPRICE = realized_price_filter(PRICE,TIME,'wall','CalendarUniform',79)
%
%   % 391 prices spread uniformly in business time
%   FILTEREDPRICE = realized_price_filter(PRICE,TIME,'wall','CalendarUniform',79)
%
%   % 5-minute prices coresponding to 9:30:00, 9:35:00, ...
%   sampleTimes =  seconds2wall(wall2seconds(93000):300:wall2seconds(160000))';
%   FILTEREDPRICE = realized_price_filter(PRICE,TIME,'wall','Fixed',sampleTimes)
%
%   % .05-calendar time sampling when using unit times
%   TIME = wall2unit(TIME,min(TIME),max(TIME));
%   FILTEREDPRICE = realized_price_filter(PRICE,TIME,'unit','CalendarTime',.05)
%
%
%  See also REALIZED_KERNEL, REALIZED_KERNEL_CORE, REALIZED_KERNEL_WEIGHTS
%  REALIZED_KERNEL_SELECT_LAG_LENGTH, REALIZED_VARIANCE

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 5/1/2008

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin~=5
    error('Five inputs required.')
end
if size(price,2)>size(price,1)
    price=price';
end
if size(price,2)>1
    error('PRICE must be a m by 1 vector.')
end
if size(time,2)>size(time,1)
    time=time';
end
if any(diff(time)<0)
    error('TIME must be sorted and increasing')
elseif any(diff(time)==0)
    warning('oxfordRealized:realizedPriceFilter','TIME contains multiple entries with the same value. This creates an ambiguity and FILTEREDPRICE will contain the last price if TIME does not only unique elements.')
end
if size(time,2)>1 || length(time)~=length(price)
    error('TIME must be a m by 1 vector.')
end
timeType=lower(timeType);
if ~ismember(timeType,{'wall','seconds','unit'})
    error('TIMETYPE must be one of ''wall'', ''seconds'' or ''unit''.');
end
% Inserted to protect against inputing integer times
time = double(time);

samplingType=lower(samplingType);
if ~ismember(samplingType,{'calendartime','calendaruniform','businesstime','businessuniform','fixed'})
    error('SAMPLINGTYPE must be one of ''CalendarTime'', ''CalendarUniform'', ''BusinessTime'', ''BusinessUniform'' or ''Fixed''.');
end

m=size(price,1);
t0Original=time(1);
tTOriginal=time(m);
if ismember(samplingType,{'calendartime','calendaruniform','businesstime','businessuniform'})
    % Must be a scalar integer, unless using unit times
    if (~isscalar(samplingInterval) || floor(samplingInterval)~=samplingInterval || samplingInterval<1) && ~strcmp(timeType,'unit')
        error('SAMPLINGINTERVAL must be a positive integer for the SAMPLINGTYPE selected.')
    end
else
    if size(samplingInterval,2)>size(samplingInterval,1)
        samplingInterval=samplingInterval';
    end
    if ~(any(samplingInterval>=t0Original) && any(samplingInterval<=tTOriginal))
        error('At least one sampling interval must be between min(TIME) and max(TIME) when using ''Fixed'' as SAMPLINGTYPE.')
    end
    if any(diff(samplingInterval)<=0)
        error('When using ''Fixed'' as SAMPLINGTYPE the vector of sampling times in SAMPLINGINTERVAL must be sorted and strictly increasing.')
    end
end

if strcmp(timeType,'unit') && strcmp(samplingType,'calendartime')
    % samplingInterval must be between 0 and 1
    if samplingInterval>1
        error('When TIMETYPE is ''unit'' and SAMPLINGTYPE is ''CalendarTime'', SAMPLINGINTERVAL must also be in ''unit'' terms, and so must be between 0 and 1')
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Get the size of the price
m=size(price,1);

% Convert times
if strcmp(samplingType,'fixed')
    time0 = min(samplingInterval);
    time1 = max(samplingInterval);
else
    time0 = min(time);
    time1 = max(time);
end
if strcmp(timeType,'wall')
    time = wall2unit(time,time0,time1);
elseif strcmp(timeType,'seconds')
    time = seconds2unit(time,time0,time1);
end
% If fixed convert these times as well
if strcmp(samplingType,'fixed')
    if strcmp(timeType,'wall')
        samplingInterval = wall2unit(samplingInterval,time0,time1);
    elseif strcmp(timeType,'seconds')
        samplingInterval = seconds2unit(samplingInterval,time0,time1);
    end
end

% Get the first and last time
t0=time(1);
tT=time(m);
% Switch based on the cases
switch samplingType
    case 'calendartime'
        % If calendar time
        % Convert sampling interval to be a fraction of a period
        % Unit strides
        if strcmp(timeType,'wall')
            timeBase = wall2seconds([time0 time1]);
            samplingInterval = samplingInterval/diff(timeBase);
        elseif strcmp(timeType,'seconds')
            samplingInterval = samplingInterval/(time1-time0);
        end
        filteredTime=(t0:samplingInterval:tT)';
        % Append the final observation
        if ~ismember(tT,filteredTime)
            filteredTime=[filteredTime; tT];
        end
        % Compute the filtered prices
        [filteredPrice,actualTime]=fasttimefilter(price,time,filteredTime);
    case 'calendaruniform'
        % Sampling interval contains the number of samples
        % Uniform spacing
        filteredTime=linspace(t0,tT,samplingInterval)';
        % Compute the filtered prices
        [filteredPrice,actualTime]=fasttimefilter(price,time,filteredTime);
    case 'businesstime'
        indices=(1:samplingInterval:m)';
        if ~ismember(m,indices)
            indices=[indices;m];
        end
        filteredPrice=price(indices);
        filteredTime=time(indices);
        actualTime = filteredTime;
    case 'businessuniform'
        % Sampling interval contains the number of samples
        indices=floor(linspace(1,m,samplingInterval));
        filteredPrice=price(indices);
        filteredTime=time(indices);
        actualTime = filteredTime;
    case 'fixed'
        filteredTime = samplingInterval;
        % Compute the filtered prices
        [filteredPrice,actualTime]=fasttimefilter(price,time,filteredTime);
    otherwise
        error('Unrecogniced SAMPLINGTYPE.')
end

% Finally convert filteredTime back to the original time format
if strcmp(timeType,'wall')
    filteredTime = unit2wall(filteredTime,time0,time1);
    actualTime = unit2wall(actualTime,time0,time1);
elseif strcmp(timeType,'seconds')
    filteredTime = unit2seconds(filteredTime,time0,time1);
    actualTime = unit2seconds(actualTime,time0,time1);
end

function [filteredPrice,actualTime]=fasttimefilter(price,time,filteredTime)

% Get the size of the inputs
m=size(price,1);
n=size(filteredTime,1);
% This uses a 2-index algorithm so that it is fast for any realistic size.
timeIndex=1;
filteredTimeIndex=1;
% Pl hold the index values.  Initializing to 1 makes the back fill easy
pl=ones(n,1);
while timeIndex<=m && filteredTimeIndex<=n
    if time(timeIndex)<=filteredTime(filteredTimeIndex)
        pl(filteredTimeIndex)=timeIndex;
        timeIndex=timeIndex+1;
    elseif filteredTimeIndex<n
        % Increment filteredTimeIndex until filteredTime>=time(timeIndex)
        while filteredTimeIndex<n && filteredTime(filteredTimeIndex)<time(timeIndex)
            filteredTimeIndex = filteredTimeIndex + 1;
            % Last price interploation for these since there is no new
            % information
            pl(filteredTimeIndex) = pl(filteredTimeIndex-1);
        end
        % Only assign if you aren't on the last one
        if filteredTimeIndex<n
            pl(filteredTimeIndex)=timeIndex;
        end
    elseif time(timeIndex)>filteredTime(n)
        % No more to assign, so break
        break
    end
end

% Clean up any trailing ones
starter = find(diff(pl)<0) + 1;
if ~isempty(starter)
    pl(starter:length(pl)) = pl(starter-1);
end
% Use pl the index the filteredPrice and filterTimeActual
filteredPrice = price(pl);
actualTime = time(pl);


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

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

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

% This is a default implementation for separableApprox children that do not
% have know how to properly implement this method.

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

% Convert rows and columns to chebfuns.
frows = simplify(chebfun(frows, dom(1:2), maxsize));
fcols = simplify(chebfun(fcols, dom(3:4), maxsize));

% 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 SEPARABLEAPPROX.
    ypts = chebpts(length(fcols), fcols.domain);
    xpts = chebpts(length(frows), frows.domain);
    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:SEPARABLEAPPROX:minandmax2:length', 'Rank is too large.');

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 [yIntervalEst,PInvIntervalEst,yFwdPred,PInvFwdPred,yFwdEnd,PInvFwdEnd]=FPInfoIntervalSmoother(yFwdPred,PInvFwdPred,yFwdPrev,PInvFwdPrev,N,z,R,H,F,Q,u,hasLastPred)
%%FPINFOINTERVALSMOOTHER Update the Fraser-Potter information smoother
%                smoother over a sliding or growing interval given a new
%                measurement. The smoothed estimates over the entire
%                interval are available. If one already computed the
%                predicted state at the current time, it can be provided
%                rather than being recomputed. This can slide an interval
%                of estimates fixed at length N or grow an interval until
%                it is length N. See the example below for how one can grow
%                the interval until it is a desired length.
%
%INPUTS: yFwdPred The xDimXN set of predicted information states. If the
%                 interval is is increasing in size (has not yet reached
%                 the desired N), then the last dimension is NCur-1 in
%                 size, where NCur is the current length of z, unless
%                 hasLastPred=true, in which case it is NCur in length
%                 with the final prediction already computed. If
%                 hasLastPred=false, then the final column of xFwdPred is
%                 the state predicted to the time-step PRIOR to the current
%                 one. Otherwise, if hasLastPred=true, then the final
%                 column is the state predicted to the time of the latest
%                 measurement in z. This cannot be an empty matrix.
%     PInvFwdPred The xDimXxDimXN set of predicted inverse covariance
%                 matrices associated with yFwdPred. The size and meaning
%                 of the final dimension is the same as for yFwdPred.
%        yFwdPrev The xDimX1 posterior information state estimate at the
%                 time of the second to latest measurement in z. When
%                 calling this function repeatedly, this can be the yFwdEnd
%                 output from the last function call. If hasLastPred=true,
%                 then an empty matrix can be passed in place of this
%                 input.
%     PInvFwdPrev The xDimXxDim inverse covariance matrix associate with
%                 yFwdPrev. When calling this function repeatedly, this can
%                 be the PInvFwdEnd output from the last function call. If
%                 hasLastPred=true, then an empty matrix can be passed in
%                 place of this input.
%               N The positive scalar integer desired length of the
%                 smoothed interval; N>=2. If omitted or an empty matrix is
%                 passed, then N is set to the number of columns of z
%                 (NCur).
%               z The zDimXN set of measurements over the interval. z(:,N)
%                 is the latest measurement. If the interval is expanding,
%                 then z is zDimXNCur in length.
%               R The zDimXzDimXN set of measurement covariance matrices
%                 associated with the values in z. If the interval is
%                 expanding, then the last dimension is NCur in size. If
%                 all the matrices are the same, then a single zDimXzDim
%                 matrix can be passed.
%               H The xDimXxDimXN set of measurement matrices associated
%                 with the values in z. If the interval is expanding, then
%                 the last dimension is NCur in size. If all the matrices
%                 are the same, then a single xDimXzDim matrix can be
%                 passed.
%               F The xDimXxDimX(N-1) set of state transition matrices over
%                 the interval. F(:,:,N-1) is the state transition matrix
%                 to the time of the latest measurerment in z. If the batch
%                 is expanding, then the last dimension is NCur-1 in size.
%                 If all F are the same, then a single xDimXxDim matrix can
%                 be passed.
%               Q The xDimXxDimX(N-1) set of process noise covariance
%                 matrices. the meaning of the third dimension is the same
%                 as for F. If all Q are the same, then a single xDimXxDim
%                 matrix can be passed.
%               u The xDimX(N-1) set of state transition control inputs,
%                 where the second dimension has the same meaning as the
%                 third dimensions of F. . If an empty matrix is passed,
%                 then it is assumed that all control inputs are zero. If
%                 aall the u vectors are the same, then a single xDimX1
%                 vector can be passed.
%     hasLastPred This boolean value indicates whether yFwdPred and
%                 PInvFwdPred contain the prediction to the time of the
%                 latest measurement in z. The default if omitted or an
%                 empty matrix is passed is false.
%
%OUTPUTS: yIntervalEst The xDimXN set of smoothed information state
%                      estimates over the interval. The oldest estimate is
%                      yIntervalEst(:,1)
%                      and the newest yIntervalEst(:,N). If the interval is
%                      expanding, the second dimension will be size NCur.
%      PInvIntervalEst The xDimXxDimXN set of inverse covariance matrices
%                      associated with those in yIntervalEst. If the
%                      interval is expanding, the third dimension will be
%                      size NCur.
% yFwdPred,PInvFwdPred The xDimXN set of predicted information states and
%                      the associated xDimXxDimX(N-1) set of predicted
%                      inverse covariance matrices. If the interval is
%                      expanding, the final dimensions will be length
%                      NCur.
%   yFwdEnd,PInvFwdEnd The xDimX1 and xDimXxDim posterior information state
%                      and its inverse covariance matrix at the latest
%                      time.
%
%The Kalman smoothing technique utilizing forward and backwards filters is
%described in [1]. The use with information filters is related.
%
%EXAMPLE:
%Given an uninformative prior, the information smoother is initialized with
%the output of an information filter at the first step. Then, as more
%measurements are obtained, the batch length increases until it reached
%length N. After that, the interval slides. We also run an information
%filter and show that not only does the smoothed estimate at the end of the
%interval (the most smoothed) outperform the information filter, but that
%the estimate at the start of the interval is the same as the real-time
%information filter output. The measurement error reduction factor (MERF)
%and the NEES of the estimates (converted to target states) of the
%information filter and the information  smoother are compared. The MERF of
%the smoother is lower (better) and the NEES of both are close to 1,
%indicating estimator consistency.
% T=1;%Sample period.
% numRuns=500;
% N=4;%The length of the interval considered.
% numSteps=100;
%
% %Parameters for the noise process dynamics and measurement.
% H=[eye(2,2),zeros(2,2)];
% zDim=size(H,1);
% xDim=size(H,2);
% R=diag([40;40]);
% SR=chol(R,'lower');
%
% %Statistics for the initial state.
% x0Mean=[1e3;0;75;-50];
% x0Cov=diag([1e3^2;100^2;25^2;25^2]);
% x0S=chol(x0Cov,'lower');
%
% %Parameters for the state dynamics.
% q=processNoiseSuggest('PolyKal-ROT',9.8,1);
% F=FPolyKal(T,xDim,1);
% Q=QPolyKal(T,xDim,1,q);
% SQ=chol(Q,'lower');
%
% %The initial uninformative state.
% yInit=zeros(xDim,1);
% PInvInit=zeros(xDim,xDim,1);
%
% xTrue=zeros(xDim,numSteps+N-1,numRuns);
% z=zeros(zDim,numSteps+N-1,numRuns);
% ySmoothed=zeros(xDim,numSteps,numRuns);
% PInvSmoothed=zeros(xDim,xDim,numSteps,numRuns);
% yInfo=zeros(xDim,numSteps,numRuns);
% PInvInfo=zeros(xDim,xDim,numSteps,numRuns);
% yInfoAlt=zeros(xDim,numSteps,numRuns);
% PInvInfoAlt=zeros(xDim,xDim,numSteps,numRuns);
% for curRun=1:numRuns
%     %Draw the initial state.
%     xTrue(:,1,curRun)=x0Mean+x0S*randn(xDim,1);
%     %Get the first measurement.
%     z(:,1,curRun)=H*xTrue(:,1,curRun)+SR*randn(zDim,1);
%
%     %Get the states and measurements for the rest of the steps plus extra
%     %steps so that the Kalman filter and smoother cover the same range.
%     for curStep=2:(numSteps+N-1)
%         xTrue(:,curStep,curRun)=F*xTrue(:,curStep-1,curRun)+SQ*randn(xDim,1);
%         z(:,curStep,curRun)=H*xTrue(:,curStep,curRun)+SR*randn(zDim,1);
%     end
%
%     %Initialize the information filters and the information smoother with a
%     %single measurement.
%     [yUpdate,PInvUpdate]=infoFilterUpdate(yInit,PInvInit,z(:,1,curRun),R,H);
%     yFwdPred=yInit;
%     PInvFwdPred=PInvInit;
%     yFwdPrev=yUpdate;
%     PInvFwdPrev=PInvUpdate;
%
%     yInfo(:,1,curRun)=yUpdate;
%     PInvInfo(:,:,1,curRun)=PInvUpdate;
%
%     yInfoAlt(:,1,curRun)=yUpdate;
%     PInvInfoAlt(:,:,1,curRun)=PInvUpdate;
%
%     %Continue with the rest of the measurements.
%     for curStep=2:numSteps
%         zSpan=z(:,max(curStep-N+1,1):curStep,curRun);
%
%         [yIntervalEst,PInvIntervalEst,yFwdPred,PInvFwdPred,yFwdPrev,PInvFwdPrev]=FPInfoIntervalSmoother(yFwdPred,PInvFwdPred,yFwdPrev,PInvFwdPrev,N,zSpan,R,H,F,Q);
%         if(curStep>=N)
%             ySmoothed(:,curStep-N+1,curRun)=yIntervalEst(:,1);
%             PInvSmoothed(:,:,curStep-N+1,curRun)=PInvIntervalEst(:,:,1);
%         end
%         yInfoAlt(:,curStep,curRun)=yIntervalEst(:,end);
%         PInvInfoAlt(:,:,curStep,curRun)=PInvIntervalEst(:,:,end);
%
%         [yPred,PInvPred]=infoFilterDiscPred(yInfo(:,curStep-1,curRun),PInvInfo(:,:,curStep-1,curRun),F,Q);
%         [yUpdate,PInvUpdate]=infoFilterUpdate(yPred,PInvPred,z(:,curStep,curRun),R,H);
%         yInfo(:,curStep,curRun)=yUpdate;
%         PInvInfo(:,:,curStep,curRun)=PInvUpdate;
%     end
%
%     %Now, go N-1 steps more with the smoother, so that the information
%     %filter estimates and the information smoother estimates cover the same
%     %range of values.
%     for curStep=(numSteps+1):(numSteps+N-1)
%         zSpan=z(:,(curStep-N+1):curStep,curRun);
%         [yIntervalEst,PInvIntervalEst,yFwdPred,PInvFwdPred,yFwdPrev,PInvFwdPrev]=FPInfoIntervalSmoother(yFwdPred,PInvFwdPred,yFwdPrev,PInvFwdPrev,N,zSpan,R,H,F,Q);
%         ySmoothed(:,curStep-N+1,curRun)=yIntervalEst(:,1);
%         PInvSmoothed(:,:,curStep-N+1,curRun)=PInvIntervalEst(:,:,1);
%     end
% end
%
% %The maximum absolute difference between the standard information filter
% %estimate and the estimate at the front of the infomration smoother batch.
% %This should be 0.
% maxInfoDiff=max(abs(yInfo(:)-yInfoAlt(:)))
%
% %Get the target states. We shall skip the first one which is not completely
% %observable.
% xSmoothed=zeros(xDim,numSteps-1,numRuns);
% PSmoothed=zeros(xDim,xDim,numSteps-1,numRuns);
% xEst=zeros(xDim,numSteps-1,numRuns);
% PEst=zeros(xDim,xDim,numSteps-1,numRuns);
% for curRun=1:numRuns
%     for curStep=2:numSteps
%         xSmoothed(:,curStep-1,curRun)=PInvSmoothed(:,:,curStep,curRun)\ySmoothed(:,curStep,curRun);
%         PSmoothed(:,:,curStep-1,curRun)=inv(PInvSmoothed(:,:,curStep,curRun));
%
%         xEst(:,curStep-1,curRun)=PInvInfo(:,:,curStep,curRun)\yInfo(:,curStep,curRun);
%         PEst(:,:,curStep-1,curRun)=inv(PInvInfo(:,:,curStep,curRun));
%     end
% end
%
% %Limit the period plotted to the range where the target state is fully
% %observable and is common to the smoother and the filter.
% xTrue=xTrue(:,2:numSteps,:);
% z=z(:,2:numSteps,:);
% posTrue=xTrue(1:2,:,:);
%
% MERFSmoothed=calcMERF(posTrue,z,xSmoothed(1:2,:,:),0,true);
% MERFInfo=calcMERF(posTrue,z,xEst(1:2,:,:),0,true);
% NEESSmoothed=calcNEES(xTrue,xSmoothed,PSmoothed);
% NEESInfo=calcNEES(xTrue,xEst,PEst);
%
% figure(1)
% clf
% hold on
% plot(2:numSteps,MERFInfo,'-k','linewidth',4)
% plot(2:numSteps,MERFSmoothed,'--r','linewidth',2)
% h1=xlabel('Step');
% h2=ylabel('MERF');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
% legend('Information Filter','Information Smoother')
%
% figure(2)
% clf
% hold on
% plot(2:numSteps,NEESInfo,'-k','linewidth',4)
% plot(2:numSteps,NEESSmoothed,'--r','linewidth',2)
% h1=xlabel('Step');
% h2=ylabel('NEES');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
% legend('Information Filter','Information Smoother','location','southeast')
%
%REFERENCES:
%[1] D. C. Fraser and J. E. Potter, "The optimal linear smoother as a
%    combination of two optimum linear filters," IEEE Transactions on
%    Automatic Control, pp. 387-390, Aug. 1969.
%
%August 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xDim=size(yFwdPred,1);
NCur=size(z,2);

if(nargin<12||isempty(hasLastPred))
    hasLastPred=false;
end

if(isempty(N))
    %Assume that the batch is already the desired size if the batch length
    %is not specified.
    N=NCur;
end

NFEnd=NCur-1;

if(size(F,3)==1)
    F=repmat(F,[1,1,NFEnd]);
end

if(size(Q,3)==1)
    Q=repmat(Q,[1,1,NFEnd]);
end

if(nargin<11||isempty(u))
    u=zeros(xDim,NFEnd);
elseif(size(u,2)==1)
    u=repmat(u,[1,NFEnd]);
end

if(size(H,3)==1)
    H=repmat(H,[1,1,NCur]);
end

if(size(R,3)==1)
    R=repmat(R,[1,1,NCur]);
end

if(hasLastPred==false)
    [yFwdPredEnd,PInvFwdPredEnd]=infoFilterDiscPred(yFwdPrev,PInvFwdPrev,F(:,:,NFEnd),Q(:,:,NFEnd),u(:,NFEnd));

    if(NCur==N&&size(yFwdPred,2)==N)
        %If the interval is sliding.
        yFwdPred=[yFwdPred(:,2:N),yFwdPredEnd];
        PInvFwdPred=cat(3,PInvFwdPred(:,:,2:N),PInvFwdPredEnd);
    else%The interval is growing; NCur is one more than the current length
        %of yFwdPred.
        yFwdPred(:,NCur)=yFwdPredEnd;
        PInvFwdPred(:,:,NCur)=PInvFwdPredEnd;
    end
end

%Run the information filter backwards.
yRev=zeros(xDim,NCur);
PInvRev=zeros(xDim,xDim,NCur);

[yRev(:,NCur),PInvRev(:,:,NCur)]=infoFilterUpdate(zeros(xDim,1),zeros(xDim,xDim),z(:,NCur),R(:,:,NCur),H(:,:,NCur));
for curStep=(NCur-1):-1:1
    [yRevPred,PInvRevPred]=infoFilterDiscPredRev(yRev(:,curStep+1),PInvRev(:,:,curStep+1),F(:,:,curStep),Q(:,:,curStep),u(:,curStep));
    [yRev(:,curStep),PInvRev(:,:,curStep)]=infoFilterUpdate(yRevPred,PInvRevPred,z(:,curStep),R(:,:,curStep),H(:,:,curStep));
end

PInvIntervalEst=PInvFwdPred+PInvRev;
yIntervalEst=yFwdPred+yRev;

if(nargout>4)
    yFwdEnd=yIntervalEst(:,end);
    PInvFwdEnd=PInvIntervalEst(:,:,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 Loglike = Loglike_MixHP(Seqs, model, alg)

% given responsibility, calcuate the expected number of sequence belonging
% to the k-th cluster
EX = model.R;

% update parameters of Hawkes processes (mu_k, A_k), k=1,...,N
% initialize
A = model.beta;%random('exp', model.beta);
mu = sqrt(pi/2)*model.b;%random('rayl', model.b);

tmp1 = A(:)./(model.beta(:));
tmp1(isnan(tmp1)) = 0;
tmp1(isinf(tmp1)) = 0;

tmp2 = mu(:).^2./(2*model.b(:).^2);
tmp2(isnan(tmp2)) = 0;
tmp2(isinf(tmp2)) = 0;

tmp3 = log(mu(:));
tmp3(isnan(tmp3)) = 0;
tmp3(isinf(tmp3)) = 0;

Loglike = sum(tmp1)+ sum(tmp2)-sum(tmp3);

% E-step: evaluate the responsibility using the current parameters
for c = 1:length(Seqs)

    Time = Seqs(c).Time;
    Event = Seqs(c).Mark;
    Tstart = Seqs(c).Start;

    if isempty(alg.Tmax)
        Tstop = Seqs(c).Stop;
    else
        Tstop = alg.Tmax;
        indt = Time < alg.Tmax;
        Time = Time(indt);
        Event = Event(indt);
    end

    N = length(Time);
    % calculate the integral decay function in the log-likelihood function
    G = Kernel_Integration(Tstop - Time, model);

    for i = 1:N

        ui = Event(i);
        ti = Time(i);

        lambdai = mu(ui,:)+eps;
        pii = lambdai;

        if i>1
            tj = Time(1:i-1);
            uj = Event(1:i-1);

            gij = Kernel(ti-tj, model);
            auiuj = A(uj, :, :, ui);
            pij = repmat(gij, [1,1,model.K,1]).* auiuj;

            tmp = sum(sum(pij,1),2);
            lambdai = lambdai + tmp(:)';

        end

        LL = LL+log(lambdai);

    end
    LL = LL - (Tstop-Tstart).*sum(mu);
    tmp = sum(sum(repmat(G, [1,1,model.K]).*sum(A(Event,:,:,:),4),2),1);
    LL = LL - tmp(:)';

    Loglike = Loglike - EX(c,:)*LL(:);

end

Loglike = -Loglike;


function toms743_test03 ( nbits, xmin, xmax, n )

%*****************************************************************************80
%
%% TOMS743_TEST03 tests WAPR(X) when X is the argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2014
%
%  Author:
%
%    Original FORTRAN77 version by Andrew Barry, S. J. Barry,
%    Patricia Culligan-Hensley.
%    This MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, integer NBITS, the number of bits in the mantissa.
%
%    Input, real XMIN, XMAX, the range.
%
%    Input, integer N, the number of equally spaced values
%    in the range at which arguments are to be chosen.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TOMS743_TEST03\n' );
  fprintf ( 1, '  Input X is the argument.\n' );

  ifmt = 1;
  l = 0;

  if ( xmax < xmin )
    temp = xmin;
    xmin = xmax;
    xmax = temp;
  end

  dx = ( xmax - xmin ) / n;
  xmin = xmin - dx;

  if ( xmax <= 0.0 )
    iw = 1;
    fprintf ( 1, '  Both branches of the W function will be checked.\n' );
  else
    iw = 0;
    fprintf ( 1, '  Wp has been selected (maximum x is > 0)\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Results for Wp(x):\n' );

  if ( ifmt == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '   Offset x    W(x) (WAPR)' );
    fprintf ( 1, '     W(x) (BISECT)  Digits Correct\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '     x     W(x) (WAPR)' );
    fprintf ( 1, '     W(x) (BISECT)  Digits Correct\n' );
  end
  fprintf ( 1, '\n' );

  for i = 1 : n + 1

    x = xmin + i * dx;
    [ w, nerror ] = wapr ( x, 0, l );

    if ( nerror == 1 )

      fprintf ( 1, '  The value of X = %g is out of range.\n', x );

    else

      [ we, ner ] = bisect ( x, 0, l );

      if ( ner == 1 )
        fprintf ( 1, '\n' );
        fprintf ( 1, ' BISECT did not converge for x = %g\n', x );
        fprintf ( 1, '  Try reducing NBITS.\n' );
      end

      if ( w == we )
        nd = floor ( log10 ( 2.0 ^ nbits ) + 0.5 );
      else
        nd = floor ( log10 ( abs ( we / ( w - we ))) + 0.5 );
      end

      fprintf ( 1, '%17.8g      %17.8g      %17.8g      %3d\n', x, w, we, nd );

    end

  end

  if ( iw == 1 )

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Results for Wm(x):\n' );

    if ( ifmt == 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, '   Offset x    W(x) (WAPR)' );
      fprintf ( 1, '     W(x) (BISECT)  Digits Correct\n' );
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, '     x     W(x) (WAPR)' );
      fprintf ( 1, '     W(x) (BISECT)  Digits Correct\n' );
    end

    for i = 1 : n + 1

      x = xmin + i * dx;
      [ w, nerror ] = wapr ( x, 1, l );

      if ( nerror == 1 )

        fprintf ( 1, '  The value of X = %g is out of range.\n', x );

      else

        [ we, ner ] = bisect ( x, 1, l );

        if ( ner == 1 )
          fprintf ( 1, '\n' );
          fprintf ( 1, ' BISECT did not converge for x = %g\n', x );
          fprintf ( 1, '  Try reducing NBITS.\n' );
        end

        if ( w == we )
          nd = log10 ( 2.0 ^ nbits ) + 0.5;
        else
          nd = log10 ( abs ( we / ( w - we ))) + 0.5;
        end

        fprintf ( 1, '%17.8g      %17.8g      %17.8g      %3d\n', x, w, we, nd );

      end

    end

  end

  return
end


function [gx] = g_goNogo(x,P,u,in)
% generates the probability of 'go' choice: P(go) = sig(V/temperature+bias)
% function [gx] = g_goNogo(x,P,u,in)
% IN:
%   - x: value of the 'go' option
%   - P: log-temperature and bias
%   - u: [useless]
%   - in: [useless]
% OUT:
%   - gx: P(go) = sig(V/temperature+bias)

gx = 1./(1+exp(P-x));


function [D,Tc,res,dD,dTc,dRes,d2Phi] = distance(varargin)
% NP, JH 2006/11/25
%
% the distance function is phrased as
% D(y) = D(R,T,y) = phi(res(y))
%
% hence
% D   = phi(res(y))
% dD  = dPhi(res(y)) * dRes(y)
% d2D = res(y)' * d2Phi(res(y)) * dRes(y) + stuff we don't consider
%
% example SSD:
%   phi = hd/2 res'*res,  dPhi = hd * res', d2Phi = hd
%   res = T(y) - R,       dRes = dT

D     = [];
Tc    = [];
res   = [];
dD    = [];
dTc   = [];
dRes  = [];
d2Phi = [];
%
% -----------------------------------------------------------------------------
persistent PARA

if nargout>0 & nargin == 0,            % return PARA
    D  = PARA;
    return;
end;

% clear PARA
if isstr(varargin{1}) & strcmp(varargin{1},'clear'),
    PARA = [];
    return;
end;
% set PARA
if isstr(varargin{1}) & strcmp(varargin{1},'set'),
    PARA = setPARA(PARA,varargin{2:end});
    return;
end;
% -----------------------------------------------------------------------------

% -----------------------------------------------------------------------------
% do the work
% -----------------------------------------------------------------------------

Rc     = varargin{1};
TD     = varargin{2};
Omega  = varargin{3};
m      = varargin{4};
Y      = varargin{5};

doDerivative = (nargout > 3);

h   = Omega./m;
hd  = prod(h);
n   = prod(m);
%X   = getGrid(Omega,m);
%Rc  = interpolation(RD,Omega,X);
[Tc,dTc] = interpolation(TD,Omega,Y,doDerivative);

switch PARA.MODE,

    case 'SSD',
        % D   = phi(res(y))
        % dD  = dPhi(res(y)) * dRes(y)
        % d2D = res(y)' * d2Phi(res(y)) * dRes(y) + stuff we don't consider
        %
        % example SSD:
        %   phi = hd/2 res'*res,  dPhi = hd * res, d2Phi = hd
        %   res = T(y) - R,       dRes = dT

        res   = (Tc-Rc);
        D     = hd/2 * res' * res;

        if ~doDerivative, return; end;

        dRes  = dTc;
        dD    = hd * res' * dRes;
        d2Phi = hd;

    case 'SSD_W',
        % D   = phi(res(y))
        % dD  = dPhi(res(y)) * dRes(y)
        % d2D = res(y)' * d2Phi(res(y)) * dRes(y) + stuff we don't consider
        %
        % example SSD:
        %   phi = hd/2 res'*res,  dPhi = hd * res, d2Phi = hd
        %   res = T(y) - R,       dRes = dT

        mask   = varargin{6};

        res    = (Tc-Rc).*mask;
        D      = hd/2 * res' * res;

        if ~doDerivative, return; end;

        dRes  = sdiag(mask) * dTc;
        dD    = hd * res' * dRes;
        d2Phi = hd;

    case 'SSD_WP',
        % D   = phi(res(y))
        % dD  = dPhi(res(y)) * dRes(y)
        % d2D = res(y)' * d2Phi(res(y)) * dRes(y) + stuff we don't consider
        %
        % example SSD:
        %   phi = hd/2 res'*res,  dPhi = hd * res, d2Phi = hd
        %   res = T(y) - R,       dRes = dT
        period = PARA.period;
        period2 = period/2;

        mask   = varargin{6};

        res1   = Tc - Rc;

        p      = zeros(size(res1));
        p(res1 > period2)  = -period;
        p(res1 < -period2) =  period;

        res1   = res1 + p;
        res    = res1.*mask;
        D      = hd/2 * res' * res;

        if ~doDerivative, return; end;

        dRes  = sdiag(mask) * dTc;
        dD    = hd * res' * dRes;
        d2Phi = hd;

    case 'NGF',
        % D   = phi(res(y))
        % dD  = dPhi(res(y)) * dRes(y)
        % d2D = res(y)' * d2Phi(res(y)) * dRes(y) + stuff we don't consider
        %
        % example NGF:
        %   phi = hd/2 res'*res,  dPhi = hd * res, d2Phi = hd
        %   dR_i  = \nabla R_i / |\nabla R_i|_edge
        %   res = (\nabla T(y_i)' * dR_i) / ( |\nabla T(y_i)|_edge )
        %   dRes = complicated, see below

        edge = PARA.edge;
        [G1,G2,G3] = getGrad('c',Omega,m);
        if isempty(G3), G3 = 0; end;

        d1R = G1 * Rc(:);    d2R = G2 * Rc(:);    d3R = G3 * Rc(:);
        d1T = G1 * Tc(:);    d2T = G2 * Tc(:);    d3T = G3 * Tc(:);

        ndR = sqrt(d1R.^2 + d2R.^2 + d3R.^2 + edge^2);
        ndT = sqrt(d1T.^2 + d2T.^2 + d3T.^2 + edge^2);

        nd1R = d1R./ndR;      %nd1T = d1T./ndT;
        nd2R = d2R./ndR;      %nd2T = d2T./ndT;
        nd3R = d3R./ndR;      %nd3T = d3T./ndT;

        res1 = (nd1R.*d1T + nd2R.*d2T + nd3R.*d3T);
        res2 = 1./ndT;
        res  = res1 .* res2;

        D    = -hd/2 * res' * res;

        if ~doDerivative, return; end;

        dRes1 = sdiag(nd1R)*G1 + sdiag(nd2R)*G2 + sdiag(nd3R)*G3;
        dRes2 = -sdiag(1./ndT.^3)*(sdiag(d1T)*G1 + sdiag(d2T)*G2 + sdiag(d3T)*G3);
        dRes  = (sdiag(res2)*dRes1 + sdiag(res1)*dRes2) * dTc;

        dD    = -hd * res' * dRes;
        d2Phi = hd; % note the missing minus sign is not a bug!

    case 'MI'
        % D   = phi(res(y))
        % dD  = dPhi(res(y)) * dRes(y)
        % d2D = res(y)' * d2Phi(res(y)) * dRes(y) + stuff we don't consider
        %
        % example MI:
        %   phi   = res' * log(res + tol) + ...
        %   dPhi  = log(res + tol) + res./(res + tol) + ...
        %   d2Phi = (res + 2*tol)./(res + tol)^2 + ...
        %   res   = rho(T(y),R)
        %   dRes  = drho, see pdfestimate
        tol         = PARA.entropyTol;
        [rho,drho]  = pdfestimate(Rc,Tc,PARA,doDerivative);
        [n1,n2]     = size(rho);

        rhoR = sum(rho,2);
        rhoT = sum(rho,1)';
        rho  = rho(:);

        res  = rho;
        D    = rhoR'*log(rhoR+tol)+rhoT'*log(rhoT+tol) - rho'*log(rho+tol);

        if ~doDerivative, return; end;

        SR    = sparse(kron(ones(1,n2),speye(n1,n1)));
        ST    = sparse(kron(speye(n2,n2),ones(1,n1)));

        dPhi  = ...
            (log(rhoR+tol)+rhoR./(rhoR+tol))'*SR ...
            +(log(rhoT+tol)+rhoT./(rhoT+tol))'*ST ...
            -(log(rho +tol)+rho ./(rho +tol))';

        dRes  = drho*dTc;
        dD    = dPhi * dRes;

        d2Phi = ...
            SR'*sdiag((rhoR + 2*tol)./(rhoR+tol).^2)*SR ...
            +ST'*sdiag((rhoT + 2*tol)./(rhoT+tol).^2)*ST ...
            -sdiag((rho + 2*tol)./(rho+tol).^2);

        a = 1/sqrt(PARA.ngvR*PARA.ngvT);
        a = 1e0;
        d2Phi = - a * d2Phi;

    otherwise, error(sprintf('MODE = %s not implemented',MODE));
end;

return;

% =============================================================================
function d = sdiag(d);
d = spdiags(d,0,length(d),length(d));
% =============================================================================
function testMe(varargin)

fprintf('test %s\n',mfilename)

if nargin == 0,
    TD = zeros(100,100);
    TD(31:60,31:60) = 100;
    RD = TD + 1e1*randn(size(TD));
    Omega = [1,1];
    m     = [32,32];
    X  = getGrid(Omega,m);
    interpolation('set','MODE','linear-smooth');
    Rc = interpolation(RD,Omega,X);

    X = X+0.1;
end;

fctn = @(X) distance(Rc,TD,Omega,m,X);
testDerivative(fctn,X);

return;

% =============================================================================
function testDerivative(fctn,xc)

fprintf('derivative(%s)\n',mfilename)

[f,Tc,res,df]  = feval(fctn,xc);
v       = randn(size(xc));
dfv     = df*v;
hh      = logspace(0,-10,11);
fprintf('%12s %12s %12s \n','h','|fc-ft|','|fc+vdfc-ft|');
for j=1:length(hh),
    xt = xc + hh(j)*v;
    ft = feval(fctn,xt);
    n1 = norm(f(:)-ft(:));
    n2 = norm(f(:)+hh(j)*dfv-ft(:));
    fprintf('%12.4e %12.4e %12.4e\n',hh(j),n1,n2);
end;
return;
% =============================================================================

% ------------------------------------------------------------------------------
% set persistent parameter
% -----------------------------------------------------------------------------
function PARA = setPARA(PARA,varargin)

%disp(mfilename)
%varargin{:}

if ~isfield(PARA,'MODE') | isempty(getfield(PARA,'MODE')),
    PARA = setfield(PARA,'MODE','SSD');
end;

for j=1:length(varargin),
    if strcmp(varargin{j},'MODE'),
        PARA.MODE = varargin{j+1};
        varargin([j,j+1]) = [];
        break;
    end;
end;

fprintf('set distance MODE to %s, ',PARA.MODE);
switch PARA.MODE,
    case 'SSD',
        fprintf('no additional parameter needed\n');
    case 'SSD_W',
        fprintf('no additional parameter needed\n');
    case 'SSD_WP',
        if ~isfield(PARA,'period'), PARA.period = [];  end;
        if isempty(PARA.period),    PARA.period = 10;  end;
        for k=1:length(varargin)/2,
            str = sprintf('PARA=setfield(PARA,''%s'',varargin{%d});',...
                varargin{2*k-1},2*k);
            %disp(str);
            eval(str);
        end;
        fprintf('period=%s\n',num2str(PARA.period));
    case 'NGF',
        if ~isfield(PARA,'edge'), PARA.edge = [];  end;
        if isempty(PARA.edge),    PARA.edge = 10;  end;
        for k=1:length(varargin)/2,
            str = sprintf('PARA=setfield(PARA,''%s'',varargin{%d});',...
                varargin{2*k-1},2*k);
            %disp(str);
            eval(str);
        end;
        fprintf('edge=%s\n',num2str(PARA.edge));
    case 'MI',
        PARA = setMIpara(PARA,varargin{:});
        fprintf('setMIpara used\n');
    otherwise,
        error(sprintf('MODE = %s',MODE))
end;

return;
% -----------------------------------------------------------------------------
% -----------------------------------------------------------------------------