classdef ConstantHeadingModelX <  DynamicModelX
% ConstantHeadingModelX class
%
% Summary of ConstantHeadingModelX
% This is a class implementation of a time-varying 2D Linear-Gaussian
% Constant Heading Dynamic Model [1].
%
% The model is described by the following SDEs:
%   dx = v*cos(h)*dt                      | Position on X-axis (m)
%   dy = v*sin(h)*dt                      | Position on Y axis (m)
%   dv = q_vel*dW_t,  W_t~N(0,t)          | Absolute velocity  (m/s)
%   dh = q_head*dB_t, B_t~N(0,t)          | Heading            (rad)
%
% Or equivalently:
%   x(t) = f(x(t-1),Dt) + w(t),  w(t)~ N(0,Q)
%
% where: (using Euler discretisation of the SDEs)
%   x = [x; y; v; h]
%   F = [x + v*cos(h)*Dt; y+ v*sin(h)*Dt; v; h]
%   Q = [0, 0, 0, 0;
%        0, 0, 0, 0;
%        0, 0, q_vel*sqrt(Dt), 0;
%        0, 0, 0, q_head*sqrt(Dt)];
%
% ConstantVelocityModelX_2D Properties:
%   - VelocityErrVariance  Value of the velocity noise diffusion coefficient q_vel
%   - HeadingErrVariance   Value of the heading noise diffusion coefficient q_head
%   - TimeVariant          Value of the time variant Dt
%
% ConstantVelocityModelX_2D Methods:
%   - feval(~)         Equivalent to applying the model transition equations
%   - random(~)        Process noise sample generator function
%   - pdf(~)           Function to evaluate the probability p(x_k|x_{k-1}) of
%                       a set of new states, given a set of (particle) state vectors
%                       e.g. eval = @(xk,xkm1) mvnpdf(xk,xkm1,Q);
%   - covariance(~)    Returns the state covariance process Q_k
%
%  February 2018 Lyudmil Vladimirov, University of Liverpool

    properties
        VelocityErrVariance
        HeadingErrVariance
        TimeVariant
    end

    properties (Access = private)
        f = @(xkm1,Dt) [xkm1(1,:)+Dt*xkm1(3,:).*cos(xkm1(4,:));
                        xkm1(2,:)+Dt*xkm1(3,:).*sin(xkm1(4,:));
                        xkm1(3,:);
                        xkm1(4,:)];
        Q = @(Dt, q_vel, q_head) Dt*blkdiag(q_vel^2, q_vel^2, q_vel, q_head);
    end

    methods
        function this = ConstantHeadingModelX(varargin)
        % CONSTANTHEADINGMODELX Constructor method
        %
        % DESCRIPTION:
        % * ConstantHeadingModelX(q_vel,q_head) instantiates aan object handle
        %   configured with the provided velocity and heading process noise
        %   diffusion coefficients q_vel and q_head (scalar).
        % * ConstantVelocityModelX_2D(config) instantiates an object handle
        %   configured with the provided velocity and heading process noise
        %   diffusion coefficients config.VelocityErrVariance and
        %   config.HeadingErrVariance (scalar).
        % * ConstantVelocityModelX_2D(___,Name,Value) instantiates an object
        %   handle, configured with additional options specified by one or
        %   more Name,Value pair arguments.
        %
        % PARAMETERS
        % * VelocityErrVariance - (Required) The VelocityErrVariance is a scalar
        %   value describing the velocity noise diffusion coefficient.
        % * HeadingErrVariance - (Required) The VelocityErrVariance is a scalar
        %   value describing the heading noise diffusion coefficient (in rads/sec).
        %
        %  See also apply, rnd, pdf, covariance.

            % Call SuperClass method
            this@DynamicModelX;

            % Return quickly if no arguments are passed
            if(nargin==0)
                this.TimeVariant = 1;
                this.VelocityErrVariance = 1;
                this.HeadingErrVariance = 1;
            end

            % First check to see if a structure was received
            if(nargin==1)
                if(isstruct(varargin{1}))
                    this.VelocityErrVariance = varargin{1}.VelocityErrVariance;
                    this.HeadingErrVariance = varargin{1}.HeadingErrVariance;
                    this.TimeVariant = varargin{1}.TimeVariant;
                    return;
                end
            end

            % Otherwise, fall back to input parser
            parser = inputParser;
            parser.KeepUnmatched = true;
            parser.addOptional('VelocityErrVariance',[]);
            parser.addOptional('HeadingErrVariance',[]);
            parser.addOptional('TimeVariant',1);
            parser.parse(varargin{:});

            this.VelocityErrVariance = parser.Results.VelocityErrVariance;
            this.HeadingErrVariance = parser.Results.HeadingErrVariance;
            this.TimeVariant = parser.Results.TimeVariant;

            this.NumStateDims = 4;

        end

        function xk = feval(this, xkm1, wk, Dt)
        % FEVAL Propagate a given state through the dynamic model
        %
        % Parameters
        % ----------
        % xkm1: (NumStateDims x Ns) matrix, optional
        %   - A matrix, whose columns correspond to individual state vectors.
        %   - If not provided, then the state transition matrix (F) will be
        %     returned.
        % wk: (NumStateDims x Ns) matrix or boolean, optional
        %   - If wk is a boolean and set True, then the default model noise
        %     generation function (this.random()) will be used to generate the
        %     noise.
        %   - Otherwise, wk should be a matrix of equal dimensions to xk,
        %     where each column corresponds to the random noise which will
        %     be added to each state vector.
        %   - If not provided, then no noise will be added to the state vectors.
        % Dt: scalar, optional
        %   A time variant. (default=this.TimeVariant)
        %
        % Returns
        % -------
        % xk: (NumStateDims x Ns) matrix or function handle
        %   - If no parameters are passed to the function, then xk will be the
        %     state transition function f.
        %   - Else, xk will be a (NumStateDims x Ns) matrix, whose columns
        %     correspond to the columns of xkm1, each propagated through the
        %     dynamic model
        %
        % Usage
        % -----
        % * xk = FEVAL(this,xkm1,wk,Dt) returns the new state xk, produced by
        %   propagating the given state xkm1 through the dynamic model for
        %   time Dt and adding random noise wk. xk, xkm1 and wk must have
        %   the same dimensions, i.e (NumStateDim x Ns), where Ns is the
        %   number of states/columns in xkm1.
        % * xk = FEVAL(this,xkm1,wk) returns the new state xk, produced by
        %   propagating the given state xkm1 through the dynamic model for
        %   time this.TimeVariant and adding random noise wk.
        %   (i.e. Default Dt = this.TimeVariant)
        % * xk = FEVAL(this,xkm1) returns the new state xk, produced by
        %   propagating the given state xkm1 through the dynamic model for
        %   time this.TimeVariant without the addition of noise.
        %   (i.e. Default wk = 0)
        % * xk = FEVAL(this) returns the model's transition matrix resulting
        %   through the application of this.TimeVariant.
        %   (i.e. Default xkm1 = 1)
        %
        % See also PDF, RANDOM, COVARIANCE.

            switch(nargin)
                case 1
                    xk = this.f;
                    return;
%                     xkm1 = 1;
%                     wk   = 0;
%                     Dt = this.TimeVariant;
                case 2
                    wk   = 0;
                    Dt = this.TimeVariant;
                case 3
                    Dt = this.TimeVariant;
                    if(islogical(wk) && wk)
                        wk = this.random(size(xkm1,2),Dt);
                    end
            end

            % Compute result
            xk = this.f(xkm1,Dt) + wk;
        end

        function cov = covariance(this,Dt)
        % COVARIANCE Returns process covariance matrix.
        %
        % Parameters
        % ----------
        % Dt: scalar, optional
        %   A time variant. (default=this.TimeVariant)
        %
        % Returns
        % -------
        % cov: (NumStateDims x NumStateDims) matrix
        %   The process noise covariance matrix
        %
        % Usage
        % -----
        % * Qk = covariance(this,Dt) returns process covariance matrix, upon
        %   application of Dt.
        % * Qk = covariance(this) returns process covariance matrix, upon
        %   application of this.TimeVariant.
        % (i.e. Default Dt = this.TimeVariant)
        %
        % See also FEVAL, RANDOM, PDF.
            switch(nargin)
                case 1
                    Dt = this.TimeVariant;
            end

            % Return process covariance
            cov = this.Q(Dt,this.VelocityErrVariance, this.HeadingErrVariance); % Time variant
        end

        function wk = random(this, Ns, Dt)
        % RANDOM Generates random samples from the dynamic model's noise
        % distribution.
        %
        % Parameters
        % ----------
        % Ns: scalar, optional
        %   The number of samples to be generated.
        %   (default = 1)
        % Dt: scalar, optional
        %   A time variant.
        %   (default=this.TimeVariant)
        %
        % Returns
        % -------
        % wk: (NumStateDims x Ns)
        %   A matrix, whose columns correspond to indivual/independent
        %   noise samples.
        %
        % Usage
        % -----
        % * wk = random(this,Ns,Dt) generates and returns a set of Ns samples
        %   generated from the noise distribution of the dynamic model, i.e.
        %   wk ~ N(0,Q(Dt)), where Q is the noise covariance upon application
        %   of the time variant Dt.
        % * wk = random(this,Ns) generates and returns a set of Ns samples
        %   generated from the noise distribution of the dynamic model, i.e.
        %   wk ~ N(0,Q), where Q is the noise covariance upon application
        %   of the time variant this.TimeIndex.
        % (i.e. Default Dt = this.TimeVariant)
        %
        % See also FEVAL, PDF, COVARIANCE

            switch(nargin)
                case 1
                    Ns = 1;
                    Dt = this.TimeVariant;
                case 2
                    Dt = this.TimeVariant;
            end

            wk = mvnrnd(zeros(this.NumStateDims,1),this.Q(Dt,this.VelocityErrVariance, this.HeadingErrVariance),Ns)';
        end

        function prob = pdf(this, xk, xkm1, Dt)
        % PDF Evaluates the probability/likelihood p(x_k|x_{k-1}) of a
        % (set of) new state vector(s), given a (set of) old state vector(s)
        %
        % Parameters
        % ----------
        % xk: (NumStateDims x Ns) matrix
        %   A matrix, whose columns correspond to individual new state vectors.
        % xkm1: (NumStateDims x Np) matrix
        %   A matrix, whose columns correspond to individual old state vectors
        % Dt: scalar, optional
        %   A time variant.
        %   (default=this.TimeVariant)
        %
        % Returns
        % -------
        % prob: (Np x Ns) matrix
        %   A matrix, where each element (i,j) corresponds to the evaluated
        %   probability p(xk(:,j)|xkm1(:,i))
        %
        % Usage
        % -----
        % * prob = pdf(x_k, x_km1, Dt) evaluates and returns a (a x b)
        %   probability/likelihood matrix given the (NumStatesDim x a) x_k
        %   and (NumStatesDim x b) x_km1 state matrices. Dt is an optional
        %   argument (Default = this.TimeVariant) time variable, which is
        %   used for computing the model's covariance.
        %
        % See also FEVAL, RANDOM, COVARIANCE

            if(nargin<4)
                Dt = this.TimeVariant;
            end

            xk_km1 = this.feval(xkm1);
            prob = zeros(size(xk,2), size(xkm1,2));
            if(size(xkm1,2)>size(xk,2))
                for i=1:size(xk,2)
                    prob(i,:) = gauss_pdf(xk(:,i), xk_km1, this.Q(Dt,this.VelocityErrVariance,this.HeadingErrVariance));
                end
            else
                for i=1:size(xkm1,2)
                    prob(:,i) = gauss_pdf(xk, xk_km1(:,i), this.Q(Dt,this.VelocityErrVariance,this.HeadingErrVariance))';
                end
            end

        end
    end
end


% TD | CP-ALS | PARAFAC/CP decomposition solved by Alternating Least Squares
% process_video('TD', 'CP-ALS', 'dataset/demo.avi', 'output/demo_CP-ALS.avi');

r = 10;
A = double(T);
L = double(cp_als(T,r,'dimorder',[3 2 1]));
S = A - L;


function y = im2jpeg2k(x,n,q)
%IM2JPEG2K Compresses an image using a JPEG 2000 approximation.
%   Y = IM2JPEG2K(X,N,Q) compresses image X using an N-scale JPEG 2K
%   wavelet transform, implicit or explicit coefficient quantization,
%   and Huffman symbol coding augmented by zero run-length coding. If
%   quantization vector Q contains two elements, they are assumed to be
%   implicit quantization parameters; else, it is assumed to contain
%   explicit subband step sizes.  Y is an encoding structure containing
%   Huffman-encoded data and additional parameters needed by JPEG2K2IM
%   for decoding.
%
%   See also JPEG2K2IM.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

narginchk(3,3);         % Check input arguments

if ~ismatrix(x) || ~isreal(x) || ~isnumeric(x) || ~isa(x,'uint8')
   error('The input must be a UINT8 image.');
end

if length(q) ~= 2 && length(q) ~= 3 * n + 1
   error('The quantization step size vector is bad.');
end

% Level shift the input and compute its wavelet transform.
x = double(x) - 128;
[c, s] = wavefast(x,n,'jpeg9.7');

% Quantize the wavelet coefficients.
q = stepsize(n,q);
sgn = sign(c);     sgn(sgn == 0) = 1;     c = abs(c);
for k = 1:n
   qi = 3 * k - 2;
   c = wavepaste('h',c,s,k,wavecopy('h',c,s,k) / q(qi));
   c = wavepaste('v',c,s,k,wavecopy('v',c,s,k) / q(qi + 1));
   c = wavepaste('d',c,s,k,wavecopy('d',c,s,k) / q(qi + 2));
end
c = wavepaste('a',c,s,k,wavecopy('a',c,s,k) / q(qi + 3));
c = floor(c);       c = c .* sgn;

% Run-length code zero runs of more than 10. Begin by creating
% a special code for 0 runs ('zrc') and end-of-code ('eoc') and
% making a run-length table.
zrc = min(c(:)) - 1;      eoc = zrc - 1;

% The RUNS variable is shared with the RUNS variable in the nested
% function runcode. See Chapter 3 of DIPUM3E for a discussion of nested
% functions.
RUNS = 65535;

% Find the run transition points: 'plus' contains the index of the
% start of a zero run; the corresponding 'minus' is its end + 1.
z = c == 0;                 z = z - [0 z(1:end - 1)];
plus = find(z == 1);        minus = find(z == -1);

% Remove any terminating zero run from 'c'.
if length(plus) ~= length(minus)
   c(plus(end):end) = [];      c = [c eoc];
end

% Remove all other zero runs (based on 'plus' and 'minus') from 'c'.
for i = length(minus):-1:1
   run = minus(i) - plus(i);
   if run > 10
      ovrflo = floor(run / 65535);    run = run - ovrflo * 65535;
      c = [c(1:plus(i) - 1) repmat([zrc 1],1,ovrflo) zrc ...
         runcode(run) c(minus(i):end)];
   end
end

% Huffman encode and add misc. information for decoding.
y.runs    = uint16(RUNS);
y.s       = uint16(s(:));
y.zrc     = uint16(-zrc);
y.q       = uint16(100 * q');
y.n       = uint16(n);
y.huffman = mat2huff(c);

   %-------------------------------------------------------------------%
   function y = runcode(x)
      % Find a zero run in the run-length table. If not found, create a
      % new entry in the table. Return the index of the run.

      y = find(RUNS == x);
      if length(y) ~= 1
         RUNS = [RUNS; x];
         y = length(RUNS);
      end
   end
end

%----------------------------------------------------------------------%
function q = stepsize(n,p)
% Create a subband quantization array of step sizes ordered by
% decomposition (first to last) and subband (horizontal, vertical,
% diagonal, and for final decomposition the approximation subband).

if length(p) == 2               % Implicit Quantization
   q = [];
   qn = 2 ^ (8 - p(2) + n) * (1 + p(1) / 2 ^ 11);
   for k = 1:n
      qk = 2 ^ -k * qn;
      q = [q (2 * qk) (2 * qk) (4 * qk)];
   end
   q = [q qk];
else                            % Explicit Quantization
   q = p;
end

q = round(q * 100) / 100;       % Round to 1/100th place
if any(100 * q > 65535)
   error('The quantizing steps are not UINT16 representable.');
end
if any(q == 0)
   error('A quantizing step of 0 is not allowed.');
end
end


function Volume=polygon2voxel(FV,VolumeSize,mode,Yxz,hollow)
% This function POLYGON2VOXEL will convert a Triangulated Mesh into a
% Voxel Volume which will contain the discretized mesh. Discretization of a
% polygon is done by splitting/refining the face, until the longest edge
% is smaller than 0.5 voxels. Followed by setting the voxel beneath the vertice
% coordinates of that small triangle to one.
%
% Volume=polygon2voxel(FV,VolumeSize,Mode,Yxz);
%
% Inputs,
%   FV : A struct containing FV.faces with a facelist Nx3 and FV.vertices
%        with a Nx3 vertice list. Such a structure is created by Matlab
%        Patch function
%   VolumeSize : The size of the output volume, example [100 100 100]
%   Mode : (optional) if set to:
%               'none', The vertices data is directly used as coordinates
%                       in the voxel volume.
%               'wrap', The vertices data is directly used as coordinates
%                       in the voxel volume, coordinates outside are
%                       circular wrapped to the inside.
%               'auto', The vertices data is translated and
%                       scaled with a scalar to fit inside the new volume.
%               'center', coordinate 0,0,0 is set as the center of the volume
%                       instead of the corner of the voxel volume.
%               'clamp', The vertices data is directly used as coordinates
%                       in the voxel volume, coordinates outside are
%                       clamped to the inside.
%   (Optional)
%   Yxz : If true (default) use Matlab convention 1th dimension Y,
%         2th dimension X, and last dimension Z. Otherwise 1th
%         dimension is X, 2th Y and last Z.
%
%
% Outputs,
%   Volume : The 3D logical volume, with all voxels part of the discretized
%           mesh one, and all other voxels zero.
%
% Example,
%   % Compile the c-coded function
%   mex polygon2voxel_double.c -v
%
%   % Load a triangulated mesh of a sphere
%   load sphere;
%
%   % Show the mesh
%   figure, patch(FV,'FaceColor',[1 0 0]); axis square;
%
%   % Convert the mesh to a voxelvolume
%   Volume=polygon2voxel(FV,[50 50 50],'auto');
%
%   % Show x,y,z slices
%   figure,
%   subplot(1,3,1), imshow(squeeze(Volume(25,:,:)));
%   subplot(1,3,2), imshow(squeeze(Volume(:,25,:)));
%   subplot(1,3,3), imshow(squeeze(Volume(:,:,25)));
%
%   %  Show iso surface of result
%   figure, patch(isosurface(Volume,0.1), 'Facecolor', [1 0 0]);
%
% Example2,
%   % Compile the c-coded function
%   mex polygon2voxel_double.c -v
%
%   % Make A Volume with a few blocks
%   I = false(120,120,120);
%   I(40:60,50:70,60:80)=1; I(60:90,45:75,60:90)=1;
%   I(20:60,40:80,20:60)=1; I(60:110,35:85,10:60)=1;
%
%   % Convert the volume to a triangulated mesh
%   FV = isosurface(I,0.8);
%
%   % Convert the triangulated mesh back to a surface in a volume
%   J = polygon2voxel(FV,[120, 120, 120],'none');
%   % Fill the volume
%   J=imfill(J,'holes');
%
%   % Differences between original and reconstructed
%   VD = abs(J-I);
%
%   % Show the original Mesh and Mesh of new volume
%   figure,
%   subplot(1,3,1),  title('original')
%     patch(FV,'facecolor',[1 0 0],'edgecolor','none'), camlight;view(3);
%   subplot(1,3,2), title('converted');
%     patch(isosurface(J,0.8),'facecolor',[0 0 1],'edgecolor','none'), camlight;view(3);
%   subplot(1,3,3), title('difference');
%     patch(isosurface(VD,0.8),'facecolor',[0 0 1],'edgecolor','none'), camlight;view(3);
%
% Function is written by D.Kroon University of Twente (May 2009)
%
% Simple modification by Jianxiong to fill in the holes

if ~exist('hollow','var')
    hollow = false;
end

if(nargin<4), Yxz=true; end

% Check VolumeSize size
if(length(VolumeSize)==1)
    VolumeSize=[VolumeSize VolumeSize VolumeSize];
end
if(length(VolumeSize)~=3)
    error('polygon2voxel:inputs','VolumeSize must be a array of 3 elements ')
end

% Volume Size must always be an integer value
VolumeSize=round(VolumeSize);

sizev=size(FV.vertices);
% Check size of vertice array
if((sizev(2)~=3)||(length(sizev)~=2))
    error('polygon2voxel:inputs','The vertice list is not a m x 3 array')
end

sizef=size(FV.faces);
% Check size of vertice array
if((sizef(2)~=3)||(length(sizef)~=2))
    error('polygon2voxel:inputs','The vertice list is not a m x 3 array')
end

% Check if vertice indices exist
if(max(FV.faces(:))>size(FV.vertices,1))
    error('polygon2voxel:inputs','The face list contains an undefined vertex index')
end

% Check if vertice indices exist
if(min(FV.faces(:))<1)
    error('polygon2voxel:inputs','The face list contains an vertex index smaller then 1')
end

% Matlab dimension convention YXZ
if(Yxz)
    FV.vertices=FV.vertices(:,[2 1 3]);
end

switch(lower(mode(1:2)))
    case {'au'} % auto
        % Make all vertices-coordinates positive
        FV.vertices=FV.vertices-min(FV.vertices(:));
        scaling=min((VolumeSize-1)./(max(FV.vertices(:))));
        % Make the vertices-coordinates to range from 0 to 100
        FV.vertices=FV.vertices*scaling+1;
        Wrap=0;
    case {'ce'} % center
        % Center the vertices
        FV.vertices=FV.vertices+repmat((VolumeSize/2),size(FV.vertices,1),1);
        Wrap=0;
    case {'wr'} %wrap
        Wrap=1;
    case{'cl'} % clamp
        Wrap=2;
    otherwise
        Wrap=0;
end

% Separate the columns;
FacesA=double(FV.faces(:,1));
FacesB=double(FV.faces(:,2));
FacesC=double(FV.faces(:,3));
VerticesX=double(FV.vertices(:,1));
VerticesY=double(FV.vertices(:,2));
VerticesZ=double(FV.vertices(:,3));

% Volume size to double
VolumeSize=double(VolumeSize);

% Call the mex function
Volume=polygon2voxel_double(FacesA,FacesB,FacesC,VerticesX,VerticesY,VerticesZ,VolumeSize,Wrap);

if ~hollow
    for i=1:size(Volume,3)
        Volume(:,:,i) = imfill(Volume(:,:,i),'holes');
    end
end


function [fTafseq] = calc_Afseqlengtwomol(fPval1,fCval1,fKval1,fBgrate,nMod,fTsecaf,fPval2,fCval2,fKval2)
    % [fTafseq] = calc_Afseqlengtwomol(fPval1,fCval1,fKval1,fBgrate,nMod,fTsecaf,fPval2,fCval2,fKval2)
    % -------------------------------------------------------------------------
    % Determines the length of an aftershock sequence by determining the
    % intersection with the DAILY backgraund rate; MOL and nested MOL are allowed
    % (only one secondary sequence)
    %
    % Incoming:
    % fPval1   : Modified Omori law p-value
    % fCval1   : Modified Omori law c-value [days]
    % fKval1   : Modified Omori law k-value
    % fBgrate  : Daily background rate of events above Mc
    % nMod     : Aftershock sequence model choice
    %               1. Modified Omori law (MOL)
    %               2. MOL with one secondary sequence
    % fTsecaf
    % fPval2   : Modified Omori law p-value
    % fCval2   : Modified Omori law c-value [days]
    % fKval2   : Modified Omori law k-value

    % Outgoing:
    % fTafseq  : Length of aftershock sequence
    %
    % last update: 08.07.04
    % jochen.woessner@sed.ethz.ch

    % Time vector in days
    vT = [0:0.1:10000];
    vT = vT';

    % Check input
    if nargin < 4; disp('Not enough input parameters'); return; end
    if nargin == 4; nMod=1; end

    % Switch between MOL and nested MOL
    switch nMod
        case 1 % MOL
            vRate = abs(fKval1.*(vT+fCval1).^-fPval1-fBgrate);
            vSel = (min(vRate) == vRate);
            fTafseq = vT(vSel);
        case 2 % Nested Models
            vSelT = (vT >= fTsecaf);
            vRate1 = abs(fKval1.*(vT(~vSel)+fCval1).^-fPval1-fBgrate);
            vRate2 = abs(fKval1.*(vT(vSel)+fCval1).^-fPval1+fKval2.*(vT(vSel,:)-fTsecaf+fCval2).^-fPval2-fBgrate);
            vRate = [vRate1; vRate2];
            vSel = (min(vRate) == vRate);
            fTafseq = vT(vSel);
        otherwise
            disp('Not a valid model!');
    end
    % % Possible plot
    % figure
    % loglog(vT,vRate)
    % xlabel('Time [days after mainshock]')
    % ylabel('Daily rate of earthquakes')


function result=RS_DF_Test(rdata1,rtrue1,grid1,hstep,el,bootMC)

% Construct PITs
pit = [];
T = size(rdata1,1);

for i = 1:T

    ygrid = grid1;
    rdata = rdata1(i,1:length(ygrid))';

    fitdens1 = fitdist(ygrid, 'normal', 'frequency',round(rdata));

    z1 = normcdf(rtrue1(i,:),fitdens1.mu, fitdens1.sigma);

    pit = [pit; z1];
end

% Construct Histogram of PITS
bin  = 10;
m    = length(pit);
rvec = (0:0.001:1);
hs   = histc(pit,0:1/bin:1);

result.rvec      = rvec;
result.m         = m;
result.bin       = bin;
result.histogram = hs;

% Test Statistic
for r = 1:size(rvec,2)
    ecdf(:,r) = mean(pit < rvec(:,r));
end
result.ecdf=ecdf;

% Critical Values
if hstep == 1
    results1CS = bear.RS_DF_rstest(pit);
    table1 = results1CS(:,1:2);
elseif hstep >= 1
%     table1 = RS_DF_CVfinalbootstrap(el,bootMC,pit,rvec); % 20.9.2018
    table1 = bear.RS_DF_CVfinalbootstrapInoue(el,bootMC,pit,rvec);
end

result.output=round(table1.*1000)./1000;


function g = tanh(f)
%TANH   Hyperbolic tangent of a BALLFUN.
%   TANH(F) computes the hyperbolic tangent of the BALLFUN F.
%
% See also TAN, SINH, COSH.

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

g = compose( f, @tanh );
end


% the Variable Span beamformer
% h = sum( gevdVec * gevdVec^H / (mu + lambda)) * PhiX * refVec
% input:    PhiN, (Nch, Nch, Nbin) the noise covariance matrix
%           PhiX, (Nch, Nch, Nbin) the speech covariance matrix
%           mu, the speech distortion/noise reduction trade-off parameter
%           span, 1 <= span <= Nch, dimension of the assumed source space
%           refMic, the reference microphone, default = 1
% output:   h, (Nch, Nbin)  the beamformer coefficients
% % Ziteng Wang @ 201812

function h = VarSpan(PhiX, PhiN, mu, span, refMic)
if nargin < 3
    mu = 1;             %%% typical value {0, 1}
elseif nargin < 4
    span = 1;           %%% default 1 for single-target
elseif nargin < 5
    refMic = 1;
end

[Nch, ~, Nbin] = size(PhiX);
refVec = zeros(Nch, 1);
refVec(refMic) = 1;
h = zeros(Nch, Nbin);

for bin = 1:Nbin
    if rcond(PhiN(:,:,bin)) < eps
        disp(['bin ' num2str(bin) ': Noise covariance ill-conditioned.']);
        PhiN(:,:,bin) = PhiN(:,:,bin) + 1e-10 * eye(Nch);
    end
    [vv, dd] = eig(PhiX(:,:,bin), PhiN(:,:,bin));
    [~, idx] = sort(diag(dd), 'descend');
    tmp = 0;
    for i = 1:span
        tmp = tmp + vv(:,idx(i)) * vv(:,idx(i))' / (mu + dd(idx(i),idx(i)));
    end
    h(:,bin) = tmp * PhiX(:,:,bin) * refVec;
end


function essential_distMinAnglePair_test
resetRands(3)
flagDegenerateCase=true;
k=2;

e3=[0;0;1];
Q1=rot_randn([],[],2);
if flagDegenerateCase
    Q1b=[Q1(:,:,1);Q1(:,:,2)];
    Q2b=essential_randomVerticalMotion(Q1b);
    Q2=cat(3,Q2b(1:3,:),Q2b(4:6,:));
else
    Q2=rot_randn([],[],2);
end
Rzt=@(t) rot(t*e3);

Q21tk=@(t,k) Rzt(t)*essential_flipAmbiguity_R1(Q2(:,:,1),k);
Q22tk=@(t,k) Rzt(t)*essential_flipAmbiguity_R2(Q2(:,:,2),k);

figure(1)
[tMin,fMin,tBreak1,tBreak2,Q2Flip]=essential_distMinAnglePair([Q1(:,:,1);Q1(:,:,2)],[Q2(:,:,1);Q2(:,:,2)],k);
tMin=modAngle(tMin);
ft=@(t) (rot_dist(Q1(:,:,1),Q21tk(t,k))^2+rot_dist(Q1(:,:,2),Q22tk(t,k))^2);
dft=@(t) 2*e3'*(Q1(:,:,1)*logrot(Q1(:,:,1)'*Q21tk(t,k))+Q1(:,:,2)*logrot(Q1(:,:,2)'*Q22tk(t,k)));
check_der(ft,dft,'angle')
hold on
plot(tBreak1,ft(tBreak1),'r+')
plot(tBreak2,ft(tBreak2),'g+')

plot(tMin,fMin,'kx','MarkerSize',20)

hold off


% elementwise production of all previous layers, which are required to have the
% same size.
%
classdef HadamardNode < GraphNode

    methods
        function obj = HadamardNode(dimOut)
            obj = obj@GraphNode('Hadamard',dimOut);
        end

        function obj = forward(obj,prev_layers)
            obj = obj.preprocessingForward(prev_layers);

            obj.a = prev_layers{1}.a;
            for i=2:length(prev_layers)
                obj.a = obj.a .* conj(prev_layers{i}.a);
            end

            obj = forward@GraphNode(obj, prev_layers);
        end

        function obj = backward(obj,prev_layers, future_layers)
            if obj.skipGrad || obj.skipBP
                return;
            end

            future_grad = obj.GetFutureGrad(future_layers);
            for i=1:length(prev_layers)
                [D(1) D(2) D(3) D(4)] = size(prev_layers{i}.a);
                if i==1
                    grad = future_grad .* prev_layers{2}.a;
                else
                    grad = future_grad .* prev_layers{1}.a;
                end
                for j = 1:4
                    if D(j) == 1
                        grad = sum(grad,j);
                    end
                end
                obj.grad{i} = conj(grad);
            end

            obj = backward@GraphNode(obj, prev_layers, future_layers);
        end

    end

end


%% Discrete-time reentry dynamics demonstration with non-linear filters
%
%  Description:
%    In this example various different non-linear filters and smoothers are
%    applied to reentry tracking problem . The filters used in this
%    demonstration are:
%      * Extended Kalman filter
%      * Unscented Kalman filter
%      * Gauss-Hermite Kalman filter (degree 3)
%      * Cubature Kalman filter
%    Additionally, the corresponding smoother results are also presented.
%
%  References:
%    Refer to the Toolbox documentation for details on the model.
%
%  See also:
%    ukf_predict1, ukf_update1, urts_smooth1,
%    ekf_predict1, ekf_update1, erts_smooth1, ghkf_predict, ghkf_update,
%    ghrts_smooth, ckf_predict, ckf_update, crts_smooth
%
%  Author:
%    Copyright (C) 2006 Simo Särkkä
%                  2007 Jouni Hartikainen
%                  2010 Arno Solin
%
%  Licence:
%    This software is distributed under the GNU General Public
%    Licence (version 2 or later); please refer to the file
%    Licence.txt, included with the software, for details.

%% Set up model parameters

  reentry_param;
  make_reentry_data;

  silent = 0;

  % Handles to dynamic and measurement model functions,
  % and to their derivatives.
  func_f  = @reentry_f;
  func_if = @reentry_if;
  func_df = @reentry_df_dx;
  func_h  = @reentry_h;
  func_dh = @reentry_dh_dx;

  % Initial values and space for EKF
  m = m0;
  P = P0;
  Q = L*Qc*L'*dt;

  MM_EKF = zeros(size(m,1),size(Y,2));
  PP_EKF = zeros(size(m,1),size(m,1),size(Y,2));
  VV_EKF = zeros(size(m,1),size(Y,2));
  EE_EKF = zeros(size(m,1),size(Y,2));

  % Check derivatives (should be OK)
  %der_check(func_a, func_da, 1, m0, {dt,b0,H0,Gm0,R0});
  %der_check(func_h, func_dh, 1, m0, {xr,yr});

  clf; clc;
  disp(['This is a demonstration for tracking a reentry vehicle ',...
        'using 1st order EKF and augmented UKF.'])
  disp(' ');

%% Extended Kalman filter

  fprintf('Running EKF...');
  % Filtering with EKF
  for k=1:size(Y,2)
    [m,P] = ekf_predict1(m,P,func_df,Q,func_f,[],{dt,b0,H0,Gm0,R0});
    [m,P] = ekf_update1(m,P,Y(:,k),func_dh,diag([vr va]),func_h,[],{xr,yr});
    MM_EKF(:,k) = m;
    PP_EKF(:,:,k) = P;
    VV_EKF(:,k) = diag(P);
    EE_EKF(:,k) = (X(:,k) - m).^2;
  end

  %
  % Calculate RMSE
  %
  ekf_rmse = sqrt(mean(sum((X(1:2,:)-MM_EKF(1:2,:)).^2)));
  ME_EKF = squeeze(PP_EKF(1,1,:)+PP_EKF(2,2,:));
  fprintf('Done!\n')

  fprintf('Running smoothers...');
  %
  % Smoother 1
  %
  [SM_ERTS,SP_ERTS] = erts_smooth1(MM_EKF,PP_EKF,func_df,Qc*dt,func_f,L,...
                          {dt,b0,H0,Gm0,R0});
  eks_rmse1 = sqrt(mean(sum((X(1:2,:)-SM_ERTS(1:2,:)).^2)));
  ME_ERTS = squeeze(SP_ERTS(1,1,:)+SP_ERTS(2,2,:));

  SV_ERTS = zeros(size(m,1),size(Y,2));
  SE_ERTS = zeros(size(m,1),size(Y,2));
  for k=1:size(Y,2)
    SV_ERTS(:,k) = diag(SP_ERTS(:,:,k));
    SE_ERTS(:,k) = (X(:,k) - SM_ERTS(:,k)).^2;
  end

  %
  % Smoother 2
  %
  [SM_ETF,SP_ETF] = etf_smooth1(MM_EKF,PP_EKF,Y,...
	func_df,Qc*dt,func_if,L,{dt,b0,H0,Gm0,R0},...
	func_dh,diag([vr va]),func_h,[],{xr,yr});

  eks_rmse2 = sqrt(mean(sum((X(1:2,:)-SM_ETF(1:2,:)).^2)));
  ME_ETF = squeeze(SP_ETF(1,1,:)+SP_ETF(2,2,:));
  fprintf('Done!\n');

%% Unscented Kalman filter

  fprintf('Running UKF...');

  % Initial values and space for (augmented) UKF
  m = m0;
  P = P0;
  MM_UKF = zeros(size(m,1),size(Y,2));
  PP_UKF = zeros(size(m,1),size(m,1),size(Y,2));
  VV_UKF = zeros(size(m,1),size(Y,2));
  EE_UKF = zeros(size(m,1),size(Y,2));

  % Filtering with UKF
  for k=1:size(Y,2)
    % Non-augmented UKF
    %[m,P] = ukf_predict1(m,P,func_a,Q,{dt,b0,H0,Gm0,R0});

    % Augmented UKF with separate sigma points
    %[m,P] = ukf_predict2(m,P,func_f,Qc*dt,{dt,b0,H0,Gm0,R0,L});

    %[m,P] = ukf_update2(m,P,Y(:,k),func_h,diag([vr va]),{xr,yr});

    % Augmented UKF with same sigma points for predict and update steps
    [m,P,X_s,w] = ukf_predict3(m,P,func_f,Qc*dt,diag([vr va]),d_param);
    [m,P] = ukf_update3(m,P,Y(:,k),func_h,diag([vr va]),X_s,w,h_param,h_param);

    MM_UKF(:,k) = m;
    PP_UKF(:,:,k) = P;
    VV_UKF(:,k) = diag(P);
    EE_UKF(:,k) = (X(:,k) - m).^2;
  end

  %
  % Calculate RMSE of UKF
  %
  ukf_rmse = sqrt(mean(sum((X(1:2,:)-MM_UKF(1:2,:)).^2)));
  ME_UKF = squeeze(PP_UKF(1,1,:)+PP_UKF(2,2,:));
  fprintf('Done!\n');

  fprintf('Running smoothers...');
  %
  % Smoother 1
  %
  [SM_URTS,SP_URTS] = urts_smooth1(MM_UKF,PP_UKF,func_f,Q,d_param);
  uks_rmse1 = sqrt(mean(sum((X(1:2,:)-SM_URTS(1:2,:)).^2)));
  ME_URTS = squeeze(SP_URTS(1,1,:)+SP_URTS(2,2,:));

  SV_URTS = zeros(size(m,1),size(Y,2));
  SE_URTS = zeros(size(m,1),size(Y,2));
  for k=1:size(Y,2)
    SV_URTS(:,k) = diag(SP_URTS(:,:,k));
    SE_URTS(:,k) = (X(:,k) - SM_URTS(:,k)).^2;
  end

  [SM_URTSb,SP_URTSb] = urts_smooth2(MM_UKF,PP_UKF,func_f,Qc*dt,d_param);
  uks_rmse1b = sqrt(mean(sum((X(1:2,:)-SM_URTSb(1:2,:)).^2)));
  ME_URTSb = squeeze(SP_URTSb(1,1,:)+SP_URTSb(2,2,:));

  %
  % Smoother 2
  %
  [SM_UTF,SP_UTF] = utf_smooth1(MM_UKF,PP_UKF,Y,...
	func_if,Qc*dt,d_param,...
	func_h,diag([vr va]),h_param);

  uks_rmse2 = sqrt(mean(sum((X(1:2,:)-SM_UTF(1:2,:)).^2)));
  ME_UTF = squeeze(SP_UTF(1,1,:)+SP_UTF(2,2,:));

  fprintf('Done!\n');

%% Gauss-Hermite Kalman filter

  fprintf('Running GHKF...');

  % Initial values and space for GHKF
  m = m0;
  P = P0;
  MM_GHKF = zeros(size(m,1),size(Y,2));
  PP_GHKF = zeros(size(m,1),size(m,1),size(Y,2));
  VV_GHKF = zeros(size(m,1),size(Y,2));
  EE_GHKF = zeros(size(m,1),size(Y,2));

  % Filtering with GHKF
  for k=1:size(Y,2)
    [m,P] = ghkf_predict(m,P,func_f,Q,{dt,b0,H0,Gm0,R0},3);
    [m,P] = ghkf_update(m,P,Y(:,k),func_h,diag([vr va]),{xr,yr},3);
    MM_GHKF(:,k) = m;
    PP_GHKF(:,:,k) = P;
    VV_GHKF(:,k) = diag(P);
    EE_GHKF(:,k) = (X(:,k) - m).^2;
  end

  %
  % Calculate RMSE
  %
  ghkf_rmse = sqrt(mean(sum((X(1:2,:)-MM_GHKF(1:2,:)).^2)));
  ME_GHKF = squeeze(PP_GHKF(1,1,:)+PP_GHKF(2,2,:));
  fprintf('Done!\n')

  fprintf('Running smoother...');
  %
  % Smoother
  %
  [SM_GHRTS,SP_GHRTS] = ghrts_smooth(MM_GHKF,PP_GHKF,func_f,Q,...
                          {dt,b0,H0,Gm0,R0},3);
  ghrts_rmse1 = sqrt(mean(sum((X(1:2,:)-SM_GHRTS(1:2,:)).^2)));
  ME_GHRTS = squeeze(SP_GHRTS(1,1,:)+SP_GHRTS(2,2,:));

  SV_GHRTS = zeros(size(m,1),size(Y,2));
  SE_GHRTS = zeros(size(m,1),size(Y,2));
  for k=1:size(Y,2)
    SV_GHRTS(:,k) = diag(SP_GHRTS(:,:,k));
    SE_GHRTS(:,k) = (X(:,k) - SM_GHRTS(:,k)).^2;
  end

  fprintf('Done!\n');

%% Cubature Kalman filter

  fprintf('Running CKF...');

  % Initial values and space for CKF
  m = m0;
  P = P0;
  MM_CKF = zeros(size(m,1),size(Y,2));
  PP_CKF = zeros(size(m,1),size(m,1),size(Y,2));
  VV_CKF = zeros(size(m,1),size(Y,2));
  EE_CKF = zeros(size(m,1),size(Y,2));

  % Filtering with CKF
  for k=1:size(Y,2)
    [m,P] = ckf_predict(m,P,func_f,Q,{dt,b0,H0,Gm0,R0});
    [m,P] = ckf_update(m,P,Y(:,k),func_h,diag([vr va]),{xr,yr});
    MM_CKF(:,k) = m;
    PP_CKF(:,:,k) = P;
    VV_CKF(:,k) = diag(P);
    EE_CKF(:,k) = (X(:,k) - m).^2;
  end

  %
  % Calculate RMSE
  %
  ckf_rmse = sqrt(mean(sum((X(1:2,:)-MM_CKF(1:2,:)).^2)));
  ME_CKF = squeeze(PP_CKF(1,1,:)+PP_CKF(2,2,:));
  fprintf('Done!\n')

  fprintf('Running smoother...');
  %
  % Smoother
  %
  [SM_CRTS,SP_CRTS] = crts_smooth(MM_CKF,PP_CKF,func_f,Q,...
                          {dt,b0,H0,Gm0,R0});
  crts_rmse1 = sqrt(mean(sum((X(1:2,:)-SM_CRTS(1:2,:)).^2)));
  ME_CRTS = squeeze(SP_CRTS(1,1,:)+SP_CRTS(2,2,:));

  SV_CRTS = zeros(size(m,1),size(Y,2));
  SE_CRTS = zeros(size(m,1),size(Y,2));
  for k=1:size(Y,2)
    SV_CRTS(:,k) = diag(SP_CRTS(:,:,k));
    SE_CRTS(:,k) = (X(:,k) - SM_CRTS(:,k)).^2;
  end

  fprintf('Done!\n');

%% Visualize methods

  if ~silent
    aa = 0.02*(-1:0.1:4);
    cx = R0 * cos(aa);
    cy = R0 * sin(aa);
    % Plot EKF estimate
    plot(xr,yr,'ko',cx,cy,'r-',X(1,:),X(2,:),'g-',...
         MM_EKF(1,:),MM_EKF(2,:),'k--');
    legend('Radar','Earth','True','Estimate');
    title('Filtering result with EKF');
    disp(' ');
    disp('Filtering result with EKF is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_1>');
    pause;

    % Error for x_1 with EKF
    semilogy(T,EE_EKF(1,:),'g-',T,VV_EKF(1,:),'b--',...
	     T,SE_ERTS(1,:),'r-',T,SV_ERTS(1,:),'k--');
    legend('EKF-RMSE','EKF-STDE',...
	   'ERTS-RMSE1','ERTS-STDE1');
    title('RMSE of estimating x_1 with EKF and ERTS')
    clc;
    disp('RMSE of estimating x_1 with EKF and ERTS is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_5>');
    pause

    % Error for x_5 with EKF
    semilogy(T,EE_EKF(5,:),'g-',T,VV_EKF(5,:),'b--',...
	     T,SE_ERTS(5,:),'r-',T,SV_ERTS(5,:),'k--');
    legend('EKF-RMSE','EKF-STDE',...
	   'ERTS-RMSE1','ERTS-STDE1');
    title('RMSE of estimating x_5 with EKF and ERTS')
    clc;
    disp('RMSE of estimating x_5 with EKF and ERTS is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_5>');
    pause

    % Plot UKF estimate
    plot(xr,yr,'ko',cx,cy,'r-',X(1,:),X(2,:),'g-',...
         MM_UKF(1,:),MM_UKF(2,:),'k--');
    legend('Radar','Earth','True','Estimate');
    clc;
    disp('Filtering result with UKF is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_1>');
    pause;

    % Error for x_1 with UKF
    semilogy(T,EE_UKF(1,:),'g-',T,VV_UKF(1,:),'b--',...
             T,SE_URTS(1,:),'r-',T,SV_URTS(1,:),'k--');
    legend('UKF-RMSE','UKF-STDE','URTS-RMSE','URTS-STDE');
    title('RMSE of estimating x_1 with UKF and URTS')
    clc;
    disp('RMSE of estimating x_1 with EKF and ERTS is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_5>');
    pause;

    % Error for x_5 with UKF
    semilogy(T,EE_UKF(5,:),'g-',T,VV_UKF(5,:),'b--',...
             T,SE_URTS(5,:),'r-',T,SV_URTS(5,:),'k--');
    legend('UKF-RMSE','UKF-STDE','URTS-RMSE','URTS-STDE');
    title('RMSE of estimating x_5 with UKF and URTS')
    clc;
    disp('RMSE of estimating x_5 with EKF and ERTS is now displayed.');

    % Plot GHKF estimate
    plot(xr,yr,'ko',cx,cy,'r-',X(1,:),X(2,:),'g-',...
         MM_GHKF(1,:),MM_GHKF(2,:),'k--');
    legend('Radar','Earth','True','Estimate');
    clc;
    disp('Filtering result with GHKF is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_1>');
    pause;

    % Error for x_1 with GHKF
    semilogy(T,EE_UKF(1,:),'g-',T,VV_UKF(1,:),'b--',...
             T,SE_URTS(1,:),'r-',T,SV_URTS(1,:),'k--');
    legend('GHKF-RMSE','GHKF-STDE','GHRTS-RMSE','GHRTS-STDE');
    title('RMSE of estimating x_1 with GHKF and GHRTS')
    clc;
    disp('RMSE of estimating x_1 with GHKF and GHRTS is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_5>');
    pause;

    % Error for x_5 with GHKF
    semilogy(T,EE_GHKF(5,:),'g-',T,VV_GHKF(5,:),'b--',...
             T,SE_GHRTS(5,:),'r-',T,SV_GHRTS(5,:),'k--');
    legend('GHKF-RMSE','GHKF-STDE','GHRTS-RMSE','GHRTS-STDE');
    title('RMSE of estimating x_5 with GHKF and GHRTS')
    clc;
    disp('RMSE of estimating x_5 with EKF and GHRTS is now displayed.');

    % Plot CKF estimate
    plot(xr,yr,'ko',cx,cy,'r-',X(1,:),X(2,:),'g-',...
         MM_CKF(1,:),MM_CKF(2,:),'k--');
    legend('Radar','Earth','True','Estimate');
    clc;
    disp('Filtering result with CKF is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_1>');
    pause;

    % Error for x_1 with CKF
    semilogy(T,EE_UKF(1,:),'g-',T,VV_UKF(1,:),'b--',...
             T,SE_URTS(1,:),'r-',T,SV_URTS(1,:),'k--');
    legend('CKF-RMSE','CKF-STDE','CRTS-RMSE','CRTS-STDE');
    title('RMSE of estimating x_1 with CKF and CRTS')
    clc;
    disp('RMSE of estimating x_1 with CKF and CRTS is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_5>');
    pause;

    % Error for x_5 with CKF
    semilogy(T,EE_CKF(5,:),'g-',T,VV_CKF(5,:),'b--',...
             T,SE_CRTS(5,:),'r-',T,SV_CRTS(5,:),'k--');
    legend('CKF-RMSE','CKF-STDE','CRTS-RMSE','CRTS-STDE');
    title('RMSE of estimating x_5 with CKF and CRTS')
    clc;
    disp('RMSE of estimating x_5 with EKF and CRTS is now displayed.');

  end

%% Show RMSE errors for each method

  disp(' ');
  disp('RMS errors:');
  fprintf('EKF-RMSE   = %.6f [%.6f]\n',ekf_rmse,sqrt(mean(ME_EKF)));
  fprintf('ERTS-RMSE  = %.6f [%.6f]\n',eks_rmse1,sqrt(mean(ME_ERTS)));
  fprintf('ETF-RMSE   = %.6f [%.6f]\n',eks_rmse2,sqrt(mean(ME_ETF)));
  fprintf('UKF-RMSE   = %.6f [%.6f]\n',ukf_rmse,sqrt(mean(ME_UKF)));
  fprintf('URTS1-RMSE = %.6f [%.6f]\n',uks_rmse1,sqrt(mean(ME_URTS)));
  fprintf('URTS2-RMSE = %.6f [%.6f]\n',uks_rmse1b,sqrt(mean(ME_URTSb)));
  fprintf('UTF-RMSE   = %.6f [%.6f]\n',uks_rmse2,sqrt(mean(ME_UTF)));

  % Cubature methods
  fprintf('GHKF-RMSE  = %.6f [%.6f]\n',ghkf_rmse,sqrt(mean(ME_GHKF)));
  fprintf('GHRTS-RMSE = %.6f [%.6f]\n',ghrts_rmse1,sqrt(mean(ME_GHRTS)));
  fprintf('CKF-RMSE   = %.6f [%.6f]\n',ckf_rmse,sqrt(mean(ME_CKF)));
  fprintf('CRTS-RMSE  = %.6f [%.6f]\n',crts_rmse1,sqrt(mean(ME_CRTS)));


% timefdetails() - details of the timef() function for time/frequency analysis
%                  of multiple epochs of single-channel event-related data.
%
% Global Description:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% timef() performs normalized time/frequency averaging using either
% FFT-, wavelet-, or multitaper DFT estimates. The wavelets are N-cycle
% Hanning-windowed sinusoids. (Note: To substitute for hanning() windowing
% gauss() or other windowing, replace the timef.m reference to hanning()).
%
% By default, the two image panels of the output plot show, respectively, the
% event-related spectral perturbation (ERSP) and inter-trial coherence (ITC)
% of the input data.
%
% The ERSP:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The ERSP (S. Makeig, Electroencephalogr Clin Neurophysiol 86:283-93, 1993)
% shows mean log event-loced deviations from epoch-mean (or baseline-mean) power
% at each frequency. If bootstrap statistics are computed (via the 'alpha'
% probability input parameter), (time, freq) points with non-significant
% differences from 0 are colored green in the image (but not in the 'ersp'
% output variable - use the output 'erspboot' variable to re-mask the 'ersp'
% output if desired). The baseline mean spectrum removed from each epoch
% is available as output parameter "powbase". Note that log(power) differences
% are equivalent to log(power ratios) between baseline and observed spectra,
% meaning the implicit ERSP model is one of (multiplicative) amplitude modulation
% of the EEG/MEG spectrum by e.g. subcortical and/or intra-cortical influences.
%
% In the default view, the thin bottom panel below the upper (ERSP) image shows
% the ERSP envelope (the most positive and most negative values at each output
% time point). The thin left panel shows the mean (or baseline) log spectrum
% (blue trace). When bootstrap statistics are computed (via the "alpha" argument),
% the left panel (green trace) also shows the bootstrap significant levels (+/-)
% at each frequency.
%
% The ITC:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% (Inter-trial Coherence, cf. Tallon-Baudry et al., "Phase-locking factor")
% The lower panel shows the degree of tendency for spectral phase at each
% (time, freq) point to repeat across the data epochs. If bootstrap statistics
% are computed (as per the 'alpha' input parameter), non-significant points
% are colored green (but again not in the 'itc' output variable - use the
% itcboot output to re-mask if desired in later plotting).
%
% The lower thin panel shows the time-domain average (ERP) of the input data
% (blue) plus a zero-line (green). The average (ERP) is created principally by
% partial phase resetting of the EEG as measured by the ITC. (While phase resetting
% dominates, event-related spectral power changes (as measured by the ERSP)
% may also play a minor role). The thin left panel shows the frequency-mean ITC
% (blue trace) and, if bootstrap statistics are computed, the ITC significance
% limits at each frequency (green trace).
%
% ITC Math Derivation:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% By definition, linear coherence is
%       R=mean(Fxy)/sqrt(mean(abs(Fxx))*mean(abs(Fyy)));
% where Fxy is the cross-spectrum (FxFy*) and Fxx and Fyy the autospectra of
% processes x and y.  We define the phase coherence to be
%       R=mean(Fxy/(abs(Fxx))*abs(Fyy));  % mean of individually normed Fxy
% To derive the ITC, we consider y to be a stimulus-locked process
% such that, at each time and frequency, angle(Fyy) = 0 and abs(Fyy) = 1.
% Thus Pxy = Pxx, and the (complex) inter-trial phase coherence between x and
% the constant stimulus-locked process y is
%       ITC=mean(Pxx/abs(Pxx));
%
% USAGE:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   >> [ersp,itc,powbase,times,freqs,erspboot,itcboot]  ...
%                = timef(data,frames,tlimits,srate,cycles,...
%                              'key1',value1,'key2',value2, ... );
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% NOTE:
% * Left-click on subplots to view and zoom in separate windows (uses axcopy()).
%
% Required Inputs:
%   data        = Single-channel data vector (1,frames*ntrials) (required)
%
%   frames      = Frames per trial                        {750}
%  Here, a frame is a data point (one channel at one time point) and the data
%  is assumed to be composed of concatenated epochs of the same length (frames).
%
%   tlimits     = [mintime maxtime] (ms) Epoch time limits {[-1000 2000]}
%  These should be the starting and ending times of the input epochs.
%
%   srate       = data sampling rate (Hz)                 {250}
%  This is sample rate per channel (i.e., data frames per second).
%
%   cycles      = >0 -> Number of cycles in each analysis wavelet
%                 =0 -> Use FFTs (with constant window length) {0}
%                 If [wavecycles factor] -> wavelet cycles increase with frequency
%                 beginning at wavecyles (0<factor<1; factor=1 -> no increase,
%                 standard wavelets; factor=0 -> fixed epoch length, as in FFT.
%                 OR multitaper decomposition (with 'mtaper').
%  Here, the user chooses either to use the FFT method (fixed window size
%  for all frequencies) or the wavelet DFT method (the data window length
%  depends inversely on the frequency, e.g. '3' means that the data
%  windows will each be three cycles wide (at each frequency). A higher
%  number here (e.g., '5') will narrow the frequency band and widen the
%  time window.
%
% Optional Inter-Irial Coherence Type:
%   'type'      = ['coher'|'phasecoher'] Compute either linear coherence
%                  ('coher') or phase coherence ('phasecoher') also known
%                  as the phase coupling factor           {'phasecoher'}.
%
% Optional Detrending:
%   'detret'    = ['on'|'off'], Detrend data in time.       {'off'}
%   'detrep'    = ['on'|'off'], Detrend data across trials (at each time point
%                 compute the linear trend across the set of ordered trials and
%                 remove it; not that this also subtract the ERP) {'off'}
%
% Optional FFT/DFT Parameters:
%   'winsize'   = If cycles==0: data subwindow length (fastest is 2^n < frames);
%                 If cycles >0: *longest* window length to use. This determines
%                 the lowest output frequency  {default: ~frames/8}
%  When cycles>0, winsize determines the lowest computed frequency. For example,
%  with srate=100 and cycles=3, a winsize of 100 means that 3 cycles must fit
%  within a 1-sec ( 100-sample) window. So, the lowest output frequency is 3 Hz.
%
%  When cycles=0, winsize is the length of data in each FFT window. This may be
%  extended with zeroes (to give more output frequencies) using padratio (below).
%
%  'timesout'  = Number of output times (int<frames-winframes) {200}
%  The number of FFTs or wavelet DFTs computed and plotted.
%
%   'padratio'  = FFT-length/winframes (2^k)                    {2}
%                  Multiplies the number of output frequencies by
%                  dividing their spacing. When cycles==0, frequency
%                  spacing is (low_freq/padratio).
%  This factor multiplies the number of output frequencies. In the FFT method
%  (cycles=0), this is done by zero-padding each analysis window. In the wavelet
%  DFT method (cycles>0), this gives the number of frequencies per Hz.
%
%   'maxfreq'   = Maximum frequency (Hz) to plot (& to output, if cycles>0)
%                  If cycles==0, all FFT frequencies are output. {50}
%   'baseline'  = Spectral baseline end-time (in ms). Use NaN for no baseline
%                 removal{0}
%   'powbase'   = Baseline spectrum to log-subtract. 'baseline' parameter is
%                 ignored if this parameter is used {def|NaN->from data}
%  This is useful only when you want to use a known baseline spectrum (e.g. from
%  another condition) instead of using the actual mean baseline spectrum of the data.
%  Otherwise, leave this out or specify as 'NaN' (not a number).
%
% Optional Multitaper Parameters:
%   'mtaper'    = If [N W], performs multitaper decomposition.
%                  (N is the time resolution and W the frequency resolution;
%                  maximum taper number is 2NW-1). Overwrites 'winsize' and
%                  'padratio'.
%                  If [N W K], forces the use of K Slepian tapers (if possible).
%                  Phase is calculated using standard methods.
%                  The use of mutitaper with wavelets (cycles>0) is not
%                  recommended (as multiwavelets are not implemented).
%                  Uses Matlab functions DPSS, PMTM.   {no multitaper}
%
% Optional Bootstrap Parameters:
%   'alpha'     = If non-0, compute two-tailed bootstrap significance prob.
%                  level. Show non-signif. output values as green.   {0}
%  This optional parameter lengthens the computation time but gives bootstrap
%  estimates of which ERSP and ITC values are significantly different from 0,
%  by setting to 0 (green) all non-significant values in the ERSP and ITC images.
%  Normal values for alpha are 0 ([], or none) -> no bootstrap computation, or
%  0.01 (which should allow about 1% of random images to appear "significant"
%
%   'naccu'     = Number of bootstrap replications to accumulate     {200}
%   'baseboot'  = Bootstrap baseline subtract (0 -> use 'baseline';
%                                                  1 -> use whole trial) {0}
% Optional Scalp Map Plotting Parameters:
%   'topovec'   = Scalp topography (map) to plot                     {none}
%   'elocs'     = Electrode location file for scalp map   {no default}
%                     File should be ascii in format of  >> topoplot example
%  This is an optional map-plotting feature. Given an input map vector
%  (one weight at each channel, and a electrode location file, timef() plots
%  a topoplot()-style 2-d scalp map on the left side of the figure. See
%  >> topoplot example % for the format of the electrode location file.
%
% Other Optional Plotting Parameters:
%   'vert'      = [vector of ms times] -> plot vertical dashed lines  {0 only}
%  Use this to add extra vertical dashed lines at significant epoch times.
%  Time 0 is marked by default.
%
%   'plotersp'  = ['on'|'off'] Plot power spectral perturbations    {'on'}
%   'plotitc'   = ['on'|'off'] Plot inter trial coherence            {'on'}
%   'title'     = Optional figure title                              {none}
%
%   'pboot'     = Bootstrap power limits (e.g., from timef())   {from data}
%   'rboot'     = Bootstrap ITC limits (e.g., from timef())     {from data}
%  These are useful if you want to apply significance limits from another condition
%  to new data. {default|NaN, compute from data}
%
%   'linewidth' = Line width for 'marktimes' traces (thick=2, thin=1) {2}
%   'axesfont'  = Axes text font size                                {10}
%   'titlefont' = Title text font size                               {8}
%
% Outputs:
%        ersp   = Matrix (nfreqs,timesout) of log spectral diffs. from baseline (dB)
%        itc    = Matrix (nfreqs,timesout) of inter-trial phase coherence (range: [0 1])
%   Note that when cycles=0, nfreqs is total number of FFT frequencies, which
%   typically include frequencies higher than maxfreq. When cycles>0, *no* extra
%   (higher) frequencies are computed.
%
%      powbase  = Baseline power spectrum (removed to compute the ERSP)
%        times  = Vector of output times (subwindow centers) (in ms).
%        freqs  = Vector of frequency bin centers (in Hz).
%
%     erspboot  = Matrix (2,nfreqs) of [lower;upper] ERSP significance diffs.
%      itcboot  = Matrix (2,nfreqs) of [lower;upper] ITC thresholds (abs., not diffs)
%  Note that the itcboot lower bound is practically meaningless.
%
%  Plot description:
%    Assuming both 'plotersp' and 'plotitc' options are 'on' (= default). The upper panel
%    presents the data ERSP (Event-Related Spectral Perturbation) in dB, with mean baseline
%    spectral activity (in dB) subtracted. Use "'baseline', NaN" to prevent timef() from
%    removing the baseline. The lower panel presents the data ITC (Inter-Trial Coherence).
%    Click on any plot axes to pop up a new window (using 'axcopy()')
%    -- Upper left marginal panel presents the mean spectrum during the baseline period
%       (blue), and when significance is set, the significance threshold at each frequency
%       (dotted green-black trace).
%    -- The marginal panel under the ERSP image shows the maximum (green) and minimum
%       (blue) ERSP values relative to baseline power at each frequency.
%    -- The lower left marginal panel shows mean ITC across the imaged time range (blue),
%       and when significance is set, the significance threshold (dotted green-black).
%    -- The marginal panel under the ITC image shows the ERP (which is produced by ITC
%       across the data spectral pass band).
%
% Author: Sigurd Enghoff, Arnaud Delorme & Scott Makeig
%          CNL / Salk Institute 1998- | SCCN/INC, UCSD 2002-
%
% See also: crossf() - event-related cross-spectral coherence between two input
%                      time series.
%
% History:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% timef() was coded by Sigurd Enghoff and Scott Makeig at The Salk
% Institute, La Jolla CA in August, 1998, using methods developed
% in Makeig, 1993. Arno Delorme added the multitaper option, recoded
% the function to use 'keyword','parameter' argument pairs, and added the
% 'type' argument with advice from Joern Anemueller at SCCN/Institute for
% Neural Computation, UCSD in early 2002.

% Copyright (C) 8/01/00 Scott Makeig, SCCN/INC/UCSD, scott@sccn.ucsd.edu
%
% 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% 01-25-02 reformated help & license -ad


function W = weight_conversion(W, wcm)
% WEIGHT_CONVERSION    Conversion of weights in input matrix
%
%   W_bin = weight_conversion(W, 'binarize');
%   W_nrm = weight_conversion(W, 'normalize');
%   L = weight_conversion(W, 'lengths');
%   W_fix = weight_conversion(W, 'autofix');
%
%   This function may either binarize an input weighted connection matrix,
%   normalize an input weighted connection matrix, convert an input
%   weighted connection matrix to a weighted connection-length matrix, or
%   fix common connection problems in binary or weighted connection matrices.
%
%       Binarization converts all present connection weights to 1.
%
%       Normalization rescales all weight magnitudes to the range [0,1] and
%   should be done prior to computing some weighted measures, such as the
%   weighted clustering coefficient.
%
%       Conversion of connection weights to connection lengths is needed
%   prior to computation of weighted distance-based measures, such as
%   distance and betweenness centrality. In a weighted connection network,
%   higher weights are naturally interpreted as shorter lengths. The
%   connection-lengths matrix here is defined as the inverse of the
%   connection-weights matrix.
%
%       Autofix removes all Inf and NaN values, remove all self connections
%   (sets all weights on the main diagonal to 0), ensures that symmetric matrices
%   are exactly symmetric (by correcting for round-off error), and ensures that
%   binary matrices are exactly binary (by correcting for round-off error).
%
%   Inputs: W           binary or weighted connectivity matrix
%           wcm         weight-conversion command - possible values:
%                           'binarize'      binarize weights
%                           'normalize'     normalize weights

%                           'lengths'       convert weights to lengths
%                           'autofix'       fixes common weights problems
%
%   Output: W_          output connectivity matrix
%
%
%   Mika Rubinov, U Cambridge, 2012

%   Modification History:
%   Sep 2012: Original
%   Jan 2015: Added autofix feature.

switch wcm
    case 'binarize'
        W=double(W~=0);         % binarize
    case 'normalize'
        W=W./max(abs(W(:)));    % scale by maximal weight
    case 'lengths'
        E=find(W);
        W(E)=1./W(E);           % invert weights
    case 'autofix'
        % clear diagonal
        n = length(W);
        W(1:n+1:end)=0;

        % remove Infs and NaNs
        idx = isnan(W) | isinf(W);
        if any(any(idx));
            W(idx)=0;
        end

        % ensure exact binariness
        U = unique(W);
        if numel(U)>1
            idx_0 = abs(U)<1e-10;
            idx_1 = abs(U-1)<1e-10;
            if all(idx_0 | idx_1)
                W(idx_0)=0;
                W(idx_1)=1;
            end
        end

        % ensure exact symmetry
        if ~isequal(W,W.');
            if max(max(abs(W-W.'))) < 1e-10;
                W=(W+W).'/2;
            end
        end
    otherwise
        error('Unknown weight-conversion command.')
end


% pixCon:  Return a matrix of pixel connections in an image or video
%
% Note:  The mex-file version runs much faster than the m-file.
%
% Usage:
%   function pc = pixCon(sz,mode)

function pc = pixCon(sz,mode)

npix = prod(sz);
ndim = length(sz);
rd = cumprod([1 sz(1:end-1)]);
d = [-rd rd];
%pc = spdiags(ones(npix,2*ndim),d,npix,npix);
nbr = zeros(npix,2*ndim);
dz = cumprod(sz(1:end-1));
for i = 1:ndim
    dnbr = ones(sz);
    [subs{1:ndim}] = deal(':');
    subs{i} = sz(i);
    subsasgn(dnbr,struct('type','()','subs',{subs}),0);
    nbr(:,i) = dnbr(:);
    dnbr = ones(sz);
    [subs{1:ndim}] = deal(':');
    subs{i} = 1;
    subsasgn(dnbr,struct('type','()','subs',{subs}),0);
    nbr(:,ndim+i) = dnbr(:);
end;
pc = spdiags(nbr,d,npix,npix);


function [ r, seed ] = r8vec_uniform_01 ( n, seed )

%*****************************************************************************80
%
%% R8VEC_UNIFORM_01 returns a unit pseudorandom R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real R(N), the vector of pseudorandom values.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  r = zeros ( n, 1 );

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8VEC_UNIFORM_01 - Fatal error!' );
  end

  for i = 1 : n

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + 2147483647;
    end

    r(i) = seed * 4.656612875E-10;

  end

  return
end


function [centroids, idx] = runkMeans(X, initial_centroids, ...
                                      max_iters, plot_progress)
%RUNKMEANS runs the K-Means algorithm on data matrix X, where each row of X
%is a single example
%   [centroids, idx] = RUNKMEANS(X, initial_centroids, max_iters, ...
%   plot_progress) runs the K-Means algorithm on data matrix X, where each
%   row of X is a single example. It uses initial_centroids used as the
%   initial centroids. max_iters specifies the total number of interactions
%   of K-Means to execute. plot_progress is a true/false flag that
%   indicates if the function should also plot its progress as the
%   learning happens. This is set to false by default. runkMeans returns
%   centroids, a Kxn matrix of the computed centroids and idx, a m x 1
%   vector of centroid assignments (i.e. each entry in range [1..K])
%

% Set default value for plot progress
if ~exist('plot_progress', 'var') || isempty(plot_progress)
    plot_progress = false;
end

% Plot the data if we are plotting progress
if plot_progress
    figure;
    hold on;
end

% Initialize values
[m n] = size(X);
K = size(initial_centroids, 1);
centroids = initial_centroids;
previous_centroids = centroids;
idx = zeros(m, 1);

% Run K-Means
for i=1:max_iters

    % Output progress
    fprintf('K-Means iteration %d/%d...\n', i, max_iters);
    if exist('OCTAVE_VERSION')
        fflush(stdout);
    end

    % For each example in X, assign it to the closest centroid
    idx = findClosestCentroids(X, centroids);

    % Optionally, plot progress here
    if plot_progress
        plotProgresskMeans(X, centroids, previous_centroids, idx, K, i);
        previous_centroids = centroids;
        fprintf('Press enter to continue.\n');
        pause;
    end

    % Given the memberships, compute new centroids
    centroids = computeCentroids(X, idx, K);
end

% Hold off if we are plotting progress
if plot_progress
    hold off;
end

end


%--------------------------------------------------------------------------
%   p3(signal,mark)
%--------------------------------------------------------------------------
%   功能：
%   复信号绘制画图工具，用处较多
%   常见于查看时域复信号，傅里叶变换后信号的幅度和相位信息
%--------------------------------------------------------------------------
%   输入：
%           signal              按照列排列复信号
%           mark                绘制标记，例如'r--','k-.'
%--------------------------------------------------------------------------
%   例子：
%   暂无
%--------------------------------------------------------------------------
function p3(signal,mark)
if nargin==2
    plot3(1:size(signal,1),real(signal),imag(signal),mark);grid on
    xlabel('点数');ylabel('实部');zlabel('虚部')
elseif nargin ==1
    plot3(1:size(signal,1),real(signal),imag(signal));grid on
    xlabel('点数');ylabel('实部');zlabel('虚部')
end


%  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
%function dwindowt
%DWINDOWT Unit test for the function DWINDOW.

%	O. Lemoine - March 1996.

N=5000;

% Rectangular window
h=tftb_window(N,'rect');
Dh1=[1;zeros(N-2,1);-1];
Dh2=dwindow(h);
if any(abs(Dh1-Dh2)>sqrt(eps)),
 error('dwindow test 1 failed');
end;

% Hanning window
h=tftb_window(N,'hanning');
Dh1=pi*sin(2*pi*(1:N)'/(N+1))/(N+1);
Dh2=dwindow(h);
if any(abs(Dh1(2:N-1)-Dh2(2:N-1))>sqrt(eps)),
 error('dwindow test 2 failed');
end;

% Bartlett window
h=tftb_window(N,'bartlett');
Dh1=[2*ones(N/2-1,1);1;-1;-2*ones(N/2-1,1)]/N;
Dh2=dwindow(h);
if any(abs(Dh1(2:N-1)-Dh2(2:N-1))>sqrt(eps)*10),
 error('dwindow test 3 failed');
end;

% Papoulis window
h=tftb_window(N,'papoulis');
Dh1=pi*cos(pi*(1:N)'/(N+1))/(N+1);
Dh2=dwindow(h);
if any(abs(Dh1(2:N-1)-Dh2(2:N-1))>sqrt(eps)*10),
 error('dwindow test 4 failed');
end;

% Gaussian window
K=0.02; h=tftb_window(N,'gauss',K);
t=linspace(-1,1,N)';
Dh1=4*log(K)*t.*exp(log(K)*t.^2)/N;
Dh2=dwindow(h);
if any(abs(Dh1(2:N-1)-Dh2(2:N-1))>sqrt(eps)*10),
 error('dwindow test 5 failed');
end;

N=4977;

% Rectangular window
h=tftb_window(N,'rect');
Dh1=[1;zeros(N-2,1);-1];
Dh2=dwindow(h);
if any(abs(Dh1-Dh2)>sqrt(eps)),
 error('dwindow test 6 failed');
end;

% Hanning window
h=tftb_window(N,'hanning');
Dh1=pi*sin(2*pi*(1:N)'/(N+1))/(N+1);
Dh2=dwindow(h);
if any(abs(Dh1(2:N-1)-Dh2(2:N-1))>sqrt(eps)),
 error('dwindow test 7 failed');
end;

% Papoulis window
h=tftb_window(N,'papoulis');
Dh1=pi*cos(pi*(1:N)'/(N+1))/(N+1);
Dh2=dwindow(h);
if any(abs(Dh1(2:N-1)-Dh2(2:N-1))>sqrt(eps)*10),
 error('dwindow test 8 failed');
end;

% Gaussian window
K=0.02; h=tftb_window(N,'gauss',K);
t=linspace(-1,1,N)';
Dh1=4*log(K)*t.*exp(log(K)*t.^2)/N;
Dh2=dwindow(h);
if any(abs(Dh1(2:N-1)-Dh2(2:N-1))>sqrt(eps)*10),
 error('dwindow test 9 failed');
end;


% In this version of this function we try to return a (NxNxN) cube, while still
% keeping the aspect ratio

function [voxel_model] = pointcloud_to_voxels_3(model,N,bbox,voxelization_type,binaryMax)

if(isempty(model.x))
    warning('empty model passed to this function!');
    voxel_model = zeros([N N N]);

    return;
end

if(~exist('bbox','var'))
    bbox = [];
end

if(~exist('voxelization_type','var'))
    voxelization_type = 'binary';
end

if(~exist('binaryMax','var'))
    binaryMax = 1; %This is only for backward compatibilty. Otherwise it should be 255 to be compatible with 'intensity' type, in addition to other benefits!
end

[X,Y,Z] = model_to_components(model);

%--- Obtain the boundaries of the object, taking into account the bbox if
%needed
if(isempty(bbox))
    mX = min(X);
    MX = max(X);
    mY = min(Y);
    MY = max(Y);
    mZ = min(Z);
    MZ = max(Z);
else
    %-- in case of a bounding box, we only need to check the 4 corners
    x1 = bbox(1);
    y1 = bbox(2);
    z1 = bbox(3);
    w = bbox(4);
    h = bbox(5);
    d = bbox(6);
    x2 = x1+w;
    y2 = y1+h;
    z2 = z1+d;
    phi = bbox(7);

    cornersX = [x1 x1 x2 x2];
    cornersY = [y1 y2 y1 y2];
    xc = mean([x1,x2]);
    yc = mean([y1,y2]);

    % rotate the coordinates around the center
    cornersX = cornersX-repmat(xc,1,size(cornersX,2));
    cornersY = cornersY-repmat(yc,1,size(cornersX,2));
    rotated = [cosd(phi) -sind(phi); sind(phi) cosd(phi)]*[cornersX;cornersY];
    cornersX = rotated(1,:)+repmat(xc,1,size(cornersX,2));
    cornersY = rotated(2,:)+repmat(yc,1,size(cornersY,2));

    %-- now pick the limits of the corner coordinates
    mX = min(cornersX);
    MX = max(cornersX);
    mY = min(cornersY);
    MY = max(cornersY);
    mZ = min([z1,z2]); %z is interpreted differently, as the rotation is only around z
    MZ = max([z1,z2]);

end

maxmax = max([MX-mX,MY-mY,MZ-mZ]);
if(maxmax == 0) maxmax = 1; end %This happens only when there's a single point in the scene (the empty scene case has been taken care of before);

N1 = round(N*(MX-mX)/maxmax);
N2 = round(N*(MY-mY)/maxmax);
N3 = round(N*(MZ-mZ)/maxmax);

% This happens when the input point cloud is in in fact less than 3D
if(~N1), N1 = 1;end;
if(~N2), N2 = 1;end;
if(~N3), N3 = 1;end;

voxel_model = zeros(N1,N2,N3);

ax = 1/(MX-mX)*(N1-1);
bx = -mX;
ay = 1/(MY-mY)*(N2-1);
by = -mY;
az = 1/(MZ-mZ)*(N3-1);
bz = -mZ;

% This happens when the input point cloud is in fact less than 3D
if isnan(ax), ax = 0; end;
if isnan(ay), ay = 0; end;
if isnan(az), az = 0; end;

X2 = round(ax*(X+bx))+1;
Y2 = round(ay*(Y+by))+1;
Z2 = round(az*(Z+bz))+1;

switch(voxelization_type )
    case 'binary'
        voxel_model(sub2ind(size(voxel_model),X2,Y2,Z2)) = binaryMax ;
    case 'density'
        [C,ia,ic] = unique([X2,Y2,Z2],'rows');
        h = hist(ic,1:max(ic));
        ind = sub2ind(size(voxel_model),C(:,1),C(:,2),C(:,3));
        voxel_model(ind) = h;

        %max normalization
        voxel_model = voxel_model ./ max(voxel_model(:)) * 255;
    otherwise
        error('voxelization type not supported');
end

% Add the required symmetric zero padding to make it a cube
Nzx = floor( (N - N1)/2 );
Nzy = floor( (N - N2)/2 );
Nzz = floor( (N - N3)/2 );
final = zeros(N,N,N);
final(Nzx+1:Nzx+N1 , Nzy+1:Nzy+N2 ,Nzz+1:Nzz+N3) = voxel_model;
voxel_model = final;


function [x, infos] = convex_mu_nmf(V, rank, in_options)
% (Kernel) Convex multiplicative update for non-negative matrix factorization ((Kernel-)Convex-MU-NMF).
%
% The problem of interest is defined as
%
%  (standard)   min || VWH - V ||_F^2,
%               where
%               {W, H} >= 0, and
%
%  (kernel)     min || K(V,V) WH - K(V,V) ||_F^2,
%               where
%               {W, H} >= 0, and K(V,V) is a kernel matrix.
%
%
% Given a V with mixed-signs, factorized non-negative matrices {W, H} are calculated.
%
%
% Inputs:
%       matrix      V
%       rank        rank
%       in_options
%           sub_mode: 'std' (default) or 'kernel'
%           kernel  : kernel type (rbf (default), polynomial, linear, sigmoid)
%
% Output:
%       x           solution of x
%       infos       information
%
% References:
%       C. Ding, T. Li, and M.I. Jordan,
%       "Convex and semi-nonnegative matrix factorizations,"
%       IEEE Transations on Pattern Analysis and Machine Intelligence,
%       vol. 32, no. 1, pp. 45-55, 2010.
%
%       T. Li and C. Ding,
%       "The Relationships Among Various Nonnegative Matrix Factorization Methods for Clustering,"
%       International Conference on Data Mining,
%       2006.
%
%       Y. Li and A. Ngom,
%       "A New Kernel Non-Negative Matrix Factorization and Its Application in Microarray Data Analysis,"
%       CIBCB,
%       2012.
%
%
% This file is part of NMFLibrary
%
% This file has been ported from
% convexnmfrule.m and kernelconvexnmf.m at https://sites.google.com/site/nmftool/home/source-code
% by Yifeng Li.
%
%%%%
% Copyright (C) <2012>  <Yifeng Li>
%
% 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 3 of the License, or
% 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, see <http://www.gnu.org/licenses/>.
%
% Contact Information:
% Yifeng Li
% University of Windsor
% li11112c@uwindsor.ca; yifeng.li.cn@gmail.com
% May 01, 2011
%%%%
%
%
% This file was originally created by Graham Grindla.
%
% 2010-01-14 Graham Grindlay (grindlay@ee.columbia.edu)
%
% Copyright (C) 2008-2028 Graham Grindlay (grindlay@ee.columbia.edu)
%
% 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 3 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, see <http://www.gnu.org/licenses/>.
%
%
% Ported by H.Kasai on June 30, 2022
%
% Change log:
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.sub_mode = 'std';
    local_options.kernel ='rbf';
    local_options.kernel_param = [];
    local_options.special_nmf_cost = @(V, W, H, R, options) convex_mu_nmf_cost_func(V, W, H, R, options);
    local_options.special_init_factors = @(V, rank, wh_flag, r_flag, options) convex_mu_initialize(V, rank, wh_flag, r_flag, options);

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    W = init_factors.W;
    H = init_factors.H;

    % initialize
    epoch = 0;
    grad_calc_count = 0;

    % initialize for this algorithm
    if strcmp(options.sub_mode, 'std')
        Ak = V' * V;
        X = V;
        options.name = 'std';
    elseif strcmp(options.sub_mode, 'kernel')
        Ak = computeKernelMatrix(V, V, options);
        X = Ak;
        options.name = sprintf('kernel-%s', options.kernel);
    else
        error('Invalid sub_mode')
    end
    method_name = sprintf('Convex-MU (%s)', options.name);

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    Ap = (abs(Ak) + Ak) ./ 2;
    An = (abs(Ak) - Ak) ./ 2;

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(X, W, H, [], options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('Convex-MU (%s): Epoch = 0000, cost = %.16e, optgap = %.4e\n', options.name, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        ApW = Ap * W;
        AnW = An * W;
        WH  = W * H;

        % update H
        H = H .* sqrt((ApW' + AnW' * WH) ./ (AnW' + ApW' * WH));
        H = max(H, eps);
        HHt = H * H';

        % update W
        W = W .* sqrt((Ap * H' + AnW * HHt) ./ (An * H' + ApW * HHt));
        W = max(W, eps);

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(X, W, H, [], options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = W;
    x.H = H;

end

function res = convex_mu_nmf_cost_func(V, W, H, R, options)

    res = norm(V - V * W * H, 'fro');

end

function [init_factors, init_factors_opts] = convex_mu_initialize(V, rank, wh_flag, r_flag, options)

    init_factors_opts = [];

    [~, n] = size(V);

    if wh_flag
        H = rand(rank, n);
        W = H' * diag(1./sum(H,2)');
    else
        W = options.x_init.W;
        H = options.x_init.H;
    end

    init_factors.W = W;
    init_factors.H = H;
    init_factors.R = zeros(size(V));

end


function BrainNet=tHOFC(BOLD)
% Topographical similarity-based high-order FC network construction
%
% Input:
% BOLD. A cell array with size of N x 1, each cell is a matrix of BOLD signals (#time points x #ROIs) from one of N subject
%       Each subject may have different #time points but should have the same #ROIs
%
% Output:
% BrainNet. High-order brain networks (#ROIs x #ROIs x #Subjects)
%
% By Han Zhang, hanzhang@med.unc.edu
% IDEA lab https://www.med.unc.edu/bric/ideagroup
% Department of Radiology and BRIC, UNC Chapel Hill
%
% Cite: 1. Zhang, H., Chen, X., Shi, F., Li, G., Kim, M., Giannakopoulos, P., Haller, S., Shen, D., Jun 28, 2016.
%          Topographic Information based High-Order Functional Connectivity and its Application in Abnormality Detection
%          for Mild Cognitive Impairment, Journal of Alzheimer's Disease. In press. DOI: 10.3233/JAD-160092.
%       2. Zhang, H., Chen, X., Zhang, Y., Shen, D., Test-retest reliability of �high-order� functional connectivity
%          in young healthy adults. Frontiers in Neuroscience, In Press. DOI: 10.3389/fnins.2017.00439.

[nTime,nROI]=size(BOLD{1});
nSubj=length(BOLD);
BrainNet=zeros(nROI,nROI,nSubj,'single');

% Compute low-order FC network
pc=PC(BOLD);   % Pearson's correlation-based network
pc=atanh(pc);       % Fisher z transformation

% Compute topographical similarity which defines high-order FC
for ns=1:nSubj
    tempNet=zeros(nROI,nROI);
    for i=1:nROI
        for j=(i+1):nROI
            temp1=pc(:,i,ns);
            temp2=pc(:,j,ns);
            temp1([i,j])=[];                    % ignore self connections
            temp2([i,j])=[];                    % ignore self connections
            tempNet(i,j)=corr(temp1,temp2);     % only upper triangle
        end
    end
    BrainNet(:,:,ns)=tempNet+tempNet';          % form symmetric adjancency matrix (main diag are zeros)
end


function wrench = fromParametrizationToForces(xi, Config)

    % delta feet size
    delta_x   =  (Config.feet_size(2,2) - Config.feet_size(2,1))/2;
    delta_x_0 = -(Config.feet_size(2,2) + Config.feet_size(2,1))/2;
    delta_y   =  (Config.feet_size(1,2) - Config.feet_size(1,1))/2;
    delta_y_0 =  (Config.feet_size(1,2) + Config.feet_size(1,1))/2;

    % linear forces
    Fz = exp(xi(3)) + Config.fZmin;
    Fx = sqrt(Config.forceFrictionCoefficient)*tanh(xi(1))*Fz/sqrt(1 + tanh(xi(2))^2);
    Fy = sqrt(Config.forceFrictionCoefficient)*tanh(xi(2))*Fz/sqrt(1 + tanh(xi(1))^2);

    % moments
    Mz = Config.torsionalFrictionCoefficient*tanh(xi(6))*Fz;
    Mx = (delta_x*tanh(xi(4)) + delta_x_0)*Fz;
    My = (delta_y*tanh(xi(5)) + delta_y_0)*Fz;

    % compute the final wrench
    wrench = [Fx; Fy; Fz; Mx; My; Mz];
end


% Image super-resolution using sparse representation
% Example code
%
% Nov. 2, 2007. Jianchao Yang
% IFP @ UIUC
%
% Revised version. April, 2009.
%
% Reference
% Jianchao Yang, John Wright, Thomas Huang and Yi Ma. Image superresolution
% via sparse representation of raw image patches. IEEE Computer Society
% Conference on Computer Vision and Pattern Recognition (CVPR), 2008.
%
% For any questions, email me by jyang29@illinois.edu

clear all;
clc;

addpath('Solver');
addpath('Sparse coding');

% =====================================================================
% specify the parameter settings

patch_size = 3; % patch size for the low resolution input image
overlap = 1; % overlap between adjacent patches
lambda = 0.1; % sparsity parameter
zooming = 3; % zooming factor, if you change this, the dictionary needs to be retrained.

tr_dir = 'Data/training'; % path for training images
skip_smp_training = true; % sample training patches
skip_dictionary_training = true; % train the coupled dictionary
num_patch = 50000; % number of patches to sample as the dictionary
codebook_size = 1024; % size of the dictionary

regres = 'L1'; % 'L1' or 'L2', use the sparse representation directly, or use the supports for L2 regression
% =====================================================================
% training coupled dictionaries for super-resolution

if ~skip_smp_training,
    disp('Sampling image patches...');
    [Xh, Xl] = rnd_smp_dictionary(tr_dir, patch_size, zooming, num_patch);
    save('Data/Dictionary/smp_patches.mat', 'Xh', 'Xl');
    skip_dictionary_training = false;
end;

if ~skip_dictionary_training,
    load('Data/Dictionary/smp_patches.mat');
    [Dh, Dl] = coupled_dic_train(Xh, Xl, codebook_size, lambda);
    save('Data/Dictionary/Dictionary.mat', 'Dh', 'Dl');
else
    load('Data/Dictionary/Dictionary.mat');
end;

% =====================================================================
% Process the test image

fname = 'Data/Test/1.bmp';
testIm = imread(fname); % testIm is a high resolution image, we downsample it and do super-resolution

if rem(size(testIm,1),zooming) ~=0,
    nrow = floor(size(testIm,1)/zooming)*zooming;
    testIm = testIm(1:nrow,:,:);
end;
if rem(size(testIm,2),zooming) ~=0,
    ncol = floor(size(testIm,2)/zooming)*zooming;
    testIm = testIm(:,1:ncol,:);
end;

imwrite(testIm, 'Data/Test/high.bmp', 'BMP');

lowIm = imresize(testIm,1/zooming, 'bicubic');
imwrite(lowIm,'Data/Test/low.bmp','BMP');

interpIm = imresize(lowIm,zooming,'bicubic');
imwrite(uint8(interpIm),'Data/Test/bb.bmp','BMP');

% work with the illuminance domain only
lowIm2 = rgb2ycbcr(lowIm);
lImy = double(lowIm2(:,:,1));

% bicubic interpolation for the other two channels
interpIm2 = rgb2ycbcr(interpIm);
hImcb = interpIm2(:,:,2);
hImcr = interpIm2(:,:,3);

% ======================================================================
% Super-resolution using sparse representation

disp('Start superresolution...');

[hImy] = L1SR(lImy, zooming, patch_size, overlap, Dh, Dl, lambda, regres);

ReconIm(:,:,1) = uint8(hImy);
ReconIm(:,:,2) = hImcb;
ReconIm(:,:,3) = hImcr;

nnIm = imresize(lowIm, zooming, 'nearest');
figure, imshow(nnIm);
title('Input image');
pause(1);
figure, imshow(interpIm);
title('Bicubic interpolation');
pause(1)

ReconIm = ycbcr2rgb(ReconIm);
figure,imshow(ReconIm,[]);
title('Our method');
imwrite(uint8(ReconIm),'Data/Test/L1SR.bmp','BMP');


%{
    Demonstration of fault detection and fault diagnosis using KPCA.
%}
clc
clear all
close all
addpath(genpath(pwd))

load('.\data\TE.mat', 'trainData', 'testData')
kernel = Kernel('type', 'gaussian', 'gamma', 1/128^2);

parameter = struct('numComponents', 0.65, ...
                   'kernelFunc', kernel,...
                   'diagnosis', [300, 500]);

% build a KPCA object
kpca = KernelPCA(parameter);
% train KPCA model
kpca.train(trainData);
% test KPCA model
results = kpca.test(testData);

% Visualization
kplot = KernelPCAVisualization();
kplot.cumContribution(kpca)
kplot.trainResults(kpca)
kplot.testResults(kpca, results)
kplot.diagnosis(results)


function [yr] = us2yr(us)
% Convert time from microseconds to Julian years (365.25 days).
% Chad Greene 2012
yr = us*3.168808781403e-14 ;


function [Y,W,SetupStruc] = Process_LCMV(s,Transfer,SetupStruc)
K = SetupStruc.LCMV.K;
hop = SetupStruc.LCMV.hop;
win = hanning(K,'periodic');
win = win/sqrt(sum(win(1:hop:K).^2));
SetupStruc.LCMV.win = win;  % Preserve 'win' in 'SetupStruc'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = size(s,2);
for i = 1:N
    X(:,:,i) = fft(enframe(s(:,i),win,hop)');
end
frame_N = size(X,2);
K_m = K/2+1;
Num = size(Transfer,3);
Y = zeros((frame_N-1)*hop+K,Num);
Y_f = zeros(size(X,1),size(X,2),Num);
WNG =zeros(K_m,Num);
%%%%%%%%%%%%%%%%%%%%%%%%%% Obtain processing matrix 'W'
theta = 10^-4;
W = zeros(Num,N,K_m);
for i = 2:K_m
    X_f = permute(X(i,:,:),[3 2 1]);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%% LCMV processing, imposing the linear constraints to the angle of sources
    Steer = permute(Transfer(i,:,:),[2 3 1]);
    R = X_f*X_f'/frame_N;
    if rcond(R)<theta
        R = R+eye(N)*min(diag(R))*theta;
    end
    R_denominator = Steer'/R*Steer;
    if rcond(R_denominator)<theta
        R_denominator = R_denominator+eye(Num)*min(diag(R_denominator))*theta;
    end
    W_f = R\Steer/R_denominator;
    W_f = W_f';
    W(:,:,i) = W_f;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    WNG(i,:) = -10*log10(diag(W_f*W_f')');
    Y_ = W_f*X_f;
    Y_f(i,:,:) = Y_.';
    if(i~=K_m)
        Y_f(K+2-i,:,:) = Y_';
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Recover signals
if(K/hop==2)
    win = ones(K,1);
end
for i = 1:Num
    Y(:,i) = overlapadd(real(ifft(Y_f(:,:,i)))',win,hop);
end
% autoPlot(WNG,'LCMV');
return;


function rr = FTS(rel_data, wins, up)
%FTS calculates the frequency spectrum of a signal using the FFT, and finds
% the RR.
%	            FTS(rel_data, wins, up)
%
%	Inputs:
%       rel_data    .t  vector of times
%                   .v  vector of resp Sig values
%                   .fs sampling freq
%       up              universal parameters structure
%       wins        .t  vector of start times
%
%	Outputs:
%       rr          .t  vector of times of estimated RRs
%                   .v  vector of estimated RRs
%                   .f  vector of freqs of power spectrum
%                   .p  vector of powers of power spectrum
%

%% Setup

downsample_freq = up.paramSet.fft_resample_freq;    % it would be worth changing this - it changes the answer (try 1 Hz e.g.)
true_fs = rel_data.fs;

%% Cycle through windows
rr.t = mean([wins.t_start(:)' ; wins.t_end(:)']); rr.t = rr.t(:);
rr.v = nan(length(rr.t),1);
rr.p = cell(length(wins.t_start),1);
rr.f = cell(length(wins.t_start),1);

for win_no = 1 : length(wins.t_start)

    % extract relevant data
    rel_els = find(rel_data.t >= wins.t_start(win_no) & rel_data.t < wins.t_end(win_no));
    data.v = rel_data.v(rel_els);
    data.t = rel_data.t(rel_els);

    good_els = ~isnan(data.v);
    data.v = data.v(good_els);
    data.t = data.t(good_els);

    % Downsample
    data.filt.t = downsample(data.t, true_fs/downsample_freq);
    data.filt.v = decimate(data.v, true_fs/downsample_freq);
    data.filt.v = detrend(data.filt.v);

    data.filt.v = data.filt.v(:);
    data.filt.t = data.filt.t(:);

    %% Now that we have a processed PPG-waveform, i.e. the 'respiratory signal', lets find the FFT

    % Find FFT
    WINLENGTH = length(data.filt.v);
    NFFT = 2^nextpow2(WINLENGTH);
    HAMMWIN = hamming(WINLENGTH);
    HAMMWIN = HAMMWIN(:);
    f_nyq = downsample_freq/2;
    FREQS = f_nyq.*linspace(0, 1, NFFT/2+1);            % Array of correspondent FFT bin frequencies, in BR (RPM)
    WINDATA = detrend(data.filt.v);                      % Remove the LSE straight line from the data
    WINDATA = WINDATA .* HAMMWIN;
    myFFT = fft(WINDATA, NFFT);
    myFFT = myFFT(1 : NFFT/2 + 1);
    myFFT = 2.*abs(myFFT/NFFT);
    psdx = (1/(downsample_freq*NFFT)) * abs(myFFT).^2;
    psdx(2:end-1) = 2*psdx(2:end-1);
    clear myFFT WINDATA f_nyq HAMMWIN NFFT WINLENGTH

    data.power = 10*log10(psdx); data.power = data.power(:);
    data.freqs = FREQS; data.freqs = data.freqs(:);

    % Find spectral peak
    [rr.v(win_no), rr.f{win_no}, rr.p{win_no}] = find_spectral_peak(data, up);

    clear data

end

end


function m = max(T)

[e,v] = eig(T);
m = max(diag(v));


function viewFourthOrderTensor(varargin)

% function viewFourthOrderTensor(C,numDigits,fontSizeIm,fontSize)

%% Parse input

numDigits=[];
fontSizeIm=[];
fontSize=[];
toleranceLevel=eps(1);
switch nargin
    case 1
        C=varargin{1};
        toleranceLevel=eps(1);
    case 2
        C=varargin{1};
        numDigits=varargin{2};
    case 3
        C=varargin{1};
        numDigits=varargin{2};
        fontSizeIm=varargin{3};
    case 4
        C=varargin{1};
        numDigits=varargin{2};
        fontSizeIm=varargin{3};
        fontSize=varargin{4};
    case 5
        C=varargin{1};
        numDigits=varargin{2};
        fontSizeIm=varargin{3};
        fontSize=varargin{4};
        toleranceLevel=varargin{5};
end

if isempty(C)
    I=eye(3,3); %The 2nd order identity tensor
    C=dyadicProduct(I,I,1); %A type of 4th order identity tensor
end

if isempty(numDigits)
    numDigits=3;
end

if isempty(fontSizeIm)
    fontSizeIm=15;
end

if isempty(fontSize)
    fontSize=15;
end

%%
% Plot settings
faceAlpha=0.5;

%%

ind_C_all=1:numel(C);
[I,J,K,L]=ind2sub(size(C),ind_C_all);

kronD_IJ=(I==J);
kronD_KL=(K==L);

switch class(C)
    case 'sym'
        CV=sym(zeros(6,6));
        logicSym=1;
    otherwise
        CV=zeros(6,6);
        logicSym=0;
end

%Create mapping indices
P=I.*kronD_IJ+(1-kronD_IJ).*(9-I-J);
Q=K.*kronD_KL+(1-kronD_KL).*(9-K-L);

%Convert to linear indices
[ind_CV]=sub2ind(size(CV),P,Q);

%Get the unique 36 entries
[~,indUni,~]=unique(ind_CV,'first');
ind_CV_uni=ind_CV(indUni);

ind_C_uni=ind_C_all(indUni);

CV(ind_CV_uni)=C(ind_C_uni);

%%

%Derive 9x9 matrix representation
[CM]=fourthOrderMat(C);

if logicSym
    symbolicVariableSet=symvar(CM);
    C_dummy=double(subs(CM,symbolicVariableSet,1:numel(symbolicVariableSet)));
    logicEntry=abs(C_dummy)>toleranceLevel;
    C_dummy_V=double(subs(CV,symbolicVariableSet,1:numel(symbolicVariableSet)));
    logicEntry_V=abs(C_dummy_V)>toleranceLevel;
else
    logicEntry=abs(CM)>toleranceLevel;
    logicEntry_V=abs(CV)>toleranceLevel;
end

%%

C_V=zeros(size(C));
C_V(ind_C_uni)=ind_CV_uni;
[CM_V]=fourthOrderMat(C_V);
logicEntry_CV=CM_V>eps(CM_V);

%%
hf1=cFigure;
title('9x9 array mapping of $4^{th}$ order tensor','fontSize',fontSize,'Interpreter','LATEX');
xlabel('$q=3(j-1)+l$','fontSize',fontSize,'Interpreter','LATEX');
ylabel('$p=3(i-1)+k$','fontSize',fontSize,'Interpreter','LATEX');

if logicSym
    [Fp,Vp,Cp]=ind2patch(logicEntry,C_dummy,'sk');
    gpatch(Fp,Vp,Cp,0.25.*ones(1,3),faceAlpha);
else
    [Fp,Vp,Cp]=ind2patch(logicEntry,CM,'sk');
    gpatch(Fp,Vp,Cp,0.25.*ones(1,3),faceAlpha);
end
colormap gjet;
if ~logicSym
    colorbar;
end

m=zeros(3,3);
[Fp,Vp,~]=ind2patch(true(size(m)),m,'sk');
Vp(:,[1 2])=3*Vp(:,[1 2])-1;
patch('Faces',Fp,'Vertices',Vp,'FaceColor','none','EdgeColor','k','lineWidth',5);

[Fp,Vp,~]=ind2patch(logicEntry_CV,logicEntry_CV,'sk');
patch('Faces',Fp,'Vertices',Vp,'FaceColor',0.25.*ones(1,3),'EdgeColor','k','lineWidth',1,'FaceAlpha',0.25);

image_numeric(CM,hf1,numDigits,fontSizeIm,'k','tex');

view(2); axis tight; axis ij; axis square;
set(gca,'FontSize',fontSize);
drawnow;

%%

hf2=cFigure;
title('Voigt array mapping of $4^{th}$ order tensor','fontSize',fontSize,'Interpreter','LATEX');
xlabel('$q=k\delta_{kl}+(1-\delta_{kl})(9-k-l)$','fontSize',fontSize,'Interpreter','LATEX');
ylabel('$p=i\delta_{ij}+(1-\delta_{ij})(9-i-j)$','fontSize',fontSize,'Interpreter','LATEX');

if logicSym
    [Fp,Vp,Cp]=ind2patch(logicEntry_V,C_dummy_V,'sk');
    gpatch(Fp,Vp,Cp,0.25.*ones(1,3),faceAlpha);
else
    [Fp,Vp,Cp]=ind2patch(logicEntry_V,CV,'sk');
    gpatch(Fp,Vp,Cp,0.25.*ones(1,3),faceAlpha);
end
colormap gjet;
if ~logicSym
    colorbar;
end

m=zeros(2,2);
[Fp,Vp,~]=ind2patch(true(size(m)),m,'sk');
Vp(:,[1 2])=3*Vp(:,[1 2])-1;
patch('Faces',Fp,'Vertices',Vp,'FaceColor','none','EdgeColor','k','lineWidth',5);

image_numeric(CV,hf2,numDigits,fontSizeIm,'k','tex');
view(2); axis tight; axis ij; axis square;
set(gca,'FontSize',fontSize);
drawnow;

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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 3 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, see <http://www.gnu.org/licenses/>.


%% DCM2euler
% Below is a demonstration of the features of the |DCM2euler| function

%%
clear; close all; clc;

%% Syntax
% |[a]=DCM2euler(Q);|

%% Description
% This function is the inverse of the euler2DCM function. The Euler angles
% |a| are derived based on the input rotatin tensor |Q|.

%% Examples
%

%%
% Plot settings
fontSize=25;

%% Retrieving the Euler andles from a rotation tensor

%%
% Get example patch data
[F,V]=parasaurolophus;

%%
% Defining sets of true Euler angles for X, Y and Z axis rotation
a_true=[0.25*pi 0.25*pi -0.25*pi]

%%
% Use |euler2DCM| function to define the rotation tensor
[Q]=euler2DCM(a_true);

%%
% Use |DCM2euler| to retrieve the Euler angles
a_fit=DCM2euler(Q)

%% Handling symbolic expressions

try
    syms a b c

    a_true=[a b c]
    Q=euler2DCM(a_true);

    a_fit=DCM2euler(Q)

catch
    warning('Symbolic toolbox likely missing')
end

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>
%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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 3 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, see <http://www.gnu.org/licenses/>.


function error_frobenius = c8mat_is_symmetric ( m, n, a )

%*****************************************************************************80
%
%% C8MAT_IS_SYMMETRIC checks a complex matrix for symmetry.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of A.
%
%    Input, complex A(M,N), the matrix.
%
%    Output, real ERROR_FROBENIUS, measures the Frobenius norm of ( A - A' ).
%
  if ( m ~= n )
    error_frobenius = r8_huge ( );
    return
  end

  error_frobenius = sqrt ( sum ( sum ( ( abs ( a - a' ) ).^2 ) ) );

  return
end


function s=sprintsi(x,d,w)
%SPRINTSI Print X with SI multiplier S=(X,D,W)
% D is number of decimal places (+ve) or significant digits (-ve) [default=-3]
% |W| is total width including multiplier
% if W<=0 then trailing 0's will be eliminated
%
% Example: sprintsi(2345,-2) gives '2.3 k'

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

if nargin<3 w=0; end;
if nargin<2 d=-3; end;
f='afpnum kMGT';
e=max(-18,min(12,floor(log10(abs(x)))));
k=floor(e/3);
dp=max([0 d 3*k-d-e-1]);
if w<=0 & dp
   w=abs(w);
   dp=max(find([1 mod(mod(round(x*10^(dp-3*k)),10^dp),10.^(dp:-1:1))]))-1;
end
if(k)
   s=sprintf(sprintf('%%%d.%df %c',max(w-2,0),dp,f(k+7)),x*1e-3^k);
else
   s=sprintf(sprintf('%%%d.%df ',max(w-1,0),dp),x*1e-3^k);
end


function value = month_length_julian ( y, m )

%*****************************************************************************80
%
%% MONTH_LENGTH_JULIAN returns the number of days in a Julian month.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year in which the month occurred.
%
%    Input, integer M, the number of the month.
%
%    Output, integer VALUE, the number of days
%    in the month.
%
  mdays = [ 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 ];
%
%  Copy the input.
%
  m2 = m;
  y2 = y;
%
%  Check the input.
%
  [ y2, m2, ierror ] = ym_check_julian ( y2, m2 );

  if ( ierror ~= 0 )
    value = 0;
    return
  end
%
%  Get the number of days in the month.
%
  value = mdays(m2);
%
%  If necessary, add 1 day for February 29.
%
  if ( m2 == 2 && year_is_leap_julian ( y2 ) )
    value = value + 1;
  end

  return
end


function [km] = mi2km(mi)
% Convert length from miles to kilometers.
% Chad A. Greene 2012
km = mi*1.609344;