% Online Bayesian model selection demo.

% We generate data from the model A->B
% and compute the posterior prob of all 3 dags on 2 nodes:
%  (1) A B,  (2) A <- B , (3) A -> B
% Models 2 and 3 are Markov equivalent, and therefore indistinguishable from
% observational data alone.

% We control the dependence of B on A by setting
% P(B|A) = 0.5 - epislon and vary epsilon
% as in Koller & Friedman book p512

% ground truth
N = 2;
dag = zeros(N);
A = 1; B = 2;
dag(A,B) = 1;

ntrials = 100;
ns = 2*ones(1,N);
true_bnet = mk_bnet(dag, ns);
true_bnet.CPD{1} = tabular_CPD(true_bnet, 1, [0.5 0.5]);

% hypothesis space
G = mk_all_dags(N);
nhyp = length(G);
hyp_bnet = cell(1, nhyp);
for h=1:nhyp
  hyp_bnet{h} = mk_bnet(G{h}, ns);
  for i=1:N
    % We must set the CPTs to the mean of the prior for sequential log_marg_lik to be correct
    % The BDeu prior is score equivalent, so models 2,3 will be indistinguishable.
    % The uniform Dirichlet prior is not score equivalent...
    fam = family(G{h}, i);
    hyp_bnet{h}.CPD{i}= tabular_CPD(hyp_bnet{h}, i, 'prior_type', 'dirichlet', ...
				    'CPT', 'unif');
  end
end

clf
seeds = 1:3;
expt = 1;
for seedi=1:length(seeds)
  seed = seeds(seedi);
  rand('state', seed);
  randn('state', seed);

  es = [0.05 0.1 0.15 0.2];
  for ei=1:length(es)
    e = es(ei);
    true_bnet.CPD{2} = tabular_CPD(true_bnet, 2, [0.5+e 0.5-e; 0.5-e 0.5+e]);

    prior = normalise(ones(1, nhyp));
    hyp_w = zeros(ntrials+1, nhyp);
    hyp_w(1,:) = prior(:)';
    LL = zeros(1, nhyp);
    ll = zeros(1, nhyp);
    for t=1:ntrials
      ev = cell2num(sample_bnet(true_bnet));
      for i=1:nhyp
	ll(i) = log_marg_lik_complete(hyp_bnet{i}, ev);
	hyp_bnet{i} = bayes_update_params(hyp_bnet{i}, ev);
      end
      prior = normalise(prior .* exp(ll));
      LL = LL + ll;
      hyp_w(t+1,:) = prior;
    end

    % Plot posterior model probabilities
    % Red = model 1 (no arcs), blue/green = models 2/3 (1 arc)
    % Blue = model 2 (2->1)
    % Green = model 3 (1->2, "ground truth")

    subplot2(length(seeds), length(es), seedi, ei);
    m = size(hyp_w,1);
    h=plot(1:m, hyp_w(:,1), 'r-',  1:m, hyp_w(:,2), 'b-.', 1:m, hyp_w(:,3), 'g:');
    axis([0 m   0 1])
    %title('model posterior vs. time')
    title(sprintf('e=%3.2f, seed=%d', e, seed));
    drawnow
    expt = expt + 1;
  end
end


function [N,E] = rentian_scaling_2d(A,XY,n,tol)
% RENTIAN_SCALING_2D    Rentian scaling for networks embedded in two dimensions.
%
% [N,E] = rentian_scaling_2d(A,XY,n,tol)
%
% Physical Rentian scaling (or more simply Rentian scaling) is a property
% of systems that are cost-efficiently embedded into physical space. It is
% what is called a "topo-physical" property because it combines information
% regarding the topological organization of the graph with information
% about the physical placement of connections. Rentian scaling is present
% in very large scale integrated circuits, the C. elegans neuronal network,
% and morphometric and diffusion-based graphs of human anatomical networks.
% Rentian scaling is determined by partitioning the system into cubes,
% counting the number of nodes inside of each cube (N), and the number of
% edges traversing the boundary of each cube (E). If the system displays
% Rentian scaling, these two variables N and E will scale with one another
% in loglog space. The Rent's exponent is given by the slope of log10(E)
% vs. log10(N), and can be reported alone or can be compared to the
% theoretical minimum Rent's exponent to determine how cost efficiently the
% network has been embedded into physical space. Note: if a system displays
% Rentian scaling, it does not automatically mean that the system is
% cost-efficiently embedded (although it does suggest that). Validation
% occurs when comparing to the theoretical minimum Rent's exponent for that
% system.
%
% INPUTS:
%
% A:              MxM adjacency matrix.
%                 Must be unweighted, binary, and symmetric.
% XY:             Matrix of node placement coordinates.
%                 Must be in the form of an Mx2 matrix [x y], where M is the
%                 number of nodes and x and y are column vectors of node
%                 coordinates.
% n:              Number of partitions to compute. Each partition is a data
%                 point. You want a large enough number to adequately
%                 estimate the Rent's exponent.
% tol:            This should be a small value (for example 1e-6).
%                 In order to mitigate the effects of boundary conditions due
%                 to the finite size of the network, we only allow partitions
%                 that are contained within the boundary of the network. This
%                 is achieved by first computing the volume of the convex
%                 hull of the node coordinates (V). We then ensure that the
%                 volume of the convex hull computed on the original node
%                 coordinates plus the coordinates of the randomly generated
%                 partition (Vnew) is within a given tolerance of the
%                 original (i.e. check abs(V - Vnew) < tol). Thus tol, should
%                 be a small value in order to make sure the partitions are
%                 contained largely within the boundary of the network, and
%                 thus the number of nodes and edges within the box are not
%                 skewed by finite size effects.
%
% OUTPUTS:
%
% N:              nx1 vector of the number of nodes in each of the n partitions.
% E:              nx1 vector of the number of edges crossing the boundary of
%                 each partition.
%
% Subsequent Analysis:
%
%     Rentian scaling plots are created by: figure; loglog(E,N,'*');
%
%     To determine the Rent's exponent, p, we need to determine
%     the slope of E vs. N in loglog space, which is the Rent's
%     exponent. There are many ways of doing this with more or less
%     statistical rigor. Robustfit in MATLAB is one such option:
%
%         [b,stats] = robustfit(log10(N),log10(E))
%
%     Then the Rent's exponent is b(1,2) and the standard error of the
%     estimation is given by stats.se(1,2).
%
% Note: n=5000 was used in Bassett et al. 2010 in PLoS CB.
%
% Reference:
% Danielle S. Bassett, Daniel L. Greenfield, Andreas Meyer-Lindenberg,
% Daniel R. Weinberger, Simon W. Moore, Edward T. Bullmore. Efficient
% physical embedding of topologically complex information processing
% networks in brains and computer circuits. PLoS Comput Biol, 2010,
% 6(4):e1000748.
%
% Modification History:
%
%     2010:     Original (Dani Bassett)
%     Dec 2016: Updated code so that both partition centers and partition
%               sizes are chosen at random. Also added in a constraint on
%               partition placement that prevents boxes from being located
%               outside the edges of the network. This helps prevent skewed
%               results due to boundary effects arising from the finite size
%               of the network. (Lia Papadopoulos)

% determine the number of nodes in the system
M = numel(XY(:,1));

% rescale coordinates so that they are all greater than unity
XYn = XY - repmat(min(XY)-1,M,1);

% compute the area of convex hull (i.e. are of the boundary) of the network
[~,V] = convhull(XYn(:,1),XYn(:,2));

% min and max network coordinates
xmin = min(XYn(:,1));
xmax = max(XYn(:,1));
ymin = min(XYn(:,2));
ymax = max(XYn(:,2));

% initialize vectors of number of nodes in box and number of edges crossing
% box

N = zeros(n,1);
E = zeros(n,1);

% create partitions, and count the number of nodes inside the partition (N)
% and the number of edges traversing the boundary of the partition (E)

nPartitions = 0;

while nPartitions<(n+1)

    % variable to check if partition center is within network boundary
    % OK if inside == 1
    inside = 0;

    while inside == 0

        % pick a random (x,y) coordinate to be the center of the box
        randx = xmin+(xmax-xmin)*rand(1);
        randy = ymin+(ymax-ymin)*rand(1);

        % make sure the point is inside the convex hull of the network
        newCoords = [XYn; [randx randy]];
        [~,Vnew] = convhull(newCoords(:,1),newCoords(:,2));

        % if the old convex hull area and new convex hull area are equal
        % then the box center must be inside the network boundary.

        if isequal(V,Vnew)==0
            inside = 0;
        else
            inside = 1;
        end

    end

    % determine the approximate maximum distance the box can extend, given
    % the center point and the  bounds of the network
    deltaY = min(abs(ymax-randy),abs(ymin-randy));
    deltaX = min(abs(xmax-randx),abs(xmin-randx));
    deltaLmin = min(deltaY,deltaX);

    % variable to check if partition is within network boundary
    % OK if inside == 1
    inside = 0;

    while inside == 0

        % pick a random (side length)/2 that is between 0 and the
        % max possible
        deltaL = deltaLmin*rand(1);

        % (x,y) coordinates for corners of box
        boxCoords = [randx - deltaL randy - deltaL; ...
            randx - deltaL randy + deltaL; ...
            randx + deltaL randy - deltaL; ...
            randx + deltaL randy + deltaL];

        % check if all corners of box are inside the convex hull of the
        % network
        newCoords = [XYn; boxCoords];
        [~,Vnew] = convhull(newCoords(:,1),newCoords(:,2));

        % make sure the new convex hull that includes the partition corners
        % is within a certain tolerance of the original convex hull area.

        if abs(V-Vnew)>tol
            inside = 0;
        else
            inside = 1;
        end
    end

    % Find nodes inside the box, edges crossing the boundary

    L = find(XYn(:,1)>(randx-deltaL) & XYn(:,1)<(randx+deltaL) ...
        & XYn(:,2)>(randy-deltaL) & XYn(:,2)<(randy+deltaL));

    if ~isempty(L) == 1
        nPartitions = nPartitions+1;
        % count edges crossing the boundary of the box
        E(nPartitions,1) = sum(sum(A(L,setdiff(1:M,L))));
        % count nodes inside of the box
        N(nPartitions,1) = numel(L);

    end

end

return;


function [fFactorHi, fStdHi, fResHi, fPerHi, fFactorLow, fStdLow, fResLow, fPerLow, fMRateFit] = calc_ratefac(mCat1, mCat2, fPeriod1, fPeriod2, fMc1, fMc2)
% [fFactorHi, fStdHi, fResHi, fPerHi, fFactorLow, fStdLow, fResLow, fPerLow, fMRateFit] = calc_ratefac(mCat1, mCat2, fPeriod1, fPeriod2, fMc1, fMc2)
% --------------------------------------------------------------------------------------------------------------------------------------------------
% Determine rate factor between amount of events between two time periods, differentiated for EQ with M<Mc and M>=Mc.
% Mc = min(fMc1, fMc2);
% N(Per2) = fFac * N (Per1)
% fFac is determined as mean of the factors in the magnitude bins, using a binning of 0.1.A factor for
% magnitudes M<Mc and M>=Mc is determined to explore a difference.Magnitude bins with no earthquakes
% are not used.
%
% Incoming variables:
% mCat1 : EQ catalog period 1
% mCat2 : EQ catalog period 2
% fPeriod1 : Time length period 1 /[dec. year]
% fPeriod2 : Time length period 2 /[dec. year]
% fMc1 : Magnitude of completeness period 1
% fMc2 : Magnitude of completeness period 2
%
% Outgoing variable:
% fFactorHi : Mean rate factor for EQ with M>=Mc
% fStdLow   : Standard deviation of fFactorHi
% fResLow   : Residuum for model application
% fPerHi    : Total percentage of change for EQ with M>=Mc
% fFactorLow : Mean rate factor for EQ with M<Mc
% fStdHi   : Standard deviation offFactorLow
% fResHi   : Residuum for model application
% fPerLow  : Total percentage of change for EQ with M<Mc
% fMRateFit: Goodness of fit by modeling second period with rate factor multiplication of first period
%
% J. Woessner, woessner@seismo.ifg.ethz.ch
% last update: 09.10.02

% Track of changes:

% Initialization
vMaxMag = [max(mCat1(:,6)) max(mCat2(:,6))];
fMaxMag =  max(vMaxMag);
fMc = min([fMc1 fMc2]);

% Binning
%% EQ with M < Mc
vSel1 = (mCat1(:,6) < fMc);
vSel2 = (mCat2(:,6) < fMc);
mCat1Mc = mCat1(vSel1,:);
mCat2Mc = mCat2(vSel2,:);
[vMag1,vBin1]=histogram(mCat1Mc(:,6),0:0.1:fMaxMag);
[vMag1sort,vBin1sort]=sort(vMag1);
[vMag2,vBin2]=histogram(mCat2Mc(:,6),0:0.1:fMaxMag);
[vMag2sort,vBin2sort]=sort(vMag2);

%% EQ with M >= Mc
mCat1McHi = mCat1(~vSel1,:);
mCat2McHi = mCat2(~vSel2,:);
[vMag1Hi,vBin1Hi]=histogram(mCat1McHi(:,6),0:0.1:fMaxMag);
[vMag1Hisort,vBin1Hisort]=sort(vMag1Hi);
[vMag2Hi,vBin2Hi]=histogram(mCat2McHi(:,6),0:0.1:fMaxMag);
[vMag2Hisort,vBin2Hisort]=sort(vMag2Hi);

%% Overall percentage of change of EQs
fPerLow = length(mCat2Mc(:,1))/length(mCat1Mc(:,1));
fPerHi = length(mCat2McHi(:,1))/length(mCat1McHi(:,1));
fPerLow = 100*fPerLow-100;
fPerHi = 100*fPerHi-100;

% Estimate factor between rates
% Factor for values below Mc of Period 1
vMag1 = vMag1/fPeriod1; % Time normalization
vMag2 = vMag2/fPeriod2; % Time normalization
fRatioLow = vMag2./vMag1;
%vSelRat = 1 - (isnan(fRatioLow) + isinf(fRatioLow));
vSelRatLow = (~isnan(fRatioLow) & ~isinf(fRatioLow));
fFactorLow = mean(fRatioLow(vSelRatLow));
fStdLow = std(fRatioLow(vSelRatLow));
vMagModelow = vMag1*fFactorLow;
fResLow = sqrt(sum((vMag2-(vMagModelow)).^2)/length(vMag2)); % Residuum

% Factor for values above or equal Mc of background
vMag1Hi = vMag1Hi/fPeriod1; % Time normalization
vMag2Hi = vMag2Hi/fPeriod2; % Time normalization
fRatioHi = vMag2Hi./vMag1Hi;
%vSelRatHi = 1 - (isnan(fRatioHi) + isinf(fRatioHi));
vSelRatHi = (~isnan(fRatioHi) & ~isinf(fRatioHi));
fFactorHi = mean(fRatioHi(vSelRatHi));
fStdHi = std(fRatioHi(vSelRatHi));
% vSelStd = (fRatioHi <= (fFactorHi+fStdHi) & fRatioHi >= (fFactorHi-fStdHi));
% fFactorHi = mean(fRatioHi(vSelStd))
vMagModel = vMag1Hi*fFactorHi;
fResHi = sqrt(sum((vMag2Hi-(vMagModel)).^2)/length(vMag2Hi)); % Residuum

% Determine goodness fit of modeled activity or M>=Mc
vEventsum2 = cumsum(vMag2Hi);
vEventsumMod = cumsum(vMagModel);
fMRateFit  = sum(abs(vEventsum2-vEventsumMod))/sum(vEventsum2);
fMRateFit = 100-fMRateFit*100;


function [basis,gradbasis]=legs(x,dir,n,scale)
% usage: [basis,gradbasis]=legs(x,dir,n,scale)
%
% returns the values and directional derivatives  of (n+1)^2-1 basis functions
% constructed from spherical harmonics at locations given in x and, for the
% gradients, for (in general non-normalized) directions given in dir.
%
% input: x      set of N locations given as an Nx3 matrix
%        dir    set of N direction vectors given as an Nx3 matrix
%                  (dir is not normalized (it hence can be a dipole moment))
%        n       order of spherical harmonics
%
% output: basis: Nx((n+1)^2-1)  matrix containing in the j.th  row the real
%                and imaginary parts of r^kY_{kl}(theta,Phi)/(N_{kl}*scale^k) ( (r,theta,phi)
%                are the spherical coordinates corresponding to  the j.th row in x)
%                for k=1 to n and l=0 to k
%                the order is:
%                          real parts for k=1 and l=0,1 (2 terms)
%                  then    imaginary parts for k=1 and l=1 (1 term)
%                  then    real parts for k=2 and l=0,1,2 (3 terms)
%                  then    imaginary parts for k=2 and l=1,2 (2 term)
%                              etc.
%                   the spherical harmonics are normalized with
%                   N_{kl}=sqrt(4pi (k+l)!/((k-l)!(2k+1)))
%                    the phase does not contain the usual (-1)^l term !!!
%                   scale is constant preferably set to the avererage radius
%
%         gradbasis: Nx((n+1)^2-1) matrix containing in the j.th row the scalar
%                     product of the gradient of the former with the j.th row of dir

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

[n1,n2]=size(x);

comi=sqrt(-1);

normalize=ones(n,n+1);
facto=ones(2*n+2,1);for i=3:2*n+2,facto(i)=facto(i-1)*(i-1);end;
for i=1:n
    for j=1:i+1
        if j==1
            normalize(i,j)=scale^i*i*sqrt(2*facto(i+j-1+1)/facto(i-j+1+1)/(2*i+1));
        else
            normalize(i,j)=scale^i*i*sqrt(facto(i+j-1+1)/facto(i-j+1+1)/(2*i+1));
        end
    end
end
normalize=reshape(repmat(reshape(normalize,n*(n+1),1),1,n1),n,n+1,n1);

rad=sqrt(x(:,1).^2+x(:,2).^2+x(:,3).^2);
phi=angle(x(:,1)+comi*x(:,2));
costheta=x(:,3)./(rad+eps);

ms=0:n;
ns=1:n;

shiftfactors=zeros(n,n+1);
shiftminusfactors=zeros(n,n+1);
for in=1:n; for im=1:n+1
        shiftfactors(in,im)=in+im-1;
        shiftminusfactors(in,im)=(in+im-2)*(in+im-1);
    end;end;
for in=1:n
    shiftminusfactors(in,1)=1;
end

leg0=zeros(n,n+1,n1);

for i=1:n
 p=legendre(i,costheta);
 leg0(i,1:i+1,:)=p;
end
 leg0=leg0.*reshape(repmat((-1).^ms,n,n1),n,n+1,n1);
ephi=exp(comi*ms'*phi');

leg0=leg0.*reshape(repmat(reshape(ephi,1,(n+1)*n1),n,1),n,n+1,n1)...
         .*reshape(repmat((repmat(rad,1,n).^repmat(ns,n1,1))',n+1,1),n,n+1,n1);

     onesx=zeros(1,n+1,n1);
     onesx(1,1,:)=ones(1,1,n1);

legshift=[onesx;leg0(1:n-1,:,:)];
legshiftminus=[-conj(legshift(:,2,:)),legshift(:,1:n,:)].*reshape(repmat(reshape(shiftminusfactors,n*(n+1),1),n1,1),n,n+1,n1);
legshiftplus=-[legshift(:,2:n+1,:),zeros(n,1,n1)];
legshift=legshift.*reshape(repmat(reshape(shiftfactors,n*(n+1),1),n1,1),n,n+1,n1);

dirp=[(dir(:,1)+dir(:,2)/comi)/2,(dir(:,1)-dir(:,2)/comi)/2,dir(:,3)];

gradleg= reshape(repmat(transpose(dirp(:,1)),n*(n+1),1),n,n+1,n1).*legshiftplus...
        +reshape(repmat(transpose(dirp(:,2)),n*(n+1),1),n,n+1,n1).*legshiftminus...
        +reshape(repmat(transpose(dirp(:,3)),n*(n+1),1),n,n+1,n1).*legshift;

leg0=leg0./normalize;
gradleg=gradleg./normalize;

    basis=zeros(n1,(n+1)^2-1);gradbasis=zeros(n1,(n+1)^2-1);
 for i=1:n
     basis(:,i^2:i^2+i)=(reshape(real(leg0(i,1:i+1,:)),i+1,n1))';
     basis(:,i^2+i+1:(i+1)^2-1)=(reshape(imag(leg0(i,2:i+1,:)),i,n1))';
     gradbasis(:,i^2:i^2+i)=(reshape(real(gradleg(i,1:i+1,:)),i+1,n1))';
     gradbasis(:,i^2+i+1:(i+1)^2-1)=(reshape(imag(gradleg(i,2:i+1,:)),i,n1))';
 end

return;


function [V,C] = calcVoronoi(v,varargin)
% compute the area of the Voronoi decomposition
%
% Input
%  v - @vector3d
%
% Output
%  V - list of Voronoi--Vertices
%  C - cell array of Voronoi--Vertices per generator
%
% See also
% voronoin

n = length(v);
v = reshape(v,[],1);

[x,y,z] = double(v);
faces = convhulln([x(:) y(:) z(:)],{'Qt','Pp','QJ'}); % delauny triangulation on sphere

% voronoi-vertices
V = normalize(cross(v.subSet(faces(:,3))-v.subSet(faces(:,1)),...
  v.subSet(faces(:,2))-v.subSet(faces(:,1))));

% voronoi-vertices around generators
[center, vertices] = sort(faces(:));

v = v.subSet(center);
vert = repmat(V,3,1);
vert = vert.subSet(vertices);

% the azimuth of a voronoi-vertex relativ to its generator
[ignore,azimuth] = polar(hr2quat(v,zvector).*cross(v,vert));

% sort the vertices clockwise around with respect to its center
[ignore,left] = sortrows([center azimuth]); %#ok<*ASGLU>

left = mod(vertices(left)'-1,length(V))+1;

% now we delete duplicated voronoi vertices
eps = 10^-10; % machine precision
[ignore,first,ind] = unique(round(squeeze(double(V))/eps)/eps,'rows');
V = V.subSet(first);
left = ind(left)';

% erase duplicated vertices in the pointer list
dublicated = find([diff(left)==0,false]);

% check whether the duplicated is in the next cell // they shouldn't be
% deleted
last = [0;find(diff(center));length(center)];
dublicated(ismember(dublicated,last)) = [];

left(dublicated) = [];
center(dublicated) = [];

C = cell(n,1);
last = [0;find(diff(center));length(center)];
for k=1:numel(last)-1
  ndx = last(k)+1:last(k+1);
  C{center(ndx(1))} = left( ndx );
end


function featuresS = ngldmToScalarFeatures(s,numVoxels)
% function featuresS = ngldmToScalarFeatures(s,numVoxels)
%
% APA, 03/16/2017

% Coarseness
Ns = sum(s(:));
Nn = size(s,2);
Ng = size(s,1);
lenV = 1:Nn;
levV = 1:Ng;

% Low dependence emphasis
sLdeM = bsxfun(@rdivide,s,lenV.^2);
featuresS.LowDependenceEmphasis = sum(sLdeM(:))/Ns;

% High dependence emphasis
sHdeM = bsxfun(@times,s,lenV.^2);
featuresS.HighDependenceEmphasis = sum(sHdeM(:))/Ns;

% Low grey level count emphasis
sLgceM = bsxfun(@rdivide,s',(1:Ng).^2);
featuresS.LowGrayLevelCountEmphasis = sum(sLgceM(:))/Ns;

% High grey level count emphasis
sHgceM = bsxfun(@times,s',(1:Ng).^2);
featuresS.HighGrayLevelCountEmphasis = sum(sHgceM(:))/Ns;

% Low dependence low grey level emphasis
sLdlgeM = bsxfun(@rdivide,bsxfun(@rdivide,s,lenV.^2)',(1:Ng).^2);
featuresS.LowDependenceLowGrayLevelEmphasis = sum(sLdlgeM(:))/Ns;

% Low dependence high grey level emphasis
sLdhgeM = bsxfun(@times,bsxfun(@rdivide,s,lenV.^2)',(1:Ng).^2);
featuresS.LowDependenceHighGrayLevelEmphasis = sum(sLdhgeM(:))/Ns;

% High dependence low grey level emphasis
sHdlgeM = bsxfun(@rdivide,bsxfun(@times,s,lenV.^2)',(1:Ng).^2);
featuresS.HighDependenceLowGrayLevelEmphasis = sum(sHdlgeM(:))/Ns;

% High dependence high grey level emphasis
sHdhgeM = bsxfun(@times,bsxfun(@times,s,lenV.^2)',(1:Ng).^2);
featuresS.HighDependenceHighGrayLevelEmphasis = sum(sHdhgeM(:))/Ns;

% Grey level non-uniformity
featuresS.GrayLevelNonuniformity = sum(sum(s,2).^2)/Ns;

% Grey level non-uniformity normalised
featuresS.GrayLevelNonuniformityNorm = sum(sum(s,2).^2)/Ns^2;

% Dependence count non-uniformity
featuresS.DependenceCountNonuniformity = sum(sum(s,1).^2)/Ns;

% Dependence count non-uniformity normalised
featuresS.DependenceCountNonuniformityNorm = sum(sum(s,1).^2)/Ns^2;

% Dependence count percentage
featuresS.DependenceCountPercentage = Ns/numVoxels;

% Grey level variance
iPij = bsxfun(@times,s'/sum(s(:)),levV);
mu = sum(iPij(:));
iMinusMuPij = bsxfun(@times,s'/sum(s(:)),(levV-mu).^2);
featuresS.GrayLevelVariance = sum(iMinusMuPij(:));

% Dependence count variance
jPij = bsxfun(@times,s/sum(s(:)),lenV);
mu = sum(jPij(:));
jMinusMuPij = bsxfun(@times,s/sum(s(:)),(lenV-mu).^2);
featuresS.DependenceCountVariance = sum(jMinusMuPij(:));

% Dependence count entropy
p = s(:)/sum(s(:));
featuresS.Entropy = -sum(p .* log2(p+eps));

% Dependence count energy
p = s(:)/sum(s(:));
featuresS.Energy = sum(p .^2);


function m = medianw( x, w, dim )
% Fast weighted median.
%
% Computes the weighted median of a set of samples.
%  http://en.wikipedia.org/wiki/Weighted_median
% "A weighted median of a sample is the 50% weighted percentile."
% For matrices computes median along each column (or dimension dim).
% If all weights are equal to 1 gives identical results to median.
%
% USAGE
%  m = medianw( x, w, [dim] )
%
% INPUTS
%  x      - vector or array of samples
%  w      - vector or array of weights
%  dim    - dimension along which to compute median
%
% OUTPUTS
%  m      - weighted median value of x
%
% EXAMPLE - simple toy example
%  x=[1 2 3]; w=[1 1 5]; medianw(x,w)
%
% EXAMPLE - comparison to median
%  n=randi(100); m=randi(100);
%  x=rand(n,m); w=ones(n,m);
%  m1=median(x); m2=medianw(x,w);
%  assert(isequal(m1,m2))
%
% See also median
%
% Piotr's Image&Video Toolbox      Version 3.24
% Copyright 2013 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

if(nargin<3), dim=find(size(x)~=1,1); end
d=dim; nd=ndims(x); n=numel(x);

if( n==1 || size(x,d)==1 )
  m=x;
elseif( length(x)==n )
  [x,o]=sort(x); w=w(o); w=cumsum(w);
  w=w/w(end); [~,j]=min(w<=.5);
  if(j==1 || w(j-1)~=.5), m=x(j);
  else m=(x(j-1)+x(j))/2; end
else
  if(d>1), p=[d 1:d-1 d+1:nd]; x=permute(x,p); w=permute(w,p); end
  [x,o]=sort(x); w=w(o); w=cumsum(w); is={':'}; is=is(ones(1,nd-1));
  w=bsxfun(@rdivide,w,w(end,is{:})); [~,j]=min(w<=.5);
  s=size(x); s=reshape(((1:n/s(1))-1)*s(1),size(j));
  j0=max(1,j-1); j0=j0+s; j=j+s;
  same=w(j0)~=.5; j0(same)=j(same); m=(x(j0)+x(j))/2;
  if(d>1), p=[2:d 1 d+1:nd]; m=permute(m,p); end
end

end


function [ n_data, x, bip ] = airy_bi_prime_values ( n_data )

%*****************************************************************************80
%
%% AIRY_BI_PRIME_VALUES returns some values of the Airy function Bi'(x).
%
%  Discussion:
%
%    The Airy functions Ai(X) and Bi(X) are a pair of linearly independent
%    solutions of the differential equation:
%
%      W'' - X * W = 0
%
%    In Mathematica, the function can be evaluated by:
%
%      AiryBiPrime[x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real BIP, the derivative of the Airy BI function.
%
  n_max = 11;

  bip_vec = [ ...
     0.4482883573538264E+00, ...
     0.4515126311496465E+00, ...
     0.4617892843621509E+00, ...
     0.4800490287524480E+00, ...
     0.5072816760506224E+00, ...
     0.5445725641405923E+00, ...
     0.5931444786342857E+00, ...
     0.6544059191721400E+00, ...
     0.7300069016152518E+00, ...
     0.8219038903072090E+00, ...
     0.9324359333927756E+00 ];

  x_vec = [ ...
     0.0E+00, ...
     0.1E+00, ...
     0.2E+00, ...
     0.3E+00, ...
     0.4E+00, ...
     0.5E+00, ...
     0.6E+00, ...
     0.7E+00, ...
     0.8E+00, ...
     0.9E+00, ...
     1.0E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    bip = 0.0;
  else
    x = x_vec(n_data);
    bip = bip_vec(n_data);
  end

  return
end


function[varargout]=cellgrid(varargin)
%CELLGRID  Interpolate a cell array of numeric arrays onto a regular grid.
%
%   [TO,Y]=CELLGRID(T,X,DT) where T is a cell array of time arrays, and
%   X is of the same size as T, linearly interpolates the elements of X
%   within T at times TO, which are regularly spaced with interval DT.
%
%   DT may either be a scalar, or an array of the same length as T and X.
%
%   CELLGRID does not modify bad data points, such as those marked with
%   NaNs.  See CELLFILL to interpolate over bad data points.
%
%   As an example, with T and X given by
%
%       T{1} = [1 2 3  5  6]';   T{2}=[3 7  9 10]';
%       X{1} = [2 4 6 10 12]';   X{2}=[5 9 11 12]';
%
%   [TO,Y]=CELLGRID(T,X,1) will return
%
%       TO{1} = [1 2 3 4  5  6]';   TO{2} = [3 4 5 6 7 8  9  10]';
%       Y{1}  = [2 4 6 8 10 12]';   Y{2}  = [5 6 7 8 9 10 11 12]';
%
%   By default, CELLGRID uses INTERP1 with the 'pchip' method of
%   interpolation.  CELLGRID(...,STR) instead uses the method specified by
%   STR, e.g. STR='linear'.  See INTERP1 for details.
%
%   [TO,Y1,Y2,...YN]=CELLGRID(T,X1,X2,...XN,DT) with multiple input
%   arguments also works provided the XN are all the same size.
%
%   CELLGRID(X1,X2,...XN); with no output arguments overwrites the
%   original input variables.
%   __________________________________________________________________
%
%   Specifying interpolated times
%
%   [TO,Y]=CELLGRID(T,X,DT,A,B) will set TO to TO=A:DT:B, where A and B
%   are either scalars or an arrays of the same size as X.
%
%   The default behavior is equivalent to choosing A and B as the first and
%   last elements of TO, that is, to setting A=CELLMIN(T) and B=CELLMAX(T).
%   __________________________________________________________________
%
%   Parallelization
%
%   CELLGRID(...,'parallel') parallelizes the computation using a PARFOR
%   loop over the various input variables.  This requires that Matlab's
%   Parallel Computing Toolbox be installed.
%   __________________________________________________________________
%
%   'cellgrid --t' runs a test.
%
%   Usage: [to,y]=cellgrid(t,x,dt);
%          [to,y1,y2,y3]=cellgrid(t,x1,x2,x3,dt);
%          [to,y1,y2,y3]=cellgrid(t,x1,x2,x3,dt,a,b);
%          cellgrid(t,x1,x2,x3,dt);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2015--2019 J.M. Lilly --- type 'help jlab_license' for details

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

str='pchip';
cores='serial';

for i=1:2
    if ischar(varargin{end})
        if strcmpi(varargin{end}(1:3),'ser')||strcmpi(varargin{end}(1:3),'par')
            cores=varargin{end};
        else
            str=varargin{end};
        end
        varargin=varargin(1:end-1);
    end
end

if strcmpi(cores(1:3),'par')
    if exist('parpool')~=2
        disp('Sorry, parallel algorithm requires the Parallel Computing Toolbox.')
        disp('Defaulting to the serial algorithm.')
        str='serial';
    end
end

t=varargin{1};

if ~iscell(varargin{end-2})&&~ischar(varargin{end-2})
    a=varargin{end-1};
    b=varargin{end};
    if length(a)==1
        a=a+zeros(size(t));
    end
    if length(b)==1
        b=b+zeros(size(t));
    end
    varargin=varargin(1:end-2);
else
    a=cellmin(t);
    b=cellmax(t);
end

dt=varargin{end};

varargin=varargin(1:end-1);

%Size check
for i=2:length(varargin)
    if size(varargin{i})~=size(varargin{1});
        error('All input arguments must be the same size.')
    end
end

if length(dt)==1
    dt=dt+0*a;
end

for i=1:length(t)
   to{i,1}=[a(i):dt(i):b(i)]';
end

if strcmpi(cores(1:3),'par')
    disp('CELLGRID employing parallel algorithm.')
    parfor j=2:length(varargin)
        for i=1:length(t)
            if length(t{i})>1
                bool=~isnan(t{i})&~isnan(varargin{j}{i});
                if length(find(bool))~=0
                    args{j}{i,1}=interp1(t{i}(bool),varargin{j}{i}(bool),to{i},str);
                else
                    args{j}{i,1}=nan*to{i};
                end
            else
                args{j}{i,1}=varargin{j}{i};
            end
        end
    end
else
    for j=2:length(varargin)
        for i=1:length(t)
            if length(t{i})>1
                bool=~isnan(t{i})&~isnan(varargin{j}{i});
                if length(find(bool))>=2
                    %i,j,length(find(bool))
                    args{j}{i,1}=interp1(t{i}(bool),varargin{j}{i}(bool),to{i},str);
                else
                    args{j}{i,1}=nan*to{i};
                end
            else
                args{j}{i,1}=varargin{j}{i};
            end
        end
    end
end

varargout=args;
if ~isempty(varargout{2})
    for i=1:length(to)
        if isempty(varargout{2}{i})
            to{i}=[];
        end
    end
else
    to=[];
end
varargout{1}=to;

eval(to_overwrite(length(varargin)))

function[]=cellgrid_test

t{1}=[1 2 3  5  6]';
t{2}=[3 7  9 10]';
x{1}=[2 4 6 10 12]';
x{2}=[5 9 11 12]';

to{1}=[1 2 3 4  5  6]';
to{2}=[3 4 5 6 7 8  9  10]';
y{1}=[2 4 6 8 10 12]';
y{2}=[5 6 7 8 9 10 11 12]';

cellgrid(t,x,1);

reporttest('CELLGRID',aresame(x,y)&&aresame(to,t))


function [f,Ph]=haralick_n(qs,nL,q, nanFlag)
% Haralick textures measurements

Ph = coocurrance_alldir_mod(q);
% last row and column corresponds to outside ROI!
if nanFlag
    Ph=Ph(1:end-1,1:end-1);
    nL=nL-1;
end

%compute features
R=sum(Ph(:));
Ph=Ph/R;
% Energy (1)
f(1)=sum(sum(Ph.^2));
% Contrast (2)
f(2)=0.0;
for n=0:nL-1
   temp=0;
   for i=1:nL
      for j=1:nL
         if (abs(i-j) == n)
            temp=temp+Ph(i,j);
         end
      end
   end
   f(2)=f(2)+n^2*temp;
end
% Correlation
%using symmetry Ph'=Ph!
Px=sum(Ph);
Py=sum(Ph');
vec=[1:nL];
ux=sum(Px .*vec);
uy=sum(Py .*vec);

varx=sum(Px .* vec.^2)-ux^2;
sigx=sqrt(varx);
vary=sum(Py .* vec.^2)-uy^2;
sigy=sqrt(vary);
u=vec*Ph(i,j)*vec';
f(3)=(u-ux*uy)/(sigx*sigy);

%Entropy (3)
res = 1e-10; % realmin
f(4)=-sum(sum(Ph.*log2(Ph+res))); % log????

% variance
f(5)=0;
for i=1:nL
   for j=1:nL
      f(5)=f(5)+(i-u)^2*Ph(i,j);
   end
end
% P(x+y)
f(6)=0;  % sum of entropy...
for k=2:2*nL
   temp=0.0;
   for i=1:nL
      for j=1:nL
         if ((i+j) == k)
            temp=temp+Ph(i,j);
         end
      end
   end
   Pxpy(k)=temp;
   f(6)=f(6)-temp*log2(temp+res);
end

% inverse different moment..
f(7)=0;
f(8)=0;  %Homogeneity (4)
for i=1:nL
   for j=1:nL
      temp1=1/(1+(i-j)^2)*Ph(i,j);
      temp2=1/(1+abs(i-j))*Ph(i,j);
      f(7)=f(7)+temp1;
      f(8)=f(8)+temp2;
   end
end

return


function [ vX, mX ] = SolveLsLInfProx( mA, vB, paramLambda, numIterations )
% ----------------------------------------------------------------------------------------------- %
%[ vX, mX ] = SolveLsL1Prox( mA, vB, lambdaFctr, numIterations )
% Solve L Infinity Regularized Least Squares Using Proximal Gradient (PGM) Method.
% Input:
%   - mA                -   Input Matirx.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vB                -   input Vector.
%                           The model known data.
%                           Structure: Vector (m X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           The L Infinity Regularization parameter.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - numIterations     -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% Output:
%   - vX                -   Output Vector.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References
%   1.  Wikipedia PGM - https://en.wikipedia.org/wiki/Proximal_gradient_method.
% Remarks:
%   1.  Using vanilla PGM.
% Known Issues:
%   1.  A
% TODO:
%   1.  B
% Release Notes:
%   -   1.0.000     23/08/2017
%       *   First realease version.
% ----------------------------------------------------------------------------------------------- %

mAA = mA.' * mA;
vAb = mA.' * vB;
vX  = pinv(mA) * vB; %<! Dealing with "Fat Matrix"

stopThr = 1e-6;

stepSize = 1 / (2 * (norm(mA, 2) ^ 2));
% stepSize = 1 / sum(mA(:) .^ 2); %<! Faster to calculate, conservative (Hence slower)

mX = zeros([size(vX, 1), numIterations]);
mX(:, 1) = vX;

for ii = 2:numIterations

    vG = (mAA * vX) - vAb;
    vV = vX - (stepSize * vG);
    vX = vV - (stepSize * paramLambda * ProjectL1Ball(vV / (paramLambda * stepSize), 1, stopThr));

    mX(:, ii) = vX;

end

end


% Impulse response invariant discretization of fractional second
% order filter.
%
% irid_fsof function is prepared to compute a discrete-time finite
% dimensional (z) transfer function to approximate a continuous-time
% fractional second order low-pass filter (LPF) [1/(s^2 + a*s + b)]^r, where "s" is
% the Laplace transform variable; "r" is a real number in the range of
% (0,1); a and b are the time constant of LPF [1/(s^2 + a*s + b)]^r, where a, b >= 0.
%
% The proposed approximation keeps the impulse response "invariant"
%
% IN:
%       a, b: the time constant of (the first order) LPF
%             (a and b are arbitrary positive real numbers)
%       r: the fractional order in (0,1)
%       Ts: the sampling period
%       norder: the finite order of the approximate z-transfer function
%       (the orders of denominator and numerator z-polynomial are the same)
% OUT:
%       sr: returns the LTI object that approximates the [1/(s^2 + a*s + b)]^r
%           in the sense of invariant impulse response.
% TEST CODE
% [sr]=irid_fsof(0.01,3,2,.8,5);
%
% Reference:
%       (1) Yan Li, Hu Sheng and YangQuan Chen.
%           "Analytical Impulse Response of A Fractional Second Order Filter
%           and Its Impulse Response Invariant Discretization".
%       (2) YangQuan Chen.
%           "Impulse-invariant discretization of fractional order low-pass filters".
%           Sept. 2008. CSOIS AFC (Applied Fractional Calculus) Seminar.
%           http://fractionalcalculus.googlepages.com/
% --------------------------------------------------------------------
% Yan Li, Ph.D,
% School of Control Science and Engineering,
% Shandong University, Jinan, Shandong 250061, P. R. China.
% --------------------------------------------------------------------
%
% --------------------------------------------------------------------
% YangQuan Chen, Ph.D, Associate Professor and Graduate Coordinator
% Department of Electrical and Computer Engineering,
% Director, Center for Self-Organizing and Intelligent Systems (CSOIS)
% Utah State University, 4120 Old Main Hill, Logan, UT 84322-4120, USA
% E: yqchen@ece.usu.edu or yqchen@ieee.org, T/F: 1(435)797-0148/3054;
% W: http://www.csois.usu.edu or http://yangquan.chen.googlepages.com
% --------------------------------------------------------------------
%
%--------------------------------------------------------------------
% Hu Sheng, Ph.D,
% Department of Electrical and Computer Engineering,
% Center for Self-Organizing and Intelligent Systems (CSOIS)
% Utah State University, 4120 Old Main Hill, Logan, UT 84322-4120, USA
% --------------------------------------------------------------------
% Note: All the codes were finished under the direction of Dr. YangQuan Chen.
%       The codes were writen by Dr. Yan Li and Hu Sheng,
%
% See also irid_fod.m
%          at http://www.mathworks.com/matlabcentral/files/21342/irid_fod.m
%          irid_doi.m
%          at
%          http://www.mathworks.com/matlabcentral/fileexchange/
%          26380-impulse-response-invariant-discretization-of-distributed-order-integrator

%*************************************************************************
function [sr]=irid_fsof(Ts,a,b,r,norder)

if nargin<5; norder=5; end
if a < 0 | b < 0 , sprintf('%s','a and b constant has to be positive'), return, end
if Ts < 0 , sprintf('%s','Sampling period has to be positive'), return, end
if r>=1 | r<= 0, sprintf('%s','The fractional order should be in (0,1)'), return, end
if norder<2, sprintf('%s','The order of the approximate transfer function has to be greater than 1'), return, end

close all;
wmax0=2*pi/Ts/2;          % rad./sec. Nyquist frequency
wmax=floor(1+ log10(wmax0) );
wmin=wmax-5;
w=logspace(wmin,wmax,1000);
j=sqrt(-1);
L=10/Ts;                  % decides the number of points of the impulse response function h(n)
t=[1:L]*Ts; y=[]; ht=[]; y1=[]; y2=[];

if a^2-4*b<0              % Case 1: a^2-4*b<0
    for k=1:length(t)
        y1(k)=quad(@(tau)realconvolution((-a)/2,sqrt(-a^2+4*b)/2,r,tau,t(k)),0,t(k));
        y2(k)=quad(@(tau)imconvolution((-a)/2,sqrt(-a^2+4*b)/2,r,tau,t(k)),0,t(k));
    end
    ht=y1+y2;
elseif a^2-4*b==0         % Case2: a^2-4*b==0
       ht = exp(-sqrt(b).*t).*t.^(2*r-1)/gamma(2*r);
else                      % Case 3: a^2-4*b>0
    for k=1:length(t)
       ht(k)=quadgk(@(x)integration2(x,a,b,r,t(k)),0,t(k));
    end
    s=(-a+sqrt(abs(a^2-4*b)))/2;
    ht= (exp(s.*t)/gamma(r)/gamma(r)).*ht;
end
h = [ht.*Ts];              % approcimation
q=norder;p=norder;
[B,A]=stmcb((h),q,p);
sprintf('Impulse response invariant Discrete approximated transfer function:')
sr=tf(B,A,Ts)
hht=impulse(sr,t);         % approcimated impulse response
                           % frequency response
srfr=(1./((j*w).^2 +a*j*w+b)).^(r);
srfr1=freqresp(sr,w);      % approcimated frequency response

figure;
subplot(3,1,1)             % comparision of impulse response
plot(t,ht,'b');
hold on;
plot(t,hht./Ts,'r-.')
axis([Ts,Ts.*L,-0.5,1]);
xlabel('Time');ylabel('Impulse response');
grid on;
legend(['impulse response of 1/(s^2 + ',num2str(a), '* s +',num2str(b),' )^{',num2str(abs(r)),'}'],'approximated impulse response');

subplot(3,1,2)            % comparision of magnitude
semilogx(w,20*log10(abs(srfr)),'b');hold on;
semilogx(w,20*log10(abs(reshape(srfr1, 1000, 1))),'r-.');
legend(['mag. Bode of 1/(s^2 + ',num2str(a), '* s +',num2str(b),' )^{',num2str(abs(r)),'}'],'approximated mag. Bode');
xlabel('Frequency (Hz)');ylabel('Magnitude (dB)');grid on;

subplot(3,1,3)            % comparision of phase
semilogx(w,(180/pi) * (angle(srfr)),'b');hold on;
semilogx(w,(180/pi) * (angle(reshape(srfr1, 1000, 1))),'r-.');
grid on
xlabel('Frequency (Hz)');ylabel('Phase (degrees)');
legend(['phase Bode of 1/(s^2 + ',num2str(a), '* s +',num2str(b),' )^{',num2str(abs(r)),'}'],'approximated phase Bode')

end


%DEMO_HURDLE Demonstration of Logit Negative-binomial hurdle model
%            using Gaussian process prior
%
%  Description
%    Hurdle models can be used to model excess number of zeros
%    compared to usual Poisson and negative binomial count models.
%    Hurdle models assume a two-stage process, where the first
%    process determines whether the count is larger than zero, and
%    the second process determines the non-zero count. Both
%    processes are given a zero mean Gaussian process prior. The
%    two stage model formulation makes it possible to make
%    inference for the two latent processes separately.
%
%    In this demo we construct logit negative binomial hurdle model
%    by using logit and zero truncated negative binomial models.
%    The posterior inference is made with parallel-EP
%    approximation.
%
%    See also  DEMO_SPATIAL2, DEMO_CLASSIFIC1
%
% Copyright (c) 2008-2010 Jarno Vanhatalo
% Copyright (c) 2010-2011 Aki Vehtari
% Copyright (c) 2011 Jaakko Riihimäki

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% load the data
S = which('demo_spatial1');
data = load(strrep(S,'demo_spatial1.m','demodata/spatial1.txt'));

x = data(:,1:2);
ye = data(:,3);
y = data(:,4);

x0=x;
x=bsxfun(@rdivide,bsxfun(@minus,x0,mean(x0)),std(x0));

% for zero-process
yz=double(y>0)*2-1;
% for count-process
ci=find(y>0);
yc=y(ci);
xc=x(ci,:);
yec=ye(ci,:);

% Create the covariance functions
pl = prior_t('s2',10);
pm = prior_sqrtunif();
cf = gpcf_matern32('lengthScale', 5, 'magnSigma2', 0.05, ...
                   'lengthScale_prior', pl, 'magnSigma2_prior', pm);

% Create the zero part
likz=lik_probit();
gpz=gp_set('lik',likz,'cf',cf,'jitterSigma2',1e-6,'latent_method','EP','latent_opt',struct('parallel','on'));
% Create the count part
likc=lik_negbinztr();
gpc=gp_set('lik',likc,'cf',cf,'jitterSigma2',1e-6,'latent_method','EP','latent_opt',struct('parallel','on'));
% Note that although both parts use 'cf', they have separate hyperparameters

% Set the options for the quasi-Newton optimization
opt=optimset('TolFun',1e-2,'TolX',1e-2,'Display','iter');
gpz=gp_optim(gpz,x,yz,'opt',opt,'optimf',@fminlbfgs);
gpc=gp_optim(gpc,xc,yc,'z',yec,'opt',opt,'optimf',@fminlbfgs);

% make prediction to the data points
[Efz, Varfz] = gp_pred(gpz, x, yz, x);
[Efc, Varfc] = gp_pred(gpc, xc, yc, xc, 'z', yec);

% Define help parameters for plotting
xii=sub2ind([60 35],x0(:,2),x0(:,1));
[X1,X2]=meshgrid(1:35,1:60);

% Plot the figures
figure
subplot(1,2,1)
G=NaN(size(X1));
G(xii)=Efz(:);
pcolor(X1,X2,G),shading flat
colorbar
axis equal
axis([0 35 0 60])
title('Posterior mean of latent zero process')

subplot(1,2,2)
G=NaN(size(X1));
G(xii(ci))=Efc(:);
pcolor(X1,X2,G),shading flat
colorbar
axis equal
axis([0 35 0 60])
title('Posterior mean of latent count process')


function x = AngNormalize(x)
%ANGNORMALIZE  Reduce angle to range [-180, 180)
%
%   X = ANGNORMALIZE(X) reduces angles in [-540, 540) to the range
%   [-180, 180).  X can be any shape.

  x(x >= 180) = x(x >= 180) - 360;
  x(x < -180) = x(x < -180) + 360;
end


function [ATilde,P,R,C]=equilibrateMat(A,algorithm,maxIter,RelTol,AbsTol)
%%EQUILIBRATEMAT Equilibrate a general (not necessarily symmetric) real or
%                complex matrix. This finds a permutation matrix and two
%                diagonal matrices such that the equilibrated matrix,
%                ATilde=P*R*A*C has the infinity-norm of each row and
%                column equal to 1.
%
%INPUTS: A An nXn real or complex matrix. Unless algorithm=1, in which case
%          A must be real and all positive.
% algorithm An optional parameter selecting the algorithm to use for
%          equilibration. Possible values are:
%          0 (The default if omitted or an empty matrix is passed) Use the
%            algorithm involving bipartite matching, as described in
%            Section 2.1.2 of [1], which is one of the algorithmic variants
%            given in [2].
%          1 This is the heuristic ALG3 in [1]. The algorithm should work
%            with all-positive, real matrices. It does not work with
%            complex matrices. Also, the algorithm usually does not work
%            with matrices containing negative elements. This algorithm
%            always produces P=eye(n,n).
%  maxIter If algorithm=1, this is the maximum number of iterations to
%          perform. The default if omitted or an empty matrix is passed is
%          500. This is not used if algorithm=0.
% RelTol, AbsTol The absolute and relative convergence criteria for
%         algorithm 1. The defaults if omitted or empty matrices are passed
%         are 1e-10 and 1e-13.
%
%OUTPUTS: ATilde The nXn equlibrated matrix.
%              P The nNn permutation matrix.
%              R The left-multiplying diagonal matrix.
%              C The right-multiplying diagonal matrix.
%
%EXAMPLE:
%Here, we create a symmetric matrix and equilibrate it. The equilibration
%makes the largest magnitude values in each row and column equal 1.
%However, it also changes the condition number (usually for the better), as
%we shall show:
% %A random complex matrix.
% A=10*rand(10,10)+10*1j*rand(10,10);
% ATilde=equilibrateMat(A);
%
% %The original maximum absolute value elements in the columns and rows.
% max(ATilde,[],1)
% max(ATilde,[],2)
%
% %After equilibration, the maximum mangitude elements in the columns and
% % %rows are all 1.
% max(ATilde,[],1)
% max(ATilde,[],2)
%
% %The condition number often but not always improves.
% cond(A,Inf)
% cond(ATilde,Inf)
%
%REFERENCES:
%[1] P. Liu, "An exploration of matrix equilibration," University of
%    British Columbia. CPSC 517, Tech. Rep., 30 Sep. 2015. [Online].
%    Available: http://dx.doi.org/10.14288/1.0103601
%[2] I. S. Duff and J. Koster, "On algorithms for permuting large entries
%    to the diagonal of a sparse matrix," SIAM Journal on Matrix Analysis
%    and Applications, vol. 22, no. 4, pp. 973-996, 2001.
%
%September 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(AbsTol))
    AbsTol=1e-13;
end

if(nargin<4||isempty(RelTol))
    RelTol=1e-10;
end

if(nargin<3||isempty(maxIter))
    maxIter=500;
end

if(nargin<2||isempty(algorithm))
    algorithm=0;
end

switch(algorithm)
    case 0%The algorithm of [2].
        n=size(A,1);
        ATrans=-log(A);

        %In the current version of the Tracker Component Library, the compiled
        %version of assign2D does not support complex numbers in the cost matrix.
        %Thus, if the mex file is in the path, we have to remove it from the path
        %and use the m file. We can add the compiled version back to the path after
        %calling assign2D.
        [filepath,~,ext]=fileparts(which('assign2D'));

        if(~strcmp(ext,'m'))
            %If it isn't the m file that will be used, we have to remove the
            %filepath from the current path.
            rmpath(filepath);
        else
            filepath=[];
        end

        %Note that u and v are reversed; the definition in [1] is different from
        %that used in assign2D. ATrans will be complex if A had any negative terms
        %or if A were complex to begin with. However, the complex values are
        %correctly handled in the m function implementation of assign2D.
        [col4row,~,~,v,u]=assign2D(ATrans,false);

        %If a compiled implementation of assign2D had to be removed from the
        %current path.
        if(~isempty(filepath))
            addpath(filepath)
        end

        if(isempty(col4row))
            %The equilibration problem is infeasible.
            P=[];
            R=[];
            C=[];
            return
        end

        %We have to obtain the dual variables for the original problem, not the
        %transformed one that is sovled in assign2D.
        ADelta=min(ATrans(:));
        %If A is all positive, the problem wasn't transformed.
        if(ADelta>0)
            ADelta=0;
        end
        u=u+ADelta;

        %Construct the permutation matrix.
        P=zeros(n,n);
        for k=1:n
            P(k,col4row(k))=1;
        end

        R=diag(exp(u));
        C=diag(exp(v));

        %R and C should be real if A is real. This deals with any possible finite
        %precision issues.
        if(isreal(A))
            R=real(R);
            C=real(C);
        end

        ATilde=P*R*A*C;
    case 1%ALG3 in [1].
        if(~isreal(A))
            error('This algorithm only works with real matrices.')
        end

        if(any(A(:)<0))
            warning('This algorithm often does not satisfy the desired norm criterion when A contains negative elements.')
        end

        n=size(A,1);

        R=eye(n,n);
        C=eye(n,n);
        ATilde=A;

        D=eye(n,n);
        for curIter=1:maxIter
            ATildeOld=ATilde;
            for i=1:n
                alphaR=1/sqrt(norm(ATilde(i,:),Inf));
                alphaC=1/sqrt(norm(ATilde(:,i),Inf));

                Dr=D;
                Dr(i,i)=alphaR;
                Dc=D;
                Dc(i,i)=alphaC;

                R=R*Dr;
                C=C*Dc;
                ATilde=Dr*ATilde*Dc;
            end

            diffMag=abs(ATilde-ATildeOld);
            if(all((diffMag(:)<=RelTol*abs(ATilde(:)))|(diffMag(:)<=AbsTol)))
               return;
            end
        end

        P=eye(n,n);

    otherwise
        error('Unknown algorithm specified.')
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 [pc,Pf,Pb] = rc2pcv(rc,R0)

%function [pc,Pf,Pb] = rc2pcv(rc,R0)
% Transforms forward reflection coefficients rcb into
% normalized correlation matrices pc.

%S. de Waele, March 2003.

s = kingsize(rc);
order = s(3)-1;
dim = s(1); I = eye(dim);
%if nargin == 1, R0 = I; end

pc = zeros(s);  pc(:,:,1)  = I;

Pf   = zeros(s); Pf(:,:,1)   = R0;
Pb   = zeros(s); Pb(:,:,1)  = R0;

for p = 1:order,
   TsqrtPf = Tsqrt(Pf(:,:,p)); %square root M defined by: M=Tsqrt(M)*Tsqrt(M)'
   TsqrtPb= Tsqrt(Pb(:,:,p));
   %partial correlation
   pc(:,:,p+1) = -inv(TsqrtPf)*rc(:,:,p+1)*TsqrtPb;
   %residual matrices
   Pf(:,:,p+1)  = (I-TsqrtPf *pc(:,:,p+1) *pc(:,:,p+1)'*inv(TsqrtPf ))*Pf(:,:,p);
   Pb(:,:,p+1) = (I-TsqrtPb*pc(:,:,p+1)'*pc(:,:,p+1) *inv(TsqrtPb))*Pb(:,:,p);
end %for p = 2:order,


function [ idx, tri ] = nearest_neighbor ( varargin )

%NEAREST_NEIGHBOR    find nearest neighbors
%   IDX = NEAREST_NEIGHBOR(X) finds the nearest neighbor by Euclidean
%   distance to each point (column) in X from X. X is a matrix with points
%   as columns. IDX is a vector of indices into X, such that X(:, IDX) are
%   the nearest neighbors to X. e.g. the nearest neighbor to X(:, 2) is
%   X(:, IDX(2))
%
%   IDX = NEAREST_NEIGHBOR(P, X) finds the nearest neighbor by Euclidean
%   distance to each point in P from X. P and X are both matrices with the
%   same number of rows, and points are the columns of the matrices. Output
%   is a vector of indices into X such that X(:, IDX) are the nearest
%   neighbors to P
%
%   IDX = NEAREST_NEIGHBOR(I, X) where I is a logical vector or vector of
%   indices, and X has at least two rows, finds the nearest neighbor in X
%   to each of the points X(:, I).
%   I must be a row vector to distinguish it from a single point.
%   If X has only one row, the first input is treated as a set of 1D points
%   rather than a vector of indices
%
%   IDX = NEAREST_NEIGHBOR(..., Property, Value)
%   Calls NEAREST_NEIGHBOR with the indicated parameters set. Property
%   names can be supplied as just the first letters of the property name if
%   this is unambiguous, e.g. NEAREST_NEIGHBOR(..., 'num', 5) is equivalent
%   to NEAREST_NEIGHBOR(..., 'NumberOfNeighbors', 5). Properties are case
%   insensitive, and are as follows:
%      Property:                         Value:
%      ---------                         ------
%         NumberOfNeighbors             natural number, default 1
%            NEAREST_NEIGHBOR(..., 'NumberOfNeighbors', K) finds the closest
%            K points in ascending order to each point, rather than the
%            closest point. If Radius is specified and there are not
%            sufficient numbers, fewer than K neighbors may be returned
%
%         Radius                         positive, default +inf
%            NEAREST_NEIGHBOR(..., 'Radius', R) finds neighbors within
%            radius R. If NumberOfNeighbors is not set, it will find all
%            neighbors within R, otherwise it will find at most
%            NumberOfNeighbors. The IDX matrix is padded with zeros if not
%            all points have the same number of neighbors returned. Note
%            that specifying a radius means that the Delaunay method will
%            not be used.
%
%         DelaunayMode                   {'on', 'off', |'auto'|}
%            DelaunayMode being set to 'on' means NEAREST_NEIGHBOR uses the
%            a Delaunay triangulation with dsearchn to find the points, if
%            possible. Setting it to 'auto' means NEAREST_NEIGHBOR decides
%            whether to use the triangulation, based on efficiency. Note
%            that the Delaunay triangulation will not be used if a radius
%            is specified.
%
%         Triangulation                  Valid triangulation produced by
%                                        delaunay or delaunayn
%            If a triangulation is supplied, NEAREST_NEIGHBOR will attempt
%            to use it (in conjunction with dsearchn) to find the
%            neighbors.
%
%   [IDX, TRI] = NEAREST_NEIGHBOR( ... )
%   If the Delaunay Triangulation is used, TRI is the triangulation of X'.
%   Otherwise, TRI is an empty matrix
%
%   Example:
%
%     % Find the nearest neighbor in X to each column of X
%     x = rand(2, 10);
%     idx = nearest_neighbor(x);
%
%     % Find the nearest neighbors to each point in p
%     p = rand(2, 5);
%     x = rand(2, 20);
%     idx = nearest_neighbor(p, x)
%
%     % Find the five nearest neighbors to points x(:, [1 6 20]) in x
%     x = rand(4, 1000)
%     idx = nearest_neighbor([1 6 20], x, 'NumberOfNeighbors', 5)
%
%     % Find all neighbors within radius of 0.1 of the points in p
%     p = rand(2, 10);
%     x = rand(2, 100);
%     idx = nearest_neighbor(p, x, 'r', 0.1)
%
%     % Find at most 10 nearest neighbors to point p from x within a
%     % radius of 0.2
%     p = rand(1, 2);
%     x = rand(2, 30);
%     idx = nearest_neighbor(p, x, 'n', 10, 'r', 0.2)
%
%
%   See also DELAUNAYN, DSEARCHN, TSEARCH

%TODO    Allow other metrics than Euclidean distance
%TODO    Implement the Delaunay mode for multiple neighbors

% Copyright 2006 Richard Brown. This code may be freely used and
% distributed, so long as it maintains this copyright line
error(nargchk(1, Inf, nargin, 'struct'));

% Default parameters
userParams.NumberOfNeighbors = []    ; % Finds one
userParams.DelaunayMode       = 'auto'; % {'on', 'off', |'auto'|}
userParams.Triangulation      = []    ;
userParams.Radius             = inf   ;

% Parse inputs
[P, X, fIndexed, userParams] = parseinputs(userParams, varargin{:});

% Special case uses Delaunay triangulation for speed.

% Determine whether to use Delaunay - set fDelaunay true or false
nX  = size(X, 2);
nP  = size(P, 2);
dim = size(X, 1);

switch lower(userParams.DelaunayMode)
    case 'on'
        %TODO Delaunay can't currently be used for finding more than one
        %neighbor
        fDelaunay = userParams.NumberOfNeighbors == 1 && ...
            size(X, 2) > size(X, 1)                    && ...
            ~fIndexed                                  && ...
            userParams.Radius == inf;
    case 'off'
        fDelaunay = false;
    case 'auto'
        fDelaunay = userParams.NumberOfNeighbors == 1 && ...
            ~fIndexed                                  && ...
            size(X, 2) > size(X, 1)                    && ...
            userParams.Radius == inf                   && ...
            ( ~isempty(userParams.Triangulation) || delaunaytest(nX, nP, dim) );
end

% Try doing Delaunay, if fDelaunay.
fDone = false;
if fDelaunay
    tri = userParams.Triangulation;
    if isempty(tri)
        try
            tri   = delaunayn(X');
        catch
            msgId = 'Nearest_Neighbor:DelaunayFail';
            msg = ['Unable to compute delaunay triangulation, not using it. ',...
                'Set the DelaunayMode parameter to ''off'''];
            warning(msgId, msg);
        end
    end
    if ~isempty(tri)
        try
            idx = dsearchn(X', tri, P')';
            fDone = true;
        catch
            warning('Nearest_Neighbor:DSearchFail', ...
                'dsearchn failed on triangulation, not using Delaunay');
        end
    end
else % if fDelaunay
    tri = [];
end

% If it didn't use Delaunay triangulation, find the neighbors directly by
% finding minimum distances
if ~fDone
    idx = zeros(userParams.NumberOfNeighbors, size(P, 2));

    % Loop through the set of points P, finding the neighbors
    Y = zeros(size(X));
    for iPoint = 1:size(P, 2)
        x = P(:, iPoint);

        % This is the faster than using repmat based techniques such as
        % Y = X - repmat(x, 1, size(X, 2))
        for i = 1:size(Y, 1)
            Y(i, :) = X(i, :) - x(i);
        end

        % Find the closest points, and remove matches beneath a radius
        dSq = sum(abs(Y).^2, 1);
        iRad = find(dSq < userParams.Radius^2);
        if ~fIndexed
            iSorted = iRad(minn(dSq(iRad), userParams.NumberOfNeighbors));
        else
            iSorted = iRad(minn(dSq(iRad), userParams.NumberOfNeighbors + 1));
            iSorted = iSorted(2:end);
        end

        % Remove any bad ones
        idx(1:length(iSorted), iPoint) = iSorted';
    end
    %while ~isempty(idx) && isequal(idx(end, :), zeros(1, size(idx, 2)))
    %    idx(end, :) = [];
    %end
    idx( all(idx == 0, 2), :) = [];
end % if ~fDone
if isvector(idx)
    idx = idx(:)';
end
end % nearest_neighbor

%DELAUNAYTEST   Work out whether the combination of dimensions makes
%fastest to use a Delaunay triangulation in conjunction with dsearchn.
%These parameters have been determined empirically on a Pentium M 1.6G /
%WinXP / 512MB / Matlab R14SP3 platform. Their precision is not
%particularly important
function tf = delaunaytest(nx, np, dim)
switch dim
    case 2
        tf = np > min(1.5 * nx, 400);
    case 3
        tf = np > min(4 * nx  , 1200);
    case 4
        tf = np > min(40 * nx , 5000);

        % if the dimension is higher than 4, it is almost invariably better not
        % to try to use the Delaunay triangulation
    otherwise
        tf = false;
end % switch
end % delaunaytest

%MINN   find the n most negative elements in x, and return their indices
%  in ascending order
function I = minn(x, n)

% Make sure n is no larger than length(x)
n = min(n, length(x));

% Sort the first n
[xsn, I] = sort(x(1:n));

% Go through the rest of the entries, and insert them into the sorted block
% if they are negative enough
for i = (n+1):length(x)
    j = n;
    while j > 0 && x(i) < xsn(j)
        j = j - 1;
    end

    if j < n
        % x(i) should go into the (j+1) position
        xsn = [xsn(1:j), x(i), xsn((j+1):(n-1))];
        I   = [I(1:j), i, I((j+1):(n-1))];
    end
end

end %minn

%PARSEINPUTS    Support function for nearest_neighbor
function [P, X, fIndexed, userParams] = parseinputs(userParams, varargin)
if length(varargin) == 1 || ~isnumeric(varargin{2})
    P           = varargin{1};
    X           = varargin{1};
    fIndexed    = true;
    varargin(1) = [];
else
    P             = varargin{1};
    X             = varargin{2};
    varargin(1:2) = [];

    % Check the dimensions of X and P
    if size(X, 1) ~= 1
        % Check to see whether P is in fact a vector of indices
        if size(P, 1) == 1
            try
                P = X(:, P);
            catch
                error('Nearest_Neighbor:InvalidIndexVector', ...
                    'Unable to index matrix using index vector');
            end
            fIndexed = true;
        else
            fIndexed = false;
        end % if size(P, 1) == 1
    else % if size(X, 1) ~= 1
        fIndexed = false;
    end

    if ~fIndexed && size(P, 1) ~= size(X, 1)
        error('Nearest_Neighbor:DimensionMismatch', ...
            'No. of rows of input arrays doesn''t match');
    end
end
% Parse the Property/Value pairs
if rem(length(varargin), 2) ~= 0
    error('Nearest_Neighbor:propertyValueNotPair', ...
        'Additional arguments must take the form of Property/Value pairs');
end

propertyNames = {'numberofneighbors', 'delaunaymode', 'triangulation', ...
    'radius'};
while length(varargin) ~= 0
    property = varargin{1};
    value    = varargin{2};

    % If the property has been supplied in a shortened form, lengthen it
    iProperty = find(strncmpi(property, propertyNames, length(property)));
    if isempty(iProperty)
        error('Nearest_Neighbor:InvalidProperty', 'Invalid Property');
    elseif length(iProperty) > 1
        error('Nearest_Neighbor:AmbiguousProperty', ...
            'Supplied shortened property name is ambiguous');
    end
    property = propertyNames{iProperty};

    switch property
        case 'numberofneighbors'
            if rem(value, 1) ~= 0 || ...
                    value > length(X) - double(fIndexed) || ...
                    value < 1
                error('Nearest_Neighbor:InvalidNumberOfNeighbors', ...
                    'Number of Neighbors must be an integer, and smaller than the no. of points in X');
            end
            userParams.NumberOfNeighbors = value;

        case 'delaunaymode'
            fOn = strcmpi(value, 'on');
            if strcmpi(value, 'off')
                userParams.DelaunayMode = 'off';
            elseif fOn || strcmpi(value, 'auto')
                if userParams.NumberOfNeighbors ~= 1
                    if fOn
                        warning('Nearest_Neighbor:TooMuchForDelaunay', ...
                            'Delaunay Triangulation method works only for one neighbor');
                    end
                    userParams.DelaunayMode = 'off';
                elseif size(X, 2) < size(X, 1) + 1
                    if fOn
                        warning('Nearest_Neighbor:TooFewDelaunayPoints', ...
                            'Insufficient points to compute Delaunay triangulation');
                    end
                    userParams.DelaunayMode = 'off';

                elseif size(X, 1) == 1
                    if fOn
                        warning('Nearest_Neighbor:DelaunayDimensionOne', ...
                            'Cannot compute Delaunay triangulation for 1D input');
                    end
                    userParams.DelaunayMode = 'off';
                else
                    userParams.DelaunayMode = value;
                end
            else
                warning('Nearest_Neighbor:InvalidOption', ...
                    'Invalid Option');
            end % if strcmpi(value, 'off')

        case 'radius'
            if isscalar(value) && isnumeric(value) && isreal(value) && value > 0
                userParams.Radius = value;
                if isempty(userParams.NumberOfNeighbors)
                    userParams.NumberOfNeighbors = size(X, 2) - double(fIndexed);
                end
            else
                error('Nearest_Neighbor:InvalidRadius', ...
                    'Radius must be a positive real number');
            end

        case 'triangulation'
            if isnumeric(value) && size(value, 2) == size(X, 1) + 1 && ...
                    all(ismember(1:size(X, 2), value))
                userParams.Triangulation = value;
            else
                error('Nearest_Neighbor:InvalidTriangulation', ...
                    'Triangulation not a valid Delaunay Triangulation');
            end
    end % switch property

    varargin(1:2) = [];
end % while
if isempty(userParams.NumberOfNeighbors)
    userParams.NumberOfNeighbors = 1;
end
end %parseinputs


function [y,mm]=v_momfilt(x,r,w,m)
%V_MOMFILT calculates moments of a signal using a sliding window Y=(X,R,W,M)
%
% Inputs: x    is the input signal
%         r    is a list of moments to calculate
%              (+ve = relative to mean, -ve = relative to zero)
%         w    is the window (or just the length of a Hamming window)
%              Note: If the window is asymmetric, you should be aware that it gets
%              flipped in the convolution process
%         m    is the sample of w to use as the centre [default=ceil(length(w)/2+0.5)]
%
%         mm   the actual value of m used. Output point y(i) is based on x(i+m-w:i+m-1).
%
% Example:
%  To calculate a running kurtosis using a Hamming window of length 30:
%             y=v_momfilt(x,[2 4],30); k=y(:,2)./y(:,1).^2

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

lx=prod(size(x));
lxx=lx+m-1;
xx=zeros(lxx,1);
xx(1:lx)=x(:);              % extend with zeros so filter() works correctly

cw=cumsum(w);
sw=cw(end);
y0=repmat(sw,lxx,1);
lxw=min(lxx,lw);
y0(1:lxw)=cw(1:lxw);
y0(lx+1:lx+m-1)=y0(lx+1:lx+m-1)-cw(1:m-1);      % equivalent to y0=filter(w,1,xx^0);
yd=y0(m:end);
yd(abs(yd)<eps)=1;

nr=length(r);
wlx=ones(lx,1);
wlxx=ones(lxx,1);
y=zeros(lx,nr);
mr=max(abs(r));                         % max moment to calculate
mk=zeros(1,mr);                       % list of moments
mk(-r(r<0))=1;                         % choose the moments we need to calculate
maxr=max(r);
if maxr>1
    mk(1:maxr)=1;
end
ml=find(mk>0);
lml=length(ml);
if lml
    mlx=mk;
    mlx(ml)=1:lml;                        % mapping from moment into ml
    wlml=ones(1,lml);
    xm=filter(w,1,xx(:,wlml).^ml(wlxx,:));    % calculate all the moments
    xm=xm(m:end,:)./yd(:,wlml);                             % remove the useless start values and normalize
end
fr=find(r<0);
if length(fr)
    y(:,fr)=xm(:,mlx(-r(fr)));    % zero-centred moments
end
fr=find(r==0);                              % 0'th moment
if length(fr)
    y(:,fr)=1;
end
fr=find(r==1);                              % 1'st moment about mean
if length(fr)
    y(:,fr)=0;
end
fr=find(r==2);                              % 2'nd moment about mean
if length(fr)
    yfr=xm(:,2)-xm(:,1).^2;
    y(:,fr)=yfr(:,ones(1,length(fr)));
end
if maxr>2
    mon=[1 -1];
    bc=[1 -2 1];
    am=zeros(lx,maxr);
    am(:,1)=xm(:,1);                            % copy the mean across
    ml=2:maxr;
    wlml=ones(1,length(ml));
    am(:,2:end)=xm(:,wlml).^ml(ones(lx,1),:);              % calculate powers of the mean
    for i=3:maxr
        bc=conv(bc,mon);                        % calculate binomial coefficients
        fr=find(r==i);
        if length(fr)
            yfr=xm(:,i)+sum(xm(:,i-1:-1:2).*am(:,1:i-2).*bc(wlx,2:i-1),2)+am(:,i)*(bc(i)+bc(i+1));
            y(:,fr)=yfr(:,ones(1,length(fr)));
        end
    end
end


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

%*****************************************************************************80
%
%% R83S_CG uses the conjugate gradient method on an R83S system.
%
%  Discussion:
%
%    The R83S storage format is used for a tridiagonal scalar matrix.
%    The vector A(3) contains the subdiagonal, diagonal, and superdiagonal
%    values that occur on every row.
%
%    The matrix A must be a positive definite symmetric band matrix.
%
%    The method is designed to reach the solution after N computational
%    steps.  However, roundoff may introduce unacceptably large errors for
%    some problems.  In such a case, calling the routine again, using
%    the computed solution as the new starting estimate, should improve
%    the results.
%
%  Example:
%
%    Here is how an R83S matrix of order 5, stored as (A1,A2,A3), would
%    be interpreted:
%
%      A2  A3   0   0   0
%      A1  A2  A3   0   0
%       0  A1  A2  A3   0
%       0   0  A1  A2  A3
%       0   0   0  A1  A2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Frank Beckman,
%    The Solution of Linear Equations by the Conjugate Gradient Method,
%    in Mathematical Methods for Digital Computers,
%    edited by John Ralston, Herbert Wilf,
%    Wiley, 1967,
%    ISBN: 0471706892,
%    LC: QA76.5.R3.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, real A(3), the matrix.
%
%    Input, real B(N), the right hand side vector.
%
%    Input/output, real X(N).
%    On input, an estimate for the solution, which may be 0.
%    On output, the approximate solution vector.
%
  b = b(:);
  x = x(:);
%
%  Initialize
%    AP = A * x,
%    R  = b - A * x,
%    P  = b - A * x.
%
  ap = r83s_mv ( n, n, a, x );

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

    if ( pap == 0.0 )
      return
    end

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

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

  end

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_KUKA_KR16_2(robot, T)
%   Solves the inverse kinematic problem for the KUKA KR16 2 robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_KUKA_KR16_2 returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('kuka', 'KUKA_KR16_2');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function q = inversekinematic_kuka_kr16_2(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

% %Evaluate the parameters
% theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
L6=abs(d(6));

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
%the other possible solution is q1 + pi
q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
%solver for q3 for both cases
q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));
% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));
A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%The inverse kinematic problem can be solved as in the IRB 140 (for example)

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));

A2 =(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%the angle phi is fixed
phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi - beta;
q3(2) = pi - phi + beta;


% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
% Copyright 2015-2018 Jonas Koennemanm, Giovanni Licitra
% Redistribution is permitted under the 3-Clause BSD License terms. Please
% ensure the above copyright notice is visible in any derived work.
%
function [solution,times,problem] = vanderpol

  problem = ocl.Problem(10, @varsfun, @daefun, @pathcosts, 'N', 30);

  % intial state bounds
  problem.setInitialBounds('x',     0);
  problem.setInitialBounds('y',     1);

  % Get and set initial guess
  initialGuess = problem.getInitialGuess();
  initialGuess.states.x.set(-0.2);

  % Run solver to obtain solution
  [solution,times] = problem.solve(initialGuess);

  % plot solution
  figure
  hold on
  plot(times.states.value,solution.states.x.value,'-.','LineWidth',2)
  plot(times.states.value,solution.states.y.value,'--k','LineWidth',2)
  stairs(times.controls.value,solution.controls.F.value,'r','LineWidth',2)
  xlabel('time')
  legend({'x','y','u'})

  snapnow;
end

function varsfun(svh)
  % Scalar x:  -0.25 <= x <= inf
  % Scalar y: unbounded
  svh.addState('x', 'lb', -0.25, 'ub', inf);
  svh.addState('y');

  % Scalar u: -1 <= F <= 1
  svh.addControl('F', 'lb', -1, 'ub', 1);
end

function daefun(daeh,x,~,u,~)
  daeh.setODE('x', (1-x.y^2)*x.x - x.y + u.F);
  daeh.setODE('y', x.x);
end

function pathcosts(ch,x,~,u,~)
  ch.add( x.x^2 );
  ch.add( x.y^2 );
  ch.add( u.F^2 );
end


%% Double Pendulum
% Clear work space and initaliza figure
clear; clc; close all; %figure;

%% Initial and Constant Values

% Initial values for tuned state (1):
% gamma0    = pi/12;
% dtgamma0  = pi;
% alpha0    = 0;
% dtalpha0  = 0;

% Initial values for tuned state (2):
% gamma0    = 0;
% dtgamma0  = 0;
% alpha0    = pi/6;
% dtalpha0  = pi/36;

% Initial values for tuned state (3):
% gamma0    = 0;
% dtgamma0  = 0;
% alpha0    = pi/24;
% dtalpha0  = pi/2;

% Initial values for chaotic solution:
% gamma0    = 0;
% dtgamma0  = 0;
% alpha0    = pi/2;
% dtalpha0  = 5;

% Initial values for chaotic solution2:
gamma0    = 0;
dtgamma0  = 0;
alpha0    = pi/2;
dtalpha0  = 5.025;

% Constant Values
beta0     = pi/2;
lambda    = 2.74; %rad/s
omega     = 5.48; %rad/s
psi       = 0.96;
eta       = 0.24;

fps       = 10;
%movie     = true;

%% Using solver ode45

duration  = 100;
ivp       = [gamma0; dtgamma0; alpha0; dtalpha0; beta0; lambda; omega; psi; eta];
[t,x]     = ode45(@dpendulum,[0 duration], ivp);

%options = odeset('RelTol',1e-4,'AbsTol',[1e-4 1e-4 1e-5]);
%[t,x]=ode45(@dpendulum,[0 duration], ivp, options);

%% Plot

nframes=duration*fps;
%t = linspace(0,duration,nframes);
%x = deval(sol,t);

gamma    = x(:,1);
dtgamma  = x(:,2);
alpha    = x(:,3);
dtalpha  = x(:,4);
% we assume:
l1=1; % b distance
l2=2; % c distance

%% Plot alpha vs Gamma
figure;
plot (t,x(:,3),'-.',t,x(:,1),'-')

%% Plot d_alpha vs d_Gamma
figure;
plot (t,x(:,4),'-.',t,x(:,2),'-')


function s = ClenshawL(c,x)
% Clenshaw algorithmus for evaluating Legendre series
%
% Description
%
% $$ s = \sum_n=0^N c_n P_n(x)$$
%
% Input
%  c - coefficients
%  x - evaluation nodes
%
% Output
%  s - Legendre series
%

% init
c= [c(:);0;0];
dn = zeros(size(x));
d1 = zeros(size(x));
d2 = zeros(size(x));

% use backward tree term recurence
for l = length(c)-2:-1:1

  d1 = d2 + (2*l+1)/(l+1) * x .* dn;
  d2 = c(l) - l/(l+1) * dn;
  dn = d1;

end

s = d2 + x .* d1;

end


function [kls,xx1,xx2] = vbmc_kldiv(vp1,vp2,Ns,gaussflag)
%VBMC_KLDIV Kullback-Leibler divergence between two variational posteriors.
%   KLS = VBMC_KLDIV(VP1,VP2) returns an estimate of the (asymmetric)
%   Kullback-Leibler (KL) divergence between two variational posterior
%   distributions VP1 and VP2. KLS is a 2-element vector whose first element
%   is KL(VP1||VP2) and the second element is KL(VP2||VP1). The symmetrized
%   KL divergence can be computed as mean(KLS).
%
%   KLS = VBMC_KLDIV(VP1,VP2,NS) uses NS random draws to estimate each
%   KL divergence (default NS=1e5).
%
%   KLS = VBMC_KLDIV(VP1,VP2,NS,GAUSSFLAG) computes the "Gaussianized"
%   KL-divergence if GAUSSFLAG=1, that is the KL divergence between two
%   multivariate normal distibutions with the same moments as the variational
%   posteriors given as inputs. Otherwise, the standard KL-divergence is
%   returned for GAUSSFLAG=0 (default).
%
%   [KLS,XX1,XX2] = VBMC_KLDIV(...) returns NS samples from the variational
%   posteriors VP1 and VP2 as, respectively, NS-by-D matrices XX1 and XX2,
%   where D is the dimensionality of the problem.
%
%   If GAUSSFLAG is 1, VP1 and/or VP2 can be N-by-D matrices of samples
%   from variational posteriors (they do not need have the same number
%   of samples).
%
%   See also VBMC, VBMC_MTV, VBMC_PDF, VBMC_RND, VBMC_DIAGNOSTICS.

if nargin < 3 || isempty(Ns); Ns = 1e5; end
if nargin < 4 || isempty(gaussflag); gaussflag = false; end

% This was removed because the comparison *has* to be in original space,
% given that the transform might change for distinct variational posteriors
% if nargin < 5 || isempty(origflag); origflag = true; end
origflag = true;

kls = NaN(1,2);

if ~gaussflag && (~vbmc_isavp(vp1) || ~vbmc_isavp(vp2))
    error('vbmc_kldiv:WrongInputs', ...
        'Unless the KL divergence is Gaussianized, VP1 and VP2 need to be variational posteriors.');
end

%try
    if gaussflag
        if Ns == 0  % Analytical calculation
            if origflag
                error('vbmc_kldiv:NoAnalyticalMoments', ...
                    'Analytical moments are available only for the transformed space.')
            end
            [q1mu,q1sigma] = vbmc_moments(vp1,0);
            [q2mu,q2sigma] = vbmc_moments(vp2,0);
            xx1 = []; xx2 = [];
        else        % Numerical moments
            if vbmc_isavp(vp1)
                [q1mu,q1sigma] = vbmc_moments(vp1,origflag,Ns);
            else
                q1mu = mean(vp1,1);
                q1sigma = cov(vp1);
            end
            if vbmc_isavp(vp2)
                [q2mu,q2sigma] = vbmc_moments(vp2,origflag,Ns);
            else
                q2mu = mean(vp2,1);
                q2sigma = cov(vp2);
            end
        end
        [kls(1),kls(2)] = mvnkl(q1mu,q1sigma,q2mu,q2sigma);

    else
        MINP = realmin;

        xx1 = vbmc_rnd(vp1,Ns,origflag,1);
        q1 = vbmc_pdf(vp1,xx1,origflag);
        q2 = vbmc_pdf(vp2,xx1,origflag);
        q1(q1 == 0 | ~isfinite(q1)) = 1;    % Ignore these points
        q2(q2 == 0 | ~isfinite(q2)) = MINP;
        kls(1) = -mean(log(q2) - log(q1));

        xx2 = vbmc_rnd(vp2,Ns,origflag,1);
        q1 = vbmc_pdf(vp1,xx2,origflag);
        q2 = vbmc_pdf(vp2,xx2,origflag);
        q1(q1 == 0 | ~isfinite(q1)) = MINP;
        q2(q2 == 0 | ~isfinite(q2)) = 1;    % Ignore these points
        kls(2) = -mean(log(q1) - log(q2));

    end

    kls = max(kls,0); % Correct for numerical errors

%catch

    % Could not compute KL divs

%end


function ccii_1_tabulate ( )

%*****************************************************************************80
%
%% CCII_1_TABULATE tabulates CCII_1 quadrature rules for the Hermite integral.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 May 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CCII_1_TABULATE\n' );
  fprintf ( 1, '  Tabulate CCII_1 quadrature rules for the Hermite integral.\n' );
  fprintf ( 1, '  Density function rho(x) = 1.\n' );
  fprintf ( 1, '  Region: -oo < x < +oo.\n' );
  fprintf ( 1, '  Exactness: NONE.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Print the first 5 rules:\n' );
  fprintf ( 1, '   I          X(I)            W(I)\n' );
  fprintf ( 1, '\n' );

  ell = 1.0;

  for n = 1 : 5

    [ x, w ] = ccii_1 ( n, ell );

    fprintf ( 1, '\n' );
    for i = 1 : n
      fprintf ( 1, '  %2d  %14.6f  %14.6f\n', i, x(i), w(i));
    end
    fprintf ( 1, ' Sum                  %14.6f  %14.6f\n', sum ( w(1:n) ) );

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Print the first 5 nested rules:\n' );
  fprintf ( 1, '   I          X(I)            W(I)\n' );
  fprintf ( 1, '\n' );

  ell = 1.0;

  for j = 1 : 5

    n = 2^j - 1;

    [ x, w ] = ccii_1 ( n, ell );

    fprintf ( 1, '\n' );
    for i = 1 : n
      fprintf ( 1, '  %2d  %14.6f  %14.6f\n', i, x(i), w(i) );
    end
    fprintf ( 1, ' Sum                  %14.6f\n', sum ( w(1:n) ) );

  end

  return
end


function ind=findn(arr);

%FINDN   Find indices of nonzero elements.
%   I = FINDN(X) returns the indices of the vector X that are
%   non-zero. For example, I = FINDN(A>100), returns the indices
%   of A where A is greater than 100. See RELOP.
%
%   This is the same as find but works for N-D matrices using
%   ind2sub function
%
%   It does not return the vectors as the third output arguement
%   as in FIND
%
%   The returned I has the indices (in actual dimensions)
%
%   x(:,:,1)            x(:,:,2)            x(:,:,3)
%       = [ 1 2 3           =[11 12 13        =[21 22 23
%           4 5 6             14 15 16          24 25 26
%           7 8 9]            17 18 19]         27 28 29]
%
%   I=find(x==25) will return 23
%   but findn(x==25) will return 2,2,3
%
%   Also see find, ind2sub

%   Loren Shure, Mathworks Inc. improved speed on previous version of findn
%   by Suresh Joel Mar 3, 2003

in=find(arr);
sz=size(arr);
if isempty(in), ind=[]; return; end;
[out{1:ndims(arr)}] = ind2sub(sz,in);
ind = cell2mat(out);


function [f,fnorm]=setnorm(f,varargin)
%SETNORM  Set a norm of the input signal to a specified value
%   Usage:  h=setnorm(f,...);
%
%
%   `setnorm(f,...)` will set the input signal *f* to a specified norm.
%
%   `[f,fnorm]=setnorm(f,...)` does the same thing, but in addition
%   returns norm *fnorm* of a signal *f*.
%
%   The norm is specified as a string and may be one of:
%
%     '1'       Normalize the $l^1$ norm to be *1*.
%
%     'area'    Normalize the area of the signal to be *1*. This is exactly the same as `'1'`.
%
%     '2'       Normalize the $l^2$ norm to be *1*.
%
%     'energy'  Normalize the energy of the signal to be *1*. This is exactly
%               the same as `'2'`.
%
%     'inf'     Normalize the $l^{\inf}$ norm to be *1*.
%
%     'peak'    Normalize the peak value of the signal to be *1*. This is exactly
%               the same as `'inf'`.
%
%     'rms'     Normalize the Root Mean Square (RMS) norm of the
%               signal to be *1*.
%
%     's0'      Normalize the S0-norm to be *1*.
%
%     'wav'     Normalize to the $l^{\inf}$ norm to be *0.99* to avoid
%               possible clipping introduced by the quantization procedure
%               when saving as a wav file. This only works with floating
%               point data types.
%
%     'null'    Do NOT normalize, output is identical to input.
%
%
%   It is possible to specify the value to which the norm is set and the dimension:
%
%     'val',v    Value to set the norm to
%
%      'dim',d
%                Work along specified dimension. The default value of `[]`
%                means to work along the first non-singleton one.
%
%   See also: rms, s0norm

if ~isnumeric(f)
  error('%s: Input must be numerical.',upper(mfilename));
end;

if nargin<1
  error('%s: Too few input parameters.',upper(mfilename));
end;

definput.import={'setnorm'};
definput.keyvals.dim=[];
definput.keyvals.val=[];

[flags,kv]=ltfatarghelper({},definput,varargin,'setnorm');

if flags.do_null || flags.do_norm_notset || isempty(f);
  return
end;

if isa(f,'integer') && ~flags.do_wav
   error('%s: Integer data types are unsupported.',upper(mfilename));
end

if isempty(kv.val)
    kv.val=1;
end
%% ------ Computation --------------------------

[f,L,Ls,W,dim,permutedsize,order]=assert_sigreshape_pre(f,[],kv.dim, ...
                                                  upper(mfilename));
fnorm = zeros(W,1);

for ii=1:W

  if flags.do_1 || flags.do_area
    fnorm(ii) =  norm(f(:,ii),1);
  end;

  if flags.do_2 || flags.do_energy
    fnorm(ii) = norm(f(:,ii),2);
  end;

  if flags.do_inf || flags.do_peak
    fnorm(ii) = norm(f(:,ii),Inf);
  end;

  if flags.do_rms
    fnorm(ii) = rms(f(:,ii));
  end;

  if flags.do_s0
    fnorm(ii) = s0norm(f(:,ii));
  end;

  if flags.do_wav
    if isa(f,'float')
       fnorm(ii) = (1/0.99)*norm(f(:,ii),Inf);
    else
       error(['%s: TO DO: Normalizing integer data types not supported ',...
              'yet.'],upper(mfilename));
    end
  end;

  fnorm = 1/kv.val*fnorm;

  if fnorm(ii) > 0
     f(:,ii)=f(:,ii)/fnorm(ii);
  end
end;

f=assert_sigreshape_post(f,kv.dim,permutedsize,order);


function Q=QCoordTurn(T,x,sigmaV2,sigmaTurn2,sigmaLin2)
%%QCOORDTURN Get the discrete-time process noise covariance matrix for a
%            2D or 3D coordinated turn model with a Cartesian state. The
%            turn rate can be specified in terms of a turn rate in radians
%            per second, or in terms of a transversal acceleration.
%            Additionally, a linear acceleration can be given.
%
%INPUTS: T The time-duration of the propagation interval in seconds.
%        x The target state for 2D or 3D motion. If there is no linear
%          acceleration (acceleration along the direction of motion), then
%          x in 2D can either be x=[x;y;xdot;ydot;omega], where omega is
%          the turn rate estimate in radians per second counterclockwise
%          from the x-axis or x can be x=[x;y;xdot;ydot;at] where at is
%          the transversal acceleration, which is orthogonal to the
%          velocity and is defined such that positive values of at map to
%          positive values of omega. In 3D, the state has z and zdot
%          components after y and ydot. If there is a linear acceleration,
%          then in 2D the target state is either x=[x;y;xdot;ydot;omega;al]
%          where omega is the turn rate and al is the linear acceleration
%          or the target state is x=[x;y;xdot;ydot;at;al] if the turn is
%          expressed in terms of a transversal acceleration. Again, in 3D,
%          z and zdot components are respectively added after y and ydot. The
%          dimensionality of the state is used to determine the
%          dimensionality and whether a linear acceleration component is
%          present. The linear acceleration component changes the speed.
%          That means that it acts in the direction of the velocity vector.
%  sigmaV2 The variance driving the process noise affecting the velocity
%          components. This has units of m^2/s^4 and is assumed to be the
%          same in both the x and y dimensions.
% sigmaTurn2 If the turn is specified in terms of a turn rate in radians
%          per second, then this is the variance driving the process noise
%          of the turn rate having units of radians squared per second
%          squared. If the turn is expressed in terms of a transverse
%          acceleration, then this is the variance of the transverse
%          acceleration noise, having units of m^2/s^4.
% sigmaLin2 This parameter is only needed if a linear acceleration is
%          present. It is the variance of the linear acceleration noise
%          having units of m^2/s^4.
%
%OUTPUTS: Q The process noise covariance matrix under a direct discrete-
%           time coordinated turn model where the velocity is specified in
%           Cartesian coordinates. Note that Q is singular, due to the
%           direct-discrete modeling of the process noise.
%
%The basic 2D coordinated turn model is described in Section VA of [1]. It
%is assumed that the continuous-time turn rate model is
%omegaDot=-(1/tau)*Omega+noise, which discretizes to
%omega[k+1]=exp(-T/tau)*omega[k]+noise.
%The turn rate for an unknown process noise is taken from Equation 73 of
%the paper and is a discrete-time approximation, not an exact
%discretization. The ordering of the elements in the state has been changed
%from the paper.
%
%The velocity and position components of the process noise model are
%similar to a discrete white noise acceleration model. Thus, a starting
%point for the process noise parameter sigmaV2 can be found using the
%processNoiseSuggest function with 'PolyKalDirectDisc-ROT' as the model and
%order=1 to cover velocity changes that are not solely due to turns or
%linear accelerations. The changes in the turn rate/transverse acceleration
%that are not due to exponential decay can similarly be covered using
%process noise  starting with sigmaTurn2 being set based on
%processNoiseSuggest with 'PolyKalDirectDisc-ROT' and order=1/2. Similarly,
%a starting point for sigmaLin2 is from processNoiseSuggest with
%'PolyKalDirectDisc-ROT' and order=2.
%
%Note that the process noise added to the turn component and the linear
%acceleration term (if present) is not integrated into the velocity/
%position components. If the only changes allowed/ expected are due to
%modifications in the linear and transverse accelerations (or turn rate),
%then sigmaV2=0. However, regardless of the size of T, the uncertainty in
%the other components would not directly integrate into the position and
%velocity components over any one step. Thus, one would expect the model to
%be particularly poor when the step size is large.
%
%The corresponding transition matrix in 2D is given by the function
%FCoordTurn2D. The corresponding continuous-time functions are
%aCoordTurn2DOmega and aCoordTurn2DTrans with diffusion matrix
%DCoordTurn2D. However, note that the discrete-time functions with
%unknown noise is a direct-discrete model and not a discretization of the
%continuous-time model.
%
%REFERENCES:
%[1] X. R. Li and V. P. Jilkov, "Survey of maneuvering target tracking.
%   Part I: Dynamic models," IEEE Transactions on Aerospace and Electronic
%   Systems, vol. 39, no. 4, pp. 1333-1364, Oct. 2003.
%
%July 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

sigmaV=sqrt(sigmaV2);
sigmaTurn=sqrt(sigmaTurn2);

switch(length(x))
    case 5%2D, there is no linear acceleration.
        G=[T^2/2;%Position x
           T^2/2;%Position y
           T;%Velocity x
           T;%Velocity y
           T];%Turn rate/ linear acceleration
        G(1:4)=G(1:4)*sigmaV;
        G(5)=G(5)*sigmaTurn;
        Q=G*G';
    case 6%2D, there is linear acceleration.
        sigmaLin=sqrt(sigmaLin2);
        G=[T^2/2;%Position x
           T^2/2;%Position y
           T;%Velocity x
           T;%Velocity y
           T;%Turn rate/ linear acceleration
           T];%Transverse acceleration
        G(1:4)=G(1:4)*sigmaV;
        G(5)=G(5)*sigmaTurn;
        G(6)=G(6)*sigmaLin;
        Q=G*G';
    case 7%3D, there is no linear acceleration.
        G=[T^2/2;%Position x
           T^2/2;%Position y
           T^2/2;%Position z
           T;%Velocity x
           T;%Velocity y
           T;%Velocity z
           T];%Turn rate/ linear acceleration
        G(1:6)=G(1:6)*sigmaV;
        G(7)=G(7)*sigmaTurn;
        Q=G*G';
    case 8%3D, there is linear acceleration.
        sigmaLin=sqrt(sigmaLin2);
        G=[T^2/2;%Position x
           T^2/2;%Position y
           T^2/2;%Position z
           T;%Velocity x
           T;%Velocity y
           T;%Velocity z
           T;%Turn rate/ linear acceleration
           T];%Transverse acceleration
        G(1:6)=G(1:6)*sigmaV;
        G(7)=G(7)*sigmaTurn;
        G(8)=G(8)*sigmaLin;
        Q=G*G';
    otherwise
        error('The length of x is neither 5 nor 6 (for 2D) and neither 7 or 8 (for 3D).');
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.