% this is an experimentaal high-performance EPI sequence
% which uses split gradients to overlap blips with the readout
% gradients combined with ramp-samping

seq=mr.Sequence();         % Create a new sequence object
fov=250e-3; Nx=64; Ny=64;  % Define FOV and resolution
thickness=3e-3;            % slice thinckness
Nslices=3;
TE=40e-3;

pe_enable=1;               % a flag to quickly disable phase encoding (1/0) as needed for the delay calibration
ro_os=1;                   % oversampling factor (in contrast to the product sequence we don't really need it)
readoutTime=4.2e-4;        % this controls the readout bandwidth
partFourierFactor=0.75;    % partial Fourier factor: 1: full sampling 0: start with ky=0

tRFex=2e-3;
tRFref=2e-3;
spoilFactor=1.5;             % spoiling gradient around the pi-pulse

% Set system limits
lims = mr.opts('MaxGrad',32,'GradUnit','mT/m',...
    'MaxSlew',130,'SlewUnit','T/m/s',...
    'rfRingdownTime', 30e-6, 'rfDeadtime', 100e-6);

% Create fat-sat pulse
B0=2.89; % 1.5 2.89 3.0
sat_ppm=-3.45;
sat_freq=sat_ppm*1e-6*B0*lims.gamma;
rf_fs = mr.makeGaussPulse(110*pi/180,'system',lims,'Duration',8e-3,...
    'bandwidth',abs(sat_freq),'freqOffset',sat_freq);
rf_fs.phaseOffset=-2*pi*rf_fs.freqOffset*mr.calcRfCenter(rf_fs); % compensate for the frequency-offset induced phase
gz_fs = mr.makeTrapezoid('z',lims,'delay',mr.calcDuration(rf_fs),'Area',1/1e-4); % spoil up to 0.1mm

% Create 90 degree slice selection pulse and gradient
[rf, gz, gzReph] = mr.makeSincPulse(pi/2,'system',lims,'Duration',tRFex,...
    'SliceThickness',thickness,'apodization',0.5,'timeBwProduct',4);

% Create 90 degree slice refocusing pulse and gradients
[rf180, gz180] = mr.makeSincPulse(pi,'system',lims,'Duration',tRFref,...
    'SliceThickness',thickness,'apodization',0.5,'timeBwProduct',4,'PhaseOffset',pi/2,'use','refocusing');
% [~, gzr_t, gzr_a]=mr.makeExtendedTrapezoidArea('z',gz180.amplitude,0,-gzReph.area+0.5*gz180.amplitude*gz180.fallTime,lims);
% gz180n=mr.makeExtendedTrapezoid('z','system',lims,'times',[0 gz180.riseTime gz180.riseTime+gz180.flatTime+gzr_t]+gz180.delay, 'amplitudes', [0 gz180.amplitude gzr_a]);
[~, gzr1_t, gzr1_a]=mr.makeExtendedTrapezoidArea('z',0,gz180.amplitude,spoilFactor*gz.area,lims);
[~, gzr2_t, gzr2_a]=mr.makeExtendedTrapezoidArea('z',gz180.amplitude,0,-gzReph.area+spoilFactor*gz.area,lims);
if gz180.delay>(gzr1_t(4)-gz180.riseTime)
    gz180.delay=gz180.delay-(gzr1_t(4)-gz180.riseTime);
else
    rf180.delay=rf180.delay+(gzr1_t(4)-gz180.riseTime)-gz180.delay;
    gz180.delay=0;
end
gz180n=mr.makeExtendedTrapezoid('z','system',lims,'times',[gzr1_t gzr1_t(4)+gz180.flatTime+gzr2_t]+gz180.delay, 'amplitudes', [gzr1_a gzr2_a]);

% define the output trigger to play out with every slice excitatuion
trig=mr.makeDigitalOutputPulse('osc0','duration', 100e-6); % possible channels: 'osc0','osc1','ext1'

% Define other gradients and ADC events
deltak=1/fov;
kWidth = Nx*deltak;

% Phase blip in shortest possible time
blip_dur = ceil(2*sqrt(deltak/lims.maxSlew)/10e-6/2)*10e-6*2; % we round-up the duration to 2x the gradient raster time
% the split code below fails if this really makes a trpezoid instead of a triangle...
gy = mr.makeTrapezoid('y',lims,'Area',-deltak,'Duration',blip_dur); % we use negative blips to save one k-space line on our way towards the k-space center
%gy = mr.makeTrapezoid('y',lims,'amplitude',deltak/blip_dur*2,'riseTime',blip_dur/2, 'flatTime', 0);

% readout gradient is a truncated trapezoid with dead times at the beginnig
% and at the end each equal to a half of blip_dur
% the area between the blips should be defined by kWidth
% we do a two-step calculation: we first increase the area assuming maximum
% slewrate and then scale down the amlitude to fix the area
extra_area=blip_dur/2*blip_dur/2*lims.maxSlew; % check unit!;
gx = mr.makeTrapezoid('x',lims,'Area',kWidth+extra_area,'duration',readoutTime+blip_dur);
actual_area=gx.area-gx.amplitude/gx.riseTime*blip_dur/2*blip_dur/2/2-gx.amplitude/gx.fallTime*blip_dur/2*blip_dur/2/2;
gx.amplitude=gx.amplitude/actual_area*kWidth;
gx.area = gx.amplitude*(gx.flatTime + gx.riseTime/2 + gx.fallTime/2);
gx.flatArea = gx.amplitude*gx.flatTime;

% calculate ADC
% we use ramp sampling, so we have to calculate the dwell time and the
% number of samples, which are will be qite different from Nx and
% readoutTime/Nx, respectively.
adcDwellNyquist=deltak/gx.amplitude/ro_os;
% round-down dwell time to 100 ns
adcDwell=floor(adcDwellNyquist*1e7)*1e-7;
adcSamples=floor(readoutTime/adcDwell/4)*4; % on Siemens the number of ADC samples need to be divisible by 4
% MZ: no idea, whether ceil,round or floor is better for the adcSamples...
adc = mr.makeAdc(adcSamples,'Dwell',adcDwell,'Delay',blip_dur/2);
% realign the ADC with respect to the gradient
time_to_center=adc.dwell*((adcSamples-1)/2+0.5); % I've been told that Siemens samples in the center of the dwell period
adc.delay=round((gx.riseTime+gx.flatTime/2-time_to_center)*1e6)*1e-6; % we adjust the delay to align the trajectory with the gradient. We have to aligh the delay to 1us
% this rounding actually makes the sampling points on odd and even readouts
% to appear misalligned. However, on the real hardware this misalignment is
% much stronger anyways due to the grdient delays

% FOV positioning requires alignment to grad. raster... -> TODO

% split the blip into two halves and produnce a combined synthetic gradient
gy_parts = mr.splitGradientAt(gy, blip_dur/2, lims);
[gy_blipup, gy_blipdown, ~]=mr.align('right',gy_parts(1),'left',gy_parts(2),gx);
gy_blipdownup=mr.addGradients({gy_blipdown, gy_blipup}, lims);

% pe_enable support
gy_blipup.waveform=gy_blipup.waveform*pe_enable;
gy_blipdown.waveform=gy_blipdown.waveform*pe_enable;
gy_blipdownup.waveform=gy_blipdownup.waveform*pe_enable;

% phase encoding and partial Fourier

Ny_pre=round(partFourierFactor*Ny/2-1); % PE steps prior to ky=0, excluding the central line
Ny_post=round(Ny/2+1); % PE lines after the k-space center including the central line
Ny_meas=Ny_pre+Ny_post;

% Pre-phasing gradients
gxPre = mr.makeTrapezoid('x',lims,'Area',-gx.area/2);
gyPre = mr.makeTrapezoid('y',lims,'Area',Ny_pre*deltak);
[gxPre,gyPre]=mr.align('right',gxPre,'left',gyPre);
% relax the PE prepahser to reduce stimulation
gyPre = mr.makeTrapezoid('y',lims,'Area',gyPre.area,'Duration',mr.calcDuration(gxPre,gyPre));
gyPre.amplitude=gyPre.amplitude*pe_enable;

% Calculate delay times
durationToCenter = (Ny_pre+0.5)*mr.calcDuration(gx);
rfCenterInclDelay=rf.delay + mr.calcRfCenter(rf);
rf180centerInclDelay=rf180.delay + mr.calcRfCenter(rf180);
delayTE1=ceil((TE/2 - mr.calcDuration(rf,gz) + rfCenterInclDelay - rf180centerInclDelay)/lims.gradRasterTime)*lims.gradRasterTime;
delayTE2=ceil((TE/2 - mr.calcDuration(rf180,gz180n) + rf180centerInclDelay - durationToCenter)/lims.gradRasterTime)*lims.gradRasterTime;
assert(delayTE1>=0);
%assert(delayTE2>=0);
% now we merge slice refocusing, TE delay and pre-phasers into a single
% block
delayTE2=delayTE2+mr.calcDuration(rf180,gz180n);
gxPre.delay=0;
gxPre.delay=delayTE2-mr.calcDuration(gxPre);
assert(gxPre.delay>=mr.calcDuration(rf180)); % gxPre may not overlap with the RF
gyPre.delay=mr.calcDuration(rf180);
assert(mr.calcDuration(gyPre)<=mr.calcDuration(gxPre)); % gyPre may not shift the timing

% Define sequence blocks

%seq.addBlock(mr.makeDelay(1)); % older scanners like Trio may need this
                                % dummy delay to keep up with timing

for s=1:Nslices
    seq.addBlock(rf_fs,gz_fs);
    rf.freqOffset=gz.amplitude*thickness*(s-1-(Nslices-1)/2);
    rf.phaseOffset=-2*pi*rf.freqOffset*mr.calcRfCenter(rf); % compensate for the slice-offset induced phase
    rf180.freqOffset=gz180.amplitude*thickness*(s-1-(Nslices-1)/2);
    rf180.phaseOffset=pi/2-2*pi*rf180.freqOffset*mr.calcRfCenter(rf180); % compensate for the slice-offset induced phase
    seq.addBlock(rf,gz,trig);
    seq.addBlock(mr.makeDelay(delayTE1));
    seq.addBlock(rf180,gz180n,mr.makeDelay(delayTE2),gxPre,gyPre);
    for i=1:Ny_meas
        if i==1
            seq.addBlock(gx,gy_blipup,adc); % Read the first line of k-space with a single half-blip at the end
        elseif i==Ny_meas
            seq.addBlock(gx,gy_blipdown,adc); % Read the last line of k-space with a single half-blip at the beginning
        else
            seq.addBlock(gx,gy_blipdownup,adc); % Read an intermediate line of k-space with a half-blip at the beginning and a half-blip at the end
        end
        gx.amplitude = -gx.amplitude;   % Reverse polarity of read gradient
    end
end

%% check whether the timing of the sequence is correct
[ok, error_report]=seq.checkTiming;

if (ok)
    fprintf('Timing check passed successfully\n');
else
    fprintf('Timing check failed! Error listing follows:\n');
    fprintf([error_report{:}]);
    fprintf('\n');
end

%% do some visualizations

seq.plot();             % Plot sequence waveforms

% trajectory calculation
[ktraj_adc1, t_adc1, ktraj1, t_ktraj1, t_excitation1, t_refocusing1] = seq.calculateKspacePP();

% plot k-spaces
figure; plot(t_ktraj1, ktraj1'); % plot the entire k-space trajectory
hold on; plot(t_adc1,ktraj_adc1(1,:),'.'); % and sampling points on the kx-axis
figure; plot(ktraj1(1,:),ktraj1(2,:),'b'); % a 2D plot
axis('equal'); % enforce aspect ratio for the correct trajectory display
hold;plot(ktraj_adc1(1,:),ktraj_adc1(2,:),'r.'); % plot the sampling points

%% prepare the sequence output for the scanner
seq.setDefinition('FOV', [fov fov thickness]);
seq.setDefinition('Name', 'epi');

seq.write('epise_rs.seq');

% seq.install('siemens');

% seq.sound(); % simulate the seq's tone

%% very optional slow step, but useful for testing during development e.g. for the real TE, TR or for staying within slewrate limits

rep = seq.testReport;
fprintf([rep{:}]); % as for January 2019 TR calculation fails for fat-sat


%CCG - Compute multiple cross- and auto-correlograms
%
%  USAGE
%
%    [ccg,t] = CCG(times,groups,<options>)
%
%    times          times of all events
%                   NOTE: spiketimes in SECONDS.
%    groups         group IDs for each event in time list (should be
%                   integers 1:nGroups)
%
%   alternate buzcode usage:
%   times           {Ncells} array of [Nspikes]  spiketimes for each cell.
%   groups          []
%
%                   spikes = bz_GetSpikes
%                   [ccg,t] = CCG(spikes.times,[])
%
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'binSize'     bin size in s (default = 0.01)
%     'duration'    duration in s of each xcorrelogram (default = 2)
%     'norm'        normalization of the CCG, 'counts' or 'rate' (DL added 8/1/17)
%                   'counts' gives raw event/spike count,
%                   'rate' returns CCG in units of spks/second (default: counts)
%    =========================================================================
%
%
%  OUTPUT
%   ccg     [t x ngroups x ngroups] matrix where ccg(t,i,j) is the
%           number (or rate) of events of group j at time lag t with
%           respect to reference events from group i
%   t       time lag vector (units: seonds)
%
%  SEE
%
%    See also ShortTimeCCG.

% Copyright (C) 2012 by Michaël Zugaro
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.

function [ccg,t] = CCG(times,groups,varargin)

% Default values
duration = 2;
binSize = 0.01;
Fs = 1/20000;
normtype = 'counts';

% Option for spike times to be in {Ncells} array of spiketimes DL2017
if iscell(times) && isempty(groups)
    numcells = length(times);
    for cc = 1:numcells
        groups{cc}=cc.*ones(size(times{cc}));
    end
    times = cat(1,times{:}); groups = cat(1,groups{:});
end

%Sort
[times,sortidx] = sort(times);
groups = groups(sortidx);

% Check parameters
if nargin < 2,
  error('Incorrect number of parameters (type ''help CCG'' for details).');
end
%if ~isdvector(times),
%	error('Parameter ''times'' is not a real-valued vector (type ''help CCG'' for details).');
%end
if ~isdscalar(groups) && ~isdvector(groups),
	error('Parameter ''groups'' is not a real-valued scalar or vector (type ''help CCG'' for details).');
end
if ~isdscalar(groups) && length(times) ~= length(groups),
	error('Parameters ''times'' and ''groups'' have different lengths (type ''help CCG'' for details).');
end
groups = groups(:);
times = times(:);

% Parse parameter list
for i = 1:2:length(varargin),
	if ~ischar(varargin{i}),
		error(['Parameter ' num2str(i+2) ' is not a property (type ''help CCG'' for details).']);
	end
	switch(lower(varargin{i})),
		case 'binsize',
			binSize = varargin{i+1};
			%if ~isdscalar(binSize,'>0'),
		%		error('Incorrect value for property ''binSize'' (type ''help CCG'' for details).');
	%		end
		case 'duration',
			duration = varargin{i+1};
			if ~isdscalar(duration,'>0'),
				error('Incorrect value for property ''duration'' (type ''help CCG'' for details).');
            end

            case 'Fs',
			Fs = varargin{i+1};
			if ~isdscalar(Fs,'>0'),
				error('Incorrect value for property ''Fs'' (type ''help CCG'' for details).');
            end
        case 'norm'
            normtype = varargin{i+1};

  end
end

% Number of groups, number of bins, etc.
if length(groups) == 1,
	groups = ones(length(times),1);
	nGroups = 1;
else
	nGroups = max(unique(groups));
end

halfBins = round(duration/binSize/2);
nBins = 2*halfBins+1;
t = (-halfBins:halfBins)'*binSize;
times = round(times/Fs);
binSize_Fs = round(binSize/Fs);
if length(times) <= 1,
	% ---- MODIFIED BY EWS, 1/2/2014 ----
    % *** Use unsigned integer format to save memory ***
    ccg = uint16(zeros(nBins,nGroups,nGroups));
    % -----------------------------------
    return
end

% Compute CCGs
nEvents = length(times);
%

counts = double(CCGHeart(times,uint32(groups),binSize_Fs,uint32(halfBins)));
% -----------------------------------
%
% Reshape the results
n = max(groups);
counts = reshape(counts,[nBins n n]);

if n < nGroups,
	counts(nBins,nGroups,nGroups) = 0;
end

%Rate normalization: counts/numREFspikes/dt to put in units of spikes/s. DL
switch normtype
    case 'rate'
        for gg = 1:nGroups
            numREFspikes = sum(groups==gg);%number of reference events for group
            counts(:,gg,:) = counts(:,gg,:)./numREFspikes./binSize;
        end
end

ccg = flipud(counts);


function y = norms( x, p, dim )

%NORMS   Internal cvx version.

%
% Size check
%

narginchk(1,3);
try
    if nargin < 3, dim = []; end
    [ x, sx, sy, zx, zy, nx, nv, perm ] = cvx_reduce_size( x, dim ); %#ok
catch exc
    error( exc.message );
end

%
% Check second argument
%

if nargin < 2 || isempty( p ),
    p = 2;
elseif ~isnumeric( p ) || numel( p ) ~= 1 || ~isreal( p ),
    error( 'Second argument must be a real number.' );
elseif p < 1 || isnan( p ),
    error( 'Second argument must be between 1 and +Inf, inclusive.' );
end

%
% Quick exit for empty matrices
%

if isempty( x ),
    y = zeros( zy );
    return
end

%
% Type check
%

persistent remap1 remap2
if isempty( remap2 ),
    remap1 = cvx_remap( 'constant', 'log-convex' );
    remap2 = cvx_remap( 'affine', 'log-convex' );
end
xc = reshape( cvx_classify( x ), sx );
if ~all( remap2( xc( : ) ) ),
    error( 'Disciplined convex programming error:\n   Invalid computation: norms( {%s}, ... )', cvx_class( x, true, true ) );
end

%
% Compute norms
%

if nx == 1,
	p = 0;
end
switch p,
	case 0,
		y = abs( x );
    case 1,
        y = sum( abs(x) );
    case Inf,
        y = max( abs(x) );
    otherwise,
        tt = all( remap1( xc ) );
        if all( tt( : ) ),
            y = sum( abs(x) .^ p ) .^ (1/p);
        elseif any( tt( : ) ),
            y  = cvx( [ 1, nv ], [] );
            xt = cvx_subsref( x, ':', tt );
            y  = cvx_subsasgn( y, tt, norms( xt, p ) );
            tt = ~tt;
            xt = cvx_subsref( x, ':', tt );
            y  = cvx_subsasgn( y, tt, norms( xt, p ) );
        elseif p == 2,
        	y = [];
            cvx_begin
                epigraph variable y( 1, nv )
                { cvx_accept_convex(x), y } == lorentz( [ nx, nv ], 1, ~isreal( x ) ); %#ok
            cvx_end
		else
			z = []; y = [];
            cvx_begin
                variable z( nx, nv )
                epigraph variable y( 1, nv )
                if isreal(x), cmode = 'abs'; else cmode = 'cabs'; end
                { cat( 3, z, y( ones(nx,1), : ) ), cvx_accept_convex(x) } ...
                    == geo_mean_cone( sw, 3, [1/p,1-1/p], cmode ); %#ok
                sum( z ) == y; %#ok
            cvx_end
        end
end

%
% Reverse the reshaping and permutation steps
%

y = reshape( y, sy );
if ~isempty( perm ),
    y = ipermute( y, perm );
end

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function MDP_DEM_Mixed_Models_Movement
% This demo illustrates a series of computational pathologies as elicited
% during a synthetic neurological examination. This focuses upon an
% examination of the biceps reflex, and a simple coordination task. Each of
% these are simulated for an arm that may move in three dimensions through
% internal and external rotation of the shoulder, flexion and extension of
% the shoulder, and flexion and extension of the elbow. These dynamics play
% out through an active Bayesian filtering scheme, where a series of
% attracting points draw the hand to different locations in 3D space. The
% selection of these attracting points involves a hierarhical Markov
% Decision Process, which identifies these sequences based upon the prior
% belief that (1) the sequence will minimise expected free energy and (2)
% the sequence is consistent with the trajectory predicted by the highest
% (contextual) level of the model.
%__________________________________________________________________________

rng default

% REFLEXES
%==========================================================================
% Demonstration 1 - Tendon reflexes (healthy, pyramidal lesion,
% cerebellar lesion)

% Specify generative model and process
%--------------------------------------------------------------------------

a = 2;                                      % length of upper arm
b = 1.5;                                    % length of forearm
c = [-1;-1;-1];                             % location of shoulder

DEM        = DEM_MDP_Movement_Disorders_GM; % Continuous state generative model
x = [0;-1;1;0;0;0];                         % Initial joint positions

DEM.G(1).g = @(x,v,a,P) Gg1Reflex(x,v,a,P); % Generative process allows for tendon tap
DEM.M(1).x = x;
DEM.G(1).x = x;
DEM.U = [MDP_DEM_MD_transform(x(1:3),a,b,c);0;0;0]*ones(1,64);
DEM.C = zeros(size(DEM.U));
DEM.C(3,10:12) = -0.25;                     % Transient stimulation of type II sensory afferents

% Solve model and plot healthy reflexes
%--------------------------------------------------------------------------
dem = spm_ADEM(DEM);

% Animate
spm_figure('GetWin','Figure 1'); clf
opengl('software')
for i = 1:size(dem.pU.x{1},2)
    MDP_DEM_MD_plot_arm(full(dem.pU.x{1}(1:3,i)),dem.M(1).pE.b1,dem.M(1).pE.b2,[-1;-1;-1]), hold on
    light
    view(10,0)
    zlim([-4 1])
    xlim([-1.2 2.1])
    drawnow
    hold off
    title('Normal')
end

clear dem

% Upper motor lesion
%--------------------------------------------------------------------------
DEM.M(1).V = exp(5);        % Overestimate precision

dem = spm_ADEM(DEM);

% Animate
spm_figure('GetWin','Figure 1'); clf
for i = 1:size(dem.pU.x{1},2)
    MDP_DEM_MD_plot_arm(full(dem.pU.x{1}(1:3,i)),dem.M(1).pE.b1,dem.M(1).pE.b2,[-1;-1;-1]), hold on
    light
    view(10,0)
    zlim([-4 1])
    xlim([-1.2 2.1])
    drawnow
    hold off
    title('Pyramidal')
end

clear dem

% Cerebellar
%--------------------------------------------------------------------------
DEM.M(1).V = exp(4);        % Restore sensory precision
DEM.M(1).E.s = 3/4;         % Overestimate smoothness

dem = spm_ADEM(DEM);

spm_figure('GetWin','Figure 1'); clf
for i = 1:size(dem.pU.x{1},2)
    MDP_DEM_MD_plot_arm(full(dem.pU.x{1}(1:3,i)),dem.M(1).pE.b1,dem.M(1).pE.b2,[-1;-1;-1]), hold on
    light
    view(10,0)
    zlim([-4 1])
    xlim([-1.2 2.1])
    drawnow
    hold off
    title('Cerebellar')
end

clear all

% COORDINATION
%==========================================================================
% Demonstration 2 - Mixed models (healthy)

% Specify discrete generatve model and mapping between continuous and
% discrete
%--------------------------------------------------------------------------

% Locations
%--------------------------------------------------------------------------

% 3D coordinates for three targets
L1  =   [   1;   -1; -3];
L2  =   [  -1;    1; -3];
L3  =   [   0;    0; -1];

% Intermediate (fictive) attracting points)
for i = 1:4
    L{i} = L1 + (i-1)*(L2 - L1)/4;
end
for i = 5:8
    L{i} = L2 + (i-5)*(L3 - L2)/4;
end
for i = 9:12
    L{i} = L3 + (i-9)*(L1 - L3)/4;
end

% MDP outcomes to DEM causes
%--------------------------------------------------------------------------
N  = 14;
nl = length(L);
nh = 3;

for i = 1:nh
    for j = 1:nl
        for k = 1:2
            c           = [L{j}; sparse(i,1,1,nh,1)];
            u           = [L{j}; sparse(i,1,1,nh,1)];
            demi.U{i,j,k} = u*ones(1,N);
            demi.C{i,j,k} = c*ones(1,N);
        end
    end
end

o     = [1 1 1];
O{1}  = spm_softmax(ones(nh,1));
O{2}  = spm_softmax(ones(nl,1));
O{3}  = spm_softmax(ones(2,1));

% generative model (Continuous)
%==========================================================================
DEM      = DEM_MDP_Movement_Disorders_GM;
DEM      = spm_MDP_DEM(DEM,demi,O,o);

% generative model (Discrete)
%==========================================================================
mdp      = MDP_DEM_Movement_Disorders_GM_1;
mdp.demi = demi;
mdp.DEM  = DEM;

mdp      = MDP_DEM_Movement_Disorders_GM_2(mdp);

% invert or solve
%--------------------------------------------------------------------------
rng default
MDP  = spm_MDP_check(mdp);
MDP  = spm_MDP_VB_X(MDP);

% illustrate belief updating - discrete
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 2'); clf
spm_MDP_VB_LFP(MDP,[],2);

% illustrate belief updating - continuous (last movement)
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 3'); clf;
spm_DEM_qU(MDP.mdp(end).dem(end).qU)

% illustrate movement
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 4'); clf

[Q, R, X, Y, Z] = MDP_DEM_Movement_Disorders_Animate(MDP,L);

spm_figure('GetWin','Figure 4'); clf
plot3(Q(1,:),Q(2,:),Q(3,:),'LineWidth',2,'Color','k'), hold on
for m = 1:length(MDP.mdp)
    for j = 1:length(MDP.mdp(m).dem)
        h = (m - 1)*length(MDP.mdp(m).dem) + j;
        H = length(MDP.mdp)*length(MDP.mdp(m).dem);
        surf(0.1*X + MDP.mdp(m).dem(j).qU.v{2}(1,i),0.1*Y + MDP.mdp(m).dem(j).qU.v{2}(2,i),0.1*Z + MDP.mdp(m).dem(j).qU.v{2}(3,2),'Facecolor','r','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'FaceAlpha',h/(1.5*H))
    end
end

light
axis equal
spm_figure('GetWin','Figure 5'); clf
subplot(5,1,1)
plot(0.05*(0:size(R,2)-1),R' - repmat(R(:,1)',size(R,2),1))
title('Normal')

% Demonstration 3 - Mixed models (Pyramidal versus extrapyramidal)
%--------------------------------------------------------------------------
% Pyramidal

mdp.MDP.DEM.M(1).V = exp(5); % Overestimate precision

% invert or solve
%--------------------------------------------------------------------------
rng default
MDP  = spm_MDP_check(mdp);
MDP  = spm_MDP_VB_X(MDP);

spm_figure('GetWin','Figure 6'); clf

[Q, R, X, Y, Z] = MDP_DEM_Movement_Disorders_Animate(MDP,L);

spm_figure('GetWin','Figure 6'); clf
plot3(Q(1,:),Q(2,:),Q(3,:),'LineWidth',2,'Color','k'), hold on
for m = 1:length(MDP.mdp)
    for j = 1:length(MDP.mdp(m).dem)
        h = (m - 1)*length(MDP.mdp(m).dem) + j;
        H = length(MDP.mdp)*length(MDP.mdp(m).dem);
        surf(0.1*X + MDP.mdp(m).dem(j).qU.v{2}(1,i),0.1*Y + MDP.mdp(m).dem(j).qU.v{2}(2,i),0.1*Z + MDP.mdp(m).dem(j).qU.v{2}(3,2),'Facecolor','r','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'FaceAlpha',h/(1.5*H))
    end
end
light
axis equal

spm_figure('GetWin','Figure 5');
subplot(5,1,2)
plot(0.05*(0:size(R,2)-1),R' - repmat(R(:,1)',size(R,2),1))
title('Pyramidal')

% Extrapyramidal
%--------------------------------------------------------------------------
mdp.MDP.DEM.M(1).V = exp(4);
mdp.MDP.beta = 50;

% invert or solve
%--------------------------------------------------------------------------
rng default
MDP  = spm_MDP_check(mdp);
MDP  = spm_MDP_VB_X(MDP);

% illustrate movement
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 8'); clf

[Q, R, X, Y, Z] = MDP_DEM_Movement_Disorders_Animate(MDP,L);

spm_figure('GetWin','Figure 8'); clf
plot3(Q(1,:),Q(2,:),Q(3,:),'LineWidth',2,'Color','k'), hold on
for m = 1:length(MDP.mdp)
    for j = 1:length(MDP.mdp(m).dem)
        h = (m - 1)*length(MDP.mdp(m).dem) + j;
        H = length(MDP.mdp)*length(MDP.mdp(m).dem);
        surf(0.1*X + MDP.mdp(m).dem(j).qU.v{2}(1,i),0.1*Y + MDP.mdp(m).dem(j).qU.v{2}(2,i),0.1*Z + MDP.mdp(m).dem(j).qU.v{2}(3,2),'Facecolor','r','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'FaceAlpha',h/(1.5*H))
    end
end
light
axis equal

spm_figure('GetWin','Figure 5');
subplot(5,1,3)
plot(0.05*(0:size(R,2)-1),R' - repmat(R(:,1)',size(R,2),1))
title('Extrapyramidal')

% Cerebellar
%--------------------------------------------------------------------------
mdp.MDP.beta = 16;
mdp.MDP.DEM.M(1).E.s = 3/4;

% invert or solve
%--------------------------------------------------------------------------
rng default
MDP  = spm_MDP_check(mdp);
MDP  = spm_MDP_VB_X(MDP);

% illustrate movement
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 9'); clf

[Q, R, X, Y, Z] = MDP_DEM_Movement_Disorders_Animate(MDP,L);

spm_figure('GetWin','Figure 9'); clf
plot3(Q(1,:),Q(2,:),Q(3,:),'LineWidth',2,'Color','k'), hold on
for m = 1:length(MDP.mdp)
    for j = 1:length(MDP.mdp(m).dem)
        h = (m - 1)*length(MDP.mdp(m).dem) + j;
        H = length(MDP.mdp)*length(MDP.mdp(m).dem);
        surf(0.1*X + MDP.mdp(m).dem(j).qU.v{2}(1,i),0.1*Y + MDP.mdp(m).dem(j).qU.v{2}(2,i),0.1*Z + MDP.mdp(m).dem(j).qU.v{2}(3,2),'Facecolor','r','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'FaceAlpha',h/(1.5*H))
    end
end
light
axis equal

spm_figure('GetWin','Figure 5');
subplot(5,1,4)
plot(0.05*(0:size(R,2)-1),R' - repmat(R(:,1)',size(R,2),1))
title('Cerebellar')

% Executive
%--------------------------------------------------------------------------
mdp.MDP.DEM.M(1).E.s = 1/2;
mdp.A{3} = ones(size(mdp.A{3})); % Disconnect higher level

% invert or solve
%--------------------------------------------------------------------------
rng default
MDP  = spm_MDP_check(mdp);
MDP  = spm_MDP_VB_X(MDP);

% illustrate movement
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 10'); clf

[Q, R, X, Y, Z] = MDP_DEM_Movement_Disorders_Animate(MDP,L);

spm_figure('GetWin','Figure 10'); clf
plot3(Q(1,:),Q(2,:),Q(3,:),'LineWidth',2,'Color','k'), hold on
for m = 1:length(MDP.mdp)
    for j = 1:length(MDP.mdp(m).dem)
        h = (m - 1)*length(MDP.mdp(m).dem) + j;
        H = length(MDP.mdp)*length(MDP.mdp(m).dem);
        surf(0.1*X + MDP.mdp(m).dem(j).qU.v{2}(1,i),0.1*Y + MDP.mdp(m).dem(j).qU.v{2}(2,i),0.1*Z + MDP.mdp(m).dem(j).qU.v{2}(3,2),'Facecolor','r','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'FaceAlpha',h/(1.5*H))
    end
end
light
axis equal

spm_figure('GetWin','Figure 5');
subplot(5,1,5)
plot(0.05*(0:size(R,2)-1),R' - repmat(R(:,1)',size(R,2),1))
title('Executive')

function MDP = MDP_DEM_Movement_Disorders_GM_1

% First level
%--------------------------------------------------------------------------

% Initial states
%--------------------------------------------------------------------------
D{1} = ones(3,1); % Location of visual cue
D{2} = zeros(12,1);  % Location of arm
D{2}(1) = 1;

% Likelihood
%--------------------------------------------------------------------------
for f1 = 1:numel(D{1})
    for f2 = 1:numel(D{2})
        A{1}(f1,f1,f2) = 1; % Vision (target)
        A{2}(f2,f1,f2) = 1; % Vision + Proprioception (arm)
        A{3}(2,f1,f2)  = (f1 == ((f2-1)/4)+1);  % At target
        A{3}(1,f1,f2)  = ~(f1 == ((f2-1)/4)+1); % Not at target
    end
end

% Transitions
%--------------------------------------------------------------------------
B{1} = eye(3);

for k = 1:length(D{2})
    for j = 1:length(D{2})
        if k == j
            B{2}(k,j,1) = 1; % Stay still
        elseif (k - j) == 1
            B{2}(k,j,2) = 1; % Move to next point
        elseif (j - k) == 1
            B{2}(k,j,3) = 1; % Move to previous point
        end
    end
end

B{2}(1,end,2) = 1;
B{2}(end,1,3) = 1;

% Preferences
%--------------------------------------------------------------------------
C{1} = zeros(3,1);
C{2} = zeros(12,1);
C{3} = [-3 3]';

% Policies
%--------------------------------------------------------------------------
V(:,:,1) = ones(8,3); % (time, policy, factor)
V(:,:,2) = [1 2 3;
    1 2 3;
    1 2 3;
    1 2 3;
    1 1 1;
    1 1 1;
    1 1 1;
    1 1 1];

E = [1 0.5 0.5]';

% First level MDP
%--------------------------------------------------------------------------
MDP.A = A;
MDP.B = B;
MDP.C = C;
MDP.D = D;
MDP.E = E;
MDP.V = V;
MDP.T = 9;
MDP.chi = -exp(64);
MDP.beta = 16;
% MDP.beta = 50;
MDP     = spm_MDP_check(MDP);

function MDP = MDP_DEM_Movement_Disorders_GM_2(mdp)

% First level
%--------------------------------------------------------------------------
MDP.MDP = mdp;

% Second level
%--------------------------------------------------------------------------

% Initial states
%--------------------------------------------------------------------------
D{1} = ones(3,1); % Target location
D{2} = zeros(9,1); % Initial locations x policies at lower level ({L1,P1},{L1,P2},{L1,P3},{L2,P1},...)
D{2}([1 2 3]) = 1;

% Likelihood
%--------------------------------------------------------------------------
for f1 = 1:length(D{1})
    for f2 = 1:length(D{2})
        A{1}(f1,f1,f2) = 1; % Target location 1st level

        if f2 < 4
            A{2}(11,f1,f2) = 0.03;  % Start location 1st level
            A{2}(12,f1,f2) = 0.12;
            A{2}(1,f1,f2)  = 0.7;
            A{2}(2,f1,f2)  = 0.12;
            A{2}(3,f1,f2)  = 0.03;

        elseif f2 <7
            A{2}(3,f1,f2) = 0.03;
            A{2}(4,f1,f2) = 0.12;
            A{2}(5,f1,f2)  = 0.7;
            A{2}(6,f1,f2)  = 0.12;
            A{2}(7,f1,f2)  = 0.03;
        else
            A{2}(7,f1,f2) = 0.03;
            A{2}(8,f1,f2) = 0.12;
            A{2}(9,f1,f2)  = 0.7;
            A{2}(10,f1,f2)  = 0.12;
            A{2}(11,f1,f2)  = 0.03;
        end

        A{3}(1,f1,f2)  = (rem(f2+2,3) == 0) + 0.5;  % Policy 1st level (slight bias towards 'stay still' policy)
        A{3}(2,f1,f2)  = (rem(f2+1,3) == 0);
        A{3}(3,f1,f2)  = (rem(f2,3) == 0);
    end
end

% Transitions
%--------------------------------------------------------------------------
B{1}(:,:,1) = ones(length(D{1}));
B{1}(:,:,2) = ones(length(D{1})); % Action that causes no change to prevent entering HMM mode
I = eye(3);
B{2} = [I, [I(3,:);I(1:2,:)],([I(3,:);I(1:2,:)])']/3;
B{2} = kron(B{2},ones(3,1));

% Preferences
%--------------------------------------------------------------------------
C{1} = zeros(3,1);
C{2} = zeros(12,1);
C{3} = zeros(3,1);

% Second level MDP
%--------------------------------------------------------------------------
MDP.A = A;
MDP.B = B;
MDP.C = C;
MDP.D = D;
MDP.T = 4;

MDP.link  = [1 0 0;
    0 1 0];
MDP.linkE = [0 0 1];

MDP     = spm_MDP_check(MDP);

function DEM = DEM_MDP_Movement_Disorders_GM
% hidden states
%--------------------------------------------------------------------------
x   = [0;-1;0.5;0;0;0]; % shoulder rotation, flexion, elbow flexion (position and velocity)

% and actions
%--------------------------------------------------------------------------
a   = [0;0;0];

% parameters
%--------------------------------------------------------------------------
P.kv = 2;              % damping
P.a = a;
P.b1 = 2;
P.b2 = 1.5;

M(1).pE = P;

% recognition model
%==========================================================================
M(1).E.s = 1/2;                               % smoothness: default 1/2 [3/4 for lesion]
M(1).E.n = 4;                                 % order of
M(1).E.d = 2;                                 % generalised motion

% level 1
%--------------------------------------------------------------------------
M(1).f  = @(x,v,P) Mf1(x,v,P);                % plant dynamics
M(1).g  = @(x,v,P) Mg1(x,v,P);                % prediction

M(1).x  = x;                                  % hidden states
M(1).V  = exp(4);                             % error precision (g)[4] [3/8 ?]
M(1).W  = exp(2);                             % error precision (f) [2]

% level 2:
%--------------------------------------------------------------------------
M(2).v  = [0;0;0;0;0;0];                      % priors (attracting point)
M(2).V  = exp(15);

% generative model
%==========================================================================

G(1).E.s = 1/2;                               % smoothness: default 1/2
G(1).E.n = 4;                                 % order of
G(1).E.d = 2;                                 % generalised motion

% first level
%--------------------------------------------------------------------------
G(1).f  = @(x,v,a,P) Gf1(x,v,a,P);
G(1).g  = @(x,v,a,P) Gg1(x,v,a,P);
G(1).x  = x;                                  % hidden states
G(1).V = exp(16);
% G(1).V  = exp([4*ones(1,6) 16*ones(1,6)]);    % error precision [16]
G(1).W  = exp(16);                            % error precision
G(1).pE = P;

% second level
%--------------------------------------------------------------------------
G(2).v  = [0;0;0;0;0;0];                      % exogenous forces
G(2).a  = spm_vec(a);                         % action forces
G(2).V  = exp(16);

DEM.G = G;
DEM.M = M;

% Equations for DEM model and process
%==========================================================================
function f = Mf1(x,v,P)
a = P.b1; % length of upper arm
b = P.b2; % length of forearm
c = [-1;-1;-1]; % location of shoulder

f = [x(4:6);MDP_DEM_MD_dthetadt(v(1:3),x,a,b,c) - P.kv*x(4:6)];

function g = Mg1(x,v,P)
a = P.b1; % length of upper arm
b = P.b2; % length of forearm
c = [-1;-1;-1]; % location of shoulder

g = [x;MDP_DEM_MD_transform(x(1:3),a,b,c);v(4:6)]; % proprioceptive, visual (hand), visual (target)

function f = Gf1(x,~,a,P)
f = [x(4:6);a  - P.kv*x(4:6)];

function g = Gg1(x,v,~,P)
a = P.b1; % length of upper arm
b = P.b2; % length of forearm
c = [-1;-1;-1]; % location of shoulder

g = [x;MDP_DEM_MD_transform(x(1:3),a,b,c);v(4:6)]; % proprioceptive, visual (hand), visual (target)

function g = Gg1Reflex(x,v,~,P)
a = P.b1; % length of upper arm
b = P.b2; % length of forearm
c = [-1;-1;-1]; % location of shoulder

g = [x + [zeros(3,1);v(1:3)];MDP_DEM_MD_transform(x(1:3),a,b,c);v(4:6)]; % proprioceptive, visual (hand), visual (target)

% Additional routines (plotting/coordinate transforms)
%==========================================================================
function y = MDP_DEM_MD_transform(x,a,b,c)
% converts angles to hand position in euclidean space
% x = angle of shoulder rotation, flexion, elbow flexion

y1 = c + [a*cos(x(2))*cos(x(1));
    a*cos(x(2))*sin(x(1));
    a*sin(x(2))];

y = y1 + [b*cos(x(3)+x(2))*cos(x(1));
    b*cos(x(3)+x(2))*sin(x(1));
    b*sin(x(3)+x(2))];

function MDP_DEM_MD_plot_arm(x,a,b,c)

y1 = c + [a*cos(x(2))*cos(x(1));
    a*cos(x(2))*sin(x(1));
    a*sin(x(2))];

y = y1 + [b*cos(x(3)+x(2))*cos(x(1));
    b*cos(x(3)+x(2))*sin(x(1));
    b*sin(x(3)+x(2))];

[S1, S2, S3] = sphere;

surf(S1*0.2+c(1),S2*0.2+c(2),S3*0.2+c(3),'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7), hold on
surf(S1*0.1+y1(1),S2*0.1+y1(2),S3*0.1+y1(3),'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7)
surf(S1*0.07+y(1),S2*0.07+y(2),S3*0.07+y(3),'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7)

plot3([c(1) y1(1)],[c(2) y1(2)],[c(3) y1(3)],'LineWidth',6,'Color',[0.1 0.1 0.8])
plot3([y1(1) y(1)],[y1(2) y(2)],[y1(3) y(3)],'LineWidth',5,'Color',[0.1 0.1 0.8])
plot3([y1(1) y(1)],[y1(2) y(2)],[y1(3)-0.1 y(3)-0.1],'LineWidth',4,'Color',[0.1 0.1 0.8])

MDP_DEM_Movements_Hand([pi,pi/2 + atan((y(3)-y1(3))/(sqrt((y(1) - y1(1))^2+(y(2) - y1(2))^2))),x(1)],0.25,y)
axis equal

function y = MDP_DEM_MD_dthetadt(v,x,a,b,c)

y1 = c + [a*cos(x(2))*cos(x(1));
    a*cos(x(2))*sin(x(1));
    a*sin(x(2))];

y2 = y1 + [b*cos(x(3)+x(2))*cos(x(1));
    b*cos(x(3)+x(2))*sin(x(1));
    b*sin(x(3)+x(2))];

y(1) = [v - y2]'*[-((a*cos(x(2)) + b*cos(x(3)+x(2))*sin(x(1))));
    ((a*cos(x(2)) + b*cos(x(3)+x(2))*cos(x(1))));
    0];

y(2) = [v - y2]'*[-((a*sin(x(2)) + b*sin(x(3) + x(2)))*cos(x(1)));
    -((a*sin(x(2)) + b*sin(x(3) + x(2))*sin(x(1))));
    (a*cos(x(2)) +(b*cos(x(2) + x(3))))];

y(3) = [v - y2]'*[-(a*sin(x(3) + x(2))*cos(x(1)));
    -(a*sin(x(3)+ x(2))*sin(x(1)));
    (b*cos(x(2) + x(3)))];

y = 0.05*y'; % 0.05

function MDP_DEM_Movements_Hand(a,s,l)
% angle    - a
% scale    - s
% location - l

% Carpals (as a single sphere)
%--------------------------------------------------------------------------
[X,Y,Z] = sphere;
Rx = makehgtform('xrotate',a(1));
Ry = makehgtform('yrotate',a(2));
Rz = makehgtform('zrotate',a(3));
T  = makehgtform('translate',l);
S  = makehgtform('scale',s);
q = hgtransform;

LW = 5;

% 2nd finger
%--------------------------------------------------------------------------

% MCP
%--------------------------------------------------------------------------
plot3([0 0],[0 0],[0 1.1],'b','LineWidth',LW,'Parent',q)
surf(0.1*X, 0.1*Y ,0.1*Z + 1.1,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% PIP
%--------------------------------------------------------------------------
plot3([0 0],[0 0.4],[1.1 1.4],'b','LineWidth',LW,'Parent',q)
surf(0.1*X, 0.1*Y + 0.4 ,0.1*Z + 1.4,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% MIP
%--------------------------------------------------------------------------
plot3([0 0],[0.4 0.7],[1.4 1.5],'b','LineWidth',LW,'Parent',q)
surf(0.1*X, 0.1*Y + 0.7 ,0.1*Z + 1.5,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% DIP
%--------------------------------------------------------------------------
plot3([0 0],[0.7 1],[1.5 1.6],'b','LineWidth',LW,'Parent',q)
surf(0.1*X, 0.1*Y + 1 ,0.1*Z + 1.6,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)

% 3rd finger
%--------------------------------------------------------------------------

% MCP
%--------------------------------------------------------------------------
plot3([-0.1 -0.2],[0 0],[0 1.05],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.2, 0.1*Y ,0.1*Z + 1.05,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% PIP
%--------------------------------------------------------------------------
plot3([-0.2 -0.2],[0 0.3],[1.05 1.4],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.2, 0.1*Y + 0.3 ,0.1*Z + 1.4,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% MIP
%--------------------------------------------------------------------------
plot3([-0.2 -0.2],[0.3 0.6],[1.4 1.5],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.2, 0.1*Y + 0.6 ,0.1*Z + 1.5,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% DIP
%--------------------------------------------------------------------------
plot3([-0.2 -0.2],[0.6 0.9],[1.5 1.6],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.2, 0.1*Y + 0.9 ,0.1*Z + 1.6,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)

% 4th finger
%--------------------------------------------------------------------------

% MCP
%--------------------------------------------------------------------------
plot3([-0.2 -0.4],[0 0],[0 1],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.4, 0.1*Y ,0.1*Z + 1,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% PIP
%--------------------------------------------------------------------------
plot3([-0.4 -0.4],[0 0.2],[1 1.4],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.4, 0.1*Y + 0.2 ,0.1*Z + 1.4,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% MIP
%--------------------------------------------------------------------------
plot3([-0.4 -0.4],[0.2 0.5],[1.4 1.5],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.4, 0.1*Y + 0.5 ,0.1*Z + 1.5,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% DIP
%--------------------------------------------------------------------------
plot3([-0.4 -0.4],[0.5 0.8],[1.5 1.6],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.4, 0.1*Y + 0.8 ,0.1*Z + 1.6,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)

% 1st finger
%--------------------------------------------------------------------------

% MCP
%--------------------------------------------------------------------------
plot3([0.1 0.2],[0 0],[0 1],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.2, 0.1*Y ,0.1*Z + 1,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% PIP
%--------------------------------------------------------------------------
plot3([0.2 0.2],[0 0.3],[1 1.4],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.2, 0.1*Y + 0.3 ,0.1*Z + 1.4,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% MIP
%--------------------------------------------------------------------------
plot3([0.2 0.2],[0.3 0.6],[1.4 1.5],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.2, 0.1*Y + 0.6 ,0.1*Z + 1.5,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% DIP
%--------------------------------------------------------------------------
plot3([0.2 0.2],[0.6 0.9],[1.5 1.6],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.2, 0.1*Y + 0.9 ,0.1*Z + 1.6,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)

% Thumb
%--------------------------------------------------------------------------

% MCP
%--------------------------------------------------------------------------
plot3([0.2 0.5],[0 0.05],[0 0.4],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.5, 0.1*Y +0.05,0.1*Z + 0.4,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% PIP
%--------------------------------------------------------------------------
plot3([0.5 0.5],[0.05 0.45],[0.4 0.6],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.5, 0.1*Y +0.45,0.1*Z + 0.6,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% DIP
%--------------------------------------------------------------------------
plot3([0.5 0.45],[0.45 0.6],[0.6 0.62],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.45, 0.1*Y +0.6,0.1*Z + 0.62,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)

set(q,'Matrix',T*Rz*Rx*Ry*S);

function [Q, R, X, Y, Z] = MDP_DEM_Movement_Disorders_Animate(MDP,L)
opengl('software')
Q = [];
R = [];

for m = 1:length(MDP.mdp)
    for j = 1:length(MDP.mdp(m).dem)
        for i = 2:size(MDP.mdp(m).dem(j).pU.x{1},2)
            MDP_DEM_MD_plot_arm(full(MDP.mdp(m).dem(j).pU.x{1}(1:3,i)),MDP.mdp(m).dem(j).M(1).pE.b1,MDP.mdp(m).dem(j).M(1).pE.b2,[-1;-1;-1]), hold on
            [X,Y,Z] = sphere;
            surf(0.1*X + MDP.mdp(m).dem(j).qU.v{2}(1,i),0.1*Y + MDP.mdp(m).dem(j).qU.v{2}(2,i),0.1*Z + MDP.mdp(m).dem(j).qU.v{2}(3,i),'Facecolor','r','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7)
            for k = 1:3
                surf(0.2*X + L{(k-1)*4+1}(1),0.2*Y + L{(k-1)*4+1}(2),0.2*Z + L{(k-1)*4+1}(3),'Facecolor',[1 1 1]-MDP.mdp(m).dem(j).Y(9+k,i),'EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7)
            end
            axis equal
            light
            drawnow
            hold off

            Q(:,end+1) = MDP_DEM_MD_transform(full(MDP.mdp(m).dem(j).pU.x{1}(1:3,i)),MDP.mdp(m).dem(j).M(1).pE.b1,MDP.mdp(m).dem(j).M(1).pE.b2,[-1;-1;-1]);
        end
        R = [R full(MDP.mdp(m).dem(j).pU.x{1}(1:3,:))];
    end
end


function [R,L] = match_query(D,SR)
% [R,L] = match_query(D,SR)
%     Match landmarks from an audio query against the database.
%     Rows of R are potential maxes, in format
%      songID  modalDTcount modalDT
%     i.e. there were <modalDTcount> occurrences of hashes
%     that occurred in the query and reference with a difference of
%     <modalDT> frames.
%     L returns the actual landmarks that this implies.
% 2008-12-29 Dan Ellis dpwe@ee.columbia.edu

%Rt = get_hash_hits(landmark2hash(find_landmarks(D,SR)));
Lq = find_landmarks(D,SR);
%Lq = fuzzify_landmarks(Lq);
% Augment with landmarks calculated half-a-window advanced too
landmarks_hopt = 0.032;
%Lq = [Lq;find_landmarks(D(round(landmarks_hopt/4*SR):end),SR)];
Lq = [Lq;find_landmarks(D(round(landmarks_hopt/2*SR):end),SR)];
%Lq = [Lq;find_landmarks(D(round(3*landmarks_hopt/4*SR):end),SR)];
% add in quarter-hop offsets too for even better recall

Hq = landmark2hash(Lq);
Rt = get_hash_hits(Hq);
nr = size(Rt,1);

if nr > 0

  % Find all the unique tracks referenced
  [utrks,xx] = unique(sort(Rt(:,1)));
  utrkcounts = diff(xx,nr);

  nutrks = length(utrks);

  R = zeros(nutrks,3);

  for i = 1:nutrks
    tkR = Rt(Rt(:,1)==utrks(i),:);
    % Find the most popular time offset
    [dts,xx] = unique(sort(tkR(:,2)),'first');
    dtcounts = 1+diff([xx',size(tkR,1)]);
    [vv,xx] = max(dtcounts);
    R(i,:) = [utrks(i),vv,dts(xx)];
  end

  % Sort by descending match count
  [vv,xx] = sort(R(:,2),'descend');
  R = R(xx,:);

  % Extract the actual landmarks
  H = Rt((Rt(:,1)==R(1,1)) & (Rt(:,2)==R(1,3)),:);
  % Restore the original times
  for i = 1:size(H,1)
    hix = find(Hq(:,3)==H(i,3));
    hix = hix(1);  % if more than one...
    H(i,2) = H(i,2)+Hq(hix,2);
    L(i,:) = hash2landmark(H(i,:));
  end

  % Return no more than 10 hits, and only down to half the #hits in
  % most popular
  if size(R,1) > 10
    R = R(1:10,:);
  end
  maxhits = R(1,2);
  nuffhits = R(:,2)>(maxhits/2);
  %R = R(nuffhits,:);

else
  R = [];
  disp('*** NO HITS FOUND ***');
end


function shadedplot(c,scale,ord)

   % Private method. See ../plot for details.

   % Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
   % $Date$
   % $Revision$

   % PREP PLOT
   figure('Color','w','Position',[50 50 850 1100]);
   box on; hold on;

   % GET TIME MAX AND MINS
   tmin =  999999;
   tmax = -999999;
   wstartrels = c.relativeStartTime(ord);
   wlengths = [c.traces(ord).nsamples];
   wFs = [c.traces(ord).samplerate];

   for i = 1:numel(ord)
      %wstartrel = 86400*(get(c.W(i),'START_MATLAB')-c.trig(i));	% relative start time (trigger is at zero)
      %tr = wstartrel + [ 0:get(c.W(i),'DATA_LENGTH')-1]'/get(c.W(i),'Fs');
      tr = wstartrels(i) + ( 0:wlengths(i)-1)'/wFs(i);

      % save min and max relative trace times
      if tr(1) < tmin
         tmin = tr(1);
      end;
      if tr(end) > tmax
         tmax = tr(end);
      end;
   end;

   % MAKE ALIGNED DATA MATRIX
   %p = 1/get(c.W(1),'Fs');         % assumes all sampling periods are thesame
   p = 1/wFs(1);         % assumes all sampling periods are the same

   tmin = round(tmin*wFs(i))*p;      % round to nearest sample
   tmax = round(tmax*wFs(i))*p;
   N = 1:length(ord);
   T =  tmin : p : tmax ;
   D = zeros(length(N),length(T));
   count = 0;
   absmax = max(abs(c.traces(ord)));
   absmax(absmax==0) = scale; %next line will be negated for zero-scale

   for n=1:numel(ord)
      c.traces(ord(n)) = c.traces(ord(n)) .* (scale ./ absmax(n));
   end;
   d = double(c.traces); %nan fill??
   % d = double(c.W(ord) .* (scale ./ absmax));
   for i = 1:numel(ord)
      count = count + 1;
      %d = get(c.W(ord(i)),'DATA');            %%%d = c.w(:,i);
      %if (max(abs(d)) ~= 0)
      %    d = scale * d/max(abs(d));		% apply a uniform amplitude scale;
      %end
      % DONE ABOVE: wstartrel = 86400*(get(c.W(i),'START_MATLAB')-c.trig(i));	% relative start time (trigger is at zero)
      [~,startindex] = min(abs(T-wstartrels(i)));
      D( count ,  startindex : (startindex+wlengths(i)-1) ) = d(:,i);
   end;
   imagesc(T,N,D);

   % ADJUST PLOT
   axis([tmin tmax 0.5 length(ord)+0.5]);
   set(gca,'YDir','reverse');
   n = length(ord);
   set(gca,'YTick',(1:round(n/50):n));
   yt = get(gca,'YTick');
   set(gca,'YTickLabel',datestr(c.trig(ord(yt))),'FontSize',6);
   xlabel('Relative Time,(s)','FontSize',8);

   % SET COLOR MAP
   cmap = [ 0 0 1;
      1 1 1
      1 0 0];
   cmap = interp1([-1 0 1],cmap, -1:.1:1, 'linear');
   colormap(cmap);

   maybeReplaceYticksWithStationNames(c,gca)

   %PRINT OUT FIGURE
   set(gcf, 'paperorientation', 'portrait');
   set(gcf, 'paperposition', [.25 .25 8 10.5] );
   % print(gcf, '-depsc2', 'FIG_alignwfm.ps')
   %!ps2pdf FIG_alignwfm.ps
   %!convert FIG_alignwfm.ps FIG_alignwfm.gif
   %!rm FIG_alignwfm.ps
end


function ang= getRelativeRigidOrientation(obj, frame, R, p)
    % Returns the symbolic representation of the Euler angles of a
    % rigid link.
    %
    % Parameters:
    % frame: the list of coordinate frame of the point
    % @type cell
    % R: ang is computed as the relative Euler angles from frame to R
    % @type matrix
    % p: the offset of the point from the origin of the frame
    % @type matrix
    %
    %
    % Return values:
    %  ang: the 3-dimensional Euler angles (roll,pitch,yaw) vector of the
    %  CoM of the system @type SymExpression
    %
    %
    % @note Syntax for ont point
    %
    % >> getEulerAngles(obj,pf,offset)
    %
    % @note Syntax for multiple points (offset should be np*3 matrix)
    %
    % >> getEulerAngles(obj,pfarray, offset)

    % the number of points (one less than the nargin)
    if nargin < 4
        p = [];
    end
    if nargin < 3
        R = eye(3);
    end
    c_str = getTwists(frame, p);
    c_str{1}.R = R;

    ang = eval_math_fun('ComputeRelativeRigidOrientation', c_str);

end


function [b, mpe] = back1_mpe(engine, bfuture, f, ev1, t)

if t ~= 1
  error('mixed up time stamps')
end
bnet = bnet_from_engine(engine);
ss = bnet.nnodes_per_slice;
maximize = 1;

int = engine.interface;
D = engine.in_clq; % from J2
C = engine.int_clq1; % from J1
phiD = marginalize_pot(bfuture.clpot{D}, int, maximize);
phiC = marginalize_pot(f.clpot{C}, int, maximize);
ratio = divide_by_pot(phiD, phiC);
f.clpot{C} = multiply_by_pot(f.clpot{C}, ratio);

[mpe, b.clpot] = find_max_config(engine.jtree_engine1, f.clpot, f.seppot, ev1);
for c=1:length(b.clpot)
  [b.clpot{c}, ll(c)] = normalize_pot(b.clpot{c});
end
b.t = t;


function [net,opts]=train_net(net,opts)

    if ~isfield(opts,'datatype')
        opts.datatype='single';
    end

    opts.training=1;

    if ~isfield(opts.parameters,'learning_method')
        opts.parameters.learning_method='sgd';
    end

    if ~isfield(opts,'display_msg')
        opts.display_msg=1;
    end
    opts.TrainMiniBatchError=[];
    opts.TrainMiniBatchError_Top5=[];
    opts.TrainMiniBatchLoss=[];

    tic

    opts.order=randperm(opts.n_train);

    if opts.parameters.selective_sgd==1
        [ net,opts ] = selective_sgd( net,opts );
    end

    batch_dim=length(size(opts.train));
    idx_nd=repmat({':'},[1,batch_dim]);

    for mini_b=1:opts.n_train_batch

        idx=opts.order(1+(mini_b-1)*opts.parameters.batch_size:mini_b*opts.parameters.batch_size);
        idx_nd{end}=idx;
        opts.idx_nd=idx_nd;
        opts.idx=idx;
        res(1).x=opts.train(idx_nd{:});

        %classification
        if strcmp(net.layers{end}.type,'softmaxloss')
            res(1).class=opts.train_labels(idx);
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%forward%%%%%%%%%%%%%%%%%%%
        [ net,res,opts ] = net_ff( net,res,opts );
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%backward%%%%%%%%%%%%%%%%
        opts.dzdy=1.0;

        [ net,res,opts ] = net_bp( net,res,opts );
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        %%summarize the current batch
        loss=double(gather(mean(res(end).x(:))));

        if strcmp(net.layers{end}.type,'softmaxloss')
            err=error_multiclass(res(1).class,res);
            opts.TrainMiniBatchError=[opts.TrainMiniBatchError;err(1)/opts.parameters.batch_size];
            opts.TrainMiniBatchError_Top5=[opts.TrainMiniBatchError_Top5;err(2)/opts.parameters.batch_size];
            if opts.display_msg==1
                disp(['Minibatch loss: ', num2str(loss),...
                    ', top 1 err: ', num2str(opts.TrainMiniBatchError(end)),...
                    ',top 5 err:,',num2str(opts.TrainMiniBatchError_Top5(end))])
            end
        end

        opts.TrainMiniBatchLoss=[opts.TrainMiniBatchLoss;loss];
        if (~isfield(opts.parameters,'iterations'))
            opts.parameters.iterations=0;
        end
        opts.parameters.iterations=opts.parameters.iterations+1;

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%stochastic gradients descent%%%%%%%%%%%%%%%%%%%%%%%%
        [ net,res,opts ] = opts.parameters.learning_method( net,res,opts );
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    end

    %%summarize the current epoch
     if ~isfield(opts,'results')||~isfield(opts.results,'TrainEpochLoss')
        opts.results.TrainEpochLoss=[];
        opts.results.TrainEpochError=[];
        opts.results.TrainEpochError_Top5=[];
     end

    opts.results.TrainEpochLoss=[opts.results.TrainEpochLoss;mean(opts.TrainMiniBatchLoss(:))];

    if strcmp(net.layers{end}.type,'softmaxloss')
        opts.results.TrainEpochError=[opts.results.TrainEpochError;mean(opts.TrainMiniBatchError(:))];
        opts.results.TrainEpochError_Top5=[opts.results.TrainEpochError_Top5;mean(opts.TrainMiniBatchError_Top5(:))];
        disp(['Epoch ',num2str(opts.parameters.current_ep),...
         ', training loss: ', num2str(opts.results.TrainEpochLoss(end)),...
                ', top 1 err: ', num2str(opts.results.TrainEpochError(end)),...
                ',top 5 err:,',num2str(opts.results.TrainEpochError_Top5(end))])

    end

    if opts.RecordStats==1
        if ~isfield(opts,'results')||~isfield(opts.results,'TrainMiniBatchLoss')
            opts.results.TrainMiniBatchLoss=[];
            opts.results.TrainMiniBatchError=[];
            opts.results.TrainMiniBatchError_Top5=[];
        end
        opts.results.TrainMiniBatchLoss=[opts.results.TrainLoss;opts.TrainMiniBatchLoss];
        opts.results.TrainMiniBatchError=[opts.results.TrainError;opts.TrainMiniBatchError];
        opts.results.TrainMiniBatchError_Top5=[opts.results.TrainError_Top5;opts.TrainMiniBatchError_Top5];

    end

    toc;

end


%% FUNCTION Logistic_iMSF
%   Incompletet Multi-Source Learning with Logistic Loss (interface). This
%   code generates structural information for MultiSource_LogisticR and
%   performs the optimization.
%
%% OBJECTIVE
%   see manual.
%
%% INPUT
%   A_Set: {n * d} * t - input. The cell array of data. Each cell should be
%          n by p_i, with missing samples denoted by a whole row of NaNs.
%   Y_set: {n * 1} * t - output. Y_i \in {-1, 1}.
%   z:     group sparsity regularization parameter (a relative value, [0,1])
%
%% OUTPUT
%   Sol: {struct} solution struct array.
%         For task i: model: struct{i}.x, bais: struct{i}.c
%   funVal: objective function values at each iteration.
%
%% LICENSE
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%   Copyright (C) 2011 - 2012 Lei Yuan, Jiayu Zhou, and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 12, 2012.
%
%% RELATED PAPERS
%   [1] Lei Yuan, Yalin Wang, Paul M. Thompson, Vaibhav A. Narayan and Jieping
%       Ye, Multi-Source Learning for Joint Analysis of Incomplete
%       Multi-Modality Neuroimaging Data, KDD 2012
%   [2] Lei Yuan, Yalin Wang, Paul M. Thompson, Vaibhav A. Narayan and Jieping
%       Ye, for the Alzheimer's Disease Neuroimaging Initiative, Multi-source
%       Feature Learning for Joint Analysis of Incomplete Multiple Heterogeneous
%       Neuroimaging Data, NeuroImage 2012 Jul 2; 61(3):622-632.
%
%% RELATED FUNCTIONS
%   init_opts, Construct_iMSF, MultiSource_LogisticR

function [Sol, funVal] = Logistic_iMSF(X_Set, Y, lambda, opts)

if nargin< 4
    opts = [];
end

[A_Set, Y_Set, W, G, ind] = Construct_iMSF(X_Set, Y);
[Sol, funVal] = MultiSource_LogisticR(A_Set, Y_Set, lambda, G, W, ind, opts);


function test06 ( sample_num )

%*****************************************************************************80
%
%% TEST06 times R4_UNI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 May 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST06\n' );
  fprintf ( 1, '  Measure the time it takes R4_UNI to generate\n' );
  fprintf ( 1, '  %d uniform deviates.\n', sample_num );

  seed = uint32 ( 123456789 );

  time1 = cputime;

  for sample = 1 : sample_num
    [ value, seed ] = r4_uni ( seed );
  end

  time2 = cputime;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  %f seconds.\n', time2 - time1 );

  return
end


function [cache, place] = score_add_to_cache(cache,j,ps,score,scoring_fn)
% [cache place] = score_add_to_cache(cache,j,ps,score,scoring_fn)
%
% j is the son node,
% ps is the list of parents of j, for example [12 5 7],
% score is the score to add for this familly.
% scoring_fn is 'bic' or 'bayesian'.
%
% place = where the entry was add.
%
% example for 2 nodes with cache of size 5 :
%
% cache =
%   5   b        0      0      0  --> number of writing in cache +1 and b==1 iff the cache is full
%   0   0        1   -239.12   1  --> 1st familly in the cache (node 1 without parents) calculate with bic
%   0   0        2   -318.98   1
%   1   0        2   -189.23   2  --> 3rd familly in the cache (node 2 with 1 as parent) calculate with bayesian
%   0   1        1   -251.09   1
% .ps2bool.      j    score  1or2 --> new entry
%   |   |        |      |      |
%   |   |        |      |      |___> 1 for 'bic' or 2 for 'bayesian'
%   |   |        |      |__________> score of the familly
%   |   |        |_________________> son node of the familly
%   |   |__________________________> ==1 iff node 2 is parent of son node
%   |______________________________> ==1 iff node 1 is parent of son node
%
% If the cache is FULL then the new place is RanDoMly choose.
%
% francois.olivier.c.h@gmail.com

N=size(cache,2)-3;
place=0;

if ~isempty(find(ps==j))
  disp('This is a cyclic entry, nothing was done.');
elseif j>N | j<0
  disp('This entry is not valid, nothing was done.');
else

  switch scoring_fn
    case 'bic',
      fn=1;
    case 'bayesian',
      fn=2;
    otherwise,
      fn=3;
      %error(['unrecognized scoring fn ' scoring_fn]);
  end
  L=size(cache,1);

  if cache(1,2)==0
    place=cache(1,1);
  else
    place=ceil(rand(1)*(L-1))+1;
  end

  cache(place,:)=0
  cache(place,ps)=1;
  cache(place,N+1)=j;
  cache(place,N+2)=score;
  cache(place,N+3)=fn;
  cache(1,1)=place+1;
  if place==L | cache(1,2)~=0
    cache(1,2)=1
  end

end


%FIG_DESCI_CASSI Demonstrate decompress snapshot compressive imaging
%(DeSCI) results of simulated coded aperture snapshot spectral imaging
%(CASSI) `toy` dataset.
% Note: Run this demonstration script after completing the reconstruction
%       process TEST_DESCI_CASSI and obtaining the saved .mat file.
%   See also TEST_DESCI_CASSI, GAPDENOISE_CACTI, GAPDENOISE.
clear; clc;
% close all
addpath('../'); % repository root
addpath(genpath('../packages/')); % packages
% [1] run the corresponding test file for recovery or load the saved
% results
% test_desci_cassi % run the corresponding test file for recovery
load('../results/savedmat/desci_cassi_toy.mat'); % load the saved results

methods   = {'gaptv','desci'}; % all methods for comparison
methnames = {'GAP-TV','DeSCI'}; % corresponding names of the methods
nim = nframe*nmask; % number of images

%% [2.1] demonstrate the reconstructed spectum
colors = {'Brown', 'Orange', 'Blue', 'Red'}; % colors of the four birds
crops  =  [150 280 24 24;  % brown [x y width height] as insertShape
           340 210 24 24;  % orange
           349 396 24 24;  % blue
           349 439 24 24]; % red
locos = {'northwest','northwest','northwest','northwest'};

markers   = {'bx--','r*-'}; % markers

linewidth = 2;
markersize = 4;
textfontsize = 14;
legendfontsize = 12;

% Change default axes fonts.
set(0,'DefaultAxesFontName','Arial');
set(0,'DefaultAxesFontSize',textfontsize);
set(0,'DefaultAxesFontWeight','normal');
% Change default text fonts.
set(0,'DefaultAxesFontName','Arial');
set(0,'DefaultTextFontSize',textfontsize);
set(0,'DefaultTextFontWeight','normal');

f = figure('position',[25 50 700 900]); % PSNR plot
h = tight_subplot(3,2,[0.07 0.08],[.07 .035],[.1 .02]);
% subplot(3,2,1); % show crops on original RGB image
axes(h(1));
imshow(insertShape(orig_rgb,'Rectangle',crops,'Color','cyan','Linewidth',5));
title('Original');
% subplot(3,2,2); % coded measurement
axes(h(2));
imshow(meas,[]);
title('Coded frame');
for ic = 1:length(colors)
    color = colors{ic};
    crop = crops(ic,:);
    sp_truth = squeeze(sum(sum(orig(crop(2):crop(2)+crop(4)-1,crop(1):crop(1)+crop(3)-1,:)/MAXB,2),1));
    % figure;
    % subplot(3,2,ic+2);
    axes(h(ic+2));
    plot(wavelength,sp_truth/max(sp_truth),'k-','linewidth',linewidth,'markersize',markersize,'DisplayName','Ground truth'); hold on;

    for imeth = 1:length(methods)
        meth = methods{imeth};
        eval(sprintf('sp_%s = squeeze(sum(sum(v%s(crop(2):crop(2)+crop(4)-1,crop(1):crop(1)+crop(3)-1,:),2),1));',meth,meth));
        eval(sprintf('corrall(ic,imeth)=corr(sp_%s,sp_truth);',meth)); % corr is scale invariant
        eval(sprintf('plot(wavelength,rescale(sp_%s,min(sp_truth)/max(sp_truth),1),markers{imeth},''linewidth'',linewidth,''markersize'',markersize,''DisplayName'',[methnames{imeth} '', corr: %s'']);',meth,num2str(corrall(ic,imeth),'%.4f')));
    end
    title(color);
    if ic==3 || ic==4
        xlabel('Wavelength (nm)');
    end
    if ic==1 || ic==3
        ylabel('Intensity (a.u.)');
    end
    ylim([0 1]);
    hlg = legend('show');
    set(hlg,'FontSize',legendfontsize);
    legend('location',locos{ic});
    legend('boxoff');
end

%% [2.2] show exemplar frames (wavelength)
bands = [1 10 20 31];

nb = length(bands);
nmeth = length(methods);
f = figure('Position',[50 50 700 950]);
h = tight_subplot(nb,nmeth+1,[.002 .002],[.03 .03],[.02 .02]);
set(f, 'Color', 'white');
for ib = 1:nb
    band = bands(ib);
    lambda = wavelength(band);
    wlmap = (gray*kron(ones(3,1),spectrumRGB(lambda))); % colormap with the corresponding RGB wavelength
    wlmap = wlmap/max(wlmap(:));

    axes(h(1+(ib-1)*(nmeth+1)));
    imshow(uint8(orig(:,:,band)),'colormap',wlmap);
    if ib==1
        title('Ground truth');
    end
    for imeth = 1:nmeth
        meth = methods{imeth};
        axes(h(imeth+1+(ib-1)*(nmeth+1)));
        eval(sprintf('imshow(v%s(:,:,band),''colormap'',wlmap);',meth));
        if ib==1
            title(sprintf('%s',methnames{imeth}));
        end
    end
end


% extract_data.m
% --------------
% This program extracts the raw signal data from the CD included in the
% Cumming/Wong book.  The data on CD are in CEOS format.
% It is assumed that the run parameters are stored in CD_run_params.mat
% in the current directory.
% Run "specify_run_parameters.m" first to create this file.
% -------------------------------------------------------------------------
% Created :   Nov 01, 2004  by Kaan Ersahin
% Modified:   Nov 22, 2004  by Kaan Ersahin
% Modified:   Dec 3,  2004  by Ian Cumming
%              - changed function to m-file
%              - fixed the parameter file name
%              - added more radar parameters
% -------------------------------------------------------------------------

clear,   home,   format compact

%  Load the input parameters from a matlab data file
load CD_run_params

disp ' '
disp '------------------------------------------------'
disp ' UBC RRSG - CEOS Reader for RADARSAT-1 RAW DATA'
disp '------------------------------------------------'
disp ' '

% -------------------------------------------------------------------------
% Quantize the range line limits and block size, if necessary
% -------------------------------------------------------------------------

% Move the first range line to the beginning of an 8-line block
first_rg_line = 8 * ( ceil(first_rg_line / 8) - 1 ) + 1;

% Make 'Nrg_lines_blk' a multiple of 8, to get complete the 8-line blocks
Nrg_lines_blk = 8 * ceil(Nrg_lines_blk / 8);

% Find the number of complete blocks required to cover the area of interest
Nblocks = ceil(Nrg_lines / Nrg_lines_blk);

% Make 'Nrg_lines' a multiple of 'Nblocks', to get complete blocks
Nrg_lines = Nrg_lines_blk * Nblocks;

% =========================================================================
% These values are specific to the data set, DO NOT CHANGE for this CD
% =========================================================================

length_replica  =  2880;         % Total length (I&Q) of replica record
tot_Nrg_cells   =  9288;         % Total number of range cells per line
tot_Nrg_lines   = 19432;         % Total number of range lines (records)
first_replica   =     7;         % First record that contains the replica
PRF             = 1256.98;       % Pulse Reputation Frequency (Hz)
Fr              = 32.317e+6;     % Radar sampling rate (Hz)
f0              = 5.300e+9;      % Radar center frequency (Hz)
c               = 2.9979e+8;     % Speed of light (m/s)
R0              = 0.0065956*c/2; % Slant range of first radar sample (m)
Nrepl           = 1349;          % No. of valid samples in the replica
Kr              = 0.72135e+12;   % FM rate of radar pulse (Hz/s)
Tr              = 41.75e-6;      % Chirp duration (s)

% -------------------------------------------------------------------------
% Save parameters in a MAT file that can be used by subsequent programs

if     SaveV6,   save -v6 CD_run_params
else,            save     CD_run_params,      end
% -------------------------------------------------------------------------

fprintf('Total number of range lines      : %5d \n', tot_Nrg_lines )
fprintf('Total number of range cells      : %5d \n', tot_Nrg_cells )
fprintf('First range cell to be extracted : %5d \n', first_rg_cell )
fprintf('First range line to be extracted : %5d \n', first_rg_line )
fprintf('Number of range cells            : %5d \n', Nrg_cells )
fprintf('Number of range lines            : %5d \n', Nrg_lines )
fprintf('Number of range lines per block  : %5d \n', Nrg_lines_blk )
fprintf('Number of blocks                 : %5d \n', Nblocks )
disp ' '
disp '------------------------------------------------'

% -------------------------------------------------------------------------
%  Check the dimensions of the selected area to be processed.
% -------------------------------------------------------------------------

if (first_rg_line <= 0) | ((first_rg_line + Nrg_lines - 1) > tot_Nrg_lines)
    disp ' ',  disp '*****************************************************'
    disp ' ERROR: Check the limits of the range lines !', beep, return
end
if (first_rg_cell <= 0) | ((first_rg_cell + Nrg_cells - 1) > tot_Nrg_cells)
    disp ' ',  disp '*****************************************************'
    disp ' ERROR: Check the limits of the range cells !', beep, return
end

% -------------------------------------------------------------------------
% EXTRACT DATA from the area of interest and write data files
% -------------------------------------------------------------------------

for blk = 1 : Nblocks
    % find the first range line of block 'blk'
    start_line_blk = Nrg_lines_blk * (blk-1) + first_rg_line;
    fprintf('\nExtracting block number%3.0f,  RC1 =%5.0f,  RL1 =%6.0f\n',...
        blk, first_rg_cell, start_line_blk )

    % create the output file name for block 'blk'
    output_file_pre = strcat(output_path,output_prefix,'_',num2str(blk));

    % Call 'read_ceos_raw' function to extract the data for block 'blk'
    read_CEOS_raw( output_file_pre, start_line_blk, blk );

end  % of the 'blk' for loop

beep,   pause(0.3),   beep


function [ x_extend ] = img_extend( x, extension_y, extension_x, extension_z )
% mirrored extension to prevent any edge distortions

% M. Fischer, April 2016

%%
x_extend = zeros(size(x,1)+extension_y,size(x,2)+extension_x,size(x,3)+extension_z);
x_extend(1:end-extension_y,1:end-extension_x,1:end-extension_z) = x;
for j = 1:(extension_z-1)
    x_extend(:,:,end-extension_z+j) = x_extend(:,:,end-extension_z);
end;
x_extend(end-extension_y+1:end,1:end-extension_x,:) = flipud(x(end-extension_y+1:end,:,:));
x_extend(1:end-extension_y,end-extension_x+1:end,:) = fliplr(x(:,end-extension_x+1:end,:));
x_extend(end-extension_y+1:end,end-extension_x+1:end,:) = rot90(x(end-extension_y+1:end,end-extension_x+1:end,:),2);

end


%% Demo: fMRI searchlights with split-half correlations, classifier, and representational similarity analysis
%
% The data used here is available from http://cosmomvpa.org/datadb.zip
%
% This example uses the following dataset:
% - 'ak6' is based on the following work (please cite if you use it):
%    Connolly et al (2012), Representation of biological classes in the
%    human brain. Journal of Neuroscience,
%    doi 10.1523/JNEUROSCI.5547-11.2012
%
%    Six categories (monkey, lemur, mallard, warbler, ladybug, lunamoth)
%    during ten runs in an fMRI study. Using the General Linear Model
%    response were estimated for each category in each run, resulting
%    in 6*10=60 t-values.
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

%% Set data paths
% The function cosmo_config() returns a struct containing paths to tutorial
% data. (Alternatively the paths can be set manually without using
% cosmo_config.)
config=cosmo_config();

ak6_study_path=fullfile(config.tutorial_data_path,'ak6');

% show readme information
readme_fn=fullfile(ak6_study_path,'README');
cosmo_type(readme_fn);

% reset citation list
cosmo_check_external('-tic');

% set result directory
output_path=config.output_data_path;

%% Example: split-half correlation measure (Haxby 2001-style)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% This example uses the 'ak6' dataset

% define data filenames & load data from even and odd runs

data_path=fullfile(ak6_study_path,'s01'); % data from subject s01
mask_fn=fullfile(data_path, 'brain_mask.nii'); % whole brain mask

data_odd_fn=fullfile(data_path,'glm_T_stats_odd.nii');
ds_odd=cosmo_fmri_dataset(data_odd_fn,'mask',mask_fn,...
                            'targets',1:6,'chunks',1);

data_even_fn=fullfile(data_path,'glm_T_stats_even.nii');
ds_even=cosmo_fmri_dataset(data_even_fn,'mask',mask_fn,...
                            'targets',1:6,'chunks',2);

% Combine even and odd runs
ds_odd_even=cosmo_stack({ds_odd, ds_even});

% print dataset
fprintf('Dataset input:\n');
cosmo_disp(ds_odd_even);

% Use cosmo_correlation_measure.
% This measure returns, by default, a split-half correlation measure
% based on the difference of mean correlations for matching and
% non-matching conditions (a la Haxby 2001).
measure=@cosmo_correlation_measure;

% define spherical neighborhood with radius of 3 voxels
radius=3; % voxels
nbrhood=cosmo_spherical_neighborhood(ds_odd_even,'radius',radius);

% Run the searchlight with a 3 voxel radius
corr_results=cosmo_searchlight(ds_odd_even,nbrhood,measure);

% print output
fprintf('Dataset output:\n');
cosmo_disp(corr_results);

% Plot the output
cosmo_plot_slices(corr_results);

% Define output location
output_fn=fullfile(output_path,'corr_searchlight.nii');

% Store results to disc
cosmo_map2fmri(corr_results, output_fn);

% Show citation information
cosmo_check_external('-cite');


%STYLIZATION  Stylization filter
%
%     dst = cv.stylization(src)
%     dst = cv.stylization(src, 'OptionName',optionValue, ...)
%
% ## Input
% * __src__ Input 8-bit 3-channel image.
%
% ## Output
% * __dst__ Output image with the same size and type as `src`.
%
% ## Options
% * __SigmaS__ Range between 0 to 200. default 60
% * __SigmaR__ Range between 0 to 1. default 0.45
% * __FlipChannels__ whether to flip the order of color channels in input
%   `src` and output `dst`, between MATLAB's RGB order and OpenCV's BGR
%   (input: RGB->BGR, output: BGR->RGB). default false
%
% Stylization aims to produce digital imagery with a wide variety of effects
% not focused on photorealism. Edge-aware filters are ideal for stylization,
% as they can abstract regions of low contrast while preserving, or enhancing,
% high-contrast features.
%
% See also: cv.pencilSketch
%


function [cum_ret, cumprod_ret, daily_ret, daily_portfolio, exp_ret] ....
    = bk(fid, data, varargins, opts)
% This program starts the BK algorithm
%
% function [cum_ret, cumprod_ret, daily_ret, daily_portfolio, exp_ret] ....
%           = bk_start(fid, data, varargins, opts)
%
% cum_ret: a number representing the final cumulative wealth.
% cumprod_ret: cumulative return until each trading period
% daily_ret: individual returns for each trading period
% daily_portfolio: individual portfolio for each trading period
%
% 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, exp_ret] ...
%          = bk_start(fid, data, {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
K  = varargins{1};
L  = varargins{2};
c  = varargins{3};
tc = varargins{4};      % transaction cost fee rate

% Call BK's run algorithm
[cum_ret, cumprod_ret, daily_ret, daily_portfolio, exp_ret] ...
    = bk_run(fid, data, K, L, c, tc, opts);

end


function drawscene(X,C,R,fig,ctypehint,scenetitle,camsId)

% drawscene ... plots calibration points, cameras and their viewing axes
%
% drawscene(X,C,R,fig,ctypehint,scenetitle)
%
% X ............ 4xPOINTS matrix containg POINTS object points
% C ............ 3xCAMS matrix containing the camera centers (in world coord.)
% R ............ 3*CAMSx3 matrix containing camera rotation matrices
%                (needed for drawing the viewing axes)
% fig .......... figure handle (defaults to 1)
% ctypehint .... calibration object type of X (defaults to 'cloud')
% scenetitle ... title of the plot (defaults to '')
% camsIs ....... 1xCAMS vector with cameas Id (default is 1:CAMS

% $Author: svoboda $
% $Revision: 2.0 $
% $Id: drawscene.m,v 2.0 2003/06/19 12:07:03 svoboda Exp $
% $State: Exp $

POINTS = size(X,2);
CAMS   = size(C,2);

if nargin < 7
  camsId = [1:CAMS];
end

if (nargin < 3)
  error('not enough input arguments');
end
if (nargin < 5)
  scenetitle = '';
end
if (nargin < 4)
  ctypehint = 'cloud';
end

figure(fig); clf
title(scenetitle)
grid on
axis equal

% plot camera positions (blue)
drawcloud(C,fig,'b');

% plot calibration object (red)
drawobject(X,ctypehint,fig,'r');

% Mean of all points
centroid = mean(X(1:3,:)');

% plot viewing axes
for i=1:CAMS
  axis_dir = -R(3*i,:); % 3rd row of i-th rotation matrix
  axis_len = 0.6*norm(C(1:3,i)-centroid');
  endpoint = C(1:3,i)+axis_len*axis_dir';
  line([C(1,i),endpoint(1)],[C(2,i),endpoint(2)],[C(3,i),endpoint(3)]);
  text(C(1,i),C(2,i),C(3,i),sprintf('%4d',camsId(i)),'Color','k');
end


function pstruct = tapas_ehgf_jget_namep(pvec)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2017-2020 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF 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/>.

pstruct = struct;

l = length(pvec)/8;

if l ~= floor(l)
    error('tapas:hgf:UndetNumLevels', 'Cannot determine number of levels');
end

pstruct.mux_0     = pvec(1:l);
pstruct.sax_0     = pvec(l+1:2*l);
pstruct.mua_0     = pvec(2*l+1:3*l);
pstruct.saa_0     = pvec(3*l+1:4*l);
pstruct.kau       = pvec(4*l+1);
pstruct.kax       = pvec(4*l+2:5*l);
pstruct.kaa       = pvec(5*l+1:6*l-1);
pstruct.omu       = pvec(6*l);
pstruct.omx       = pvec(6*l+1:7*l);
pstruct.oma       = pvec(7*l+1:8*l);

end


function plot_frcst_(frcsts,y,time,options)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Filippo Ferroni, 6/1/2015
% Revised, 2/15/2017
% Revised, 3/21/2018
% Revised, 8/08/2019

% input : frcsts
% 1st dimension: horizon
% 2nd dimension: variable
% 3rdth dimension: draws
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if length(time) ~= length(y)
    error('Mismatch between time and in sample data (y)');
end
if size(time,2)>size(time,1)
    time=time';
end

hor     = size(frcsts,1);
nvar    = size(frcsts,2);
ndraws  = size(frcsts,3);
nplots  = [ceil(sqrt(nvar)) ceil(sqrt(nvar))];
savefig_yes = 0;
conf_sig    = 0.68;
add_frcst_yes = 0;
add_multiple_bands_yes = 0;
fnam_dir    = '.';
fnam_suffix = 'frcsts';
trasf_yes   =  0;

% retreive the frequency
integerTest = ~mod(time,1);
indx        = find(integerTest==1);
frq         = indx(2)-indx(1);

if frq == 12 % monthly
       timefor = time(end) + 1/12 : 1/12 : time(end) + hor/12;
       time = [time; timefor'];
elseif frq == 4 % quarterly
       timefor = time(end) + 1/4 : 1/4 : time(end) + hor/4;
       time = [time; timefor'];
elseif frq == 1 % annual
       timefor = time(end) + 1 : 1 : time(end) + hor;
       time = [time; timefor'];
elseif frq == 48 % weekly
       timefor = time(end) + 1/48 : 1/48 : time(end) + hor/48;
       time = [time; timefor'];
else
    error('Frequency not defined.')
end
%
% if strmatch(freq,'m') == 1 %#ok<*MATCH2>
%        timefor = time(end) + 1/12 : 1/12 : time(end) + hor/12;
%        time = [time; timefor'];
% elseif strmatch(freq,'q') == 1
%        timefor = time(end) + 1/4 : 1/4 : time(end) + hor/4;
%        time = [time; timefor'];
% elseif strmatch(freq,'a') == 1
%        timefor = time(end) + 1 : 1 : time(end) + hor;
%        time = [time; timefor'];
% else
%     error('You need to provide a frequency: ''m'', ''q'' or ''a''.')
% end
time_start = 1;
time_end = length(time);

if nargin < 4
    disp('You did not provided names for variables.')
    disp('I call them Var 1, Var 2, ... ')
    for v = 1 : nvar
        eval(['varnames{'   num2str(v) '} =  ''Var  ' num2str(v) ''';'])
    end
else
    if isfield(options,'time_start') ==1
        time_start = find(options.time_start==time);
        if isempty(time_start) ==1
            error('''time_start'' is not included in ''T''.')
        end
    end
    if isfield(options,'order_transform') ==1
        trasf_yes       = 1;
        order_trasform  = options.order_transform;
        if length(order_trasform) ~= nvar
            error('Mismatch between the ''order_transform'' size and # of variables.')
        end
    end
    if isfield(options,'varnames') ==1
        varnames = options.varnames;
        if length(varnames) ~= nvar
            error('Mismatch between the # varnames and # of variables to plot')
        end
    else
        disp('You did not provided names for the endogenous variables.')
        disp('I call them Var 1, Var 2, ...')
        for v = 1 : nvar
            eval(['varnames{'   num2str(v) '} =  ''Var  ' num2str(v) ''';'])
        end
    end
    if isfield(options,'nplots') ==1
        nplots = options.nplots;
    end
    if isfield(options,'saveas_strng') ==1
        savefig_yes = 1;
        % setting the names of the figure to save
        fnam_suffix = [ fnam_suffix options.saveas_strng ];
    end
    if isfield(options,'saveas_dir') == 1
        savefig_yes = 1;
        % setting the folder where to save the figure
        fnam_dir = options.saveas_dir;
        if exist(fnam_dir,'dir') == 0
            mkdir(fnam_dir)
        end
    end
    if isfield(options,'add_frcst') ==1
        add_frcst = options.add_frcst;
        if size(add_frcst) ~= [length(time), nvar]
            error('The ''add_frcst'' dimensions must be in-sample + output-of-sample length and the # of variables to plot')
        end
        add_frcst_yes = 1;
    end
    if isfield(options,'conf_sig') ==1
        conf_sig = options.conf_sig;
    end
    if isfield(options,'conf_sig_2') ==1
        if options.conf_sig_2 < conf_sig
            error('Additional confidence bands should be larger than ''options.conf_sig''.')
        end
        add_multiple_bands_yes = 1;
        sort_idx_2   = round((0.5 + [-options.conf_sig_2, options.conf_sig_2, 0]/2) * ndraws);
    end
end

nfigs  = ceil(length(varnames)/( nplots(1)*nplots(2)) );
nplots = repmat(nplots,nfigs,1);
for j=1:size(nplots,1),
    nbofplots(j)=nplots(j,1)*nplots(j,2);
end

ntotplots = sum(nbofplots);
if ntotplots<length(varnames),
    nfigplus = ceil((length(pplotvar)-ntotplots)/nbofplots(end));
    lastrow=nplots(end,:);
    lastrow=repmat(lastrow,nfigplus,1);
    nplots = [nplots;lastrow];
    nbofplots = [nbofplots repmat(nbofplots(end),1,nfigplus)];
end

if ndraws > 1
    frcsts_ = nan(length(time),nvar,ndraws);
    for kk = 1 : ndraws
        frcsts_(:,:,kk) = [y; frcsts(:,:,kk)];
    end
    frcsts = frcsts_;

    sort_idx   = round((0.5 + [-conf_sig, conf_sig, 0]/2) * ndraws);

    if trasf_yes ==1
        frcsts_ = nan(size(frcsts));
        for var = 1 : nvar
            if order_trasform(var) == 1 % period over period
                frcsts_(2:end,var,:) = (frcsts(2:end,var,:) - frcsts(1:end-1,var,:)) ;

            elseif order_trasform(var) == 100 % percentage period over period
                frcsts_(2:end,var,:) = 100*(frcsts(2:end,var,:) - frcsts(1:end-1,var,:));

            elseif order_trasform(var) == 12 % percentage 12 periord over 12 period (year over year % change f  or monthly data)
                frcsts_(13:end,var,:) = 100*(frcsts(13:end,var,:) - frcsts(1:end-12,var,:));

            elseif order_trasform(var) == 4 % percentage 4 periord over 4 period (year over year % change for quarterly data)
                frcsts_(5:end,var,:) = 100*(frcsts(5:end,var,:) - frcsts(1:end-4,var,:));

            elseif order_trasform(var) == 400
                frcsts_(2:end,var,:) = 400*(frcsts(2:end,var,:) - frcsts(1:end-1,var,:));

            elseif order_trasform(var) == 1200
                frcsts_(2:end,var,:) = 1200*(frcsts(2:end,var,:) - frcsts(1:end-1,var,:));

            else
                frcsts_(:,var,:) = frcsts(:,var,:);
            end
        end
        frcsts = frcsts_;
    end

    frcsts_sort   = sort(frcsts,3);
    if sort_idx(1) == 0
        sort_idx(1) = 1;
        warning('Bands not reliable. You have too few draws.')
    end
    irf_Median = squeeze(frcsts_sort(:, :, sort_idx(3) ));
    irf_low    = squeeze(frcsts_sort(:, :, sort_idx(1) ));
    irf_up     = squeeze(frcsts_sort(:, :, sort_idx(2) ));
    if  add_multiple_bands_yes == 1
        irf_low_low  = squeeze(frcsts_sort(:, :, sort_idx_2(1) ));
        irf_up_up    = squeeze(frcsts_sort(:, :, sort_idx_2(2) ));
    end

else
    irf_Median = [y; frcsts];

    if trasf_yes ==1
        irf_Median_ = nan(size(irf_Median));
        for var = 1 : nvar
            if order_trasform(var) == 1
                irf_Median_(2:end,var) = diff(irf_Median(:,var)) ;

            elseif order_trasform(var) == 12 % percentage 12 periord over 12 period (year over year % change f  or monthly data)
                irf_Median_(13:end,var) = 100*(irf_Median(13:end,var) - irf_Median(1:end-12,var));

            elseif order_trasform(var) == 4 % percentage 4 periord over 4 period (year over year % change for quarterly data)
                irf_Median_(5:end,var) = 100*(irf_Median(5:end,var) - irf_Median(1:end-4,var));

            elseif order_trasform(var) == 100
                irf_Median_(2:end,var) = 100*diff(irf_Median(:,var));

            elseif order_trasform(var) == 400
                irf_Median_(2:end,var) = 400*diff(irf_Median(:,var));

            elseif order_trasform(var) == 1200
                irf_Median_(2:end,var) = 1200*diff(irf_Median(:,var));

            else
                irf_Median_(:,var) = irf_Median(:,var);
            end
        end
        irf_Median = irf_Median_;
    end
    irf_low    = irf_Median;
    irf_up     = irf_Median;

end

if trasf_yes ==1 && add_frcst_yes ==1
    add_frcst_ = nan(size(add_frcst));
    for var = 1 : nvar
        if order_trasform(var) == 1
            add_frcst_(2:end,var) = diff(add_frcst(:,var)) ;

        elseif order_trasform(var) == 12 % percentage 12 periord over 12 period (year over year % change for monthly data)
            add_frcst_(13:end,var) = 100*(add_frcst(13:end,var) - add_frcst(1:end-12,var));

%         elseif order_trasform(var) == 4 % percentage 4 periord over 4 period (year over year % change for quarterly data)
%             add_frcst_(4:end,var) = 100*(add_frcst(4:end,var) - add_frcst(1:end-3,var));

        elseif order_trasform(var) == 4 % percentage 4 periord over 4 period (m over m % change for monthly data or YoY for Q data)
            add_frcst_(5:end,var) = 100*(add_frcst(5:end,var) - add_frcst(1:end-4,var));

        elseif order_trasform(var) == 100
            add_frcst_(2:end,var) = 100*diff(add_frcst(:,var));

        elseif order_trasform(var) == 400
            add_frcst_(2:end,var) = 400*diff(add_frcst(:,var));

        elseif order_trasform(var) == 1200
            add_frcst_(2:end,var) = 1200*diff(add_frcst(:,var));

        else
            add_frcst_(:,var) = add_frcst(:,var);
        end
    end
    add_frcst = add_frcst_;
end

jplot = 0;
jfig  = 0;

index_time = time_start:time_end;

for var= 1: nvar

    if jplot==0,
        figure('name',['Forecasts'] );
        jfig=jfig+1;
    end

    jplot=jplot+1;
    subplot(nplots(1),nplots(2),jplot)

    if add_multiple_bands_yes == 1

        h = area([time(index_time)],[irf_low_low(index_time,var),...
            irf_low(index_time,var) - irf_low_low(index_time,var),...
            irf_up(index_time,var) - irf_low(index_time,var),...
            irf_up_up(index_time,var) - irf_up(index_time,var)]);%,'FaceColor',[.85 .85 .85]);
        set(h(4),'FaceColor',[.95 .95 .95])
        set(h(3),'FaceColor',[.85 .85 .85])
        set(h(2),'FaceColor',[.95 .95 .95])
        set(h(1),'FaceColor',[1 1 1])
        set(h,'linestyle','none')
        hold on
        min_=min(irf_low_low(index_time,var));
        max_=max(irf_up_up(index_time,var));
    else
        h = area([time(index_time)],[irf_low(index_time,var),...
            irf_up(index_time,var) - irf_low(index_time,var)]);%,'FaceColor',[.85 .85 .85]);
        set(h(2),'FaceColor',[.85 .85 .85])
        set(h(1),'FaceColor',[1 1 1])
        set(h,'linestyle','none')
        min_=min(irf_low(index_time,var));
        max_=max(irf_up(index_time,var));
    end
    hold on;
    plot(time(index_time),irf_Median(index_time,var),'k');
    hold on;
    plot(time(index_time),zeros(length(index_time),1),'k');
    hold on;
    axis tight
    ylim([min_,max_]);
    if add_frcst_yes == 1
        plot(time(index_time),add_frcst(index_time,var),'b','LineWidth',2);
        ylim([min([min_,min(add_frcst(index_time,var))]) , max([max_,max(add_frcst(index_time,var))])  ] );
    end
    title(varnames{var})
    set(gcf,'position' ,[50 50 800 650])
    if jplot==nbofplots(jfig) || var==length(varnames)
        %legend(legenda);
        if savefig_yes == 1
            STR_RECAP = [ fnam_dir '/' fnam_suffix  '_' int2str(jfig)];
            saveas(gcf,STR_RECAP,'fig');
            if strcmp(version('-release'),'2022b') == 0
                saveas(gcf,STR_RECAP,'eps');
                % savefigure_pdf(STR_RECAP);
                savefigure_pdf([STR_RECAP '.pdf']);
            end
        end
        jplot=0;
    end
end


function y = horzcat(varargin)
    % Horizontal concatenation for sym arrays.
    %   C = HORZCAT(A, B, ...) horizontally concatenates the sym arrays A,
    %   B, ... .  For matrices, all inputs must have the same number of rows.  For
    %   N-D arrays, all inputs must have the same sizes except in the second
    %   dimension.
    %
    %   C = HORZCAT(A,B) is called for the syntax [A B].
    %
    %   See also VERTCAT.

    X = cellfun(@(x)SymExpression(x),varargin,'UniformOutput',false);
        %     X = SymExpression(varargin);
    str = cellfun(@(x)x.s,X,'UniformOutput',false);
    % evaluate the operation in Mathematica and return the
    % expression string
    sstr = ['Join[Sequence@@ToMatrixForm/@' cell2tensor(str,'ConvertString',false) ',2]'];

    % create a new object with the evaluated string
    y = SymExpression(sstr);

end


function S = sparse2 (i,j,s,m,n,nzmax)					    %#ok
%SPARSE2 replacement for SPARSE
%
%   Example:
%   S = sparse2 (i,j,s,m,n,nzmax)
%
%   Identical to the MATLAB sparse function (just faster).
%   An additional feature is added that is not part of the MATLAB sparse
%   function, the Z matrix.  With an extra output,
%
%   [S Z] = sparse2 (i,j,s,m,n,nzmax)
%
%   the matrix Z is a binary real matrix whose nonzero pattern contains the
%   explicit zero entries that were dropped from S.  Z only contains entries
%   for the sparse2(i,j,s,...) usage.  [S Z]=sparse2(X) where X is full always
%   returns Z with nnz(Z) = 0, as does [S Z]=sparse2(m,n).  More precisely,
%   Z is the following matrix (where ... means the optional m, n, and nzmax
%   parameters).
%
%       S = sparse (i,j,s, ...)
%       Z = spones (sparse (i,j,1, ...)) - spones (S)
%
%   See also sparse.

%   Copyright 2006-2007, Timothy A. Davis, http://www.suitesparse.com

error ('sparse2 mexFunction not found') ;


function nav=decode_sp3b(nav,headinfo,fid)
global glc gls
peph=gls.peph;
NMAX=10000; nav.peph=repmat(gls.peph,NMAX,1);

if headinfo.type=='P'
    ns=headinfo.ns;
else
    ns=2*headinfo.ns;
end

%traversal by epoch
while ~feof(fid)

    line=fgetl(fid);
    if strcmp(line,'EOF'),continue;end

    if line(1)~='*'
        fprintf('sp3 invalid epoch\n');
        continue;
    end

    time=str2time(line(4:31));
    if strcmp(headinfo.tsys,'UTC'),time=utc2gpst(time);end
    peph.time=time;

    %initialize peph struct
    for i=1:glc.MAXSAT
        for j=1:4
            peph.pos(i,j)=0;
            peph.std(i,j)=0;
            peph.vel(i,j)=0;
            peph.vst(i,j)=0;
        end
        for j=1:3
            peph.cov(i,j)=0;
            peph.vco(i,j)=0;
        end
    end

    for i=1:ns
        line=fgetl(fid);
        if size(line,2)<4||(line(1)~='P'&&line(1)~='V'),continue;end

        if line(2)==' '
            sys=glc.SYS_GPS;
        else
            sys=code2sys(line(2));
        end
        prn=str2double(line(3:4));
        if sys==glc.SYS_QZS,prn=prn+192;end
        sat=satno(sys,prn);
        if ~sat,continue;end

        for j=1:4
            if j<=3
                m=2;k1=1000;base=headinfo.bfact(1);k2=10^-3;k3=0.1;k4=10^-7;
            else
                m=3;k1=10^-6;base=headinfo.bfact(2);k2=10^-12;k3=10^-10;k4=10^-16;
            end

            val=str2double(line(4+14*(j-1)+1:4+14*(j-1)+14));
            if size(line,2)>61
                std=str2double(line(61+3*(j-1)+1:61+3*(j-1)+m));
            else
                std=0;
            end

            if line(1)=='P'
                if val~=0&&abs(val-999999.999999)>=10^-6
                    peph.pos(sat,j)=val*k1; v=1;
                end
                if base>0&&std>0
                    peph.std(sat,j)=base^std*k2;
                end
            elseif v
                if val~=0&&abs(val-999999.999999)>=10^-6
                    peph.vel(sat,j)=val*k3;
                end
                if base>0&&std>0
                    peph.vst(sat,j)=base^std*k4;
                end
            end
        end
    end

    if v
        if nav.np>size(nav.peph,1)
            nav.peph(nav.np+1:nav.np+NMAX,1)=repmat(gls.peph,NMAX,1);
        end
        nav.peph(nav.np+1)=peph;
        nav.np=nav.np+1;
    end

end
fclose(fid);

if nav.np<size(nav.peph,1)
    nav.peph(nav.np+1:end,:)=[];
end

return


% FILTERSD  Solve a DENSE NLP using FilterSD
%
% THIS IS A LOW LEVEL FUNCTION - USE opti_filtersd() INSTEAD!
%
% FilterSD is a NLP solver written in FORTRAN and available from:
% https://projects.coin-or.org/filterSD
%
%   [x,fval,exitflag,stats,lambda] = filtersd(fun, grad, x0, lb, ub, nlcon, nljac, cl, cu, opts)
%
%   Input arguments:
%       fun - nonlinear function handle
%       grad - gradient of nonlinear function handle
%       x0 - initial solution guess
%       lb - decision variable lower bounds
%       ub - decision variable upper bounds
%       nlcon - nonlinear constraints handle
%       nljac - Jacobian of nonlinear constraints handle (dense matrix)
%       cl - nonlinear constraints lower bound
%       cu - nonlinear constraints upper bound
%       opts - solver options (see below)
%
%   Return arguments:
%       x - solution vector
%       fval - objective value at the solution
%       exitflag - exit status (see below)
%       stats - statistics structure
%       lambda - Lagrange multipliers at solution [bounds;nl constraints]
%
%   Option Fields (all optional):
%       maxiter - maximum major solver iterations
%       maxtime - maximum solver execution time [won't return last solution on bounded problems]
%       maxfeval - maximum function evaluations [won't return last solution on bounded problems]
%       display - solver display level [0-2]
%       rho - initial trust region radius
%       htol - tolerance allowed in sum h(x) of constraint infeasibilities
%       rgtol - tolerance allowed in reduced gradient L2 norm
%       iterfun - iteration callback of the form ifun(feval,fval,x)
%
%   Return Status:
%       0 - Successful run
%       1 - Unbounded NLP (f(x) <= fmin at htol feasible point)
%       2 - Bounds on x are inconsistent
%       3 - Local minimum of feasibility problem and h(x) > htol (nonlinear constraints are locally inconsistent)
%       4 - Initial point x has h(x) > ubd
%       5 - Iteration limit reached
%       6 - Termination with rho <= htol
%       7 - Not enough workspace memory in ws or lws
%       8 - Insufficient space for filter
%       9 and above - LCP solver problem
%       105 - User Exit
%
%   Copyright (C) 2013 Jonathan Currie (I2C2)