function outputImage = imfill(this, locations, nD)
% Fills image slice-wise; mimicks imfill in matlab functionality
%
%   Y = MrImage()
%   filledY = Y.imfill(locations, nD)
%
% This is a method of class MrImage.
%
% IN
%   locations
%           array of 2D coordinates or string 'holes' to fill all holes
%   nD      dimensionality to perform operation
%           '2d' = slicewise application, separate 2d images
%           '3d' = as volume
%
% OUT
%   outputImage
%           MrImage where data matrix is inflated
%
% EXAMPLE
%   Y = MrImage();
%   filledY = Y.imfill()
%   filledY = Y.imfill('holes')
%
%
%   See also MrImage imfill MrImage.imerode perform_unary_operation

% Author:   Saskia Klein & Lars Kasper
% Created:  2014-08-04
% Copyright (C) 2014 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public Licence (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.

% update geometry-header with flipping left-right
if nargin < 2
    locations = 'holes';
end

if nargin < 3
    nD = '2d';
end

if isreal(this)
    outputImage = this.perform_unary_operation(...
        @(x) imfill(x, locations), nD);
else
    outputImage = this.abs.perform_unary_operation(...
        @(x) imfill(x, locations), nD);
end


function test_suite=test_align
% tests for cosmo_align
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #
    try % assignment of 'localfunctions' is necessary in Matlab >= 2016
        test_functions=localfunctions();
    catch % no problem; early Matlab versions can use initTestSuite fine
    end
    initTestSuite;

function test_align_basics
    x=cosmo_synthetic_dataset('type','meeg','size','big');
    y=cosmo_slice(x,x.fa.chan<=6,2);
    orig_ds=cosmo_dim_transpose(y,'time');

    [nsamples,nfeatures]=size(orig_ds.samples);

    ds1=cosmo_slice(orig_ds,randperm(nsamples));
    ds2=cosmo_slice(orig_ds,randperm(nsamples));

    [mp,pm]=cosmo_align({ds1.sa.targets,ds1.sa.chunks,ds1.sa.time},...
                            {ds2.sa.targets,ds2.sa.chunks,ds2.sa.time});
    assertEqual(cosmo_slice(ds1,mp),ds2);
    assertEqual(cosmo_slice(ds2,pm),ds1);

    [mp,pm]=cosmo_align([2 3 4],[4 2 3]);
    assertEqual(mp,[3 1 2]);
    assertEqual(pm,[2 3 1]);

    [mp,pm]=cosmo_align({{'b','c','c'},[3 2 3]},...
                            {{'c','b','c'},[3 3 2]});
    assertEqual(mp,[3 1 2]);
    assertEqual(pm,[2 3 1]);

    % test structs
    p=struct();
    p.x={'b','c','c'};
    p.y=[3 2 3];
    q=struct();
    q.y=[3 3 2];
    q.x={'c','b','c'};
    [mp,pm]=cosmo_align(p,q);
    assertEqual(mp,[3 1 2]);
    assertEqual(pm,[2 3 1]);

    % test NaN
    [mp,pm]=cosmo_align([2 NaN 4],[4 2 NaN]);
    assertEqual(mp,[3 1 2]);
    assertEqual(pm,[2 3 1]);

    % test exceptions
    ds_small=cosmo_slice(ds1,1:nsamples-1);
    aet=@(varargin)assertExceptionThrown(@()...
                    cosmo_align(varargin{:}),'');
    aet(ds1,ds2);
    aet([2 3 4 3],[4 2 3 4]);
    aet([2 3 4 3],[4 2 3 4]);
    aet([2 3 4 3],[4 2 3]);
    aet([2 3 4],{[2,3,4],[2,3,4]});

    aet([2 3 NaN NaN],[2 3 NaN Inf]);
    aet([2 NaN 3],[4 2 NaN]);

    aet(struct,struct('a',1));
    aet(struct('a',1),1);

    aet({'b','c','d','d'},{'d','b','c','d'});

function test_align_multiple_nans
    n_rows_half=20+ceil(rand()*20);
    x=ceil(rand(n_rows_half*2,2)*sqrt(n_rows_half));
    rp=randperm(2*n_rows_half);

    rp1=rp(1:n_rows_half);
    rp2=rp(n_rows_half+(1:n_rows_half));

    % half of the rows become NaN
    x(rp1,1)=NaN;
    x(rp1,2)=1:n_rows_half;
    x(rp2,1)=1:n_rows_half;
    x(rp2,2)=1:n_rows_half;

    rp2=randperm(n_rows_half*2);
    y=x(rp2,:);

    assertExceptionThrown(@()cosmo_align(...
                            {x(:,1),x(:,2)},{y(:,1),y(:,2)}),'');


function C = complex( A, B )
%COMPLEX Construct complex SEPARABLEAPPROX from real and imaginary parts.
%   C = COMPLEX(A, B) returns the complex SEPARABLEAPPROX A + Bi, where A and B are
%   real valued SEPARABLEAPPROX objects with the same domain.
%
%   C = COMPLEX(A) for real SEPARABLEAPPROX A returns the complex result C with real
%   part A and all zero imaginary part. isreal(C) returns false.
%
% See also IMAG, CONJ, ABS, REAL.

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

if ( nargin == 2 )
    if ( ~isa(B, 'separableApprox') )
        error('CHEBFUN:SEPARABLEAPPROX:complex:inputs', ...
            'Second input must be a CHEBFUN2.');
    elseif ( ~isreal( A ) || ~isreal( B ) )
        error('CHEBFUN:SEPARABLEAPPROX:complex:notReal1', ...
            'Inputs must be real valued.');
    end
    C = A + 1i*B;
else
    if ( ~isreal( A ) )
        error('CHEBFUN:SEPARABLEAPPROX:complex:notReal2', ...
            'Input must be real valued.');
    end
    % Make complex.
    C = A + 0*1i;
end

end


function varargout = removeMeshEars(varargin)
%REMOVEMESHEARS Remove vertices that are connected to only one face.
%
%   [V, F] = removeMeshEars(V, F)
%   [V, F] = removeMeshEars(MESH)
%   Remove vertices that are connected to only one face. This removes also
%   "pending" faces.
%   Note that if the mesh has boundary, this may remove some regular faces
%   located on the boundary.
%
%   Example
%   removeMeshEars
%
%   See also
%     meshes3d, ensureManifoldMesh
%

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2019-01-08, using Matlab 8.6.0.267246 (R2015b)
% Copyright 2019-2022 INRA - Cepia Software Platform

[vertices, faces] = parseMeshData(varargin{:});

nVertices = size(vertices, 1);

% for each vertex, determine the number of faces it belongs to
vertexDegree = zeros(nVertices, 1);
for iv = 1:nVertices
    vertexDegree(iv) = sum(sum(faces == iv, 2) > 0);
end

% remove vertices with degree 1
inds = find(vertexDegree == 1);
[vertices, faces] = removeMeshVertices(vertices, faces, inds);

%% Format output

varargout = formatMeshOutput(nargout, vertices, faces);


%% This is for testing the Plotting functions in the robotics Toolbox
function tests = TransformationsTest
  tests = functiontests(localfunctions);
  clc
end

function teardownOnce(tc)
    close all
end

%    tranimate                  - animate a coordinate frame
function tranimate_test(tc)
    X1 = eye(3,3); X2 = rotx(pi/2);
    tranimate(X1, X2);
    tranimate(X1, X2, 'nsteps', 10);

    clf
    tranimate(X1, X2, 'axis', [-10 10 -20 20 -30 30]);
    ax = gca; v = [ax.XLim ax.YLim ax.ZLim];
    tc.verifyEqual(v, [-10 10 -20 20 -30 30]); % 19b doesnt give

    tranimate(X1, X2, 'noxyz');
    tranimate(X1, X2, 'rgb');
    tranimate(X1, X2, 'retain');
    tranimate(X1, X2, 'fps', 20);

    X1 = eye(4,4); X2 = transl(1,2,3)*trotx(pi/2);
    tranimate(X1, X2);
    tranimate(X1, X2, 'nsteps', 10);

    clf
    tranimate(X1, X2, 'axis', [-10 10 -20 20 -30 30]);
    %v = axis;
    ax = gca; v = [ax.XLim ax.YLim ax.ZLim];
    tc.verifyEqual(v, [-10 10 -20 20 -30 30]);

    tranimate(X1, X2, 'noxyz');
    tranimate(X1, X2, 'rgb');
    tranimate(X1, X2, 'retain');
    tranimate(X1, X2, 'fps', 20);
end

function tranimate2_test(tc)
    X1 = eye(2,2); X2 = rot2(pi/2);
    tranimate2(X1, X2);
    tranimate2(X1, X2, 'nsteps', 10);

    clf
    tranimate2(X1, X2, 'axis', [-10 10 -20 20]);
    v = axis;
    tc.verifyEqual(v, [-10 10 -20 20]);

    tranimate2(X1, X2, 'noxyz');
    tranimate2(X1, X2, 'rgb');
    tranimate2(X1, X2, 'retain');
    tranimate2(X1, X2, 'fps', 20);

    X1 = eye(3,3); X2 = transl2(1,2)*trot2(pi/2);
    tranimate2(X1, X2);
    tranimate2(X1, X2, 'nsteps', 10);

    clf
    tranimate2(X1, X2, 'axis', [-10 10 -20 20]);
    v = axis;
    tc.verifyEqual(v, [-10 10 -20 20]);

    tranimate2(X1, X2, 'noxyz');
    tranimate2(X1, X2, 'rgb');
    tranimate2(X1, X2, 'retain');
    tranimate2(X1, X2, 'fps', 20);
end

%    trplot                     - plot HT as a coordinate frame
function trplot_test(tc)
    %%
    Rt1 = [1.0000         0         0         0
                0    0.8253   -0.5646         0
                0    0.5646    0.8253         0
                0         0         0    1.0000];
    trplot(Rt1);
    clf
    h = trplot(Rt1);
    trplot(Rt1, 'handle', h);
    trplot(Rt1, 'color', 'r');
    trplot(Rt1, 'color', [1 0 1]);
    trplot(Rt1, 'noaxes');
    trplot(Rt1, 'frame', 'bob');
    trplot(Rt1, 'frame', 'A', 'text_opts', {'FontSize', 10, 'FontWeight', 'bold'})
    trplot(Rt1, 'view', [10 20]);
    trplot(Rt1, 'arrow')
    trplot(Rt1, '3d')
    trplot(Rt1, '3d', 'anaglyph', 'mo')
    trplot(Rt1, '3d', 'dispar', 0.3);
end

 function trplot2_test(tc)
     %%
    Rt1 = [
    0.9553   -0.2955    1.0000
    0.2955    0.9553    2.0000
         0         0    1.0000];
    trplot2(Rt1);
    clf
    h = trplot2(Rt1);
    trplot2(Rt1, 'handle', h);
    h = trplot2(Rt1, 'handle', h);
    trplot2(Rt1, 'color', 'r');
    trplot2(Rt1, 'color', [1 0 1]);
    trplot2(Rt1, 'noaxes');
    trplot2(Rt1, 'frame', 'bob');
    trplot2(Rt1, 'frame', 'A', 'text_opts', {'FontSize', 10, 'FontWeight', 'bold'})
    trplot2(Rt1, 'view', [10 20]);
    trplot2(Rt1, 'arrow')
    trplot2(Rt1, 'framelabel', 'A')

    clf
    a = gca;
    trplot2(Rt1, 'axhandle', a);

    h = trplot2(Rt1);
    tc.verifyWarning( @() trplot2(h, Rt1), 'SMTB:trplot2:deprecated')
end

%    plot2                      - plot trajectory
function plot2_test(tc)
    %%
        th = [0:0.1:pi];
        sth = sin(th);
        p = [th' sth'];
        plot2(p);

        p = [th' sth' sth'];
        plot2(p);
end

%    plot_box                   - draw a box
function plot_box_test(tc)
        plot_box(1,1,5,5,'b');
        plot_box(2,2,4,4,'g');
end

%    plot_circle                - draw a circle
function plot_circle_test(tc)
    plot_circle([1 2],2,'g');
    plot_circle([1 2],2,'fillcolor', 'g');
    plot_circle([1 2],2,'fillcolor', 'g', 'alpha', 0.5);
    plot_circle([1 2],2,'edgecolor', 'b');
    plot_circle([1 2],2,'fillcolor', 'g', 'edgecolor', 'b');
end

%    plot_ellipse               - draw an ellipse
function plot_ellipse_test(tc)
    %%
    %2d

    C = diag([1,4]);
    plot_ellipse(C,[2 3],'r');
    plot_ellipse(C,[2 3],'fillcolor', 'g');
    plot_ellipse(C,[2 3],'fillcolor', 'g', 'alpha', 0.5);
    plot_ellipse(C,[2 3],'edgecolor', 'g');
    plot_ellipse(C,[2 3],'fillcolor', 'g', 'edgecolor', 'b');

    % with 3d centre
    plot_ellipse([1 0; 0 4],[2 3 1],'r');

    %3d

    C = diag([1,4,2]);
    plot_ellipse(C,[2 3 1],'r');
    plot_ellipse(C,[2 3 1],'fillcolor', 'g');
    plot_ellipse(C,[2 3 1],'fillcolor', 'g', 'alpha', 0.5);
    plot_ellipse(C,[2 3 1],'edgecolor', 'g');
    plot_ellipse(C,[2 3 1],'fillcolor', 'g', 'edgecolor', 'b');
end

%    plot_homline               - plot homogeneous line
function plot_homline_test(tc)
    plot_homline([1 2 3]','y');
end

%    plot_point                 - plot points
function plot_point_test(tc)
    plot_point([1; 2]);
end

%    plot_poly                  - plot polygon
function plot_poly_test(tc)
    p = [1 2 2 1; 1 1 2 2];
    plot_poly(p,'g');
end

%    plot_sphere                - draw a sphere
function plot_sphere_test(tc)
     plot_sphere([1 2 3],5,'r');
end

function xyzlabel_test(tc)

    plot3(1, 2, 3)
    xyzlabel()
end

function plotvol_test(tc)

    % 2D cases
    plotvol([2 3])
    tc.verifyEqual(axis, [-2 2 -3 3]);
    plotvol([2 3 4 5])
    tc.verifyEqual(axis, [2 3 4 5]);

    % 3D cases
    plotvol(3)
    ax = gca; v = [ax.XLim ax.YLim ax.ZLim];
    tc.verifyEqual(v, 3*[-1 1 -1 1 -1 1]);

    plotvol([1 2 3]);
    ax = gca; v = [ax.XLim ax.YLim ax.ZLim];
    tc.verifyEqual(v, [-1 1 -2 2 -3 3]);

    plotvol([1 2 3 4 5 6]);
    ax = gca; v = [ax.XLim ax.YLim ax.ZLim];
    tc.verifyEqual(v, [1 2 3 4 5 6]);
end


function [SPM] = cl_ext_spm_spm(SPM)
% [Re]ML Estimation of a General Linear Model
%
% :Usage:
% ::
%
%     FORMAT [SPM] = spm_spm(SPM)
%
% :Required fields of SPM:
%
% xY.VY - nScan x 1 struct array of image handles (see spm_vol)
%         Images must have the same orientation, voxel size and data type
%       - Any scaling should have already been applied via the image handle
%         scalefactors.
%
% xX    - Structure containing design matrix information
%       - Required fields are:
%         xX.X      - Design matrix (raw, not temporally smoothed)
%         xX.name   - cellstr of parameter names corresponding to columns
%                     of design matrix
%       - Optional fields are:
%         xX.K      - cell of session-specific structures (see spm_filter)
%                   - Design & data are pre-multiplied by K
%                     (K*Y = K*X*beta + K*e)
%                   - Note that K should not smooth across block boundaries
%                   - defaults to speye(size(xX.X,1))
%         xX.W      - Optional whitening/weighting matrix used to give
%                     weighted least squares estimates (WLS). If not specified
%                     spm_spm will set this to whiten the data and render
%                     the OLS estimates maximum likelihood
%                     i.e. W*W' = inv(xVi.V).
%
% xVi   - Structure describing intrinsic temporal non-sphericity
%       - Required fields are:
%         xVi.Vi    - array of non-sphericity components
%                   - defaults to {speye(size(xX.X,1))} - i.i.d.
%                   - specifying a cell array of constraints (Qi)
%                     These constraints invoke spm_reml to estimate
%                     hyperparameters assuming V is constant over voxels.
%                     that provide a high precise estimate of xX.V
%       - Optional fields are:
%         xX.V      - Optional non-sphericity matrix.  Cov(e) = sigma^2*V
%                     If not specified spm_spm will compute this using
%                     a 1st pass to identify significant voxels over which
%                     to estimate V.  A 2nd pass is then used to re-estimate
%                     the parameters with WLS and save the ML estimates
%                     (unless xX.W is already specified).
%
% xM    - Structure containing masking information, or a simple column vector
%         of thresholds corresponding to the images in VY [default: -Inf]
%       - If a structure, the required fields are:
%         xM.TH - nVar x nScan matrix of analysis thresholds, one per image
%         xM.I  - Implicit masking (0=>none, 1 => implicit zero/NaN mask)
%         xM.VM - struct array of explicit mask image handles
%       - (empty if no explicit masks)
%               - Explicit mask images are >0 for valid voxels to assess.
%               - Mask images can have any orientation, voxel size or data
%                 type. They are interpolated using nearest neighbour
%                 interpolation to the voxel locations of the data Y.
%       - Note that voxels with constant data (i.e. the same value across
%         scans) are also automatically masked out.
%
% swd   - Directory where the output files will be saved [default: pwd]
%         If exists, it becomes the current working directory.
%
% In addition, global SPM "defaults" variable is used (see spm_defaults):
%
% stats.<modality>.UFp - critical F-threshold for selecting voxels over
%                        which the non-sphericity is estimated (if
%                        required) [default: 0.001]
%
% stats.maxres         - maximum number of residual images for smoothness
%                        estimation
%
% stats.maxmem         - maximum amount of data processed at a time (in bytes)
%
% modality             - SPM modality {'PET','FMRI','EEG'}
%
%__________________________________________________________________________
%
% spm_spm is the heart of the SPM package. Given image files and a
% General Linear Model, it estimates the model parameters, variance
% hyperparameters, and smoothness of standardised residual fields, writing
% these out to disk in the current working directory for later
% interrogation in the results section. (NB: Existing analyses in the
% current working directory are overwritten).  This directory
% now becomes the working directory for this analysis and all saved
% images are relative to this directory.
%
% The model is expressed via the design matrix (xX.X). The basic model
% at each voxel is of the form is Y = X*B + e, for data Y, design
% matrix X, (unknown) parameters B and residual errors e. The errors
% are assumed to have a normal distribution.
%
% Sometimes confounds (e.g. drift terms in fMRI) are necessary. These
% can be specified directly in the design matrix or implicitly, in terms
% of a residual forming matrix K to give a generalised linear model
% K*Y = K*X*B + K*e.  In fact K can be any matrix (e.g. a convolution
% matrix).
%
% In some instances i.i.d. assumptions about errors do not hold. For
% example, with serially correlated (fMRI) data or correlations among the
% levels of a factor in repeated measures designs. This non-sphericity
% can be specified in terms of components (SPM.xVi.Vi{i}). If specified
% these covariance components will then be estimated with ReML (restricted
% maximum likelihood) hyperparameters. This estimation assumes the same
% non-sphericity for voxels that exceed the global F-threshold. The ReML
% estimates can then be used to whiten the data giving maximum likelihood
% (ML) or Gauss-Markov estimators. This entails a second pass of the data
% with an augmented model K*W*Y = K*W*X*B + K*W*e where W*W' = inv(xVi.V).
% xVi.V is the non-sphericity based on the hyperparameter estimates.
% W is stored in xX.W and cov(K*W*e) in xX.V. The covariance of the
% parameter estimates is then xX.Bcov = pinv(K*W*X)*xX.V*pinv(K*W*X)'.
%
% If you do not want ML estimates but want to use ordinary least squares
% (OLS) then simply set SPM.xX.W to the identity matrix. Any non-sphericity
% V will still be estimated but will be used to adjust the degrees of freedom
% of the ensuing statistics using the Satterthwaite approximation (c.f.
% the Greenhouse-Geisser corrections).
%
% If [non-spherical] variance components Vi are not specified xVi.Vi and
% xVi.V default to the identity matrix (i.e. i.i.d). The parameters are
% then estimated by OLS.  In this instance the OLS and ML estimates are
% the same.
%
% Note that only a single voxel-specific hyperparameter (i.e. variance
% component) is estimated, even if V is not i.i.d.  This means spm_spm
% always implements a fixed-effects model.
% Random effects models can be emulated using a multi-stage procedure:
% This entails summarising the data with contrasts such that the fixed
% effects in a second model on the summary data are those effects of
% interest (i.e. the population effects). This means contrasts are
% re-entered into spm_spm to make an inference (SPM) at the next
% level. At this higher hierarchical level the residual variance for the
% model contains the appropriate variance components from lower levels.
% See spm_RandFX.man for further details and below.
%
% Under the additional assumption that the standardised error fields
% are non-stationary standard Gaussian random fields, results from
% Random field theory can be applied to estimate the significance
% statistic images (SPM's) adjusting p values for the multiple tests
% at all voxels in the search volume. The parameters required for
% this random field correction are the volume, and Lambda, the covariance
% matrix of partial derivatives of the standardised error fields, estimated
% by spm_est_smoothness.
%
%                           ----------------
%
% The volume analysed is the intersection of the threshold masks,
% explicit masks and implicit masks. See spm_spm_ui for further details
% on masking options.
%
%--------------------------------------------------------------------------
%
% The output of spm_spm takes the form of an SPM.mat file of the analysis
% parameters, and 'float' flat-file images of the parameter and variance
% [hyperparameter] estimates. An 8bit zero-one mask image indicating the
% voxels assessed is also written out, with zero indicating voxels outside
% tha analysed volume.
%
%                           ----------------
%
% The following SPM.fields are set by spm_spm (unless specified)
%
%     xVi.V      - estimated non-sphericity trace(V) = rank(V)
%     xVi.h      - hyperparameters  xVi.V = xVi.h(1)*xVi.Vi{1} + ...
%     xVi.Cy     - spatially whitened <Y*Y'> (used by ReML to estimate h)
%     xVi.CY     - <(Y - <Y>)*(Y - <Y>)'>    (used by spm_spm_Bayes)
%
%                           ----------------
%
%     Vbeta     - struct array of beta image handles (relative)
%     VResMS    - file struct of ResMS image handle  (relative)
%     VM        - file struct of Mask  image handle  (relative)
%
%                           ----------------
%
%     xX.W      - if not specified W*W' = inv(x.Vi.V)
%     xX.V      - V matrix (K*W*Vi*W'*K') = correlations after K*W is applied
%     xX.xKXs   - space structure for K*W*X, the 'filtered and whitened'
%                 design matrix
%               - given as spm_sp('Set',xX.K*xX.W*xX.X) - see spm_sp
%     xX.pKX    - pseudoinverse of K*W*X, computed by spm_sp
%     xX.Bcov   - xX.pKX*xX.V*xX.pKX - variance-covariance matrix of
%                 parameter estimates
%                 (when multiplied by the voxel-specific hyperparameter ResMS
%                 of the parameter estimates (ResSS/xX.trRV = ResMS) )
%     xX.trRV   - trace of R*V
%     xX.trRVRV - trace of RVRV
%     xX.erdf   - effective residual degrees of freedom (trRV^2/trRVRV)
%     xX.nKX    - design matrix (xX.xKXs.X) scaled for display
%                 (see spm_DesMtx('sca',... for details)
%
%                           ----------------
%
%     xVol.M    - 4x4 voxel->mm transformation matrix
%     xVol.iM   - 4x4 mm->voxel transformation matrix
%     xVol.DIM  - image dimensions - column vector (in voxels)
%     xVol.XYZ  - 3 x S vector of in-mask voxel coordinates
%     xVol.S    - Lebesgue measure or volume       (in voxels)
%     xVol.R    - vector of resel counts           (in resels)
%     xVol.FWHM - Smoothness of components - FWHM, (in voxels)
%
%                           ----------------
%
%     xCon      - Contrast structure (created by spm_FcUtil.m)
%     xCon.name - Name of contrast
%     xCon.STAT - 'F', 'T' or 'P' - for F/T-contrast ('P' for PPMs)
%     xCon.c    - (F) Contrast weights
%     xCon.X0   - Reduced design matrix (spans design space under Ho)
%                 It is in the form of a matrix (spm99b) or the
%                 coordinates of this matrix in the orthogonal basis
%                 of xX.X defined in spm_sp.
%     xCon.iX0  - Indicates how contrast was specified:
%                 If by columns for reduced design matrix then iX0 contains
%                 the column indices. Otherwise, it's a string containing
%                 the spm_FcUtil 'Set' action: Usually one of {'c','c+','X0'}
%                 (Usually this is the input argument F_iX0.)
%     xCon.X1o  - Remaining design space (orthogonal to X0).
%                 It is in the form of a matrix (spm99b) or the
%                 coordinates of this matrix in the orthogonal basis
%                 of xX.X defined in spm_sp.
%     xCon.eidf - Effective interest degrees of freedom (numerator df)
%     xCon.Vcon - ...for handle of contrast/ESS image (empty at this stage)
%     xCon.Vspm - ...for handle of SPM image (empty at this stage)
%
%                           ----------------
%
%
% The following images are written to file
%
% mask.{img,hdr}                                   - analysis mask image
% 8-bit (uint8) image of zero-s & one's indicating which voxels were
% included in the analysis. This mask image is the intersection of the
% explicit, implicit and threshold masks specified in the xM argument.
% The XYZ matrix contains the voxel coordinates of all voxels in the
% analysis mask. The mask image is included for reference, but is not
% explicitly used by the results section.
%
%                           ----------------
%
% beta_????.{img,hdr}                                 - parameter images
% These are 32-bit (float32) images of the parameter estimates. The image
% files are numbered according to the corresponding column of the
% design matrix. Voxels outside the analysis mask (mask.img) are given
% value NaN.
%
%                           ----------------
%
% ResMS.{img,hdr}                    - estimated residual variance image
% This is a 64-bit (float64) image of the residual variance estimate.
% Voxels outside the analysis mask are given value NaN.
%
%                           ----------------
%
% RPV.{img,hdr}                      - estimated resels per voxel image
% This is a 64-bit (float64) image of the RESELs per voxel estimate.
% Voxels outside the analysis mask are given value 0.  These images
% reflect the nonstationary aspects the spatial autocorrelations.
%
%                           ----------------
%
% ResI_????.{img,hdr}        - standardised residual (temporary) images
% These are 64-bit (float64) images of standardised residuals. At most
% maxres images will be saved and used by spm_est_smoothness, after which
% they will be deleted.
%
%--------------------------------------------------------------------------
%
% References:
%
% Christensen R (1996) Plane Answers to Complex Questions
%       Springer Verlag
%
% Friston KJ, Holmes AP, Worsley KJ, Poline JB, Frith CD, Frackowiak RSJ (1995)
% ``Statistical Parametric Maps in Functional Imaging:
%   A General Linear Approach''
%       Human Brain Mapping 2:189-210
%
% Worsley KJ, Friston KJ (1995)
% ``Analysis of fMRI Time-Series Revisited - Again''
%       NeuroImage 2:173-181
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging
%
% ..
%    % Andrew Holmes, Jean-Baptiste Poline & Karl Friston
%    $Id: spm_spm.m 3960 2010-06-30 17:41:24Z ged $
% ..

SVNid     = '$Rev: 3960 $';

%-Say hello
%--------------------------------------------------------------------------
SPMid     = spm('FnBanner',mfilename,SVNid);
Finter    = spm('FigName','Stats: estimation...'); spm('Pointer','Watch');

%-Get SPM.mat[s] if necessary
%--------------------------------------------------------------------------
if nargin == 0
    P     = cellstr(spm_select(Inf,'^SPM\.mat$','Select SPM.mat[s]'));
    for i = 1:length(P)
        swd     = fileparts(P{i});
        load(fullfile(swd,'SPM.mat'));
        SPM.swd = swd;
        spm_spm(SPM);
    end
    return
end

%-Change to SPM.swd if specified
%--------------------------------------------------------------------------
try
    cd(SPM.swd);
catch
    SPM.swd = pwd;
end

%-Ensure data are assigned
%--------------------------------------------------------------------------
try
    SPM.xY.VY;
catch
    spm('alert!','Please assign data to this design', mfilename);
    spm('FigName','Stats: done',Finter); spm('Pointer','Arrow')
    return
end

%-Delete files from previous analyses
%--------------------------------------------------------------------------
if exist(fullfile(SPM.swd,'mask.img'),'file') == 2

    str = {'Current directory contains SPM estimation files:',...
        'pwd = ',SPM.swd,...
        'Existing results will be overwritten!'};
    if spm_input(str,1,'bd','stop|continue',[1,0],1)
        spm('FigName','Stats: done',Finter); spm('Pointer','Arrow')
        return
    else
        warning('Overwriting old results\n\t (pwd = %s) ',SPM.swd);
        try, SPM = rmfield(SPM,'xVol'); end
    end
end

files = {'^mask\..{3}$','^ResMS\..{3}$','^RPV\..{3}$',...
         '^beta_.{4}\..{3}$','^con_.{4}\..{3}$','^ResI_.{4}\..{3}$',...
         '^ess_.{4}\..{3}$', '^spm\w{1}_.{4}\..{3}$'};

for i = 1:length(files)
    j = spm_select('List',SPM.swd,files{i});
    for k = 1:size(j,1)
        spm_unlink(deblank(j(k,:)));
    end
end

%==========================================================================
% - A N A L Y S I S   P R E L I M I N A R I E S
%==========================================================================

%-Initialise
%==========================================================================
fprintf('%-40s: %30s','Initialising parameters','...computing');        %-#
xX            = SPM.xX;
[nScan nBeta] = size(xX.X);

%-If xM is not a structure then assume it's a vector of thresholds
%--------------------------------------------------------------------------
try
    xM = SPM.xM;
catch
    xM = -Inf(nScan,1);
end
if ~isstruct(xM)
    xM = struct('T',    [],...
                'TH',   xM,...
                'I',    0,...
                'VM',   {[]},...
                'xs',   struct('Masking','analysis threshold'));
end

%-Check confounds (xX.K) and non-sphericity (xVi)
%--------------------------------------------------------------------------
if ~isfield(xX,'K')
    xX.K  = 1;
end
try
    %-If covariance components are specified use them
    %----------------------------------------------------------------------
    xVi   = SPM.xVi;
catch

    %-otherwise assume i.i.d.
    %----------------------------------------------------------------------
    xVi   = struct( 'form',  'i.i.d.',...
                    'V',     speye(nScan,nScan));
end

%-Get non-sphericity V
%==========================================================================
try
    %-If xVi.V is specified proceed directly to parameter estimation
    %----------------------------------------------------------------------
    V     = xVi.V;
    str   = 'parameter estimation';

catch

    % otherwise invoke ReML selecting voxels under i.i.d assumptions
    %----------------------------------------------------------------------
    V     = speye(nScan,nScan);
    str   = '[hyper]parameter estimation';
end

%-Get whitening/Weighting matrix: If xX.W exists we will save WLS estimates
%--------------------------------------------------------------------------
try
    %-If W is specified, use it
    %----------------------------------------------------------------------
    W     = xX.W;
catch

    if isfield(xVi,'V')

        % otherwise make W a whitening filter W*W' = inv(V)
        %------------------------------------------------------------------
        W     = spm_sqrtm(spm_inv(xVi.V));
        W     = W.*(abs(W) > 1e-6);
        xX.W  = sparse(W);

    else
        % unless xVi.V has not been estimated - requiring 2 passes
        %------------------------------------------------------------------
        W     = speye(nScan,nScan);
        str   = 'hyperparameter estimation (1st pass)';
    end
end

%-Design space and projector matrix [pseudoinverse] for WLS
%==========================================================================
xX.xKXs   = spm_sp('Set',spm_filter(xX.K,W*xX.X));       % KWX
xX.xKXs.X = full(xX.xKXs.X);
xX.pKX    = spm_sp('x-',xX.xKXs);                        % projector
erdf      = spm_SpUtil('trRV',xX.xKXs);                  % Working error df

%-If xVi.V is not defined compute Hsqr and F-threshold under i.i.d.
%--------------------------------------------------------------------------
if ~isfield(xVi,'V')

    Fcname = 'effects of interest';
    iX0    = [SPM.xX.iB SPM.xX.iG];
    xCon   = spm_FcUtil('Set',Fcname,'F','iX0',iX0,xX.xKXs);
    X1o    = spm_FcUtil('X1o', xCon(1),xX.xKXs);
    Hsqr   = spm_FcUtil('Hsqr',xCon(1),xX.xKXs);
    trRV   = spm_SpUtil('trRV',xX.xKXs);
    trMV   = spm_SpUtil('trMV',X1o);

    % Threshold for voxels entering non-sphericity estimates
    %----------------------------------------------------------------------
    try
        modality = lower(spm_get_defaults('modality'));
        UFp      = spm_get_defaults(['stats.' modality '.ufp']);
    catch
        UFp      = 0.001;
    end
    UF           = spm_invFcdf(1 - UFp,[trMV,trRV]);
end

%-Image dimensions and data
%==========================================================================
VY       = SPM.xY.VY;
spm_check_orientations(VY);

% check files exists and try pwd
%--------------------------------------------------------------------------
for i = 1:numel(VY)
    if ~spm_existfile(VY(i).fname)
        [p,n,e]     = fileparts(VY(i).fname);
        VY(i).fname = [n,e];
    end
end

M        = VY(1).mat;
DIM      = VY(1).dim(1:3)';
xdim     = DIM(1); ydim = DIM(2); zdim = DIM(3);
YNaNrep  = spm_type(VY(1).dt(1),'nanrep');

%-Maximum number of residual images for smoothness estimation
%--------------------------------------------------------------------------
MAXRES   = spm_get_defaults('stats.maxres');
nSres    = min(nScan,MAXRES);

fprintf('%s%30s\n',repmat(sprintf('\b'),1,30),'...done');               %-#

%-Initialise output images (unless this is a 1st pass for ReML)
%==========================================================================
if isfield(xX,'W')
    fprintf('%-40s: %30s','Output images','...initialising');           %-#

    %-Initialise new mask name: current mask & conditions on voxels
    %----------------------------------------------------------------------
    VM    = struct('fname',  'mask.img',...
                   'dim',    DIM',...
                   'dt',     [spm_type('uint8') spm_platform('bigend')],...
                   'mat',    M,...
                   'pinfo',  [1 0 0]',...
                   'descrip','spm_spm:resultant analysis mask');
    VM    = spm_create_vol(VM);

    %-Initialise beta image files
    %----------------------------------------------------------------------
    Vbeta(1:nBeta) = deal(struct(...
        'fname',    [],...
        'dim',      DIM',...
        'dt',       [spm_type('float32') spm_platform('bigend')],...
        'mat',      M,...
        'pinfo',    [1 0 0]',...
        'descrip',  ''));

    for i = 1:nBeta
        Vbeta(i).fname   = sprintf('beta_%04d.img',i);
        Vbeta(i).descrip = sprintf('spm_spm:beta (%04d) - %s',i,xX.name{i});
    end
    Vbeta = spm_create_vol(Vbeta);

    %-Initialise residual sum of squares image file
    %----------------------------------------------------------------------
    VResMS = struct('fname',    'ResMS.img',...
        'dim',      DIM',...
        'dt',       [spm_type('float64') spm_platform('bigend')],...
        'mat',      M,...
        'pinfo',    [1 0 0]',...
        'descrip',  'spm_spm:Residual sum-of-squares');
    VResMS = spm_create_vol(VResMS);

    %-Initialise standardised residual images
    %----------------------------------------------------------------------
    VResI(1:nSres) = deal(struct(...
        'fname',    [],...
        'dim',      DIM',...
        'dt',       [spm_type('float64') spm_platform('bigend')],...
        'mat',      M,...
        'pinfo',    [1 0 0]',...
        'descrip',  'spm_spm:StandardisedResiduals'));

    for i = 1:nSres
        VResI(i).fname   = sprintf('ResI_%04d.img', i);
        VResI(i).descrip = sprintf('spm_spm:ResI (%04d)', i);
    end
    VResI = spm_create_vol(VResI);
    fprintf('%s%30s\n',repmat(sprintf('\b'),1,30),'...initialised');    %-#
end % (xX,'W')

%==========================================================================
% - F I T   M O D E L   &   W R I T E   P A R A M E T E R    I M A G E S
%==========================================================================

%-MAXMEM is the maximum amount of data processed at a time (bytes)
%--------------------------------------------------------------------------
MAXMEM = spm_get_defaults('stats.maxmem');
mmv    = MAXMEM/8/nScan;
blksz  = min(xdim*ydim,ceil(mmv));                             %-block size
nbch   = ceil(xdim*ydim/blksz);                                %-# blocks
nbz    = max(1,min(zdim,floor(mmv/(xdim*ydim))));   nbz = 1;   %-# planes
blksz  = blksz * nbz;

%-Initialise variables used in the loop
%==========================================================================
[xords, yords] = ndgrid(1:xdim, 1:ydim);
xords = xords(:)'; yords = yords(:)';           % plane X,Y coordinates
S     = 0;                                      % Volume (voxels)
s     = 0;                                      % Volume (voxels > UF)
Cy    = 0;                                      % <Y*Y'> spatially whitened
CY    = 0;                                      % <(Y - <Y>) * (Y - <Y>)'>
EY    = 0;                                      % <Y>    for ReML
i_res = round(linspace(1,nScan,nSres))';        % Indices for residual

%-Initialise XYZ matrix of in-mask voxel co-ordinates (real space)
%--------------------------------------------------------------------------
XYZ   = zeros(3,xdim*ydim*zdim);

%-Cycle over bunches blocks within planes to avoid memory problems
%==========================================================================
spm_progress_bar('Init',100,str,'');

for z = 1:nbz:zdim                       %-loop over planes (2D or 3D data)

    % current plane-specific parameters
    %----------------------------------------------------------------------
    CrPl    = z:min(z+nbz-1,zdim);       %-plane list
    zords   = CrPl(:)*ones(1,xdim*ydim); %-plane Z coordinates
    CrBl    = [];                        %-parameter estimates
    CrResI  = [];                        %-residuals
    CrResSS = [];                        %-residual sum of squares
    Q       = [];                        %-in mask indices for this plane

    for bch = 1:nbch                     %-loop over blocks

        %-Print progress information in command window
        %------------------------------------------------------------------
        if numel(CrPl) == 1
            str = sprintf('Plane %3d/%-3d, block %3d/%-3d',...
                z,zdim,bch,nbch);
        else
            str = sprintf('Planes %3d-%-3d/%-3d',z,CrPl(end),zdim);
        end
        if z == 1 && bch == 1
            str2 = '';
        else
            str2 = repmat(sprintf('\b'),1,72);
        end
        fprintf('%s%-40s: %30s',str2,str,' ');

        %-construct list of voxels in this block
        %------------------------------------------------------------------
        I     = (1:blksz) + (bch - 1)*blksz;       %-voxel indices
        I     = I(I <= numel(CrPl)*xdim*ydim);     %-truncate
        xyz   = [repmat(xords,1,numel(CrPl)); ...
                 repmat(yords,1,numel(CrPl)); ...
                 reshape(zords',1,[])];
        xyz   = xyz(:,I);                          %-voxel coordinates
        nVox  = size(xyz,2);                       %-number of voxels

        %-Get data & construct analysis mask
        %=================================================================
        fprintf('%s%30s',repmat(sprintf('\b'),1,30),'...read & mask data')
        Cm    = true(1,nVox);                      %-current mask

        %-Compute explicit mask
        % (note that these may not have same orientations)
        %------------------------------------------------------------------
        for i = 1:length(xM.VM)

            %-Coordinates in mask image
            %--------------------------------------------------------------
            j = xM.VM(i).mat\M*[xyz;ones(1,nVox)];

            %-Load mask image within current mask & update mask
            %--------------------------------------------------------------
            Cm(Cm) = spm_get_data(xM.VM(i),j(:,Cm),false) > 0;
        end

        %-Get the data in mask, compute threshold & implicit masks
        %------------------------------------------------------------------
        Y     = zeros(nScan,nVox);
        for i = 1:nScan

            %-Load data in mask
            %--------------------------------------------------------------
            if ~any(Cm), break, end                %-Break if empty mask
            Y(i,Cm)  = spm_get_data(VY(i),xyz(:,Cm),false);

            Cm(Cm)   = Y(i,Cm) > xM.TH(i);         %-Threshold (& NaN) mask
            if xM.I && ~YNaNrep && xM.TH(i) < 0    %-Use implicit mask
                Cm(Cm) = abs(Y(i,Cm)) > eps;
            end
        end

        %-Mask out voxels where data is constant
        %------------------------------------------------------------------
        Cm(Cm) = any(diff(Y(:,Cm),1));
        Y      = Y(:,Cm);                          %-Data within mask
        CrS    = sum(Cm);                          %-# current voxels

        %==================================================================
        %-Proceed with General Linear Model (if there are voxels)
        %==================================================================
        if CrS

            %-Whiten/Weight data and remove filter confounds
            %--------------------------------------------------------------
            fprintf('%s%30s',repmat(sprintf('\b'),1,30),'...filtering');%-#

            KWY   = spm_filter(xX.K,W*Y);

            %-General linear model: Weighted least squares estimation
            %--------------------------------------------------------------
            fprintf('%s%30s',repmat(sprintf('\b'),1,30),'...estimation');%-#

            beta  = xX.pKX*KWY;                    %-Parameter estimates
            res   = spm_sp('r',xX.xKXs,KWY);       %-Residuals
            ResSS = sum(res.^2);                   %-Residual SSQ
            clear KWY                              %-Clear to save memory

            %-If ReML hyperparameters are needed for xVi.V
            %--------------------------------------------------------------
            if ~isfield(xVi,'V')

                %-F-threshold & accumulate spatially whitened Y*Y'
                %----------------------------------------------------------
                j   = sum((Hsqr*beta).^2,1)/trMV > UF*ResSS/trRV;
                j   = find(j);
                if ~isempty(j)
                    q  = size(j,2);
                    s  = s + q;
                    q  = spdiags(sqrt(trRV./ResSS(j)'),0,q,q);
                    Y  = Y(:,j)*q;
                    Cy = Cy + Y*Y';
                end

            end % (xVi,'V')

            %-if we are saving the WLS (ML) parameters
            %--------------------------------------------------------------
            if isfield(xX,'W')

                %-sample covariance and mean of Y (all voxels)
                %----------------------------------------------------------
                CY         = CY + Y*Y';
                EY         = EY + sum(Y,2);

                %-Save betas etc. for current plane as we go along
                %----------------------------------------------------------
                CrBl       = [CrBl,    beta];
                CrResI     = [CrResI,  res(i_res,:)];
                CrResSS    = [CrResSS, ResSS];

            end % (xX,'W')
            clear Y                         %-Clear to save memory

        end % (CrS)

        %-Append new inmask voxel locations and volumes
        %------------------------------------------------------------------
        XYZ(:,S + (1:CrS)) = xyz(:,Cm);     %-InMask XYZ voxel coords
        Q                  = [Q I(Cm)];     %-InMask XYZ voxel indices
        S                  = S + CrS;       %-Volume analysed (voxels)

    end % (bch)

    %-Plane complete, write plane to image files (unless 1st pass)
    %======================================================================
    if isfield(xX,'W')

        fprintf('%s%30s',repmat(sprintf('\b'),1,30),'...saving plane'); %-#

        jj = NaN(xdim,ydim,numel(CrPl));

        %-Write Mask image
        %------------------------------------------------------------------
        if ~isempty(Q), jj(Q) = 1; end
        VM    = spm_write_plane(VM, ~isnan(jj), CrPl);

        %-Write beta images
        %------------------------------------------------------------------
        for i = 1:nBeta
            if ~isempty(Q), jj(Q) = CrBl(i,:); end
            Vbeta(i) = spm_write_plane(Vbeta(i), jj, CrPl);
        end

        %-Write standardised residual images
        %------------------------------------------------------------------
        for i = 1:nSres
            if ~isempty(Q), jj(Q) = CrResI(i,:)./sqrt(CrResSS/erdf); end
            VResI(i) = spm_write_plane(VResI(i), jj, CrPl);
        end

        %-Write ResSS into ResMS (variance) image scaled by tr(RV) above
        %------------------------------------------------------------------
        if ~isempty(Q), jj(Q) = CrResSS; end
        VResMS  = spm_write_plane(VResMS, jj, CrPl);

    end % (xX,'W')

    %-Report progress
    %----------------------------------------------------------------------
    fprintf('%s%30s',repmat(sprintf('\b'),1,30),'...done');             %-#
    spm_progress_bar('Set',100*(bch + nbch*(z - 1))/(nbch*zdim));

end % (for z = 1:zdim)
fprintf('\n');                                                          %-#
spm_progress_bar('Clear')

%==========================================================================
% - P O S T   E S T I M A T I O N   C L E A N U P
%==========================================================================
if S == 0, spm('alert!','No inmask voxels - empty analysis!'); return; end

%-average sample covariance and mean of Y (over voxels)
%--------------------------------------------------------------------------
CY   = CY/S;
EY   = EY/S;
CY   = CY - EY*EY';

%-If not defined, compute non-sphericity V using ReML Hyperparameters
%==========================================================================
if ~isfield(xVi,'V')

    %-check there are signficant voxels
    %----------------------------------------------------------------------
    if s == 0
        spm('FigName','Stats: no significant voxels',Finter);
        spm('Pointer','Arrow');
        if isfield(SPM.xGX,'rg')&&~isempty(SPM.xGX.rg)
            figure(Finter);
            plot(SPM.xGX.rg);
            spm('alert*',{'Please check your data'; ...
                'There are no significant voxels';...
                'The globals are plotted for diagnosis'});
        else
            spm('alert*',{'Please check your data'; ...
                'There are no significant voxels'});
        end
        warning('Please check your data: There are no significant voxels.');
        return
    end

    %-ReML estimate of residual correlations through hyperparameters (h)
    %----------------------------------------------------------------------
    str    = 'Temporal non-sphericity (over voxels)';
    fprintf('%-40s: %30s\n',str,'...ReML estimation');                  %-#
    Cy     = Cy/s;

    % ReML for separable designs and covariance components
    %----------------------------------------------------------------------
    if isstruct(xX.K)
        m     = length(xVi.Vi);
        h     = zeros(m,1);
        V     = sparse(nScan,nScan);
        for i = 1:length(xX.K)

            % extract blocks from bases
            %--------------------------------------------------------------
            q     = xX.K(i).row;
            p     = [];
            Qp    = {};
            for j = 1:m
                if nnz(xVi.Vi{j}(q,q))
                    Qp{end + 1} = xVi.Vi{j}(q,q);
                    p           = [p j];
                end
            end

            % design space for ReML (with confounds in filter)
            %--------------------------------------------------------------
            Xp     = xX.X(q,:);
            try
                Xp = [Xp xX.K(i).X0];
            end

            % ReML
            %--------------------------------------------------------------
            fprintf('%-30s\n',sprintf('  ReML Block %i',i));
            [Vp,hp] = spm_reml(Cy(q,q),Xp,Qp);
            V(q,q)  = V(q,q) + Vp;
            h(p)    = hp;
        end
    else
        [V,h] = spm_reml(Cy,xX.X,xVi.Vi);
    end

    % normalize non-sphericity and save hyperparameters
    %----------------------------------------------------------------------
    V         = V*nScan/trace(V);
    xVi.h     = h;
    xVi.V     = V;                  % Save non-sphericity xVi.V
    xVi.Cy    = Cy;                 % spatially whitened <Y*Y'>
    SPM.xVi   = xVi;                % non-sphericity structure

    % If xX.W is not specified use W*W' = inv(V) to give ML estimators
    %----------------------------------------------------------------------
    if ~isfield(xX,'W')
        if spm_matlab_version_chk('7') >=0
            save('SPM','SPM','-V6');
        else
            save('SPM','SPM');
        end
        clear
        load SPM
        SPM = spm_spm(SPM);
        return
    end
end

%-Use non-sphericity xVi.V to compute [effective] degrees of freedom
%==========================================================================
xX.V            = spm_filter(xX.K,spm_filter(xX.K,W*V*W')');% KWVW'K'
[trRV trRVRV]   = spm_SpUtil('trRV',xX.xKXs,xX.V);          % trRV (for X)
xX.trRV         = trRV;                                     % <R'*y'*y*R>
xX.trRVRV       = trRVRV;                                   %-Satterthwaite
xX.erdf         = trRV^2/trRVRV;                            % approximation
xX.Bcov         = xX.pKX*xX.V*xX.pKX';                      % Cov(beta)

%-Set VResMS scalefactor as 1/trRV (raw voxel data is ResSS)
%--------------------------------------------------------------------------
VResMS.pinfo(1) = 1/xX.trRV;
VResMS          = spm_create_vol(VResMS);

%-Smoothness estimates of component fields and RESEL counts for volume
%==========================================================================
try
    FWHM = SPM.xVol.FWHM;
    VRpv = SPM.xVol.VRpv;
    R    = SPM.xVol.R;
catch
    [FWHM,VRpv,R] = spm_est_smoothness(VResI,VM,[nScan erdf]);
end

%-Delete the residuals images
%==========================================================================
% j = spm_select('List',SPM.swd,'^ResI_.{4}\..{3}$');
% for  k = 1:size(j,1)
%     spm_unlink(deblank(j(k,:)));
% end

%-Compute scaled design matrix for display purposes
%--------------------------------------------------------------------------
xX.nKX        = spm_DesMtx('sca',xX.xKXs.X,xX.name);

%-Save remaining results files and analysis parameters
%==========================================================================
fprintf('%-40s: %30s','Saving results','...writing');                   %-#

%-place fields in SPM
%--------------------------------------------------------------------------
SPM.xVol.XYZ   = XYZ(:,1:S);        %-InMask XYZ coords (voxels)
SPM.xVol.M     = M;                 %-voxels -> mm
SPM.xVol.iM    = inv(M);            %-mm -> voxels
SPM.xVol.DIM   = DIM;               %-image dimensions
SPM.xVol.FWHM  = FWHM;              %-Smoothness data
SPM.xVol.R     = R;                 %-Resel counts
SPM.xVol.S     = S;                 %-Volume (voxels)
SPM.xVol.VRpv  = VRpv;              %-Filehandle - Resels per voxel

SPM.Vbeta      = Vbeta;             %-Filehandle - Beta
SPM.VResMS     = VResMS;            %-Filehandle - Hyperparameter
SPM.VM         = VM;                %-Filehandle - Mask

SPM.xVi        = xVi;               % non-sphericity structure
SPM.xVi.CY     = CY;                %-<(Y - <Y>)*(Y - <Y>)'>

SPM.xX         = xX;                %-design structure

SPM.xM         = xM;                %-mask structure

SPM.xCon       = struct([]);        %-contrast structure

SPM.SPMid      = SPMid;
SPM.swd        = pwd;

%-Save analysis parameters in SPM.mat file
%--------------------------------------------------------------------------
if spm_matlab_version_chk('7') >=0
    save('SPM','SPM','-V6');
else
    save('SPM','SPM');
end

%==========================================================================
%- E N D: Cleanup GUI
%==========================================================================
fprintf('%s%30s\n',repmat(sprintf('\b'),1,30),'...done')                %-#
spm('FigName','Stats: done',Finter); spm('Pointer','Arrow')
fprintf('%-40s: %30s\n','Completed',spm('time'))                        %-#
fprintf('...use the results section for assessment\n\n')                %-#


classdef CasadiSolver < handle

  properties
    timeMeasures
    stageList
    collocationList
    nlpData
    stats
  end

  properties (Access = private)
    controls_regularization
    controls_regularization_value
  end

  methods

    function self = CasadiSolver(stageList, transitionList, ...
                                 nlp_casadi_mx, ...
                                 controls_regularization, controls_regularization_value, ...
                                 casadi_options, verbose, problem_userdata, transition_type)

      ocl.utils.assert(length(stageList)==length(transitionList)+1, ...
                'You need to specify Ns-1 transitions for Ns stages.');

      self.controls_regularization = controls_regularization;
      self.controls_regularization_value = controls_regularization_value;

      constructTotalTic = tic;

      % create variables as casadi symbolics
      if nlp_casadi_mx
        casadi_sym = @casadi.MX.sym;
      else
        casadi_sym = @casadi.SX.sym;
      end

      vars = cell(length(stageList), 1);
      costs = cell(length(stageList), 1);
      constraints = cell(length(stageList), 1);
      constraints_LB = cell(length(stageList), 1);
      constraints_UB = cell(length(stageList), 1);

      v_stage = [];

      collocationList = cell(length(stageList), 1);

      for k=1:length(stageList)
        stage = stageList{k};

        x_struct = stage.x_struct;
        z_struct = stage.z_struct;
        u_struct = stage.u_struct;
        p_struct = stage.p_struct;
        x_order = stage.x_order;

        daefh = stage.daefh;
        pathcostsfh = stage.pathcostsfh;
        gridcostsfh = stage.gridcostsfh;
        gridconstraintsfh = stage.gridconstraintsfh;
        terminalcostfh = stage.terminalcostfh;

        userdata = stage.userdata;

        nx = stage.nx;
        nu = stage.nu;
        np = stage.np;
        d = stage.d;
        H_norm = stage.H_norm;
        T = stage.T;
        N = stage.N;

        if length(stageList) > 1
          name_suffix = ['_s',mat2str(k)];
        else
          name_suffix = '';
        end

        % create casadi primitives for model functions
        x = ocl.casadi.structToSym(x_struct, casadi_sym, name_suffix);
        z = ocl.casadi.structToSym(z_struct, casadi_sym, name_suffix);
        u = ocl.casadi.structToSym(u_struct, casadi_sym, name_suffix);
        p = ocl.casadi.structToSym(p_struct, casadi_sym, name_suffix);

        % casadi dae function
        [casadi_ode_sym, casadi_alg_sym] = ocl.model.dae( ...
          daefh, x_struct, z_struct, u_struct, p_struct, x_order, ...
          x, z, u, p, userdata);
        casadi_dae_fun = casadi.Function('odefun', {x,z,u,p}, {casadi_ode_sym, casadi_alg_sym});
        daefun = @(x,z,u,p) ocl.casadi.daefun(casadi_dae_fun,x,z,u,p);

        % casadi pathcost function
        pathcosts_sym = ocl.model.pathcosts(...
          pathcostsfh,x_struct, z_struct, u_struct, p_struct, ...
          x, z, u, p, userdata);
        casadi_pathcost_fun = casadi.Function('pathcosts', {x,z,u,p}, {pathcosts_sym});
        pathcostfun = @(x,z,u,p) ocl.casadi.pathcostfun(casadi_pathcost_fun,x,z,u,p);

        collocation = ocl.collocation.Collocation(x_struct, z_struct, u_struct, p_struct, x_order, daefun, pathcostfun, d);
        ni = collocation.num_i;

        x = casadi_sym(['x','_s',mat2str(k)], nx);
        vi = casadi_sym(['vi','_s',mat2str(k)], ni);
        u = casadi_sym(['u','_s',mat2str(k)], nu);
        h = casadi_sym(['h','_s',mat2str(k)]);
        p = casadi_sym(['p','_s',mat2str(k)], np);

        [xF, cost_integr, equations] = ocl.collocation.equations(collocation, x, vi, u, h, p);
        integrator_fun = casadi.Function('sys', {x,vi,u,h,p}, {xF, cost_integr, equations});

        integratormap = integrator_fun.map(N, 'serial');

        nv_stage = ocl.simultaneous.nvars(N, nx, ni, nu, np);
        v_last_stage = v_stage;
        v_stage = casadi_sym(['v','_s',mat2str(k)], nv_stage);

        gridcostfun = @(k,K,x,p) ocl.model.gridcosts(gridcostsfh, x_struct, p_struct, k, K, x, p, userdata);
        gridconstraintfun = @(k,K,x,p) ocl.model.gridconstraints(gridconstraintsfh, x_struct, p_struct, k, K, x, p, userdata);
        terminalcostfun = @(x,p) ocl.model.terminalcost(terminalcostfh, x_struct, p_struct, x, p, userdata);

        [costs_stage, constraints_stage, ...
         constraints_LB_stage, constraints_UB_stage] = ...
            ocl.simultaneous.equations(H_norm, T, nx, ni, nu, np, ...
                                       gridcostfun, gridconstraintfun, ...
                                       terminalcostfun, ...
                                       integratormap, v_stage, ...
                                       controls_regularization, controls_regularization_value);

        collocationList{k} = collocation;

        transition_eq = [];
        transition_lb = [];
        transition_ub = [];
        if k >= 2
          [x0_cur, p_cur] = ocl.simultaneous.getFirstState(stageList{k}, collocationList{k}, v_stage);
          [xF_prev, p_prev] = ocl.simultaneous.getLastState(stageList{k-1}, collocationList{k-1}, v_last_stage);
          transition_fun = transitionList{k-1};

          tansition_handler = ocl.Constraint(problem_userdata);

          x0_cur = ocl.Variable.create(stageList{k}.x_struct, x0_cur);
          xF_prev = ocl.Variable.create(stageList{k-1}.x_struct, xF_prev);

          p_cur = ocl.Variable.create(stageList{k}.p_struct, p_cur);
          p_prev = ocl.Variable.create(stageList{k-1}.p_struct, p_prev);

          if transition_type == 1
            transition_fun(tansition_handler,x0_cur,xF_prev);
          elseif transition_type == 2
            transition_fun(tansition_handler,x0_cur,xF_prev,p_cur,p_prev);
          else
            ocl.error('Transition type invalid.');
          end

          transition_eq = tansition_handler.values;
          transition_lb = tansition_handler.lowerBounds;
          transition_ub = tansition_handler.upperBounds;
        end

        vars{k} = v_stage;
        costs{k} = costs_stage;
        constraints{k} = vertcat(transition_eq, constraints_stage);
        constraints_LB{k} = vertcat(transition_lb, constraints_LB_stage);
        constraints_UB{k} = vertcat(transition_ub, constraints_UB_stage);

      end

      v = vertcat(vars{:});
      costs = sum([costs{:}]);
      constraints = vertcat(constraints{:});
      constraints_LB = vertcat(constraints_LB{:});
      constraints_UB = vertcat(constraints_UB{:});

      % get struct with nlp for casadi
      casadiNLP = struct;
      casadiNLP.x = v;
      casadiNLP.f = costs;
      casadiNLP.g = constraints;
      casadiNLP.p = [];

      if ~verbose
        casadi_options.ipopt.print_level = 0;
        casadi_options.print_time = 0;
      end

      constructSolverTic = tic;
      casadiSolver = casadi.nlpsol('my_solver', 'ipopt', casadiNLP, casadi_options);
      constructSolverTime = toc(constructSolverTic);

      nlpData = struct;
      nlpData.casadiNLP = casadiNLP;
      nlpData.constraints_LB = constraints_LB;
      nlpData.constraints_UB = constraints_UB;
      nlpData.solver = casadiSolver;

      timeMeasures.constructTotal = toc(constructTotalTic);
      timeMeasures.constructSolver = constructSolverTime;

      self.stageList = stageList;
      self.nlpData = nlpData;
      self.timeMeasures = timeMeasures;
      self.collocationList = collocationList;
    end

    function igMerged = getInitialGuessWithUserData(self)

      stage_list = self.stageList;
      colloc_list = self.collocationList;

      igMerged = cell(length(stage_list), 1);

      ig = self.getInitialGuess();
      for k=1:length(stage_list)
        stage = stage_list{k};
        colloc = colloc_list{k};

        igMerged{k} = ocl.simultaneous.getInitialGuessWithUserData(ig{k}, stage, colloc);
      end
    end

    function igList = getInitialGuess(self)
      stage_list = self.stageList;

      igList = cell(length(stage_list),1);
      for k=1:length(stage_list)
        stage = stage_list{k};

        colloc = self.collocationList{k};

        N = stage.N;
        x_struct = stage.x_struct;
        z_struct = stage.z_struct;
        u_struct = stage.u_struct;
        p_struct = stage.p_struct;

        nx = stage.nx;
        nu = stage.nu;
        np = stage.np;
        H_norm = stage.H_norm;
        T = stage.T;

        vi_struct = colloc.vars;
        ni = colloc.num_i;

        x_bounds = stage.x_bounds;
        x0_bounds = stage.x0_bounds;
        xF_bounds = stage.xF_bounds;
        z_bounds = stage.z_bounds;
        u_bounds = stage.u_bounds;
        p_bounds = stage.p_bounds;

        [x_lb, x_ub] = ocl.model.bounds(x_struct, x_bounds);
        [x0_lb, x0_ub] = ocl.model.bounds(x_struct, x0_bounds);
        [xF_lb, xF_ub] = ocl.model.bounds(x_struct, xF_bounds);

        [z_lb, z_ub] = ocl.model.bounds(z_struct, z_bounds);
        [u_lb_traj, u_ub_traj] = ocl.model.boundsTrajectory(u_struct, u_bounds, N);
        [p_lb, p_ub] = ocl.model.bounds(p_struct, p_bounds);

        varsStruct = ocl.simultaneous.variablesStruct(N, x_struct, vi_struct, u_struct, p_struct);

        ig = ocl.simultaneous.getInitialGuess(H_norm, T, nx, ni, nu, np, ...
                                    x0_lb, x0_ub, xF_lb, xF_ub, x_lb, x_ub, ...
                                    z_lb, z_ub, u_lb_traj, u_ub_traj, p_lb, p_ub, ...
                                    vi_struct);

        igList{k} = ocl.Variable.create(varsStruct, ig);
      end

    end

    function [sol,times,info] = solve(self,v0)
      % solve(initialGuess)

      solveTotalTic = tic;

      stage_list = self.stageList;
      collocation_list = self.collocationList;

      ig_list = v0;

      for k=1:length(v0)
        v0{k} = v0{k}.value;
      end

      lbv = cell(length(stage_list),1);
      ubv = cell(length(stage_list),1);
      for k=1:length(stage_list)

        stage = stage_list{k};
        colloc = collocation_list{k};

        nx = stage.nx;
        nu = stage.nu;
        np = stage.np;
        H_norm = stage.H_norm;
        T = stage.T;
        N = length(H_norm);

        x_struct = stage.x_struct;
        z_struct = stage.z_struct;
        u_struct = stage.u_struct;
        p_struct = stage.p_struct;

        x_bounds = stage.x_bounds;
        x0_bounds = stage.x0_bounds;
        xF_bounds = stage.xF_bounds;
        z_bounds = stage.z_bounds;
        u_bounds = stage.u_bounds;
        p_bounds = stage.p_bounds;

        vi_struct = colloc.vars;
        ni = colloc.num_i;

        [x_lb, x_ub] = ocl.model.bounds(x_struct, x_bounds);
        [x0_lb, x0_ub] = ocl.model.bounds(x_struct, x0_bounds);
        [xF_lb, xF_ub] = ocl.model.bounds(x_struct, xF_bounds);

        [z_lb, z_ub] = ocl.model.bounds(z_struct, z_bounds);
        [u_lb_traj, u_ub_traj] = ocl.model.boundsTrajectory(u_struct, u_bounds, N);
        [p_lb, p_ub] = ocl.model.bounds(p_struct, p_bounds);

        [vi_lb, vi_ub] = ocl.collocation.bounds(vi_struct, x_lb, x_ub, z_lb, z_ub);

        [lbv_stage, ubv_stage] = ocl.simultaneous.bounds(H_norm, T, nx, ni, nu, np, ...
                                      x_lb, x_ub, x0_lb, x0_ub, xF_lb, xF_ub, ...
                                      vi_lb, vi_ub, u_lb_traj, u_ub_traj, p_lb, p_ub);
        lbv{k} = lbv_stage;
        ubv{k} = ubv_stage;
      end

      v0 = vertcat(v0{:});
      lbv = vertcat(lbv{:});
      ubv = vertcat(ubv{:});

      args = struct;
      args.lbg = self.nlpData.constraints_LB;
      args.ubg = self.nlpData.constraints_UB;
      args.p = [];
      args.lbx = lbv;
      args.ubx = ubv;
      args.x0 = v0;

      % execute solver
      solveCasadiTic = tic;
      sol = self.nlpData.solver.call(args);
      solveCasadiTime = toc(solveCasadiTic);

      if strcmp(self.nlpData.solver.stats().return_status, 'NonIpopt_Exception_Thrown')
        ocl.utils.warning('Solver was interrupted by user.');
      end

      sol_values = sol.x.full();

      sol = cell(length(stage_list),1);
      times = cell(length(stage_list),1);
      i_stage = 1;
      nlpFunEvalTic = tic;
      for k=1:length(stage_list)

        stage = stage_list{k};
        colloc = collocation_list{k};

        nx = stage.nx;
        nu = stage.nu;
        np = stage.np;
        N = stage.N;
        H_norm = stage.H_norm;

        ni = colloc.num_i;
        nt = colloc.num_t;

        nv_stage = ocl.simultaneous.nvars(N, nx, ni, nu, np);

        % unpack solution of this stage to state/controls trajectories
        V = sol_values(i_stage:i_stage+nv_stage-1);
        sol_out = ocl.Variable.create(ig_list{k}.type, V);

        [~,~,~,~,H] = ocl.simultaneous.variablesUnpack(V, N, nx, ni, nu, np);
        colloc_times = ocl.simultaneous.times(H(1)*H_norm, colloc);
        times_struct = ocl.simultaneous.timesStruct(N, nt);
        times_out = ocl.Variable.create(times_struct, colloc_times);

        i_stage = i_stage + nv_stage;

        sol{k} = sol_out;
        times{k} = times_out;
      end
      nlpFunEvalTime = toc(nlpFunEvalTic);

      self.timeMeasures.solveTotal      = toc(solveTotalTic);
      self.timeMeasures.solveCasadi     = solveCasadiTime;
      self.timeMeasures.nlpFunEval      = nlpFunEvalTime;

      self.stats = self.nlpData.solver.stats();

      info = self.info();
    end

    function r = info(self)
      r = struct;
      r.timeMeasures = self.timeMeasures;
      r.success = self.stats.success;
      r.ipopt_stats = self.stats;
    end
  end

end


function RtOut = transformCameraRt(RtIn)

RtOut = [RtIn(1:3,1:3)', - RtIn(1:3,1:3)'* RtIn(1:3,4)];


%% Example
% real time control of the KUKA iiwa 7 R 800
% the impedence control is turned on
% Moving first joint of the robot, using a sinisoidal function

% The external torques are plotted in real-time during the test
% the feedback from the measured torques can be used to for a closed loop
% control

% An example script, it is used to show how to use the different
% functions of the KUKA Sunrise matlab toolbox

% First run the following script in Matlab
% Then start the client on the KUKA iiwa controller

% Mohammad SAFEEA, 24th of April 2018

close all,clear all;clc;

warning('off')

ip='172.31.1.147'; % The IP of the controller
% start a connection with the server
global t_Kuka;
t_Kuka=net_establishConnection( ip );

if ~exist('t_Kuka','var') || isempty(t_Kuka) || strcmp(t_Kuka.Status,'closed')
  warning('Connection could not be establised, script aborted');
  return;
else

      %% Move point to point to an initial position
        jPos={0,0,0,-pi/2,0,pi/2,0};
      setBlueOff(t_Kuka); % turn Off blue light

      relVel=0.15;
      movePTPJointSpace( t_Kuka , jPos, relVel); % move to initial configuration
     %% Pause for 3 seocnds
     pause(3);
        %% Start direct servo in joint space
        massOfTool=0.0; % the mass of the tool attached to flange in Kg
        cOMx=0; % X coordinate of the center of mass of the tool in (mm)
        cOMy=0; % Y coordinate of the center of mass of the tool in (mm)
        cOMz=40; % Z coordinate of the center of mass of the tool in (mm)
        cStiness=900; % cartizian stifness
        rStifness=80; % rotational stifness
        nStifness=50; % null space stifness

        % Start the realtime control with impedence
        realTime_startImpedanceJoints(t_Kuka,massOfTool,cOMx,cOMy,cOMz,...
        cStiness,rStifness,nStifness);

       w=0.6; % motion constants, frequency rad/sec
       A=0.2; % motion constants, amplitude of motion

       a=datevec(now);
       t0=a(6)+a(5)*60+a(4)*60*60; % calculate initial time

     dt=0;
     tstart=t0;
     counter=0;
     duration=20*60; %1 minutes
     %% Control loop
     % real time plot handles
         colors={'k','b','r','g','c','m','y'};
         figureHandle=figure('Units','inches','Position',[0 0 5 3.75]);
         numOfSamples=120; % number of samples to show in the plot
         timeVec=1:numOfSamples;
         tawVec=zeros(7,numOfSamples);
         plotHandle=[];
         for i=1:7
            plotHandle=[plotHandle,plot(timeVec,tawVec(i,:),colors{i},'LineWidth',2)];
            hold on;
         end
        % format the plot
        ylim([0,8]);
        temp=title('External torques');
        set(temp,'FontSize',16);
        temp=xlabel('Time (seconds)');
        set(temp,'FontSize',14);
        temp=ylabel('Distance (m)');
        set(temp,'FontSize',14);

       while(dt<duration)
         %% perform trajectory calculation here
          a=datevec(now);
          time=a(6)+a(5)*60+a(4)*60*60;
          dt=time-t0;

          temp=A*(1-cos(w*dt));
            for dacount=1:7
                jPosCommand{dacount}=jPos{dacount}+temp;
            end
          counter=counter+1;
          %% Send joint positions to robot
          taw=sendJointsPositionsExTorque( t_Kuka ,jPosCommand);

          tawTemp=zeros(7,1);
          for i=1:7
              tawTemp(i)=taw{i};
              torque_array(i,counter)=taw{i};
          end
          % delete first measurment
          tawVec(:,1)=[];
          % add new measurment to end of arrays
          tawVec=[tawVec,tawTemp];
          % update plots data
          for i=1:6
            set(plotHandle(i),'YData',tawVec(i,:));
          end
          set(plotHandle(7),'YData',tawVec(i,:));
          % show plot data
          set(figureHandle,'Visible','on');

       end

       %% Stop the realtime control with impedence motion
       realTime_stopImpedanceJoints( t_Kuka );

       fprintf('\n Motion stopped \n');
       pause(2);
       %% turn off light
       setBlueOff(t_Kuka);

      %% turn off the server
       net_turnOffServer( t_Kuka );

       fclose(t_Kuka);

end
warning('on')


function [outdata] = rev_spatial_parse(indata)
% rev_spatial_parse  User-defined function for reverse spatial parsing.
% Input: 'inbits'
%    - input bits for spatial parsing.
% Output: 'outbits'
%    - spatial parsing output bits.

% Get spatial parsing info.
rate   = indata(1);
Nss    = indata(2);
ssize  = indata(3);
remdat = indata(4:end);
inbits = remdat(1:(end/2));
inbits = inbits(1:Nss*ssize);
mscale = remdat((1+end/2):end);
mscale = mscale(1:Nss*ssize);

% Init. rev. spatial parsing
switch rate
    case {1}
        nbits1 = 1;
    case {2,3}
        nbits1 = 2;
    case {4,5}
        nbits1 = 4;
    case {6,7,8}
        nbits1 = 6;
    otherwise
        nbits1 = 1;
end
nbits2 = nbits1;  nbits3 = nbits1;  nbits4 = nbits1;

switch Nss
    case 1
        totbits = nbits1;
    case 2
        totbits = nbits1 + nbits2;
    case 3
        totbits = nbits1 + nbits2 + nbits3;
    case 4
        totbits = nbits1 + nbits2 + nbits3 + nbits4;
end

% Bits: input/output streams...
ssbits1 = zeros(ssize, 1);
ssbits2 = zeros(ssize, 1);
ssbits3 = zeros(ssize, 1);
ssbits4 = zeros(ssize, 1);
outbits = zeros(4*ssize, 1);

% Metric scaling: input/output streams...
ssmscl1 = zeros(ssize, 1);
ssmscl2 = zeros(ssize, 1);
ssmscl3 = zeros(ssize, 1);
ssmscl4 = zeros(ssize, 1);
outmscl = zeros(4*ssize, 1);

% Do rev. spatial parsing...
s1 = 0;
s2 = nbits1;
s3 = nbits1+nbits2;
s4 = nbits1+nbits2+nbits3;
cnt = length(inbits)/totbits - 1;
if (Nss>=1)
    ssbits1 = inbits(1+end*0/Nss : end*1/Nss);
    ssmscl1 = mscale(1+end*0/Nss : end*1/Nss);
    for i=1:nbits1 % stream #1
        outbits(i+s1:totbits:i+s1+totbits*cnt) = ssbits1(i:nbits1:i+nbits1*cnt);
        outmscl(i+s1:totbits:i+s1+totbits*cnt) = ssmscl1(i:nbits1:i+nbits1*cnt);
    end
end
if (Nss>=2)
    ssbits2 = inbits(1+end*1/Nss : end*2/Nss);
    ssmscl2 = mscale(1+end*1/Nss : end*2/Nss);
    for i=1:nbits2 % stream #2
        outbits(i+s2:totbits:i+s2+totbits*cnt) = ssbits2(i:nbits2:i+nbits2*cnt);
        outmscl(i+s2:totbits:i+s2+totbits*cnt) = ssmscl2(i:nbits2:i+nbits2*cnt);
    end
end
if (Nss>=3)
    ssbits3 = inbits(1+end*2/Nss : end*3/Nss);
    ssmscl3 = mscale(1+end*2/Nss : end*3/Nss);
    for i=1:nbits3 % stream #3
        outbits(i+s3:totbits:i+s3+totbits*cnt) = ssbits3(i:nbits3:i+nbits3*cnt);
        outmscl(i+s3:totbits:i+s3+totbits*cnt) = ssmscl3(i:nbits3:i+nbits3*cnt);
    end
end
if (Nss>=4)
    ssbits4 = inbits(1+end*3/Nss : end*4/Nss);
    ssmscl4 = mscale(1+end*3/Nss : end*4/Nss);
    for i=1:nbits4 % stream #4
        outbits(i+s4:totbits:i+s4+totbits*cnt) = ssbits4(i:nbits4:i+nbits4*cnt);
        outmscl(i+s4:totbits:i+s4+totbits*cnt) = ssmscl4(i:nbits4:i+nbits4*cnt);
    end
end

% Reverse spatial parsing output
outdata = [outbits; outmscl];


%{
This code demonstrates how to warp a depth map (togeter with a label map) from a camera to another view.
It create a mesh using the depth map from a camera and render the mesh in OpenGL.
Therefore, it will not create artifacts with a lot of gaps in a warpped result.

compile
   mex WarpMesh.cpp -lGLU -lOSMesa
   you need to install osmesa to compile it
   in Mac OS X 10.9.2, you just need to install X11 on mac (xquartz) comes with mesa
   mex WarpMesh.cpp -lGLU -lOSMesa -I/opt/X11/include/ -L/opt/X11/lib/

Email Jianxiong if you have questions. Cite the following paper if you use this code:

J. Xiao, A. Owens and A. Torralba
SUN3D: A Database of Big Spaces Reconstructed using SfM and Object Labels
Proceedings of 14th IEEE International Conference on Computer Vision (ICCV2013)

%}

close all;
clear

load demo.mat

[label,depth] = WarpMeshMatlab(XYZcamera,labelNow,K, Rt);

% warp color
[imageWarp,depth] = WarpMeshMatlabColor(XYZcamera,double(image),K, Rt);

figure
subplot(2,3,1);
imagesc(XYZcamera(:,:,3));
axis equal; axis tight; axis off; title('before warping');

subplot(2,3,2);
imagesc(labelNow);
axis equal; axis tight; axis off; title('before warping');

subplot(2,3,4);
imagesc(depth);
axis equal; axis tight; axis off; title('after warping');

subplot(2,3,5);
imagesc(label);
axis equal; axis tight; axis off; title('after warping');

subplot(2,3,3);
imagesc(image);
axis equal; axis tight; axis off; title('before warping');

subplot(2,3,6);
imagesc(imageWarp/255);
axis equal; axis tight; axis off; title('after warping');


function options = selectSOSmodel(F,options,NonLinearParameterization,noRANK,IntegerData,UncertainData,obj)
switch options.sos.model
    case 0
        constraint_classes = constraintclass(F);
        noCOMPLICATING = ~any(ismember([7 8 9 10 12 13 14 15],constraint_classes));
        if noCOMPLICATING & ~NonLinearParameterization && noRANK && ~IntegerData && ~isa(obj,'logdet')
            options.sos.model = 1;
            if options.verbose>0;disp('Using kernel representation (options.sos.model=1).');end
        else
            if NonLinearParameterization
                if options.verbose>0;disp('Using image representation (options.sos.model=2). Nonlinear parameterization found');end
            elseif ~noRANK
                if options.verbose>0;disp('Using image representation (options.sos.model=2). SOS-rank constraint was found.');end
            elseif IntegerData
                if options.verbose>0;disp('Using image representation (options.sos.model=2). Integrality constraint was found.');end
            elseif UncertainData
                if options.verbose>0;disp('Using image representation (options.sos.model=2). Uncertain data was found.');end
            elseif isa(obj,'logdet')
                if options.verbose>0;disp('Using image representation (options.sos.model=2). Logdet objective was found.');end
            else
                if options.verbose>0;disp('Using image representation (options.sos.model=2). Integer data, KYPs or similar was found.');end
            end
            options.sos.model = 2;
        end
    case 1
        if NonLinearParameterization
            if options.verbose>0;disp('Switching to image model due to nonlinear parameterization (not supported in kernel model).');end
            options.sos.model = 2;
        end
        if ~noRANK
            if options.verbose>0;disp('Switching to image model due to SOS-rank constraints (not supported in kernel model).');end
            options.sos.model = 2;
        end
        if IntegerData
            if options.verbose>0;disp('Switching to image model due to integrality constraints (not supported in kernel model).');end
            options.sos.model = 2;
        end
    case 3
    otherwise
end


function [rr,t,sqi] = Analyze_ABP_PPG_Waveforms(Waveform,Type,HRVparams,detectedQRS,subjectID)
%
%   Analyze_ABP_PPG_Waveforms(Waveform,Type,HRVparams,detectedQRS,subjectID)
%	OVERVIEW:
%       Analyze ABP or PPG waveform
%
%   INPUT:
%       Waveform    - matrix containing the a raw signal in each column
%       Type        - array containing the signal type of waveforms in each column:
%                     'APB' for ABP waveform
%                     'PPG' for PPG waveform
%       HRVparams   - struct of settings for HRV analysis
%       subjectID   - string to identify current subject
%
%   OUTPUT
%       rr          - (seconds) Vector containing RR interval
%       t           - (seconds) Time of the rr interval data
%       sqi         - (optional) Signal Quality Index; Requires a
%                     matrix with at least two columns. Column 1
%                     should be timestamps of each sqi measure, and
%                     Column 2 should be SQI on a scale from 0 to 1.
%
%   DEPENDENCIES & LIBRARIES:
%       PhysioNet Cardiovascular Signal Toolbox
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%
%   REFERENCE:
%   Vest et al. "An Open Source Benchmarked HRV Toolbox for Cardiovascular
%   Waveform and Interval Analysis" Physiological Measurement (In Press), 2018.
%
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Written by Giulia Da Poian (giulia.dap@gmail.com) on Sep 6, 2017.
%       Dependent scripts written by various authors
%       (see functions for details)
%	COPYRIGHT (C) 2018
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

NmbOfSigs = size(Waveform,2);
AnnotationFolder = strcat(HRVparams.writedata, filesep, 'Annotation', filesep);
if ~exist(AnnotationFolder, 'dir')
   mkdir(AnnotationFolder)
end
addpath(AnnotationFolder)

rr = [];
t = [];
sqi = [];

for i = 1:NmbOfSigs

    current_type = Type{i};
    switch current_type
        case 'PPG'
            % PPG Detection - qppg
            [PPGann] = qppg(Waveform(:,i),HRVparams.Fs);
            % PPG SQI
            [ppgsqi_numeric, ~, ppgsqi]= calculate_ppgsqi(PPGann,Waveform(:,i),HRVparams.Fs);
            % Write PPG  annotations
            write_ann(strcat(AnnotationFolder, subjectID),HRVparams,'ppg',PPGann);
            write_ann(strcat(AnnotationFolder, subjectID),HRVparams,'sqippg',PPGann(1:length(ppgsqi_numeric)),char(ppgsqi),ppgsqi_numeric);

            rr = diff(PPGann)./HRVparams.Fs;
            t = PPGann(2:end)./HRVparams.Fs;
            sqi = [PPGann'./HRVparams.Fs, ppgsqi_numeric'./100];

        case 'ABP'
            % ABP
            ABPann = run_wabp(Waveform(:,i));
            % ABP SQI
            ABPfeatures =  abpfeature(Waveform(:,i), ABPann, HRVparams.Fs);
            [BeatQ, ~] = jSQI(ABPfeatures, ABPann, Waveform(:,i));

            if ~isempty(detectedQRS)
                % Pulse Transit Time
                ptt = pulsetransit(detectedQRS, ABPann);
                % Plot BP vs PTT
                syst = ABPfeatures(:,2);
                if HRVparams.gen_figs
                    figure;
                    plot(syst,ptt(:,3)./HRVparams.Fs,'o');
                    xlabel('BP (mmHg)'); ylabel('PTT (s)');
                    title('Pulse Transit Time - BP vs PTT (ABP - QRS)')
                end
            end
            % Write ABP annotations
            write_ann(strcat(AnnotationFolder, subjectID),HRVparams,'abpm',ABPann);
            write_ann(strcat(AnnotationFolder, subjectID),HRVparams,'sqiabp',ABPann,'N',int8(BeatQ(:,1)));%write_ann(strcat(AnnotationFolder, subjectID),HRVparams,'sqiabp',BeatQ(:,1));

    end
end


function []=panel2disp(n,N,m,p,k,T,Ymat,Xmat,Units,endo,exo,const,beta_gibbs,B_median,beta_median,beta_std,beta_lbound,beta_ubound,sigma_gibbs,sigma_median,D_estimates,gamma_estimates,ar,lambda1,lambda3,lambda4,startdate,enddate,forecast_record,forecast_estimates,Fcperiods,stringdates3,Fstartdate,Fcenddate,Feval,Fcomp,data_endo_c,data_endo_c_lags,data_exo_c,It,Bu,IRF,IRFt,pref,names,PriorExcel)

% obtain first a point estimate betatilde of the VAR coefficients
% this is simply the median
betatilde=beta_median;
Btilde=reshape(betatilde,k,n);

% check whether the model is stationary
[stationary eigmodulus]=bear.checkstable(betatilde,n,p,k);

% the other measures are unit-specific, hence, loop over units
for ii=1:N

% obtain predicted values
Yp(:,:,ii)=Xmat(:,:,ii)*B_median;
% then produce the corresponding residuals, using (1.9.4)
EPS(:,:,ii)=Ymat(:,:,ii)-Yp(:,:,ii);

% Compute then the sum of squared residuals
% compute first the RSS matrix, defined in (1.9.5)
RSS(:,:,ii)=EPS(:,:,ii)'*EPS(:,:,ii);
% retain only the diagonal elements to get the vector of RSSi values
rss(:,:,ii)=diag(RSS(:,:,ii));

% Go on calculating R2
% generate Mbar
Mbar=eye(T)-ones(T,T)/T;
% then compute the TSS matrix, defined in (1.9.8)
TSS(:,:,ii)=Ymat(:,:,ii)'*Mbar*Ymat(:,:,ii);
% generate the R2 matrix in (1.9.9)
R2(:,:,ii)=eye(n)-RSS(:,:,ii)./TSS(:,:,ii);
% retain only the diagonal elements to get the vector of R2 values
r2(:,:,ii)=diag(R2(:,:,ii));

% then calculate the adjusted R2, using (1.9.11)
R2bar(:,:,ii)=eye(n)-((T-1)/(T-k))*(eye(n)-R2(:,:,ii));
% retain only the diagonal elements to get the vector of R2bar values
r2bar(:,:,ii)=diag(R2bar(:,:,ii));

end

% now start displaying and saving the results

% preliminary task: create and open the txt file used to save the results

filelocation=fullfile(pref.results_path, [pref.results_sub '.txt']);
fid=fopen(filelocation,'wt');

% print toolbox header

fprintf('%s\n','');
fprintf(fid,'%s\n','');

% print the list of contributors
bear.printcontributors(fid);

% print then estimation results

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

toolboxinfo='BEAR toolbox estimates';
fprintf('%s\n',toolboxinfo);
fprintf(fid,'%s\n',toolboxinfo);

time=clock;
datestring=datestr(time);
dateinfo=['Date: ' datestring(1,1:11) '   Time: ' datestring(1,13:17)];
fprintf('%s\n',dateinfo);
fprintf(fid,'%s\n',dateinfo);

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

VARtypeinfo='Panel VAR: pooled estimator';
fprintf('%s\n',VARtypeinfo);
fprintf(fid,'%s\n',VARtypeinfo);

if IRFt==1
SVARinfo='structural decomposition: none';
fprintf('%s\n',SVARinfo);
fprintf(fid,'%s\n',SVARinfo);
elseif IRFt==2
SVARinfo='structural decomposition: choleski factorisation';
fprintf('%s\n',SVARinfo);
fprintf(fid,'%s\n',SVARinfo);
elseif IRFt==3
SVARinfo='structural decomposition: triangular factorisation';
fprintf('%s\n',SVARinfo);
fprintf(fid,'%s\n',SVARinfo);
end

temp='units: ';
for ii=1:N
temp=[temp ' ' Units{ii,1} ' '];
end
unitinfo=temp;
fprintf('%s\n',unitinfo);
fprintf(fid,'%s\n',unitinfo);

temp='endogenous variables: ';
for ii=1:n
temp=[temp ' ' endo{ii,1} ' '];
end
endoinfo=temp;
fprintf('%s\n',endoinfo);
fprintf(fid,'%s\n',endoinfo);

temp='exogenous variables: ';
if const==0 && m==0
temp=[temp ' none'];
elseif const==1 && m==1
temp=[temp ' constant '];
elseif const==0 && m>0
   for ii=1:m-1
   temp=[temp ' ' exo{ii,1} ' '];
   end
elseif const==1 && m>1
temp=[temp ' constant '];
   for ii=1:m-1
   temp=[temp ' ' exo{ii,1} ' '];
   end
end
exoinfo=temp;
fprintf('%s\n',exoinfo);
fprintf(fid,'%s\n',exoinfo);

sampledateinfo=['estimation sample: ' startdate '-' enddate];
fprintf('%s\n',sampledateinfo);
fprintf(fid,'%s\n',sampledateinfo);

samplelengthinfo=['sample size (omitting initial conditions): ' num2str(T)];
fprintf('%s\n',samplelengthinfo);
fprintf(fid,'%s\n',samplelengthinfo);

laginfo=['number of lags included in regression: ' num2str(p)];
fprintf('%s\n',laginfo);
fprintf(fid,'%s\n',laginfo);

hyperparam1='hyperparameters:';
fprintf('%s\n',hyperparam1);
fprintf(fid,'%s\n',hyperparam1);

if PriorExcel==1
    arprint=[];
    for ii=1:n
        arprint=[arprint num2str(ar(ii,1)) '  '];
    end
 hyperparam2=['autoregressive coefficients (ar):                ' arprint];
else
 hyperparam2=['autoregressive coefficients (ar):                ' num2str(ar(1,1))];
end
fprintf('%s\n',hyperparam2);
fprintf(fid,'%s\n',hyperparam2);

hyperparam3=['overall tightness (lambda1):              ' num2str(lambda1)];
fprintf('%s\n',hyperparam3);
fprintf(fid,'%s\n',hyperparam3);

hyperparam4=['lag decay (lambda3):                      ' num2str(lambda3)];
fprintf('%s\n',hyperparam4);
fprintf(fid,'%s\n',hyperparam4);

hyperparam5=['exogenous variable tightness (lambda4):   ' num2str(lambda4(1,1))];
fprintf('%s\n',hyperparam5);
fprintf(fid,'%s\n',hyperparam5);

% display coefficient estimates

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

coeffinfo=['VAR coefficients (Common to all units):'];
fprintf('%s\n',coeffinfo);
fprintf(fid,'%s\n',coeffinfo);

for ii=1:n
fprintf('%s\n','');
fprintf(fid,'%s\n','');
if ii~=1
fprintf('%s\n','');
fprintf(fid,'%s\n','');
end

endoinfo=['Endogenous: ' endo{ii,1}];
fprintf('%s\n',endoinfo);
fprintf(fid,'%s\n',endoinfo);

coeffheader=fprintf('%25s %15s %15s %15s %15s\n','','Median','St.dev','Low.bound','Upp.bound');
coeffheader=fprintf(fid,'%25s %15s %15s %15s %15s\n','','Median','St.dev','Low.bound','Upp.bound');

% handle the endogenous
   for jj=1:n
      for kk=1:p
      values=[beta_median((ii-1)*k+n*(kk-1)+jj,1) beta_std((ii-1)*k+n*(kk-1)+jj,1) beta_lbound((ii-1)*k+n*(kk-1)+jj,1) beta_ubound((ii-1)*k+n*(kk-1)+jj,1)];
      fprintf('%25s %15.3f %15.3f %15.3f %15.3f\n',strcat(endo{jj,1},'(-',int2str(kk),')'),values);
      fprintf(fid,'%25s %15.3f %15.3f %15.3f %15.3f\n',strcat(endo{jj,1},'(-',int2str(kk),')'),values);
      end
   end

% handle the exogenous
   % if there is no constant:
   if const==0
      % if there is no exogenous at all, obvioulsy, don't display anything
      if m==0
      % if there is no constant but some other exogenous, display them
      else
         for jj=1:m
         values=[beta_median(ii*k-m+jj,1) beta_std(ii*k-m+jj,1) beta_lbound(ii*k-m+jj,1) beta_ubound(ii*k-m+jj,1)];
         fprintf('%25s %15.3f %15.3f %15.3f %15.3f\n',exo{jj,1},values);
         fprintf(fid,'%25s %15.3f %15.3f %15.3f %15.3f\n',exo{jj,1},values);
         end
      end
   % if there is a constant
   else
   % display the results related to the constant
         values=[beta_median(ii*k-m+1,1) beta_std(ii*k-m+1,1) beta_lbound(ii*k-m+1,1) beta_ubound(ii*k-m+1,1)];
         fprintf('%25s %15.3f %15.3f %15.3f %15.3f\n','Constant',values);
         fprintf(fid,'%25s %15.3f %15.3f %15.3f %15.3f\n','Constant',values);
      % if there is no other exogenous, stop here
      if m==1
      % if there are other exogenous, display their results
      else
         for jj=1:m-1
         values=[beta_median(ii*k-m+jj+1,1) beta_std(ii*k-m+jj+1,1) beta_lbound(ii*k-m+jj+1,1) beta_ubound(ii*k-m+jj+1,1)];
         fprintf('%25s %15.3f %15.3f %15.3f %15.3f\n',exo{jj,1},values);
         fprintf(fid,'%25s %15.3f %15.3f %15.3f %15.3f\n',exo{jj,1},values);
         end
      end
   end

fprintf('%s\n','');
fprintf(fid,'%s\n','');

% display evaluation measures

   % loop over units
   for jj=1:N
   unitinfo=['unit: ' Units{jj,1}];
   fprintf('%s\n',unitinfo);
   fprintf(fid,'%s\n',unitinfo);

   rssinfo=['Sum of squared residuals: ' num2str(rss(ii,1,jj),'%.2f')];
   fprintf('%s\n',rssinfo);
   fprintf(fid,'%s\n',rssinfo);

   r2info=['R-squared: ' num2str(r2(ii,1,jj),'%.3f')];
   fprintf('%s\n',r2info);
   fprintf(fid,'%s\n',r2info);

   adjr2info=['adj. R-squared: ' num2str(r2bar(ii,1,jj),'%.3f')];
   fprintf('%s\n',adjr2info);
   fprintf(fid,'%s\n',adjr2info);

   fprintf('%s\n','');
   fprintf(fid,'%s\n','');

   end

end

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

% display VAR stability results
eigmodulus=reshape(eigmodulus,p,n);
stabilityinfo1=['Roots of the characteristic polynomial (modulus):'];
fprintf('%s\n',stabilityinfo1);
fprintf(fid,'%s\n',stabilityinfo1);
for jj=1:p
temp=num2str(eigmodulus(jj,1),'%.3f');
   for kk=2:n
   temp=[temp,'  ',num2str(eigmodulus(jj,kk),'%.3f')];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end
if stationary==1;
stabilityinfo2=['No root lies outside the unit circle.'];
stabilityinfo3=['The estimated VAR model satisfies the stability condition'];
fprintf('%s\n',stabilityinfo2);
fprintf(fid,'%s\n',stabilityinfo2);
fprintf('%s\n',stabilityinfo3);
fprintf(fid,'%s\n',stabilityinfo3);
else
stabilityinfo2=['Warning: at leat one root lies on or outside the unit circle.'];
stabilityinfo3=['The estimated VAR model will not be stable'];
end
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

% display posterior for sigma
sigmainfo=['sigma (residual covariance matrix): posterior estimates'];
fprintf('%s\n',sigmainfo);
fprintf(fid,'%s\n',sigmainfo);
% calculate the (integer) length of the largest number in sigma, for formatting purpose
width=length(sprintf('%d',floor(max(abs(bear.vec(sigma_median))))));
% add a separator, a potential minus sign, and three digits (total=5) to obtain the total space for each entry in the matrix
width=width+5;
for ii=1:n
temp=[];
   for jj=1:n
   % convert matrix entry into string
   number=num2str(sigma_median(ii,jj),'% .3f');
      % pad potential missing blanks
      while numel(number)<width
      number=[' ' number];
      end
   number=[number '  '];
   temp=[temp number];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end

% then display the results for D and gamma, if a structural decomposition was selected

if IRF==1 && IRFt~=1
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

svarinfo1=['D (structural decomposition matrix): posterior estimates'];
fprintf('%s\n',svarinfo1);
fprintf(fid,'%s\n',svarinfo1);

% recover D
D=reshape(D_estimates,n,n);
% calculate the (integer) length of the largest number in D, for formatting purpose
width=length(sprintf('%d',floor(max(abs(bear.vec(D))))));
% add a separator, a potential minus sign and three digits (total=5) to obtain the total space for each entry in the matrix
width=width+5;
for ii=1:n
temp=[];
   for jj=1:n
   % convert matrix entry into string
   number=num2str(D(ii,jj),'% .3f');
      % pad potential missing blanks
      while numel(number)<width
      number=[' ' number];
      end
   number=[number '  '];
   temp=[temp number];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

svarinfo2=['gamma (structural disturbances covariance matrix): posterior estimates'];
fprintf('%s\n',svarinfo2);
fprintf(fid,'%s\n',svarinfo2);

% recover gamma
gamma=reshape(gamma_estimates,n,n);
% calculate the (integer) length of the largest number in D, for formatting purpose
width=length(sprintf('%d',floor(max(abs(bear.vec(gamma))))));
% add a separator, a potential minus sign and three digits (total=5) to obtain the total space for each entry in the matrix
width=width+5;
for ii=1:n
temp=[];
   for jj=1:n
   % convert matrix entry into string
   number=num2str(gamma(ii,jj),'% .3f');
      % pad potential missing blanks
      while numel(number)<width
      number=[' ' number];
      end
   number=[number '  '];
   temp=[temp number];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end

end

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

% finally, estimate and print the unit-specific elements: for this model, only forecast evaluation remains to be printed
% first note that forecast evaluation can only be conducted if it is activated, and if there is some observable data after the beginning of the forecast
if Feval==1 && Fcomp==1

   % loop over units
   for ii=1:N
   unitinfo=['unit-specific components: ' Units{ii,1}];
   fprintf('%s\n',unitinfo);
   fprintf(fid,'%s\n',unitinfo);
   fprintf('%s\n','');
   fprintf(fid,'%s\n','');
   % compute forecast evaluation
   [RMSE MAE MAPE Ustat CRPS_estimates S1_estimates S2_estimates]=bear.panelfeval(n,p,k,beta_gibbs,sigma_gibbs,forecast_record(:,:,ii),forecast_estimates(:,:,ii),Fcperiods,data_endo_c(:,:,ii),data_endo_c_lags(:,:,ii),data_exo_c,const,It,Bu);
   % then display and save the results
   bear.panelfprint(n,endo,RMSE,MAE,MAPE,Ustat,CRPS_estimates,S1_estimates,S2_estimates,stringdates3,Fstartdate,Fcenddate,Fcperiods,fid);
   fprintf('%s\n','');
   fprintf(fid,'%s\n','');
   fprintf('%s\n','');
   fprintf(fid,'%s\n','');
   fprintf('%s\n','');
   fprintf(fid,'%s\n','');
   fprintf('%s\n','');
   fprintf(fid,'%s\n','');
   end

% if forecast evaluation is activated but not possible, return a message to signal it
elseif Feval==1 && Fcomp==0

finfo1=['Forecast evaluation cannot be conducted.'];
fprintf('%s\n',finfo1);
fprintf(fid,'%s\n',finfo1);
finfo2=['Forecasts start in ' Fstartdate ', while observable data is available only until ' names{end,1} '.'];
fprintf('%s\n',finfo2);
fprintf(fid,'%s\n',finfo2);
finfo3=['To obtain forecast evaluation, the forecast start date must be anterior to the end of the data set.'];
fprintf('%s\n',finfo3);
fprintf(fid,'%s\n',finfo3);

% if forecast evaluation is not activated altogether, do not do anything
end

fclose(fid);


function array = cell2array(cellarray, nD)

% The reason for not taking nD as ndims(cellarray{1}) is that an array
% intended to be 3D can look 2D because the size of the 3rd dimension is 1.
chkarg(istypesizeof(nD, 'int') && nD > 0, '"nD" should be positive integer.');
chkarg(istypesizeof(cellarray, 'complexcell', [1 0], zeros(1, nD)), ...
	'"cellarray" should be a row cell array whose each element is %dD array with complex elements.', nD);

ncell = length(cellarray);
if ncell == 0
	array = [];
else
	assert(ncell >= 1);
	dims = size(cellarray{1});
	chkarg(istypesizeof(cellarray, 'complexcell', [1 0], dims), 'elements of "cellarray" should be have the same size.');
	array = cat(nD+1, cellarray{:});
	array = permute(array, [nD+1, 1:nD]);
end


function [cum_ret, cumprod_ret, daily_ret, daily_portfolio] ...
    = m0_start(fid, data, varargins, opts)
% This file is an entry for the M0 strategy.
%
% function [cum_ret, cumprod_ret, daily_ret, daily_portfolio] ...
%           = m0_start(fid, data, varargins, opts)
% cum_ret: cumulative wealth achived at the end of a period.
% cumprod_ret: cumulative wealth achieved till the end each period.
% daily_ret: daily return achieved by a strategy.
% daily_portfolio: daily portfolios
%
% data: market sequence vectors
% fid: handle for write log file
% varargins: variable parameters
% opts: option parameter for behvaioral control
%
% Example: [cum_ret, cumprod_ret, daily_ret, daily_portfolio] ...
%          = m0_start(fid, data, {0.5, 0}, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of OLPS: http://OLPS.stevenhoi.org/
% Original authors: Bin LI, Steven C.H. Hoi
% Contributors:
% Change log:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Extract the parameters
beta = varargins{1};    %
tc = varargins{2};      % transaction cost fee rate

% Run the M0 algorithm
[cum_ret, cumprod_ret, daily_ret, daily_portfolio] ...
    = m0_run(fid, data, beta, tc, opts);

end