function[varargout]=mspec(varargin)
% MSPEC  Multitaper power and cross spectra.
%   _______________________________________________________________________
%
%   *|* mspec.png --- Figure illustrating the multitaper spectrum.
%   Type 'jhelp mspec' to view this image. *|*
%   _______________________________________________________________________
%
%   MSPEC implements spectral and cross-spectral analysis using the multi-
%   taper method for real or complex-valued data, and along any dimension.
%
%   MSPEC is to be run after calling SLEPTAP to compute the multitapers.
%
%   Confidence intervals can be computed by calling MCONF.
%   _______________________________________________________________________
%
%   Real-valued time series
%
%   [F,S]=MSPEC(X,PSI) returns the power spectrum of the real-valued time
%   series X at positive frequencies using data tapers PSI.  The spectrum
%   at negative frequencies, which is not returned, is identical.
%
%   X may be an array with an arbitrary number of dimensions and with time
%   along it first dimensions.  Let M be its number of rows, M=SIZE(X,1).
%
%   PSI is a then matrix of K data tapers having M rows and K columns.
%
%   F is an array of frequencies with FlOOR(M/2)+1 rows, while the spectral
%   matrix S has FlOOR(M/2)+1 rather than M rows, and the same size as X
%   along all of its other dimensions.
%
%   The spectrum can also be computed with time oriented along a different
%   dimension, as described below.
%
%   By default, MSPEC removes the mean from each time series before
%   computing the spectra. This is suppressed by MSPEC(...,'nodemean').
%   ______________________________________________________________________
%
%   Cross-spectra of real-valued data
%
%   [F,SXX,SYY,SXY]=MSPEC(X,Y,PSI) computes the cross-spectrum of two
%   real-valued time series or sets of time series.  Here SXX and SYY are
%   the one-sided spectra of X and Y, while SXY is their cross spectrum.
%
%   See TWOSPECPLOT for plotting SXX and SYY simultaneously.
%   ______________________________________________________________________
%
%   Rotary spectra of complex-valued data
%
%   [F,SPP,SNN,SPN]=MSPEC(Z,PSI) where Z is complex-valued computes the so-
%   called "rotary spectra". Here SPP and SNN are the positively-rotating
%   and negatively rotating spectra, and SPN is the rotary cross spectrum.
%   the one-sided spectra of X and Y, while SPN is their cross spectrum.
%
%   Note that the rotary spectra are defined such that SXX+SYY=SPP+SNN.
%
%   The rotary spectra SPP and SNN are normalized such that the sum of SPP
%   over all frequencies plus that of SNN approximates the variance of Z.
%
%   See TWOSPECPLOT for plotting SPP and SNN simultaneously.
%   ______________________________________________________________________
%
%   Periodogram
%
%   MPSEC can be used to form the naive spectral estimator, known as the
%   periodogram. Although this is not generally a good way to estimate the
%   spectrum, it can be useful as a comparision.
%
%   MSPEC(X,[]) or MSPEC(X,Y,[]) with PSI empty uses the default, or boxcar
%   taper, normalized to unit energy. This returns the periodogram.
%   ______________________________________________________________________
%
%   Sample rate
%
%   [F,S]=MSPEC(DT,...) specifies the sample interval to be used in the
%   calculation of the frequency array F. DT defaults to unity.
%
%   Spectral values depend linearly upon the sample rate in order that the
%   integral of the spectra over frequency approximate the variance.
%   ______________________________________________________________________
%
%   Spectra along arbitary dimension
%
%   MSPEC(...,DIM) computes the spectrum with time oriented along dimension
%   DIM, with the default behavior corresponding to DIM=1.
%
%   Let M be the length of the input X, Z, or X and Y along dimension DIM.
%   PSI is again a matrix of K data tapers having M rows and K columns.
%
%   F will be again an array of frequencies with FlOOR(M/2)+1 rows, while
%   the output spectral matrices will the same size as the input arrays,
%   but with FlOOR(M/2)+1 rather than M elements along dimension DIM.
%   ______________________________________________________________________
%
%   Normalizations
%
%   By default, MSPEC uses *radian* frequency as in cos(f t).  Optionally
%   MSPEC(,...,'cyclic') will use *cyclic* frequency, as in cos(2 pi f t).
%
%   MSPEC is normalized to approximately recover the time series variance.
%   For the MSPEC periodogram, this recovery is exact, although the
%   expressions are complicated somewhat by the use of one-sided spectra.
%
%   For simplicity, the normalizations will be explained for the case of a
%   single time series with M oriented along rows, that is, with DIM=1.
%
%   Real-valued data
%
%   [F,S]=MSPEC(DT,X,[]) where X is a real-valued time series of length M
%   recovers the variance of X, STD(X,1).^2, as follows:
%
%     2*(1/2/pi)*(F(2)-F(1))*SUM(S(2:end))               -- M odd
%     2*(1/2/pi)*(F(2)-F(1))*(SUM(S(2:end-1))+S(end)/2)  -- M even
%
%   where the initial factor of two accounts for the fact that the spectrum
%   at negative frequencies is the same as that at positive frequencies.
%
%   Note that the zero frequency is omitted in the summation, and for even
%   time series length, the power at the Nyquist S(end) must be divided by
%   two to avoid double-counting by the one-sided spectrum.  The "1" in the
%   argument of STD forces STD to use an N rather than N-1 normalization.
%
%   Complex-valued data
%
%   [F,SPP,SNN]=MSPEC(DT,Z,[]) where Z is a complex-valued time series of
%   length M recovers the variance of Z, STD(Z,1).^2, as follows:
%
%     (1/2/pi)*(F(2)-F(1))*(SUM(SPP(2:end))+SUM(SNN(2:end)))   -- M odd
%     (1/2/pi)*(F(2)-F(1))*(SUM(SPP(2:end))+SUM(SNN(2:end-1))) -- M even
%
%   Again the modification for even M prevents the power at the Nyquist
%   from being double-counted.  This modification is necessary because the
%   negative rotary spectrum duplicates the Nyquist when M is even.
%   ______________________________________________________________________
%
%   Cross-spectra of complex-valued data
%
%   To compute the cross-spectra of two complex-valued time series or sets
%   of time series Z1 and Z2, run MSPEC repeatedly.
%
%   [F,SP1P1,SP2P2,SP1P2]=MSPEC(Z1,Z2,PSI);
%   [F,SN1N1,SN2N2,SN1N2]=MSPEC(CONJ(Z1),CONJ(Z2),PSI);
%
%   The first call returns the spectra and cross-spectra of Z1 and Z2 at
%   positive frequencies, while the second returns their spectra and the
%   *conjugate* of the cross-spectrum at negative frequencies.  Finally
%
%   [F,SP1P1,SN2N2,SP1N2]=MSPEC(Z1,CONJ(Z2),PSI);
%   [F,SN1N1,SP2P2,SN1P2]=MSPEC(CONJ(Z1),Z2,PSI);
%
%   returns the so-called outer or complementary cross-spectra.
%   ______________________________________________________________________
%
%   Adaptive spectra
%
%   MSPEC(...,LAMBDA,'adaptive'), where LAMBDA contains the eigenvalues of
%   the tapers as computed by SLEPTAP, alternately uses the "adaptive"
%   multitaper method of Thomson (1982).
%
%   This implementation follows that of Park et al. (1987a), JGR.
%
%   For cross-spectra or for rotary spectra, the weights appearing in the
%   adaptive spectra are derived for the total spectrum of each signal
%   compoment, that is for SXX+SYY or SPP+SNN as appropriate.  Then the
%   separate spectra and co-spectra are computed using identical weights.
%   ______________________________________________________________________
%
%   Cell array input / output
%
%   MSPEC generates cell array output given cell array input.
%
%   Let's say one has P different time series, X1, X2,..., XP.  Put these
%   into a cell array X{1}=X1, X{2}=X2, ..., X{P}=XP, and then use
%   "[psi,lambda]=sleptap(cellength(x))" to make a cell array of tapers.
%
%   [F,S]=MSPEC(X,PSI) then returns cell arrays F and S corresponding
%   to the Fourier frequencies and spectra of the P arrays.
%
%   The other argument forms given above also work.  In particular,
%   specifiying the sample time through MPSEC(DT,...) works, with DT either
%   a scalar or an array of the same length as the cell array X.
%
%   The spectra can then be plotted with CELLPLOT(F,S), or TWOSPECPLOT for
%   a pair of output spectra.
%   ______________________________________________________________________
%
%   Parallelization
%
%   MSPEC(..., 'parallel') when the input fields X, X and Y, or Z are cell
%   arrays, parallelizes the spectral estimation by looping over the cells
%   with a PARFOR loop.  This requires Matlab's Parallel Computing Toolbox.
%   ______________________________________________________________________
%
%   Example
%
%   The example at the top of this help file shows clockwise (left) and
%   counterclockwise (right) rotary spectra from moored current meter
%   measurements of the ocean currents in the Labrador Sea.
%
%   The periodogram is in gray, and blue and red are multitaper spectra
%   with P=4 and P=32, respectively.  The local Coriolis frequency is
%   marked with a dashed line.  Tidal and inertial peaks are apparent.
%
%   The main point of this figure is to show that increasing P increases
%   the degree of frequency-domain smoothing.
%   ______________________________________________________________________
%
%   'mspec --t' runs some tests.
%   'mspec --f' generates the above sample figure from Bravo mooring data.
%
%   See also:  SLEPTAP, MCONF, HERMFUN, MSVD, TWOSPECPLOT.
%
%   Usage   [f,s]=mspec(x,psi);
%           [f,s]=mspec(dt,x,psi);
%           [f,s]=mspec(dt,x,psi,dim);
%           [f,spp,snn,spn]=mspec(z,psi);
%           [f,sxx,syy,sxy]=mspec(x,y,psi);
%           [f,sxx,syy,sxy]=mspec(x,y,psi,dim);
%   _________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2000--2020 J.M. Lilly --- type 'help jlab_license' for details

%   The Cartesian and rotary spectra are then related by a unitary
%   transformation.  For details see
%
%       Lilly and Olhede (2010).  Bivariate instantaneous frequency and
%           bandwidth.  IEEE Trans. Sig. Proc.

if strcmpi(varargin{1},'--t')
    mspec_test;return
end
if strcmpi(varargin{1},'--f')
    type makefigs_mspec;
    makefigs_mspec;
    return
end

%Sort out input arguments
dim=1;
deltat=1;
lambda=1;  %This means use the average multitaper spectrum
detrendstr='demean';
normstr='rad';
cores='serial';

for i=1:4
    if ischar(varargin{end})
        if ~isempty(strfind(varargin{end},'ada'))
            lambda=varargin{end-1};
            varargin=varargin(1:end-2);
            if length(lambda)~=numel(lambda)
                if lambda~=floor(lambda)
                    error('Looks like you forgot to input LAMBDA with the adaptive algorithm.')
                end
            end
        elseif ~isempty(strfind(varargin{end},'dem'))||~isempty(strfind(varargin{end},'nod'))
             detrendstr=varargin{end};
             varargin=varargin(1:end-1);
        elseif strcmpi(varargin{end}(1:3),'par')||strcmpi(varargin{end}(1:3),'ser')
             cores=varargin{end};
             varargin=varargin(1:end-1);
        else
             normstr=varargin{end};
             varargin=varargin(1:end-1);
        end
    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 standard algorithm.')
        cores='serial';
    end
end

if isscalar(varargin{end})&&~iscell(varargin{end})
    dim=varargin{end};
    varargin=varargin(1:end-1);
end

if isscalar(varargin{1})
  deltat=varargin{1};
  varargin=varargin(2:end);
else
  if length(varargin)>2
      bool1=~iscell(varargin{2})&&(numel(varargin{1})==size(varargin{2},2));
      bool2= iscell(varargin{2})&&(numel(varargin{1})==length(varargin{2}));
      if bool1||bool2
          deltat=varargin{1};
          deltat=deltat(:)';
          varargin=varargin(2:end);
      end
  end
end

x=varargin{1};
if length(varargin)==1
    error('Taper not specified.')
end
psi=varargin{end};
na=length(varargin);

y=[];
if na==3
    y=varargin{2};
end

if iscell(x)||iscell(y)
    %All of this is just to handle cell array input
    if isempty(y)
        y=cell(size(x));
    elseif isempty(x)
        x=cell(size(y));
    end
    if ~iscell(psi)
        psio=psi;
        psi=cell(size(x));
        for i=1:length(x)
            psi{i}=psio;
        end
    end
    if size(lambda,2)==1
        lambda=vrep(lambda,length(x),2);
    end
    if length(deltat)==1
        deltat=deltat+zeros(size(x));
    end

    if strcmpi(cores(1:3),'ser')
        for i=1:length(x)
            disp(['SLEPTAP computing spectra for time series #' int2str(i) ' of ' int2str(length(x)) '.'])
            %iscell(deltat),iscell(x),iscell(y),iscell(psi)
            %vsize(deltat(i),x{i},y{i},psi{i},lambda(:,i))
            cellout{i}=mspec_one(deltat(i),x{i},y{i},psi{i},lambda(:,i),detrendstr,normstr,dim);
        end
    elseif strcmpi(cores(1:3),'par')
        %Exactly the same but with a parfor
        parfor i=1:length(x)
            disp(['SLEPTAP computing spectra for time series #' int2str(i) ' of ' int2str(length(x)) '.'])
            %iscell(deltat),iscell(x),iscell(y),iscell(psi)
            %vsize(deltat(i),x{i},y{i},psi{i},lambda(:,i))
            cellout{i}=mspec_one(deltat(i),x{i},y{i},psi{i},lambda(:,i),detrendstr,normstr,dim);
        end
    end

    for i=1:length(x)
        for j=1:length(cellout{i})
            varargout{j}{i,1}=cellout{i}{j};
        end
    end
else
    varargout=mspec_one(deltat,x,y,psi,lambda,detrendstr,normstr,dim);
end

function[cellout]=mspec_one(dt,x,y,psi,lambda,detrendstr,normstr,dim)

if ~isscalar(dt)
    dt=vrep(dt,length(fourier(size(x,1))),1);
end

if ~isreal(x)&&isempty(y)
    y=conj(x);
end

if strcmpi(detrendstr(1:3),'dem')
    x=x-vrep(vmean(x,dim),size(x,dim),dim);
    if ~isempty(y)
       y=y-vrep(vmean(y,dim),size(y,dim),dim);
    end
end

if isempty(psi)
    psi=frac(1,sqrt(size(x,1)))+zeros(size(x(:,1)));
end

[f,mmatx,mmaty]=mtrans(x,y,psi,dim);

N=lnsd(x)+1;
if isempty(y) %One time series
     if length(lambda)==1
         cellout{2}=avgspec(mmatx,mmatx,N).*dt;
     else
         var=vrep(squared(vstd(x,dim)),floor(size(x,dim)/2)+1,dim);
         %Variance same size as original input field
         cellout{2}=adaptspec(abs(mmatx).^2,lambda,var,N).*dt;
     end
     cellout{3}=zeros(size(cellout{2}));
     cellout{4}=zeros(size(cellout{2}));
else         %Two time series
     if length(lambda)==1
        cellout{2}=avgspec(mmatx,mmatx,N).*dt;
        cellout{3}=avgspec(mmaty,mmaty,N).*dt;
        cellout{4}=avgspec(mmatx,mmaty,N).*dt;
        cellout{4}(isnan(cellout{4}))=nan+1i*nan;%cross-spectrum should have complex nans
     else
        %For two time series one should do the adaptive spectra on both
        %with the same coefficients
        var=vrep(squared(vstd(x,dim))+squared(vstd(y,dim)),floor(size(x,dim)/2)+1,dim);

        [~,dk]=adaptspec(abs(mmatx).^2+abs(mmaty).^2,lambda,var,N);
        cellout{2}=frac(sum(dk.^2.*abs(mmatx).^2.*dt,N),sum(abs(dk).^2,N));
        cellout{3}=frac(sum(dk.^2.*abs(mmaty).^2.*dt,N),sum(abs(dk).^2,N));
        cellout{4}=frac(sum(dk.^2.*mmatx.*conj(mmaty).*dt,N),sum(abs(dk).^2,N));
        cellout{4}(isnan(cellout{4}))=nan+1i*nan;%cross-spectrum should have complex nans
     end
end

%Corrections for zero component, and for Nyquist with even and odd length
%Both zero are Nyquist are shared for even length time series, so divide both by two.
%Only zero is shared for odd length, since the Nyquist does not appear.
%This is best visualized by drawing N equally spaced points on the unit circle.

% for i=2:length(cellout)
%     cellout{i}(1,:)=cellout{i}(1,:)./2;
%     if iseven(size(x,1))
%        cellout{i}(end,:)=cellout{i}(end,:)./2;
%     end
% end

if contains(normstr,'cyc')
    f=f/2/pi;
    %for i=2:length(cellout)
    %    cellout{i}=cellout{i}/2/pi;
    %end
end

if isscalar(dt)
    cellout{1}=f./dt;
else
    cellout{1}=vrep(f,size(dt,2),2)./dt;
end

function[S]=avgspec(mmat1,mmat2,N)
eigspec=mmat1.*conj(mmat2);
S=mean(eigspec,N);

function[s,dk]=adaptspec(eigspec,lambda,var,N)

sold=vindex(eigspec,1,N);
s=frac(1,2)*(vindex(eigspec,1,N)+vindex(eigspec,2,N));
%Start just with first two

tol=1e-4;

var=vrep(var,length(lambda),N);
lambdamat=zeros(size(var));
%vsize(var,lambdamat,eigspec)
for i=1:length(lambda)
    lambdamat=vindexinto(lambdamat,lambda(i),i,N);
end
%figure,plot(lambdamat)
bkmat=var.*(1-lambdamat);  %Outer product;

% sold=s;
% smat=vrep(s,size(eigspec,N),N);
% dk=(smat.*real(sqrt(lambdamat)))./(smat.*lambdamat+bkmat);  %Outer products
% s=frac(sum(dk.^2.*eigspec,N),sum(abs(dk).^2,N));

i=0;
%maxmax(abs(s-sold)./sold)
while anyany(abs(s-sold)./sold>tol)&&i<20
    i=i+1;
	sold=s;
    smat=vrep(s,size(eigspec,N),N);
	dk=(smat.*real(sqrt(lambdamat)))./(smat.*lambdamat+bkmat);  %Outer products
	s=frac(sum(dk.^2.*eigspec,N),sum(abs(dk).^2,N));
 %   maxmax(abs(s-sold)./sold)
end

if i~=20
   disp(['Adaptive spectral estimate took ' int2str(i) ' iterations.'])
else
   disp(['Adaptive spectral loop terminated at ' int2str(i) ' iterations.'])
end

function[varargout]=mtrans(varargin)
% MTRANS  Multitaper "eigentransform" computation.
%
%   [F,W]=MTRANS(X,PSI,DIM) returns the multitaper "eigentransform" matrix
%   for use in multitaper spectral estimates or eigenspectral SVD analysis.
%
%       X  --  M x N matrix containing N length M time series
%     PSI  --  M x K matrix of K data tapers
%       W  --  [M/2] x K x  N eigentransform matrix (for real X)
%              [M/2] x K x 2N eigentransform matrix (for complex X)
%
%   In the above, [M/2] means M/2 if M is even, and (M-1)/2 is M is odd.
%
%   F is the angular Fourier frequency, in radians.
%
%   [F,W1,W2,...,WN]=MTRANS(X1,X2,...,XN,PSI,DIM) also works, where X1,...,
%   XN are all M x N matrices.
%
%   MTRANS(X,[]) with PSI empty uses the default taper, so that the square
%   of W corresponds to the periodogram.
%
%   See also: SLEPTAP, HERMFUN, MSPEC, MSVD.
%
%   Usage:  [f,w]=mtrans(x,psi,dim);
%           [f,wx,wy]=mtrans(x,y,psi,dim);
%           [f,wx,wy,wz]=mtrans(x,y,z,psi,dim);
%   _________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2000--2015 J.M. Lilly --- type 'help jlab_license' for details

%Sort out input arguments
dim=varargin{end};
psi=varargin{end-1};
x=varargin(1:end-2);

%Remove empties
clear bool
for i=1:length(x)
    bool(i)=isempty(x{i});
end
x=x(~bool);

%Size check
bool=false(4,length(x));
for i=1:4
    for j=1:length(x)
       bool(i,j)=aresame(size(x{j},i),size(x{1},i));
    end
end
if ~allall(bool)
   error('All input arguments should be the same size')
end

%This complicated code makes a vector that permutes PSI such that it has
%its first dimension along DIM and its last along dimension LNSD(X{1})+1.
n=3;
permutevec=zeros(lnsd(x{1})+1,1);
for i=1:lnsd(x{1})
    if i==dim
        permutevec(i)=1;
    else
        permutevec(i)=n;
        n=n+1;
    end
end
permutevec(end)=2;

if isempty(psi)
    psimat=[];
else
    psimat=permute(psi,permutevec);
    for i=1:lnsd(x{1})
        if i~=dim
            psimat=vrep(psimat,size(x{1},i),i);
        end
    end
end

f=fourier(size(x{1},dim));

index=1:length(f);
varargout{1}=f;
varargout{2}=[];
varargout{3}=[];

for i=1:size(x,2)
    Nnans=length(find(~isfinite(x{i})));
    if Nnans>0
        disp(['MSPEC finding non-finite data values.  Spectrum will be undefined.'])
    end

    N=lnsd(x{i})+1;
    xmat=vrep(x{i},size(psimat,N),N);
    mmat=fft(psimat.*xmat,[],dim);

    mmat(isnan(mmat))=nan;%This is to prevent imaginary NaNs
    %    varargout{i+1}=mmat(index,:,:);
    varargout{i+1}=vindex(mmat,index,dim);
end

function[]=mspec_test

tol=1e-10;
load bravo94
cv=bravo94.rcm.cv(:,2:end);
[psi,lambda]=sleptap(length(cv),8);

[~,spp,snn,spn]=mspec(cv,psi,lambda,'adaptive');
[~,spp2,snn2,spn2]=mspec(conj(cv)',psi,2,lambda,'adaptive');
bool=aresame(spp,spp2',1e-9)&&aresame(snn,snn2',1e-9)&&aresame(spn,conj(spn2)',1e-9);
reporttest('MSPEC transposed input for adaptive spectrum',bool)

[~,sxx,syy,sxy]=mspec(real(cv),imag(cv),psi);
[~,spp,snn,spn]=mspec(cv,psi);

[~,spp2,snn2,spn2]=mspec(conj(cv)',psi,2);
bool=aresame(spp,spp2',1e-9)&&aresame(snn,snn2',1e-9)&&aresame(spn,conj(spn2)',1e-9);
reporttest('MSPEC transposed input for average spectrum',bool)

S(1,1,:,:)=sxx;
S(2,2,:,:)=syy;
S(1,2,:,:)=sxy;
S(2,1,:,:)=conj(sxy);

SZ(1,1,:,:)=spp;
SZ(2,2,:,:)=snn;
SZ(1,2,:,:)=spn;
SZ(2,1,:,:)=conj(spn);

T=sqrt(2)*vrep(vrep(tmat,size(S,3),3),size(S,4),4);

SZ2=matmult(matmult(T,S,1),conj(permute(T,[2 1 3 4])),1);

reporttest('MSPEC for (x,y) and (z,z^*) are orthogonal transforms with matrix SQRT(2)*T',aresame(SZ,SZ2,1e-10))

tol=1e-10;
x=bravo94.rcm.cv(:,3);
xo=vfilt(x,24,'nonans');
%num=yf2num(bravo.rcm.yearf);
%t=num-yf2num(floor(bravo.rcm.yearf(1)));
t=yearfrac(bravo94.rcm.num);

psi=sleptap(length(x),8);

p0=vsum(abs(real(x)-vmean(real(x),1)).^2,1)./length(x);
[f,sp]=mspec(real(x),psi);
p1=2*frac(1,2*pi)*(vsum(sp,1)).*(f(2)-f(1));
reporttest('MSPEC satisfies Parseval''s theorem to within 4% for real Bravo, unit sample rate',abs(p1-p0)./p0<4/100);

[f,sp]=mspec(t(2)-t(1),real(x),psi);
p1=2*frac(1,2*pi)*(vsum(sp,1)).*(f(2)-f(1));
reporttest('MSPEC satisfies Parseval''s theorem to within 4% for real Bravo, non-unit sample rate',abs(p1-p0)./p0<4/100);

[f,sp]=mspec(t(2)-t(1),real(x),[]);
p1=2*frac(1,2*pi)*(vsum(sp,1)).*(f(2)-f(1));
reporttest('MSPEC satisfies Parseval''s theorem to within 4% for real Bravo, non-unit sample rate periodogram',abs(p1-p0)./p0<4/100);

[f,sp]=mspec(t(2)-t(1),real(x),psi,'cyc');
p1=2*(vsum(sp,1)).*(f(2)-f(1));
reporttest('MSPEC satisfies Parseval''s theorem to within 4% for real Bravo, non-unit sample rate, cyclic frequency',abs(p1-p0)./p0<4/100);

p0=vsum(abs(x-vmean(x,1)).^2,1)./length(x);
[f,sp,sn]=mspec(x,psi);
p1=frac(1,2*pi)*(vsum(sp,1)+vsum(sn,1)).*(f(2)-f(1));
reporttest('MSPEC satisfies Parseval''s theorem to within 4% for complex Bravo, unit sample rate',abs(p1-p0)./p0<4/100);

[f,sp,sn]=mspec(t(2)-t(1),x,psi);
p1=frac(1,2*pi)*(vsum(sp,1)+vsum(sn,1)).*(f(2)-f(1));
reporttest('MSPEC satisfies Parseval''s theorem to within 4% for complex Bravo, non-unit sample rate',abs(p1-p0)./p0<4/100);

[f,sx,sy]=mspec(real(x),imag(x),psi);
p1=2*frac(1,2*pi)*(vsum(sx+sy,1)).*(f(2)-f(1));
reporttest('MSPEC satisfies Parseval''s theorem to within 4% for bivariate Bravo, unit sample rate',abs(p1-p0)./p0<4/100);

[f,sx,sy]=mspec(t(2)-t(1),real(x),imag(x),psi);
p1=2*frac(1,2*pi)*(vsum(sx+sy,1)).*(f(2)-f(1));
reporttest('MSPEC satisfies Parseval''s theorem to within 4% for bivariate Bravo, non-unit sample rate',abs(p1-p0)./p0<4/100);

reporttest('MSPEC 2*(SXX+SYY)=SPP+SNN for Bravo, non-unit sample rate',aresame(2*sx+2*sy,sp+sn,tol));

N=1001;
cv=randn(N,1)+sqrt(-1)*randn(N,1);
cv=cv-vmean(cv,1);
[~,spp,snn]=mspec(cv,[]);
reporttest('MSPEC periodogram matches variance exactly for zero-mean complex signal and odd length', aresame(sum(spp)./N+sum(snn(2:end))./N,sum(abs(cv).^2)./N,1e-10))

N=1001;
cv=randn(N,1)+sqrt(-1)*randn(N,1)+17;
[~,spp,snn]=mspec(cv,[],'nodemean');
reporttest('MSPEC periodogram matches variance exactly for non-zero-mean complex signal and odd length', aresame(sum(spp)./N+sum(snn(2:end))./N,sum(abs(cv).^2)./N,1e-10))

N=1000;
cv=randn(N,1)+sqrt(-1)*randn(N,1);
cv=cv-vmean(cv,1);
[~,spp,snn]=mspec(cv,[]);
reporttest('MSPEC periodogram matches variance exactly for zero-mean complex signal and even length', aresame(sum(spp)./N+sum(snn(2:end-1))./N,sum(abs(cv).^2)./N,1e-10))

N=1000;
cv=randn(N,1)+sqrt(-1)*randn(N,1)+17;
[~,spp,snn]=mspec(cv,[],'nodemean');
reporttest('MSPEC periodogram matches variance exactly for non-zero-mean complex signal and even length', aresame(sum(spp)./N+sum(snn(2:end-1))./N,sum(abs(cv).^2)./N,1e-10))

N=1000;
dt=3600;
cv=randn(N,1)+sqrt(-1)*randn(N,1)+17;
[~,spp,snn]=mspec(dt,cv,[],'nodemean');
reporttest('MSPEC periodogram matches variance exactly for non-zero-mean complex signal and even length, and non-unit sample rate', aresame(sum(spp)./(dt*N)+sum(snn(2:end-1))./(dt*N),sum(abs(cv).^2)./N,1e-10))

N=1000;
cv=randn(N,1)+sqrt(-1)*randn(N,1)+17;
tic;[~,spp,snn]=mspec(cv,[],'nodemean');toc
tic;S=squared(fft(cv));toc;
b1=aresame(S(1:length(spp)),spp*N,1e-6);
b2=aresame(flipud(S(length(spp)+1:end)),snn(2:end-1)*N,1e-6);

reporttest('MSPEC recovers periodogram for even N', b1&&b2)

N=1001;
cv=randn(N,1)+sqrt(-1)*randn(N,1)+17;
tic;[~,spp,snn]=mspec(cv,[],'nodemean');toc
tic;S=squared(fft(cv));toc;
b1=aresame(S(1:length(spp)),spp*N,1e-6);
b2=aresame(flipud(S(length(spp)+1:end)),snn(2:end)*N,1e-6);
reporttest('MSPEC recovers periodogram for odd N', b1&&b2)

mspec_test_frequency;

function[]=mspec_test_frequency

T=10000;
fo=100./T;
x=cos([1:T]'*2*pi*fo);
psi=sleptap(T,1,1);
[f,sp,sn]=mspec(x+sqrt(-1)./1e10,psi);

[~,jp]=max(sp);
[~,jn]=max(sn);
bool=aresame(frac(1,2*pi)*f(jp),fo,1e-12)&&aresame(frac(1,2*pi)*f(jn),fo,1e-12);
reporttest('MSPEC frequency matches expected exactly, even number of points',bool);

T=10000-1;
fo=100/T;
x=cos([1:T]'*2*pi*fo);
psi=sleptap(T,1,1);
[f,sp,sn]=mspec(x+sqrt(-1)./1e10,psi);

[~,jp]=max(sp);
[~,jn]=max(sn);
bool=aresame(frac(1,2*pi)*f(jp),fo,1e-12)&&aresame(frac(1,2*pi)*f(jn),fo,1e-12);
reporttest('MSPEC frequency matches expected exactly, odd number of points',bool);

% [f,Cuv]=mspec(real(cv),imag(x),psi);
% [f,Suu]=mspec(real(cv),psi);
% [f,Svv]=mspec(imag(cv),psi);
%
% gammauv=Cuv;
% for i=1:size(Suu,2)
%   gammauv(:,i)=Cuv(:,i)./sqrt(Suu(:,i).*Svv(:,3));
% end
% figure,
%
%
% plot(f,abs(gammauv)),xlog,yoffset 1
% title('Cross-spectrum of u(t) at each depth vs. u(t) at #3')


function gT = rbfinfwhiteKernGradX(kern, t1, t2)

% RBFINFWHITEKERNGRADX Gradient of RBF-WHITE kernel (with integration limits
% between minus infinity and infinity) with respect to a point t.
% FORMAT
% DESC computes the gradient of the RBF-WHITE kernel with respect to the
% input positions.
% ARG kern : kernel structure for which gradients are being computed.
% ARG t1 : locations against which gradients are being computed.
% RETURN gT : the returned gradients. The gradients are returned in
% a matrix which is numData x numInputs x numData. Where numData is
% the number of data points and numInputs is the number of input
% dimensions in t1 (currently always one).
%
% FORMAT
% DESC computes the gradient of the RBF-WHITE kernel with respect to the
% input positions where both the row positions and column positions are
% provided separately.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG t1 : row locations against which gradients are being computed.
% ARG t2 : column locations against which gradients are being computed.
% RETURN gT : the returned gradients. The gradients are returned in
% a matrix which is numData2 x numInputs x numData1. Where numData1 is
% the number of data points in t1, numData2 is the number of data
% points in t2 and numInputs is the number of input dimensions in t1
% and t2 (currently always one).
%
% SEEALSO rbfinfwhiteKernParamInit, kernGradX, rbfinfwhiteKernDiagGradX
%
% COPYRIGHT : David Luengo, 2009

% KERN

if nargin < 3
  t2 = t1;
end
if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end

gT = zeros(size(t1, 1), 1, size(t2, 1));

% Parameters of the kernel required in the computation
variance = kern.variance;
invWidth = kern.inverseWidth;

for i = size(t1, 1)
    gT(i, 1, :) = - ( variance * invWidth * (t1(i)-t2) * sqrt(invWidth/pi) / 4) ...
        .* exp(- invWidth * ((t1(i)-t2).^2) / 4);
end


function [tfr,t,f] = tfrcw(x,t,N,g,h,sigma,trace);
%TFRCW	Choi-Williams time-frequency distribution.
%	[TFR,T,F]=TFRCW(X,T,N,G,H,SIGMA,TRACE) computes the Choi-Williams
%	distribution of a discrete-time signal X, or the
%	cross Choi-Williams representation between two signals.
%
%	X     : signal if auto-CW, or [X1,X2] if cross-CW.
%	T     : time instant(s)          (default : 1:length(X)).
%	N     : number of frequency bins (default : length(X)).
%	G     : time smoothing window, G(0) being forced to 1.
%	                                 (default : Hamming(N/10)).
%	H     : frequency smoothing window, H(0) being forced to 1.
%	                                 (default : Hamming(N/4)).
%	SIGMA : kernel width             (default : 1)
%	TRACE : if nonzero, the progression of the algorithm is shown
%                                        (default : 0).
%	TFR   : time-frequency representation. When called without
%         output arguments, TFRCW runs TFRQVIEW.
%	F     : vector of normalized frequencies.
%
%	Example:
%	 sig=fmlin(128,0.05,0.3)+fmlin(128,0.15,0.4);
%	 g=tftb_window(9,'Kaiser'); h=tftb_window(27,'Kaiser');
%	 t=1:128; tfrcw(sig,t,128,g,h,3.6,1);
%
%	See also all the time-frequency representations listed in
%	 the file CONTENTS (TFR*)

%	F. Auger, May-August 1994, July 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  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 should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin == 0),
 error('At least 1 parameter required');
end;
[xrow,xcol] = size(x);
if (xcol==0)|(xcol>2),
 error('X must have one or two columns');
end

if (nargin <= 2),
 N=xrow;
elseif (N<0),
 error('N must be greater than zero');
elseif (2^nextpow2(N)~=N),
 fprintf('For a faster computation, N should be a power of two\n');
end;

hlength=floor(N/4); hlength=hlength+1-rem(hlength,2);
glength=floor(N/10);glength=glength+1-rem(glength,2);

if (nargin == 1),
 t=1:xrow; g = tftb_window(glength); h = tftb_window(hlength); sigma = 1.0; trace = 0;
elseif (nargin == 2)|(nargin == 3),
 g = tftb_window(glength); h = tftb_window(hlength); sigma = 1.0; trace = 0;
elseif (nargin == 4),
 h = tftb_window(hlength); sigma = 1.0; trace = 0;
elseif (nargin == 5),
 sigma = 1.0; trace = 0;
elseif (nargin == 6),
 trace = 0;
end;

[trow,tcol] = size(t);
if (trow~=1),
 error('T must only have one row');
end;

[grow,gcol]=size(g); Lg=(grow-1)/2;
if (gcol~=1)|(rem(grow,2)==0),
 error('G must be a smoothing window with odd length');
end;

[hrow,hcol]=size(h); Lh=(hrow-1)/2; h=h/h(Lh+1);
if (hcol~=1)|(rem(hrow,2)==0),
  error('H must be a smoothing window with odd length');
end;

if (sigma<=0.0),
 error('SIGMA must be strictly positive');
end;

normfac = 16.0*pi/sigma; spreadfac = 16.0/sigma;
taumax = min([round(N/2),Lh]); tau = 1:taumax; points = -Lg:Lg;
CWKer = exp(-kron( points.' .^2, 1.0 ./ (spreadfac*tau.^2)));
CWKer = diag(g) * CWKer;

tfr= zeros (N,tcol) ;
if trace, disp('Choi-Williams distribution'); end;
for icol=1:tcol,
 ti= t(icol); taumax=min([ti+Lg-1,xrow-ti+Lg,round(N/2)-1,Lh]);
 if trace, disprog(icol,tcol,10); end;
 tfr(1,icol)= x(ti,1) .* conj(x(ti,xcol));

 for tau=1:taumax,
  points= -min([Lg,xrow-ti-tau]):min([Lg,ti-tau-1]);
  g2 = CWKer(Lg+1+points,tau); g2=g2/sum(g2);
  R=sum(g2 .* x(ti+tau-points,1) .* conj(x(ti-tau-points,xcol)));
  tfr(  1+tau,icol)=h(Lh+tau+1)*R;
  R=sum(g2 .* x(ti-tau-points,1) .* conj(x(ti+tau-points,xcol)));
  tfr(N+1-tau,icol)=h(Lh-tau+1)*R;
 end;

 tau=round(N/2);
 if (ti<=xrow-tau)&(ti>=tau+1)&(tau<=Lh),
  points= -min([Lg,xrow-ti-tau]):min([Lg,ti-tau-1]);
  g2 = CWKer(Lg+1+points,tau); g2=g2/sum(g2);
  tfr(tau+1,icol) = 0.5 * ...
   (h(Lh+tau+1)*sum(g2 .* x(ti+tau-points,1) .* conj(x(ti-tau-points,xcol)))+...
    h(Lh-tau+1)*sum(g2 .* x(ti-tau-points,1) .* conj(x(ti+tau-points,xcol))));
 end;
end;

clear CWKer;

if trace, fprintf('\n'); end;
tfr= fft(tfr);
if (xcol==1), tfr=real(tfr); end ;

if (nargout==0),
 tfrqview(tfr,x,t,'tfrcw',g,h,sigma);
elseif (nargout==3),
 f=(0.5*(0:N-1)/N)';
end;


function cut = disjunctivecut(varargin)
%
%
% x = sdpvar(2,1);
% A1 = randn(8,2);
% b1 = rand(8,1)*2-A1*[3;3];
% A2 = randn(8,2);
% b2 = rand(8,1)*2-A2*[-3;3];
% A3 = randn(8,2);
% b3 = rand(8,1)*2-A3*[3;-3];
% C1 = A1*x < b1;
% C2 = A2*x < b2;
% C3 = A3*x < b3;
% cut = disjunctivecut(C1,C2,C3,[-8;0]);cut = disjunctivecut(C1,C2,C3,[-8;0]);
% plot([cut,-10<x<10]);plot(hull(C1,C2,C3),x);plot(C1,[],'y');plot(C2,[],'y');plot(C3,[],'y')
xstar = varargin{end};
x = [];
beta = sdpvar(1);
alpha = sdpvar(length(xstar),1);
for i = 1:nargin-1
    [Imodel,Iax1,Iax2,p{i}] = export(varargin{i},[],sdpsettings,[],[],0);
    neq(i) = p{i}.K.f;
    b{i} = -p{i}.F_struc(:,1);
    A{i} = p{i}.F_struc(:,2:end);
    mu{i} = sdpvar(length(b{i}),1);
end

Objective  = alpha'*xstar-beta;
Constraints = [-1<alpha<1];

summu = 0;
for i = 1:nargin-1
    summu = summu + sum(mu{i});
    Constraints = [Constraints,alpha' == mu{i}'*A{i}];
    Constraints = [Constraints,beta  <= mu{i}'*b{i}];
    Constraints = [Constraints,mu{i}(p{i}.K.f+1:end)>0];
end

solvesdp(Constraints,Objective,sdpsettings('verbose',0));

x = recover(p{1}.used_variables);
cut = (-double(beta)+double(alpha)'*x >= 0);

return

%x = recover(sdpvar(C1));

%[Imodel,Iax1,Iax2,p1] = export(C1,[],sdpsettings,[],[],0);
%[Omodel,Oax1,Oax2,p2] = export(C2,[],sdpsettings,[],[],0);

%neq1 = p1.K.f;
%neq2 = p2.K.f;

%e1 = p1.F_struc*[1;x];
%e2 = p2.F_struc*[1;x];
%Model1 = [e2(1+p1.K.f:end)>=0];
%Model2 = [e2(1+p2.K.f:end)>=0];
if 0
Ab1 = getbase(sdpvar(Model1));
Ab2 = getbase(sdpvar(Model2));
b1 = -Ab1(:,1);
A1 =  Ab1(:,2:end);
b2 = -Ab2(:,1);
A2 = Ab2(:,2:end);

b1 = -p1.F_struc(:,1);
A1 = p1.F_struc(:,2:end);
b2 = -p2.F_struc(:,1);
A2 = p2.F_struc(:,2:end);
end

alpha = sdpvar(length(xstar),1);
beta = sdpvar(1);
mu1 = sdpvar(length(b1),1);
mu2 = sdpvar(length(b2),1);

Objective  = alpha'*xstar-beta;
Constraint = [alpha' == mu1'*A1,alpha' == mu2'*A2, beta <= mu1'*b1, beta <= mu2'*b2,mu1(neq1+1:end)>0,mu2(neq2+1:end)>0];
Constraint = [Constraint,-1<alpha<1,sum(mu1)+sum(mu2)<10];

solvesdp(Constraint,Objective,sdpsettings('verbose',0));

cut = (-double(beta)+double(alpha)'*x >= 0);


% TENSORLAB
% Version 2.01, 2014-02-05
%
% BLOCK TERM DECOMPOSITION
% Algorithms
%    btd_minf     - BTD by unconstrained nonlinear optimization.
%    btd_nls      - BTD by nonlinear least squares.
% Initialization
%    btd_rnd      - Pseudorandom initialization for BTD.
% Utilities
%    btdgen       - Generate full tensor given a BTD.
%    btdres       - Residual of a BTD.
%
% CANONICAL POLYADIC DECOMPOSITION
% Algorithms
%    cpd          - Canonical polyadic decomposition.
%    cpd_als      - CPD by alternating least squares.
%    cpd_minf     - CPD by unconstrained nonlinear optimization.
%    cpd_nls      - CPD by nonlinear least squares.
%    cpd3_sd      - CPD by simultaneous diagonalization.
%    cpd3_sgsd    - CPD by simultaneous generalized Schur decomposition.
% Initialization
%    cpd_gevd     - CPD by a generalized eigenvalue decomposition.
%    cpd_rnd      - Pseudorandom initialization for CPD.
% Line and plane search
%    cpd_aels     - CPD approximate enhanced line search.
%    cpd_els      - CPD exact line search.
%    cpd_eps      - CPD exact plane search.
%    cpd_lsb      - CPD line search by Bro.
% Utilities
%    cpderr       - Errors between factor matrices in a CPD.
%    cpdgen       - Generate full tensor given a polyadic decomposition.
%    cpdres       - Residual of a polyadic decomposition.
%    rankest      - Estimate rank.
%
% COMPLEX OPTIMIZATION
% Nonlinear least squares
%    nls_gncgs    - Nonlinear least squares by Gauss-Newton with CG-Steihaug.
%    nls_gndl     - Nonlinear least squares by Gauss-Newton with dogleg trust region.
%    nls_lm       - Nonlinear least squares by Levenberg-Marquardt.
%    nlsb_gndl    - Bound-constrained NLS by projected Gauss-Newton dogleg TR.
% Unconstrained nonlinear optimization
%    minf_lbfgs   - Minimize a function by L-BFGS with line search.
%    minf_lbfgsdl - Minimize a function by L-BFGS with dogleg trust region.
%    minf_ncg     - Minimize a function by nonlinear conjugate gradient.
% Utilities
%    deriv        - Approximate gradient and Jacobian.
%    ls_mt        - Strong Wolfe line search by More-Thuente.
%    mpcg         - Modified preconditioned conjugate gradients method.
%
% LOW MULTILINEAR RANK APPROXIMATION
% Algorithms
%    lmlra        - Low multilinear rank approximation.
%    lmlra_hooi   - LMLRA by higher-order orthogonal iteration.
%    lmlra_minf   - LMLRA by unconstrained nonlinear optimization.
%    lmlra_nls    - LMLRA by nonlinear least squares.
%    lmlra3_dgn   - LMLRA by a differential-geometric Newton method.
%    lmlra3_rtr   - LMLRA by a Riemannian trust region method.
%    mlsvd        - (Truncated) multilinear singular value decomposition.
% Initialization
%    lmlra_aca    - LMLRA by adaptive cross-approximation.
%    lmlra_rnd    - Pseudorandom initialization for LMLRA.
% Utilities
%    lmlraerr     - Errors between factor matrices in a LMLRA.
%    lmlragen     - Generate full tensor given a core tensor and factor matrices.
%    lmlrares     - Residual of a LMLRA.
%    mlrank       - Multilinear rank.
%    mlrankest    - Estimate multilinear rank.
%
% STRUCTURED DATA FUSION
% Algorithms
%   sdf_minf      - Structured data fusion by unconstrained nonlinear optimization.
%   sdf_nls       - Structured data fusion by nonlinear least squares.
% Structure
%   struct_abs        - Absolute value.
%   struct_band       - Band matrix.
%   struct_cell2mat   - Convert the contents of a cell array into a matrix.
%   struct_conj       - Complex conjugate.
%   struct_ctranspose - Complex conjugate transpose.
%   struct_diag       - Diagonal matrix.
%   struct_gram       - Gramian matrix.
%   struct_hankel     - Hankel matrix.
%   struct_inv        - Matrix inverse.
%   struct_invsqrtm   - Matrix inverse square root.
%   struct_invtransp  - Matrix inverse transpose.
%   struct_LL1        - Structure of third factor matrix in a rank-(Lr,Lr,1) BTD.
%   struct_log        - Natural logarithm.
%   struct_matvec     - Matrix-vector and matrix-matrix product.
%   struct_nonneg     - Nonnegative array.
%   struct_normalize  - Normalize columns to unit norm.
%   struct_orth       - Rectangular matrix with orthonormal columns.
%   struct_plus       - Plus.
%   struct_poly       - Matrix with columns as polynomials.
%   struct_power      - Array power.
%   struct_rational   - Matrix with columns as rational functions.
%   struct_rbf        - Matrix with columns as sums of Gaussian RBF kernels.
%   struct_sigmoid    - Constrain array elements to an interval.
%   struct_sqrt       - Square root.
%   struct_sum        - Sum of elements.
%   struct_times      - Times.
%   struct_toeplitz   - Toeplitz matrix.
%   struct_transpose  - Transpose.
%   struct_tridiag    - Tridiagonal matrix.
%   struct_tril       - Lower triangular matrix.
%   struct_triu       - Upper triangular matrix.
%   struct_vander     - Vandermonde matrix.
%
% UTILITIES
% Clustering
%    gap          - Optimal clustering based on the gap statistic.
%    kmeans       - Cluster multivariate data using the k-means++ algorithm.
% Polynomials
%    polymin      - Minimize a polynomial.
%    polymin2     - Minimize bivariate and real polyanalytic polynomials.
%    polyval2     - Evaluate bivariate and univariate polyanalytic polynomials.
%    polysol2     - Solve a system of two bivariate polynomials.
%    ratmin       - Minimize a rational function.
%    ratmin2      - Minimize bivariate and real polyanalytic rational functions.
% Statistics
%    cum4         - Fourth-order cumulant tensor.
%    scov         - Shifted covariance matrices.
% Tensors
%    dotk         - Dot product in K-fold precision.
%    fmt          - Format data set.
%    frob         - Frobenius norm.
%    ful          - Convert formatted data set to an array.
%    kr           - Khatri-Rao product.
%    kron         - Kronecker product.
%    mat2tens     - Tensorize a matrix.
%    mtkronprod   - Compute a matricized tensor Kronecker product.
%    mtkrprod     - Compute a matricized tensor Khatri-Rao product.
%    noisy        - Generate a noisy version of a given array.
%    sumk         - Summation in K-fold precision.
%    tens2mat     - Matricize a tensor.
%    tens2vec     - Vectorize a tensor.
%    tmprod       - Mode-n tensor-matrix product.
%    vec2tens     - Tensorize a vector.
% Visualization
%    slice3       - Visualize a third-order tensor with slices.
%    spy3         - Visualize a third-order tensor's sparsity pattern.
%    surf3        - Visualize a third-order tensor with surfaces.
%    voxel3       - Visualize a third-order tensor with voxels.


function R = QuatToRot(q)
%QuatToRot Converts a Quaternion to Rotation matrix
%   written by Daniel Mellinger

% normalize q
q = q./sqrt(sum(q.^2));

qahat(1,2) = -q(4);
qahat(1,3) = q(3);
qahat(2,3) = -q(2);
qahat(2,1) = q(4);
qahat(3,1) = -q(3);
qahat(3,2) = q(2);

R = eye(3) + 2*qahat*qahat + 2*q(1)*qahat;

end


%% calculation of RMS error
%After putting breakpoint in DPMpcOneBeam_aavc line 284, run the following code:
[xGrid,yGrid,weightMap] = plotIntensityMap(whichBeam,planC);
wtAtPB = [];
for i=1:length(xPosV)
    wtAtPB(i) = interp2(xGrid,yGrid,weightMap,xPosV(i)/10,yPosV(i)/10,'nearest',0);
end
MUweight = sum(planC{indexS.IM}(end).IMDosimetry.LeafSeq.MU{whichBeam});
disp('RMS ERROR:')
disp(sqrt(sum((wtAtPB-w_field*MUweight).^2)/length(w_field))/max(w_field*MUweight))

disp(sqrt(sum((wtAtPB-w_field*MUweight).^2)/length(w_field))/max(w_field*MUweight))

indKeep = find(wtAtPB>max(wtAtPB)*0.1);
x = wtAtPB(indKeep);
y = w_field(indKeep)*MUweight;
sqrt(1/length(x)*sum(((x-y)./x).^2))

%% Calculate RMS error for dose
baseIndex = 5;
newIndex = 6;
indKeep = find(planC{indexS.dose}(baseIndex).doseArray>max(planC{indexS.dose}(baseIndex).doseArray(:))*0.5);
x = planC{indexS.dose}(baseIndex).doseArray(indKeep);
y = planC{indexS.dose}(newIndex).doseArray(indKeep);
sqrt(1/length(x)*sum(((x-y)./x).^2))

%% Scale beamletWeights
% for i=1:length(planC{indexS.IM}(end).IMDosimetry.beams)
%     for j=1:size(planC{indexS.IM}(end).IMDosimetry.beams(i).beamlets,1)
%         for k=1:size(planC{indexS.IM}(end).IMDosimetry.beams(i).beamlets,2)
%             planC{indexS.IM}(end).IMDosimetry.beams(i).beamlets(j,k).maxInfluenceVal = planC{indexS.IM}(end).IMDosimetry.beams(i).beamlets(j,k).maxInfluenceVal * 11.2/23.668;
%         end
%     end
% end


function x = mrdivide (b,F,is_inverse)
%MRDIVIDE x = b/A using the factorization F = factorize(A)
%
% Example
%   F = factorize(A) ;
%   x = b/F ;               % same as x=b/A
%
% See also factorize.

% Copyright 2009, Timothy A. Davis, University of Florida

if (nargin < 3)
    is_inverse = F.is_inverse ;
end

if (is_inverse)

    % x=b/inverse(A) is a double inverse, so it becomes simply x=b*A
    x = b*F.A ;

else

    bT = b' ;
    kind = F.kind ;
    switch kind

        case 1

            % minimum 2-norm solution of a sparse underdetermined problem
            % Q-less econonmy sparse QR factorization: (A*q)'*(A*q) = R'*R
            A = F.A ;
            R = F.R ;
            q = F.q ;
            x = A * (q * (R \ (R' \ (q' * bT)))) ;
            e = A * (q * (R \ (R' \ (q' * (bT - A' * x))))) ;
            x = (x + e)' ;

        case 2

            % minimum 2-norm solution of a dense underdetermined problem
            % dense economy QR factorization: A = Q*R
            % x = (Q * (R' \ b'))' ;
            Q = F.Q ;
            R = F.R ;
            if (issparse (bT))
                bT = full (bT) ;
            end
            opUT.UT = true ;
            opUT.TRANSA = true ;
            x = (Q * linsolve (R, bT, opUT))' ;

        case 3

            % least-squares solution of a sparse overdetermined problem
            % Q-less economy sparse QR factorization: (p*A)*(p*A)' = R'*R
            A = F.A ;
            R = F.R ;
            p = F.p ;
            x = p' * (R \ (R' \ (p * (A * bT)))) ;
            e = p' * (R \ (R' \ (p * (A * (bT - A' * x))))) ;
            x = (x + e)' ;

        case 4

            % least-squares solution of a dense overdetermined problem
            % dense economy QR factorization: A' = Q*R
            % x = (R \ (Q' * b'))'
            Q = F.Q ;
            R = F.R ;
            if (issparse (bT))
                bT = full (bT) ;
            end
            opU.UT = true ;
            x = linsolve (R, Q' * bT, opU)' ;

        case 5

            % sparse Cholesky factorization: q*A*q' = L*L'
            L = F.L ;
            q = F.q ;
            x = (q * (L' \ (L \ (q' * bT))))' ;

        case 6

            % dense Cholesky factorization: A = R'*R
            % x = (R \ (R' \ b'))'
            R = F.R ;
            if (issparse (bT))
                bT = full (bT) ;
            end
            opU.UT = true ;
            opUT.UT = true ;
            opUT.TRANSA = true ;
            x = linsolve (R, linsolve (R, bT, opUT), opU)' ;

        case 7

            % sparse LU factorization: p*A*q = L*U
            L = F.L ;
            U = F.U ;
            p = F.p ;
            q = F.q ;
            x = (p' * (L' \ (U' \ (q' * bT))))' ;

        case 8

            % dense LU factorization: p*A = L*U
            % x = (P' * (L' \ (U' \ b')))'
            L = F.L ;
            U = F.U ;
            p = F.p ;
            if (issparse (bT))
                bT = full (bT) ;
            end
            opUT.UT = true ;
            opUT.TRANSA = true ;
            opLT.LT = true ;
            opLT.TRANSA = true ;
            x = (p' * linsolve (L, linsolve (U, bT, opUT), opLT))' ;

    end
end


function rgb = CIELab_to_sRGB(Lab)
% Convert a matrix of CIELAB L* a* b* values to sRGB R G B values.
%
% (c) 2018-2020 Stephen Cobeldick
%
%%% Syntax:
% rgb = CIELab_to_sRGB(Lab)
%
% https://en.wikipedia.org/wiki/Lab_color_space
% https://en.wikipedia.org/wiki/SRGB
%
%% Examples %%
%
% >> CIELab_to_sRGB([68.18,2.14,-43.8])*255
% ans =
%   109.0000  169.0000  245.0000
%
% >> CIELab_to_sRGB([0,0,0;100,0,0])
% ans =
%          0         0         0
%     1.0000    1.0000    1.0000
%
%% Inputs and Outputs
%
%%% Input Argument:
% Lab = Numeric Array, size Nx3 or RxCx3, where the last dimension
%       encodes CIELAB values [L*,a*,b*] in the range 0<=L*<=100.
%
%%% Output Argument:
% rgb = Numeric Array, same size as <Lab>, where the last dimension
%       encodes sRGB values [R,G,B] in the range 0<=RGB<=1.
%
% See also CIELAB_TO_DIN99 SRGB_TO_CIELAB SRGB_TO_CAM02UCS SRGB_TO_OSAUCS
% MAXDISTCOLOR MAXDISTCOLOR_VIEW MAXDISTCOLOR_DEMO

%% Input Wrangling %%
%
isz = size(Lab);
assert(isnumeric(Lab),...
	'SC:CIELab_to_sRGB:Lab:NotNumeric',...
	'1st input <Lab> must be numeric.')
assert(isreal(Lab),...
	'SC:CIELab_to_sRGB:Lab:ComplexValue',...
	'1st input <Lab> cannot be complex.')
assert(isz(end)==3,...
	'SC:CIELab_to_sRGB:Lab:InvalidSize',...
	'1st input <Lab> last dimension must have size 3 (e.g. Nx3 or RxCx3).')
Lab = reshape(Lab,[],3);
assert(all(Lab(:,1)>=0&Lab(:,1)<=100),...
	'SC:CIELab_to_sRGB:Lab:OutOfRange',...
	'1st input <Lab> L values must be within the range 0<=L<=100')
%
if ~isfloat(Lab)
	Lab = double(Lab);
end
%
%% Lab2RGB %%
%
M = [... High-precision sRGB to XYZ matrix:
	0.4124564,0.3575761,0.1804375;...
	0.2126729,0.7151522,0.0721750;...
	0.0193339,0.1191920,0.9503041];
% Source: http://brucelindbloom.com/index.html?Eqn_RGB_XYZ_Matrix.html
%
wpt = [0.95047,1,1.08883]; % D65
%
% Approximately equivalent to this function, requires Image Toolbox:
%rgb = applycform(lab,makecform('lab2srgb','AdaptedWhitePoint',wpt))
%
% Lab2XYZ
tmp = bsxfun(@rdivide,Lab(:,[2,1,3]),[500,Inf,-200]);
tmp = bsxfun(@plus,tmp,(Lab(:,1)+16)/116);
idx = tmp>(6/29);
tmp = idx.*(tmp.^3) + ~idx.*(3*(6/29)^2*(tmp-4/29));
XYZ = bsxfun(@times,tmp,wpt);
%
% XYZ2RGB
rgb = max(0,min(1,sGammaCor(XYZ / M.')));
%
rgb = reshape(rgb,isz);
%
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Lab_to_sRGB
function rgb = sGammaCor(rgb)
% Gamma correction of sRGB data.
idx = rgb <= 0.0031308;
rgb(idx) = 12.92 * rgb(idx);
rgb(~idx) = real(1.055 * rgb(~idx).^(1/2.4) - 0.055);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%sGammaCor


function [x,P,K] = KalmanFilterX_UpdatePDA(xPred,PPred,Y,W,yPred,S,Pxy)
% KALMANFILTERX_UPDATEPDA Perform the discrete-time Probabilistic Data
% Association (PDA) KF update step, under the assumption of additive process
% noise, for multiple measurements (as a Gaussian Mixture)
%
% Parameters
% ----------
% xPred: column vector
%   The (xDim x 1) predicted state estimate.
% PPred: matrix
%   The (xDim x xDim) predicted state covariance matrix.
% Y: matrix
%   The (yDim x nY) measurement vector.
% W: row vector
%   The (1 x nY+1) measurement association/mixture weights
%   vector. (dummy measurement assumed at index 1)
% yPred: column vector
%   The (yDim x 1) predicted measurement estimate.
% S: matrix
%   The (yDim x yDim) innovation covariance matrix.
% Pxy: matrix
%   The (xDim x yDim) cross-covariance matrix.
%
% Returns
% -------
% x: column vector
%   The (xDim x 1) state estimate at the current time-step.
% P: matrix
%   The (xDim x xDim) state covariance matrix at the current
%   time-step.
% K: matrix
%   The (xDim x yDim) Kalman gain matrix at the current
%   time-step.
%
%October 2017 Lyudmil Vladimirov, University of Liverpool.

    % Get size of observation vector
    [yDim,nY] = size(Y);

    % Compute Kalman gain
    K = Pxy/S;

    % Compute innovation mean and (cross) covariance
    innov_err       = Y - yPred(:,ones(1,nY));
    tot_innov_err   = innov_err*W(2:end)';
    Pc              = PPred - K*S*K';
    Pgag            = K*((innov_err.*W(ones(yDim,1),2:end))*innov_err' - tot_innov_err*tot_innov_err')*K';

    % Compute filtered estimates
    x    = xPred + K*tot_innov_err;
    P    = W(1)*PPred + (1-W(1))*Pc + Pgag;
end


function [output_signal,output_time] = mvstat_final(Inputdata_signal,Inputdata_time,windowrange,reductionrange,fn)
%------------ Introduction ----------------------
%This file is created to calculate the mathematical operation like 'max'
%on a pre-defined window range and with a pre-defined reduction range of an vector data.
%This function will need the following inputs in order to calculate the
%wished results:
%   1- Inputdata_signal: A vector containg your Y-Values that need to be
%   filtered.
%   2- Inputdata_time: A vector containg your x-Values that need to be
%   filtered (Normally is a time signal where the sampling rate can be
%   extracted).
%   3- windowrange: in unit of time (s) is the range to calculate the needed
%   mathematical function (The moving range ex. mvmean(y,x,0.02,0.02) or mvrms(y,x,0.02,0.02))
%   4- reductionrange: in unit of time (s) is the scalling reference for
%   the new data
%   5- fn : is used to define the wished mathmatical operation (ex. @max, @min, @mean, @rms, @int)
%The relation between the windowrange input and the reductionrange input
%must be an integer value of 1:1.2:1..10:1. If not the reductionrange would
%be corrected to get one of those relation. this specification give us the
%possibility to calculate the 'mvstat_final' without using a for-Loop which will save a whole amount of time
% for example:
% fn = @mean
% [output_signal,out_put_time] = mvstat_final (y,x,10,10)
% the moving average will be calculated as a result of this function. every
% 10 s the mean value for the last 10 s will be calculated.
% fn = @max
% [output_signal,out_put_time] = mvstat_final (y,x,20,10)
% the moving max will be calculated as a result of this function. every
% 10 s the maximum value for the last 20 s will be calculated.
% fn = @min
% [output_signal,out_put_time] = mvstat_final (y,x,5,10)
% the moving min will be calculated as a result of this function. every
% 10 s the minmum value for the last 5 s will be calculated.
%Notice: when the input values of windowrange and reductionrange are not
%the same then at the beginning of the calculation the fn value of the a
%available data will be calculated.
% Is m.file will also allows you to calculate the intgration of your input
% data in a spesific windowrange. in this case the reductionrange input
% should be defined as in the example:
%fn = @int;
%[Ua_sin,Ua_sin_time] = mvstat(Ua_sin,U1_time,(1/f_grid),delta_t,fn); Ua_sin = Ua_sin*2*f_grid;
% in this example the integration of your input data (in this case U1_sin)
% will be calculated ever one period (normally this mean 0.02 s or 1/50 or
% 1/f_grid in our case). and then the reductionrange is in this case equal
% to the sampling time of the inputdata (delta = U1_time(2)-U1_time(1)).
% this work was done in order to bring FAMOS (IMC) software function into
% matlab.
%Auther: Msc.Eng. Aubai Alkhatib
%Datum: 10.09.2013
%-------- End of introduction ------------------
delta = Inputdata_time(2)-Inputdata_time(1);
length_window = windowrange/delta;
length_reduction = reductionrange/delta;
time = Inputdata_time(end);
if windowrange > reductionrange
    number = windowrange/reductionrange;
    integer = floor(number);
    fract = number-integer;
    if fract >= 0.5
        if 0 == 1

        else
            A = windowrange/(integer+1);
        end
    else
        A = windowrange/(integer);
    end
    reductionrange_new = A;
     integer_A = floor(A);
    fract_A = A-integer_A;
    if 0 == 1
        if windowrange/(reductionrange+1) < windowrange/reductionrange
            reductionrange_new = reductionrange;
            while rem(windowrange,reductionrange_new) > 0
                reductionrange_new = reductionrange_new - 1;
            end
        else
            reductionrange_new = reductionrange;
            while rem(windowrange,reductionrange_new) == 0
                reductionrange_new = reductionrange_new + 1;
            end
        end
    end
    [~,b] = rat(A);
    if b > 1
        length_reduction_new = round(reductionrange_new/delta);
    else
        length_reduction_new = reductionrange_new/delta;
    end
    i = length_reduction_new;
    ii = 1;
    output = zeros();
    while i < length(Inputdata_signal)
        if i >= length_window
            if fract_A == 0
                switch func2str(fn)
                    case 'mean'
                        output(ii) = mean(Inputdata_signal((i-length_window)+1:i));
                    case 'max'
                        output(ii) = max(Inputdata_signal((i-length_window)+1:i));
                    case 'sum'
                        output(ii) = sum(Inputdata_signal((i-length_window)+1:i));
                    case 'min'
                        output(ii) = min(Inputdata_signal((i-length_window)+1:i));
                    case 'int'
                        output(ii) = sum(Inputdata_signal((i-length_window)+1:i))*reductionrange_new;
                    otherwise
                        output(ii) = rms(Inputdata_signal((i-length_window)+1:i));
                end
            else
                switch func2str(fn)
                    case 'mean'
                        output(ii) = mean(Inputdata_signal((i-length_window):i));
                    case 'max'
                        output(ii) = max(Inputdata_signal((i-length_window):i));
                    case 'sum'
                        output(ii) = sum(Inputdata_signal((i-length_window):i));
                    case 'min'
                        output(ii) = min(Inputdata_signal((i-length_window):i));
                    case 'int'
                        output(ii) = sum(Inputdata_signal((i-length_window)+1:i))*reductionrange_new;
                    otherwise
                        output(ii) = rms(Inputdata_signal((i-length_window):i));
                end
            end
        else
            switch func2str(fn)
                case 'mean'
                    output(ii) = mean(Inputdata_signal(1:i));
                case 'max'
                    output(ii) = max(Inputdata_signal(1:i));
                case 'sum'
                    output(ii) = sum(Inputdata_signal(1:i));
                case 'int'
                    output(ii) = sum(Inputdata_signal(1:i))*reductionrange_new;
                case 'min'
                    output(ii) = min(Inputdata_signal(1:i));
                otherwise
                    output(ii) = rms(Inputdata_signal(1:i));
            end
        end
        i = i + length_reduction_new;
        ii = ii + 1;
    end

    if strcmp(func2str(fn),'int')
        output_signal = output(length_window:length(output));
        output_time = windowrange:reductionrange_new:(length(output)*reductionrange_new);
    elseif isinteger(reductionrange_new)
        output_signal = output;
        output_time = 0:reductionrange_new:(length(output)*reductionrange_new)-reductionrange_new;
    else
        output_signal = output;
        output_time = 0:(length_reduction_new*delta):(length(output)*(length_reduction_new*delta))-(length_reduction_new*delta);
    end
elseif reductionrange > windowrange
    Length_Output = fix(time/windowrange);
    Length_Input_New = Length_Output*windowrange/delta;
    delta_new = length_window;
    delta_time = windowrange;
    Input_reshaped = reshape(Inputdata_signal(1:Length_Input_New),delta_new,Length_Input_New/delta_new);
    switch func2str(fn)
        case 'mean'
            output_signal = mean(Input_reshaped);
        case 'max'
            output_signal = max(Input_reshaped);
        case 'sum'
            output_signal = sum(Input_reshaped);
        case 'int'
            output = sum(Input_reshaped)*delta_time;
        case 'min'
            output_signal = min(Input_reshaped);
        otherwise
            output_signal = rms(Input_reshaped);
    end
    if strcmp(func2str(fn),'int')
        output_signal = output(length_window:length(output));
        output_time = windowrange:delta_time:(Length_Output*delta_time)-delta_time;
    else
        output_time = 0:delta_time:(Length_Output*delta_time)-delta_time;
    end
elseif windowrange == reductionrange
    Length_Output = fix(time/windowrange);
    Length_Input_New = Length_Output*reductionrange/delta;
    delta_new = length_reduction;
    delta_time = windowrange;
    Input_reshaped = reshape(Inputdata_signal(1:Length_Input_New),delta_new,Length_Input_New/delta_new);
    switch func2str(fn)
        case 'mean'
            output_signal = mean(Input_reshaped);
        case 'max'
            output_signal = max(Input_reshaped);
        case 'sum'
            output_signal = sum(Input_reshaped);
        case 'int'
            output = sum(Input_reshaped)*delta_time;
        case 'min'
            output_signal = min(Input_reshaped);
        otherwise
            output_signal = rms(Input_reshaped);
    end
    if strcmp(func2str(fn),'int')
        output_signal = output(length_window:length(output));
        output_time = windowrange:delta_time:(Length_Output*delta_time)-delta_time;
    else
        output_time = 0:delta_time:(Length_Output*delta_time)-delta_time;
    end
end
end


function spm_MDP_trust
% Demo of active inference for trust games
%__________________________________________________________________________
%
% This routine uses the Markov decision process formulation of active
% inference (with variational Bayes) to model a simple trust game. In trust
% games, one plays an opponent who can either cooperate or defect. The
% payoff contingencies depend upon the joint choices of you and your
% opponent, which in turn depend upon your inferences about the nature of
% the opponent (pro-social or non-social). This example illustrates single
% round games with a special focus on Bayesian belief updating between
% games. This is illustrated in terms of evidence accumulation about
% the nature of the opponent by using the posterior marginal distributions
% following one game as the prior distribution over beliefs about the
% opponent in the next. This accumulation is shown in the final figures.
%
% In this example, there are nine states. The first is a starting state
% and the subsequent eight states model the four combinations of
% cooperation and defection (between you and your opponent) under the
% prior beliefs that the opponent is either pro-social or non-social.
% Initially, these prior beliefs are uninformative but are subsequently
% informed through experience. prior beliefs about behaviour are based on
% relative entropy or KL divergence in the usual way - which requires the
% specification of utility functions over states based upon standard payoff
% tables in these sorts of games. It is interesting to see how precision
% or confidence in beliefs about choices, fluctuates with beliefs about
% the nature of one's opponent.
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_MDP_trust.m 6044 2014-06-14 10:22:46Z karl $

% set up and preliminaries
%==========================================================================
rng('default')
%
% Payoffs (reward):
% _________________________________________________________________________
%                                 Trustee
% _________________________________________________________________________
% Investor              Cooperate (fT high) Defect (fT low)
% _________________________________________________________________________
% Cooperate            A (e.g.=26)         C (e.g.= 10)
% (fI high)            a (e.g.=26)         b (e.g.= 42)
%
% Defect               B (e.g.=21)         D (e.g.= 18)
% (fI low)             c (e.g.= 7)         d (e.g.= 10)
% _________________________________________________________________________

U{1} = [26 10;
        21 18]/8;                   % self payoff (utility)

U{2} = [26 42;
         7 10]/8;                   % other payoff (utility)

% initial state - encoding the actual type of trustee
%--------------------------------------------------------------------------
S    = [0 1]';                      % indicator - [prosocial nonsocial]
S    = kron(S,[1 0 0 0 0]');

% prior beliefs about initial state
%--------------------------------------------------------------------------
k    = [1 1];
p    = spm_softmax(k(:));
k    = log(p);
D    = kron(p,[1 0 0 0 0]');

% investor's payoffs or prior beliefs (softmax(utility))
%--------------------------------------------------------------------------
a    = 1/2;
pp   = [0; spm_softmax(spm_vec((1 - a)*U{1} + a*U{2}))];
pn   = [0; spm_softmax(spm_vec(        U{1}         ))];

C    = [pp*p(1); pn*p(2)];

% investor's belief (based on a prosocial and nonsocial trustee)
%--------------------------------------------------------------------------
cp   = spm_softmax(((1 - a)*U{2}(1,:) + a*U{1}(1,:))');
dp   = spm_softmax(((1 - a)*U{2}(2,:) + a*U{1}(2,:))');
cn   = spm_softmax((        U{2}(1,:)              )');
dn   = spm_softmax((        U{2}(2,:)              )');

% transition probabilities (B{1} - (c)ooperate; B{2} - (d)efect)
%--------------------------------------------------------------------------
B{1} = ...                        % cooperate:
   [0     0 0 0 0  0 0 0 0 0;     % start - prosocial trustee
    cp(1) 1 0 0 0  0 0 0 0 0;     % cc - prosocial - state 2
    0     0 1 0 0  0 0 0 0 0;     % dc - prosocial - state 3
    cp(2) 0 0 1 0  0 0 0 0 0;     % cd - prosocial - state 4
    0     0 0 0 1  0 0 0 0 0;     % dd - prosocial - state 5the

    0 0 0 0 0  0     0 0 0 0;     % cc - nonsocial
    0 0 0 0 0  cn(1) 1 0 0 0;     % cc - nonsocial
    0 0 0 0 0  0     0 1 0 0;     % dc - nonsocial
    0 0 0 0 0  cn(2) 0 0 1 0;     % cd - nonsocial
    0 0 0 0 0  0     0 0 0 1];    % dd - nonsocial

B{2} = ...                        % defect:
   [0     0 0 0 0  0 0 0 0 0;     % start - nonsocial trustee
    0     1 0 0 0  0 0 0 0 0;     % ...
    dp(1) 0 1 0 0  0 0 0 0 0;
    0     0 0 1 0  0 0 0 0 0;
    dp(2) 0 0 0 1  0 0 0 0 0;

    0 0 0 0 0  0     0 0 0 0;
    0 0 0 0 0  0     1 0 0 0;
    0 0 0 0 0  dn(1) 0 1 0 0;
    0 0 0 0 0  0     0 0 1 0;
    0 0 0 0 0  dn(2) 0 0 0 1];

% observation probabilities
%--------------------------------------------------------------------------
A    = kron([1 1],speye(5,5));

% allowable policies - (of depth T); here, simply defect will cooperate
%--------------------------------------------------------------------------
V    = [1 2;
        1 1];

% MDP Structure
%==========================================================================
MDP.N = 8;                          % number of variational iterations
MDP.T = 2;                          % process depth (one-shot game)
MDP.S = S;                          % true initial state
MDP.A = A;                          % observation model
MDP.B = B;                          % transition probabilities (priors)
MDP.C = C;                          % terminal cost probabilities (priors)
MDP.D = D;                          % initial state probabilities (priors)
MDP.V = V;                          % allowable policies

MDP.alpha = 2;                      % gamma hyperparameters
MDP.beta  = 1/2;

% Solve - an example game
%==========================================================================
spm_figure('GetWin','Figure 1'); clf
MDP.plot = gcf;
MDP      = spm_MDP_game(MDP);

% now iterate repeated games accumulating posterior beliefs
%==========================================================================
MDP.plot = 0;
NG       = 64;

for i = 1:NG

    % solve and marginalise over posterior beliefs about hidden states
    %----------------------------------------------------------------------
    MDP    = spm_MDP_game(MDP);
    Q(:,i) = spm_softmax(k);
    O(:,i) = MDP.O(:,end);
    W(:,i) = MDP.W(:,end);
    P(:,i) = MDP.P(:,1);

    % update prior beliefs about initial state (pro-social or
    % non-social) and associated utility functions
    %----------------------------------------------------------------------
    a      = find(MDP.U(:,1));
    p      = MDP.O(:,2)'*MDP.A*MDP.B{a}(:,[1 6]);
    p      = p(:)/sum(p);
    k      = k + log(p);
    p      = spm_softmax(k);
    MDP.D  = kron(p,[1 0 0 0 0]');
    MDP.C  = [pp*p(1); pn*p(2)];

end

% graphics
%==========================================================================
spm_figure('GetWin','Figure 2'); clf

% posterior beliefs about hidden states (prosocial versus nonsocial)
%--------------------------------------------------------------------------
subplot(2,2,1)
plot(1:NG,Q)
title('Beliefs about other','FontSize',16)
xlabel('Number of games','FontSize',12)
ylabel('True and posterior expectations','FontSize',12)
spm_axis tight, axis square
legend({'prosocial','nonsocial'})

subplot(2,2,2)
plot(1:NG,P)
title('Beliefs about control','FontSize',16)
xlabel('Number of games','FontSize',12)
ylabel('True and posterior expectations','FontSize',12)
spm_axis tight, axis square
legend({'cooperate','defect'})

subplot(2,2,3)
imagesc(O)
title('Outcomes','FontSize',16)
xlabel('Number of games','FontSize',12)
ylabel('Observed outcome','FontSize',12)
axis square

subplot(2,2,4)
plot(W)
title('Precision','FontSize',16)
xlabel('Number of games','FontSize',12)
ylabel('Expected precision','FontSize',12)
axis square


function sparse_test06 ( )

%*****************************************************************************80
%
%% SPARSE_TEST06 times the zeroing out operation for full and sparse matrices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Nicholas Higham,
%    Algorithm 694: A Collection of Test Matrices in MATLAB,
%    ACM Transactions on Mathematical Software,
%    Volume 17, Number 3, September 1991, pages 289-305.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_TEST06:\n' );
  fprintf ( 1, '  Zero out a portion of a matrix.\n' );
  fprintf ( 1, '  Compare the time required when using full or sparse storage.\n' );
  fprintf ( 1, '  The sparse matrix takes longer to modify, and it takes longer\n' );
  fprintf ( 1, '  when there are more elements to remove.\n' );

  n = 2000;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix is of size %d by %d\n', n, n );
  fprintf ( 1, '  Initial number of nonzeros is %d\n', n * n );

  for amax = [ 1, n * n / 2, n * n - 1 ]

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Zero out all entries greater than %d\n', amax );

    A = magic ( n );
    tic
    A(A>amax) = 0.0;
    t = toc;
    fprintf ( 1, '  Full storage matrix required %g seconds\n', t );

    A = magic ( n );
    A = sparse ( A );
    tic
    A(A>amax) = 0.0;
    t = toc;
    fprintf ( 1, '  Sparse storage matrix required %g seconds\n', t );
    fprintf ( 1, '  Number of nonzeros is %d\n', nnz ( A ) );

  end

  return
end


function [ n_data, n, x, fx ] = l_polynomial_values ( n_data )

%*****************************************************************************80
%
%% L_POLYNOMIAL_VALUES returns some values of the Laguerre polynomial.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      LaguerreL[n,x]
%
%  First terms:
%
%      1
%     -X    +  1
%   (  X^2 -  4 X     +  2 ) / 2
%   ( -X^3 +  9 X^2 -  18 X    +    6 ) / 6
%   (  X^4 - 16 X^3 +  72 X^2 -   96 X +      24 ) / 24
%   ( -X^5 + 25 X^4 - 200 X^3 +  600 X^2 -  600 x    +  120 ) / 120
%   (  X^6 - 36 X^5 + 450 X^4 - 2400 X^3 + 5400 X^2 - 4320 X + 720 ) / 720
%   ( -X^7 + 49 X^6 - 882 X^5 + 7350 X^4 - 29400 X^3 + 52920 X^2 - 35280 X
%     + 5040 ) / 5040
%
%  Recursion:
%
%    L(0,X) = 1,
%    L(1,X) = 1-X,
%    N * L(N,X) = (2*N-1-X) * L(N-1,X) - (N-1) * L(N-2,X)
%
%  Orthogonality:
%
%    Integral ( 0 <= X < oo ) exp ( - X ) * L(N,X) * L(M,X) dX
%    = 0 if N /= M
%    = 1 if N == M
%
%  Special values:
%
%    L(N,0) = 1.
%
%  Relations:
%
%    L(N,X) = (-1)^N / N% * exp ( x ) * (d/dx)^n ( exp ( - x ) * x^n )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 March 2012
%
%  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, integer N, the order of the polynomial.
%
%    Output, real X, the point where the polynomial is evaluated.
%
%    Output, real FX, the value of the function.
%
  n_max = 17;

  fx_vec = [ ...
      0.1000000000000000E+01, ...
      0.0000000000000000E+00, ...
     -0.5000000000000000E+00, ...
     -0.6666666666666667E+00, ...
     -0.6250000000000000E+00, ...
     -0.4666666666666667E+00, ...
     -0.2569444444444444E+00, ...
     -0.4047619047619048E-01, ...
      0.1539930555555556E+00, ...
      0.3097442680776014E+00, ...
      0.4189459325396825E+00, ...
      0.4801341790925124E+00, ...
      0.4962122235082305E+00, ...
     -0.4455729166666667E+00, ...
      0.8500000000000000E+00, ...
     -0.3166666666666667E+01, ...
      0.3433333333333333E+02  ];

  n_vec = [ ...
     0,  1,  2, ...
     3,  4,  5, ...
     6,  7,  8, ...
     9, 10, 11, ...
    12,  5,  5, ...
     5,  5 ];

  x_vec = [ ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     1.0E+00, ...
     0.5E+00, ...
     3.0E+00, ...
     5.0E+00, ...
     1.0E+01 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function [T, DISTR, df, M, S] = dtiDirTest(Sbar1, N1, Sbar2, N2)

% Computes voxel-wise directional test statistics of two groups.
%
%   [T, DISTR, df, M, S] = dtiDirTest(Sbar1, N1, Sbar2, [N2])
%
% Input:
%   Sbar1, Sbar2    XxYxZx6 (or nx6) arrays of scatter matrices for each group in dt6 format.
%                   X, Y, Z are the volume dimensions (n is the number of voxels).
%   N1, N2          Number of subjects in each group (N2 defaults to 1).
%
% Output:
%   T               XxYxZx1 (or nx1) array of test statistics.
%   DISTR           The string 'f'
%   df              The degrees of freedom of the f distribution
%   M               XxYxZx3 (or nx3) array of pooled mean directions
%   S               XxYxZx1 (or nx1) array of pooled dispersions
%
% Utilities:    ndfun.m, dtiEig.m, dti33to6.m
%
% WARNING: If using Pentium 4, eliminate NaN's from array before running
% (processor bug).
%
% E.g.:
%   [vec,val] = dtiEig(dt6);
%   [M, S, N, Sbar1] = dtiDirMean(squeeze(vec(:,:,1,1:6)));
%   [M1, S1, N1, Sbar2] = dtiDirMean(squeeze(vec(:,:,1,7)));
%   [T, DISTR, df] = dtiDirTest(Sbar, N, Sbar1);
%
% Reference:
%   A. Schwartzman, R. F. Dougherty, J. E. Taylor (2005),
%       "Cross-subject comparison of principal diffusion direction maps",
%       Magnetic Resonance in Medicine 53(6):1423-1431.
%
% Copyright by Armin Schwartzman, 2004

% HISTORY:
%   2004.06.23 ASH (armins@stanford.edu) wrote it.
%   2006.07.18 ASH added indexed format capability.
%   2006.07.25 ASH changed input parameters.

% Check inputs
if ~exist('N2'),
    N2 = 1;
end
if ((ndims(Sbar1)==2) & (ndims(Sbar2)==2)),
    Ind = 1;    % Data in indexed nx6 format
    Sbar1 = shiftdim(Sbar1, -2);
    Sbar2 = shiftdim(Sbar2, -2);
elseif ((ndims(Sbar1)==4) & (ndims(Sbar2)==4)),
    Ind = 0;    % Data in XxYxZx6 format
else
    error('Wrong input format');
end

% Constants
N  = N1 + N2;
p = 3;
DISTR = 'f';
df = [p-1, (p-1)*(N-2)];

Sbar = (N1 * Sbar1 + N2 * Sbar2)/(N1 + N2);
[vec1, val1] = dtiEig(Sbar1); % ndfun('eig', Sbar1);
[vec2, val2] = dtiEig(Sbar2); % ndfun('eig', Sbar2);
[vec, val] = dtiEig(Sbar); % ndfun('eig', Sbar);
M = vec(:,:,:,:,1);
S = (N - N1*val1(:,:,:,1) - N2*val2(:,:,:,1)) / (N-2);
T = (N1*val1(:,:,:,1) + N2*val2(:,:,:,1) - N*val(:,:,:,1)) ./ S;

% Adjust output
if Ind,
    T = shiftdim(T, 2);
    M = shiftdim(M, 2);
    S = shiftdim(S, 2);
end

return


function r=rotqr2ro(q)
%ROTQR2RO converts a real quaternion to a 3x3 rotation matrix
% Inputs:
%
%     Q(4,1)   real-valued quaternion (with magnitude = 1)
%
% Outputs:
%
%     R(3,3)   Input rotation matrix
%              Plots a diagram if no output specified
%
% In the quaternion representation of a rotation, and q(1) = cos(t/2)
% where t is the angle of rotation in the range 0 to 2pi
% and q(2:4)/sin(t/2) is a unit vector lying along the axis of rotation
% a positive rotation about [0 0 1] takes the X axis towards the Y axis.
%
%      Copyright (C) Mike Brookes 2007
%      Version: $Id: rotqr2ro.m,v 1.5 2008/12/03 09:53:15 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

persistent a b c d e f g
if isempty(a)
    a=[1 5 9];
    b=[11 16 6];
    c=[16 6 11];
    d=[4 8 3];
    e=[10 15 14];
    f=[4 2 3];
    g=[2 6 7];
end
p=2*(q*q.')/(q.'*q);            % force normalized
r=zeros(3,3);
r(a)=1-p(b)-p(c);
r(d)=p(e)-p(f);
r(g)=p(e)+p(f);
if ~nargout
    % display rotated pyramid
    cla % clear current axis
%     vv=[0,0,0;1,0,0;0,1,0;0,0,1]*r';  % pyramid
%     ff=[1 2 4; 2 1 3; 3 1 4; 4 2 3];
%     cc=[0 1 0; 0 0 1; 1 0 0; 1 1 0];
    vv=[0,0,0;1,0,0;0,1,0;0,0,1;0 1 1; 1 0 1; 1 1 0; 1 1 1]*r';    % cube
    ff=[1 2 6 4; 2 7 8 6; 7 3 5 8; 4 5 3 1; 3 7 2 1; 6 8 5 4];
    cc=[0 1 0; 1 0 0; 0 1 0; 1 0 0; 0 0 1; 0 0 1];
    pa=patch('Vertices',vv,'Faces',ff,'FaceVertexCData',cc,'FaceColor','Flat');
    xlabel('x axis');
    ylabel('y axis');
    zlabel('z axis');
    title(sprintf('qr = [%.2f, %.2f, %.2f, %.2f]''    initial xyz=0 are rgb',q))
    axis([-1 1 -1 1 -1 1 0 1]*sqrt(3));
    grid on
    view(3);
    axis equal;
end


function [max_r,ind] = lp_s(p,set)
%
% Computation of the maximal magnitude of the rational ADI function over
% a discrete subset of the left complex half plane.
%
%   Calling sequence:
%
%     [max_r,ind] = lp_s(p,set)
%
%   Input:
%
%     p        vector of ADI parameters;
%     set      vector representing the discrete set.
%
%   Output:
%
%     max_r    maximal magnitude of the rational ADI function over set;
%     ind      index - maximum is attained for set(ind).
%
%
%   LYAPACK 1.0 (Thilo Penzl, Jan 1999)
%

max_r = -1;
ind = 0;

for i = 1:length(set)

  x = set(i);

  rr = 1;
  for j = 1:length(p)

    rr = rr*abs(p(j)-x)/abs(p(j)+x);

  end

  if rr > max_r

    max_r = rr;
    ind = i;

  end

end


% NURBS Toolbox.
% Version 1.0
%
% demos        - NURBS demonstrations
%
% nrbmak       - Construct a NURBS from control points and knots.
% nrbtform     - Applying scaling, translation or rotation operators.
% nrbkntins    - Knot insertion/refinement.
% nrbdegelev   - Degree elevation.
% nrbderiv     - NURBS representation of the derivative.
% nrbdeval     - Evaluation of the NURBS derivative.
% nrbkntmult   - Find the multiplilicity of a knot vector.
% nrbreverse   - Reverse evaluation direction of the NURBS.
% nrbtransp    - Swap U and V for NURBS surface.
% nrbline      - Construct a straight line.
% nrbcirc      - Construct a circular arc.
% nrbrect      - Construct a rectangle.
% nrb4surf     - Surface defined by 4 corner points.
% nrbeval      - Evalution of NURBS curve or surface.
% nrbextrude   - Extrude a NURBS curve along a vector.
% nrbrevolve   - Construct surface by revolving a profile.
% nrbruled     - Ruled surface between twp NURBS curves.
% nrbcoons     - Construct Coons bilinearly blended surface patch.
% nrbplot      - Plot NURBS curve or surface.
%
% bspeval      - Evaluate a univariate B-Spline.
% bspderiv     - B-Spline representation of the derivative
% bspkntins    - Insert a knot or knots into a univariate B-Spline.
% bspdegelev   - Degree elevation of a univariate B-Spline.
%
% vecnorm      - Normalise the vectors.
% vecmag       - Magnitaude of the vectors.
% vecmag2      - Squared Magnitude of the vectors.
% vecangle     - Alternative to atan2 (0 <= angle <= 2*pi)
% vecdot       - Dot product of two vectors.
% veccross     - Cross product of two vectors.
% vecrotx      - Rotation matrix around the x-axis.
% vecroty      - Rotation matrix around the y-axis.
% vecrotz      - Rotation matrix around the z-axis.
% vecscale     - Scaling matrix.
% vectrans     - Translation matrix.
%
% deg2rad      - Convert degrees to radians.
% rad2deg      - Convert radians to degrees.


function a = rutis1_eigen_right ( )

%*****************************************************************************80
%
%% RUTIS1_EIGEN_RIGHT returns the right eigenvectors of the RUTIS1 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real A(4,4), the right eigenvector matrix.
%
  a(1:4,1:4) = [ ...
     1.0,  1.0,  1.0,  1.0; ...
     1.0,  0.0,  0.0, -1.0; ...
     0.0,  1.0, -1.0,  0.0; ...
     1.0, -1.0, -1.0,  1.0 ]';

  return
end


function X = hnormalise(X)
% 	hnormalise   Normalise homogeneous coordinates to have 1 as the last component
% 		Xn = hnormalise(X)
%
%		Inputs:
%				X		ndims x npts array, where each column is a vector of length ndims,
%							the homogeneous coordinates of a point in ndims-1 space
%
%		Outputs:
%				Xn		ndims x npts array, where each column represents the same point as in X
%							but normalised to have the last component equal to 1

%	Author:		Oliver Whyte <oliver.whyte@ens.fr>
%	Date:		November 2011
%	Copyright:	2011, Oliver Whyte
%	Reference:  O. Whyte, J. Sivic and A. Zisserman. "Deblurring Shaken and Partially Saturated Images". In Proc. CPCV Workshop at ICCV, 2011.
%	URL:		http://www.di.ens.fr/willow/research/saturation/

for i=1:size(X,1)-1
	X(i,:) = X(i,:)./X(end,:);
end
X(end,:) = 1;


function x = perform_l1_recovery(D,y,options)

% perform_l1_recovery - run an L1 solver
%
%   x = perform_l1_recovery(D,y,options);
%
%   options.method can be
%       'bp', 'omp', 'itthresh' (nb.iter is options.niter_inversion).
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;

if isfield(options, 'method')
    method = options.method;
else
    method = 'bp';
end
if isfield(options, 's')
    s = options.s;
else
    s = 20;
end
if isfield(options, 'niter_inversion')
    mcaIters = options.niter_inversion;
else
    mcaIters = 200;
end

m = size(D,2);

% OMP options
maxItersOMP = s;
solFreq = 0; verbose = 0; lambdaStop = 0;

% BP options
maxIters = getoptions(options, 'niterbp', 20);
lambda_bp = getoptions(options, 'lambda_bp', 0);
OptTol = getoptions(options, 'tol', 1e-5);

switch lower(method)
    case 'bp'
        x = SolveBP(D, y, m, maxIters, lambda_bp, OptTol);
    case 'omp'
        % x = SolveOMP(D, y, m, maxItersOMP, lambdaStop, solFreq, verbose, OptTol);
        options.nbr_max_atoms = s;
        x = perform_omp(D,y,options);
    case 'itthresh'
        options.niter = mcaIters;
        [x,E] = perform_iterative_thresholding(D,y,options);
    otherwise
        error('Unknown method');
end


function triangle_node = sphere_grid_t3 ( lat_num, long_num )

%*****************************************************************************80
%
%% SPHERE_GRID_T3 produces a triangle grid on a sphere.
%
%  Discussion:
%
%    The point numbering system is the same used in SPHERE_GRIDPOINTS,
%    and that routine may be used to compute the coordinates of the points.
%
%    A sphere in 3D satisfies the equation:
%
%      sum ( ( P(1:DIM_NUM) - pc(1:DIM_NUM) )^2 ) = R^2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer LAT_NUM, LONG_NUM, the number of latitude
%    and longitude lines to draw.  The latitudes do not include the North
%    and South poles, which will be included automatically, so LAT_NUM = 5,
%    for instance, will result in points along 7 lines of latitude.
%
%    Output, integer TRIANGLE_NODE(3,(LAT_NUM+1)*LONG_NUM*2), the
%    triangle vertices.
%
  triangle_node = zeros ( 3, ( lat_num + 1 ) * long_num * 2 );

  triangle_num = 0;
%
%  The first row.
%
  n = 1;

  sw = 2;
  se = sw + 1;

  s_min = 2;
  s_max = long_num + 1;

  for j = 0 : long_num - 1

    triangle_num = triangle_num + 1;
    triangle_node(1:3,triangle_num) = [ sw, se, n ]';

    sw = se;

    if ( se == s_max )
      se = s_min;
    else
      se = se + 1;
    end

  end
%
%  The intermediate rows.
%
  for i = 1 : lat_num

    n_max = s_max;
    n_min = s_min;

    s_max = s_max + long_num;
    s_min = s_min + long_num;

    nw = n_min;
    ne = nw + 1;
    sw = s_min;
    se = sw + 1;

    for j = 0 : long_num - 1

      triangle_num = triangle_num + 1;
      triangle_node(1:3,triangle_num) = [ sw, se, nw ]';

      triangle_num = triangle_num + 1;
      triangle_node(1:3,triangle_num) = [ ne, nw, se ]';

      sw = se;
      nw = ne;

      if ( se == s_max )
        se = s_min;
      else
        se = se + 1;
      end

      if ( ne == n_max )
        ne = n_min;
      else
        ne = ne + 1;
      end

    end

  end
%
%  The last row.
%
  n_max = s_max;
  n_min = s_min;

  s = n_max + 1;

  nw = n_min;
  ne = nw + 1;

  for j = 0 : long_num - 1

    triangle_num = triangle_num + 1;
    triangle_node(1:3,triangle_num) = [ ne, nw, s ]';

    nw = ne;

    if ( ne == n_max )
      ne = n_min;
    else
      ne = ne + 1;
    end

  end

  return
end


function qInv=quatInv(q)
%%QUATINV Find the inverse of a quaternion. The inverse of a quaternion
%         left-or-right multiplied by the quaternion (using quatMult) is
%         equal to one (a real number). As with real numbers, issues arise
%         taking the inverse of the zero quaternion. Note that the inverse
%         does not depend on the handedness of the quaternion algebra, even
%         though the quatMult function does.
%
%%INPUTS: q  A 4XN set of N quaternions, each whose inverse is desired,
%           where the first element in each column is the scalar part of
%           the quaternion (sometimes called q0 or q4) and the next three
%           elements are the (hypercomplex) vector part. That is, the
%           hypercomplex quaternion given by q(:,1) can be written in
%           hypercomplex, non-vector form as
%           q(1,1)+i*q(2,1)+j*q(3,1)+k*q(4,1), where i, j, and k are all
%           roots of -1.
%
%OUTPUTS: qInv The 4XN set of inverses of the quaternions.
%
%Properties of quaternions including inversion are described in [1]. When a
%quaternion has unit magnitude, its conjugate it also its inverse.
%
%REFERENCES:
%[1] Weisstein, Eric W. "Quaternion." From MathWorld--A Wolfram Web
%    Resource. http://mathworld.wolfram.com/Quaternion.html
%
%September 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

qC=quatConj(q);
%The multiplication of a quaternion with its conjugate produces scalar
%values given in the first entries of qMags
qMags=quatMult(q,qC);
qMags=qMags(1,:);

qInv=bsxfun(@rdivide,qC,qMags);
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 vol = imVolumeEstimate(img, varargin)
% Estimate volume of a 3D binary structure with edge correction.
%
%   Vest = imVolumeEstimate(IMG);
%   Vest = imVolumeEstimate(IMG, DELTA);
%
%   Example
%   imVolumeEstimate
%
%   See also
%     imVolume, imSurfaceEstimate

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2010-01-21,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

%% Input arguments processing

% check image dimension
if ndims(img) ~= 3
    error('first argument should be a 3D binary or label image');
end

% in case of a label image, return a vector with a set of results
if ~islogical(img)
    labels = unique(img);
    labels(labels==0) = [];
    vol = zeros(length(labels), 1);
    for i = 1:length(labels)
        vol(i) = imVolumeEstimate(img==labels(i), varargin{:});
    end
    return;
end

% check image resolution
delta = [1 1 1];
if ~isempty(varargin)
    delta = varargin{1};
end

%% main processing

% compute volume in whole image
vol = sum(img(:));

% compute volume on border faces
f1 = sum(sum(sum(img([1 end], :, :))));
f2 = sum(sum(sum(img(:, [1 end], :))));
f3 = sum(sum(sum(img(:, :, [1 end]))));

% compute volume on border edges
e1 = sum(sum(sum(img(:, [1 end], [1 end]))));
e2 = sum(sum(sum(img([1 end], :, [1 end]))));
e3 = sum(sum(sum(img([1 end], [1 end], :))));

% compute volume on corners
v = sum(sum(sum(img([1 end], [1 end], [1 end]))));

% estimate area using edge weighting according to multiplicity
vol = vol -(f1+f2+f3)/2 + (e1+e2+e3)/4 - v/8;

% multiply by volume of a single voxel
vol = vol * prod(delta);


function [ i_lt, i_gt ] = r8vec_sorted_split ( n, a, split )

%*****************************************************************************80
%
%% R8VEC_SORTED_SPLIT "splits" a sorted R8VEC, given a splitting value.
%
%  Discussion:
%
%    Given SPLIT, the routine seeks indices I_LT and I_GT so that
%
%      A(I_LT) < SPLIT < A(I_GT),
%
%    and if there are intermediate index values between I_LT and
%    I_GT, then those entries of A are exactly equal to SPLIT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters
%
%    Input, integer N, the number of entries in A.
%
%    Input, real A(N), a sorted array.
%
%    Input, real SPLIT, a value to which the entries in A are
%    to be compared.
%
%    Output, integer I_LT:
%    0 if no entries are less than SPLIT;
%    N if all entries are less than SPLIT;
%    otherwise, the index of the last entry in A less than SPLIT.
%
%    Output, integer I_GT:
%    1 if all entries are greater than SPLIT;
%    N+1 if no entries are greater than SPLIT;
%    otherwise the index of the first entry in A greater than SPLIT.
%
  if ( n < 1 )
    i_lt = -1;
    i_gt = -1;
    return
  end

  if ( split < a(1) )
    i_lt = 0;
    i_gt = 1;
    return
  end

  if ( a(n) < split )
    i_lt = n;
    i_gt = n + 1;
    return
  end

  lo = 1;
  hi = n;

  while ( 1 )

    if ( lo + 1 == hi )
      i_lt = lo;
      break
    end

    mid = round ( ( lo + hi ) / 2 );

    if ( split <= a(mid) )
      hi = mid;
    else
      lo = mid;
    end

  end

  for i = i_lt + 1 : n
    if ( split < a(i) )
      i_gt = i;
      return
    end
  end

  i_gt = n + 1;

  return
end


function phi = rad(phi)
%RAD returns the given angle in radians
%
%   Usage: phi = rad(phi)
%
%   Input parameters:
%       phi     - angle, can be a scalar or matrix / degree
%
%   Output parameters:
%       phi     - angle / rad
%
%   See also: deg

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input parameters ====================================
nargmin = 1;
nargmax = 1;
narginchk(nargmin,nargmax);

%% ===== Computation =====================================================
phi = phi./180*pi;