function MDP = DEMO_MDP_questions
% Demo of active inference for visual salience
%__________________________________________________________________________
%
% This routine provide simulations of reading to demonstrate deep temporal
% generative models. It builds upon the scene construction simulations to
% equip the generative model with a second hierarchical level. In effect,
% this creates an agent that can accumulate evidence at the second level
% based upon epistemic foraging at the first. In brief, the agent has to
% categorise a sentence or narrative into one of two categories (happy or
% sad), where it entertains six possible sentences. Each sentence comprises
% four words, which are themselves constituted by two pictures or graphemes
% These are the same visual outcomes used in previous illustrations of
% scene construction and saccadic searches.
%
% Here, the agent has policies at two levels. The second level policy (with
% just one step into the future) allows it to either look at the next word
% or stay on the current page and make a decision. Concurrently, a first
% level policy entails one of four saccadic eye movements to each quadrant
% of the current page, where it will sample a particular grapheme.
%
% This provides a rough simulation of reading - that can be made more
% realistic by terminating first level active inference, when there can be
% no further increase in expected free energy (i.e., all uncertainty about
% the current word has been resolved). The subsequent inferred hidden
% states then become the outcome for the level above.
%
% To illustrate the schemes biological plausibility, one can change the
% agent's prior beliefs and repeat the reading sequence under violations of
% either local (whether the graphemes are flipped vertically) or globally
% (whether the sentence is surprising) expectations. This produces a
% mismatch negativity (MMN) under local violations) and a MMN with a
% P300 with global violations.
%
% see also: DEM_demo_MDP_habits.m and spm_MPD_VB_X.m
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: DEMO_MDP_questions.m 7766 2020-01-05 21:37:39Z karl $

% set up and preliminaries: first level
%==========================================================================
%
%--------------------------------------------------------------------------

rng('default')

% first level (lexical)
%==========================================================================
% question 1: {'noun (7)'}
% question 2: {'noun (7)','adverb (9)'}
% question 3: {'adjective (8)','noun (7)','adverb (9)'}
%--------------------------------------------------------------------------
% probabilistic mapping from hidden states to outcomes: A
%--------------------------------------------------------------------------
sentence{1} = {'Is there a ','&1_1','?'};
sentence{2} = {'Is any ','&2_1','&2_3','?'};
sentence{3} = {'Is a ','&3_2','&3_1','&3_3','?'};
sentence{4} = {'Yes','!'};
sentence{5} = {'No','!'};
sentence{6} = {'Not sure','!'};
sentence{7} = {'Ready','?'};

syn{1}      = {'Ready','OK','please begin'};
syn{2}      = {'Not sure','I dont know','Cannot say'};
syn{3}      = {'No','Sorry, no','There is not'};
syn{4}      = {'Yes','Yes there is','Well done'};

% assemble hidden states and transition probabilities
%--------------------------------------------------------------------------
name  = {};
for i = 1:numel(sentence)
    for j = 1:numel(sentence{i})
        phrase = sentence{i}(j);
        if ~any(ismember(name,phrase))
        name(end + 1) = phrase;
        end
    end
end

label.factor{1} = 'noun';   label.name{1}  = {'square ','triangle '};
label.factor{2} = 'adj.';   label.name{2}  = {'green ','red '};
label.factor{3} = 'adverb'; label.name{3}  = {'above','below'};
label.factor{4} = 'syntax'; label.name{4}  = name;

% prior beliefs about initial states D
%--------------------------------------------------------------------------
for i = 1:numel(label.factor)
    n    = numel(label.name{i});
    D{i} = ones(n,1)/n;
end

% restricted initial states to the beginning of a sentence
%--------------------------------------------------------------------------
state = label.name{4};
D{4}  = spm_zeros(D{4});
for i = 1:numel(sentence)
    j = ismember(state,sentence{i}(1));
    D{4}(j) = 1;
end

% probabilistic mapping from hidden states to outcomes: A
%--------------------------------------------------------------------------
Nf    = numel(D);
for f = 1:Nf
    Ns(f) = numel(D{f});
end

outcome = {};
for i = 1:numel(label.name)
    outcome = [outcome,label.name{i}];
end
for i = 1:numel(syn)
    outcome = [outcome,syn{i}];
end
outcome = unique(outcome);
j     = [];
for i = 1:numel(outcome)
    if outcome{i}(1) ~= '&'
        j = [j,i];
    end
end
outcome = outcome(j);

% single outcome modality with multiple phrases
%--------------------------------------------------------------------------
label.modality{1} = 'phrase';   label.outcome{1} = outcome;

for f1 = 1:Ns(1)
    for f2 = 1:Ns(2)
        for f3 = 1:Ns(3)
            for f4 = 1:Ns(4)

                % indices of the state
                %----------------------------------------------------------
                j = {f1,f2,f3,f4};

                % assemble phrases under this state
                %----------------------------------------------------------
                name = label.name{4}{f4};
                if name(1) == '&'
                    p   = eval(name(end));
                    q   = eval(['f' name(end)]);
                    out = label.name{p}(q);
                else
                    out = name;
                    for i = 1:numel(syn)
                        if any(ismember(syn{i},name))
                            out = syn{i};
                        end
                    end
                end

                % placing likelihood matrix
                %----------------------------------------------------------
                i = ismember(outcome,out);
                A{1}(i,j{:}) = 1/sum(i);

            end
        end
    end
end

% transitions: B{f} for each factor
%--------------------------------------------------------------------------
for f = 1:Nf
    B{f} = eye(Ns(f));
end

% specifies syntax
%--------------------------------------------------------------------------
B{4}  = spm_zeros(B{4});
for s = 1:numel(sentence)
    for t = 2:numel(sentence{s})
        i = find(ismember(state,sentence{s}(t - 1)));
        j = find(ismember(state,sentence{s}(t)));
        B{4}(j,i) = 1;
    end
    B{4}(j,j) = 1;
end

% MDP Structure
%--------------------------------------------------------------------------
mdp.T = 5;                      % number of updates
mdp.A = A;                      % observation model
mdp.B = B;                      % transition probabilities
mdp.D = D;                      % prior over initial states

mdp.label = label;
mdp.chi   = 1/32;

clear A B D

MDP   = spm_MDP_check(mdp);

% % check belief updates (and behaviour)
% %------------------------------------------------------------------------
% MDP.s = [1 2 1 8]';
% MDP   = spm_MDP_VB_X(MDP);
%
% spm_figure('GetWin','Figure 1'); clf
% spm_MDP_VB_trial(MDP,[3 4],1);
%
% % illustrate phase-precession and responses
% %------------------------------------------------------------------------
% spm_figure('GetWin','Figure 2'); clf
% spm_MDP_VB_LFP(MDP,[],4);
%
% cell2mat(MDP.label.outcome{1}(MDP.o))

% set up and preliminaries: first level
%==========================================================================
%
%--------------------------------------------------------------------------

%% second level (narrative)
%==========================================================================
% question 1: {'noun (7)'}
% question 2: {'noun (7)','adverb (9)'}
% question 3: {'adjective (8)','noun (7)','adverb (9)'}
%--------------------------------------------------------------------------
label.factor{1}  = 'narrative';    label.name{1}  = {'ready','question','answer'};
label.factor{2}  = 'question';     label.name{2}  = {'is?','where?','what?'};
label.factor{3}  = 'upper colour'; label.name{3}  = {'green','red'};
label.factor{4}  = 'lower colour'; label.name{4}  = {'green','red'};
label.factor{5}  = 'upper shape';  label.name{5}  = {'square','triangle'};
label.factor{6}  = 'lower shape';  label.name{6}  = {'square','triangle'};
label.factor{7}  = 'noun';         label.name{7}  = {'square','triangle'};
label.factor{8}  = 'adjective';    label.name{8}  = {'green','red'};
label.factor{9}  = 'adverb';       label.name{9}  = {'above','below'};

% prior beliefs about initial states D
%--------------------------------------------------------------------------
for i = 1:numel(label.factor)
    n    = numel(label.name{i});
    D{i} = ones(n,1)/n;
end

% known initial states
%--------------------------------------------------------------------------
D{1}(1)  = 128;

% probabilistic mapping from hidden states to outcomes: A
%--------------------------------------------------------------------------
label.modality{1} = 'noun';   label.outcome{1}  = MDP.label.name{1};
label.modality{2} = 'adj.';   label.outcome{2}  = MDP.label.name{2};
label.modality{3} = 'adverb'; label.outcome{3}  = MDP.label.name{3};
label.modality{4} = 'syntax'; label.outcome{4}  = MDP.label.name{4}(find(MDP.D{4}));

Nf    = numel(D);
for f = 1:Nf
    Ns(f) = numel(D{f});
end
for f1 = 1:Ns(1)
    for f2 = 1:Ns(2)
        for f3 = 1:Ns(3)
            for f4 = 1:Ns(4)
                for f5 = 1:Ns(5)
                    for f6 = 1:Ns(6)
                        for f7 = 1:Ns(7)
                            for f8 = 1:Ns(8)
                                for f9 = 1:Ns(9)

                                    % indices
                                    %--------------------------------------
                                    j = {f1,f2,f3,f4,f5,f6,f7,f8,f9};

                                    % answer: depending on question and beliefs
                                    %--------------------------------------
                                    Y  = 0;
                                    if f1 == 3
                                        if f2 == 1
                                            Y = (f7 == f5) | (f7 == f6);
                                        elseif f2 == 2
                                            if f9 == 1
                                                Y = (f7 == f5);
                                            elseif f9 == 2
                                                Y = (f7 == f6);
                                            end
                                        elseif f2 == 3
                                            if f9 == 1
                                                Y = (f7 == f5) & (f8 == f3);
                                            elseif f9 == 2
                                                Y = (f7 == f6) & (f8 == f4);
                                            end
                                        end
                                    end

                                    % A{1} noun:
                                    %======================================
                                    A{1}(f7,j{:}) = 1;

                                    % A{2} adjective:
                                    %======================================
                                    A{2}(f8,j{:}) = 1;

                                    % A{3} adverb:
                                    %======================================
                                    A{3}(f9,j{:}) = 1;

                                    % A{4} syntax: {'1','2','3','Y','N','?'}
                                    %======================================
                                    if f1 == 1
                                        A{4}(7,j{:})     = 1;
                                    elseif f1 == 2
                                        A{4}(f2,j{:})    = 1;
                                    elseif f1 == 3
                                        if Y
                                            A{4}(4,j{:}) = .98;
                                        else
                                            A{4}(5,j{:}) = .98;
                                        end
                                         A{4}(6,j{:})    = .02;
                                    end

                                end
                            end
                        end
                    end
                end
            end
        end
    end
end

% likelihood mappings
%--------------------------------------------------------------------------
Ng    = numel(A);
for g = 1:Ng
    No(g) = size(A{g},1);
    if any(spm_vec(sum(A{g})) ~= 1), disp('check A'), return, end
end

% controlled transitions: B{f} for each factor
%--------------------------------------------------------------------------
for f = 1:Nf
    B{f} = eye(Ns(f));
    label.action{f} = {'stay'};
end

% transitions B(1): {'ready','question','answer'}
%--------------------------------------------------------------------------
B{1}(:,:,1) = spm_speye(Ns(1),Ns(1),-1); B{1}(1,Ns(1),1) = 1;

% control states B(2): question {'1','2' or '3'} & control states B(7):B{9}
%--------------------------------------------------------------------------
% D{2} ;      % question;     {'1','2','3'};
% D{7} ;      % noun:         {'square','triangle'}
% D{8} ;      % adjective:    {'green','red'}
% D{9} ;      % adverb:       {'above','below'}
%--------------------------------------------------------------------------
for f = [2 7 8 9]
    for k = 1:Ns(f)
        B{f}(:,:,k + 1) = 0;
        B{f}(k,:,k + 1) = 1;
    end
    label.action{f} = ['stay', label.name{f}];
end

% allowable policies (time x polcy x factor)
%--------------------------------------------------------------------------
% question 1: {'noun (7)'}
% question 2: {'noun (7)','adverb (9)'}
% question 3: {'adjective (8)','noun (7)','adverb (9)'}
%--------------------------------------------------------------------------
V         = ones(2,14,Nf);
V(1,:,2)  = 1 + [1 1 2 2 2 2 3 3 3 3 3 3 3 3];
V(1,:,7)  = 1 + [1 2 1 1 2 2 1 1 1 1 2 2 2 2];
V(1,:,8)  = 1 + [1 1 1 1 1 1 1 1 2 2 1 1 2 2];
V(1,:,9)  = 1 + [1 1 1 2 1 2 1 2 1 2 1 2 1 2];
V(2,:,:)  = 1;

% priors: (utility) C: A{4} syntax: {'1','2','3','Y','N','S'}
%--------------------------------------------------------------------------
for g = 1:Ng
    C{g}  = zeros(No(g),1);
end

% the agent expects affirmative answers
%--------------------------------------------------------------------------
C{4}(4,:) =  1/4;
C{4}(5,:) = -1/4;

% actual state of the world
%--------------------------------------------------------------------------
s    = ones(Nf,1);
s(4) = 2;

% MDP Structure
%--------------------------------------------------------------------------
mdp.MDP    = MDP;
mdp.label  = label;             % names of factors and outcomes
mdp.tau    = 4;                 % time constant of belief updating
mdp.erp    = 4;                 % initialization
mdp.chi    = 0;                 % initialization

mdp.V = V;                      % allowable policies
mdp.A = A;                      % observation model
mdp.B = B;                      % transition probabilities
mdp.C = C;                      % preferred outcomes
mdp.D = D;                      % prior over initial states (context)
mdp.s = s;                      % initial state
mdp.o = [];                     % outcomes

mdp.link = spm_MDP_link(mdp);
MDP      = spm_MDP_check(mdp);
%   spm_MDP_factor_graph(mdp);

%% illustrate questioning
%==========================================================================
clear MDP
OPTIONS.D    = 1;
[MDP(1,1:6)] = deal(mdp);

MDP   = spm_MDP_VB_X(MDP,OPTIONS);

% show belief updates (and behaviour)
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 1'); clf
spm_MDP_VB_trial(MDP(1),[1 2 4],[1 3 4]);

% illustrate phase-precession and responses
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 2'); clf
spm_MDP_VB_LFP(MDP,[],3);

spm_figure('GetWin','Figure 2A'); clf
spm_MDP_VB_LFP(MDP,[],3,1);

spm_figure('GetWin','Figure 3'); clf
spm_MDP_VB_ERP(MDP(4:6),[3,2]);

spm_figure('GetWin','Figure 4'); clf
for i = 1:size(MDP,2)
    subplot(4,3,i)
    spm_questions_plot(MDP(1,i))
end

% illustrate violations
%==========================================================================
j      = 5;                               % which answer
NDP    = MDP(j);                          % get states and outcomes
if NDP.o(4,3) == 4                        % switch the answer
    NDP.o(4,3) = 5;
else
    NDP.o(4,3) = 4;
end
NDP = rmfield(NDP,'link');                % remove link (outomes are given)
NDP = spm_MDP_VB_X(NDP);

% find greatest effect on belief updating (about the scene)
%--------------------------------------------------------------------------
for f = 3:6
    v(f) =  norm(spm_vec(MDP(j).X{f}) - spm_vec(NDP.X{f}),'inf');
end
[v,f] = max(v);

% responses to appropriate and inappropriate answers
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 6 expected' ); clf, spm_MDP_VB_LFP(MDP(5),[],f);
spm_figure('GetWin','Figure 7 violation'); clf, spm_MDP_VB_LFP(NDP   ,[],f);

% illustrate 'communication'
%==========================================================================

% increase efficiency (i.e., suppress neurophysiological correlates)
%--------------------------------------------------------------------------
mdp.tau    = 3;                         % time constant of belief updating
mdp.erp    = 1;                         % initialization
mdp.chi    = 1/64;                      % initialization

% create two agents
%--------------------------------------------------------------------------
clear MDP
[MDP(1:2,1:6)] = deal(mdp);

% give a second subject veridical beliefs about the scene
%--------------------------------------------------------------------------
for i = [3 4 5 6]
    MDP(2,1).D{i} = sparse(s(i),1,1,Ns(i),1);
end

% confident player answers then asks
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 8'); clf
for i = 1:length(MDP)

    if i < 5
        % first model asks and the second answers
        %------------------------------------------------------------------
        MDP(1,i).n = ones(Ng,1)*[2 1 2];
        MDP(2,i).n = ones(Ng,1)*[2 1 2];
    else
        % switch roles
        %------------------------------------------------------------------
        MDP(1,i).n = ones(Ng,1)*[1 2 1];
        MDP(2,i).n = ones(Ng,1)*[1 2 1];
    end
end

TDP   = spm_MDP_VB_X(MDP,OPTIONS);
for i = 1:size(TDP,2)
    subplot(4,3,i), spm_questions_plot(TDP(:,i))
end

% confident player asks then answers
%--------------------------------------------------------------------------
for i = 1:length(MDP)

    if i < 5
        % first model asks and the second answers
        %------------------------------------------------------------------
        MDP(1,i).n = ones(Ng,1)*[1 2 1];
        MDP(2,i).n = ones(Ng,1)*[1 2 1];
    else
        % switch roles
        %------------------------------------------------------------------
        MDP(1,i).n = ones(Ng,1)*[2 1 2];
        MDP(2,i).n = ones(Ng,1)*[2 1 2];
    end
end

TDP   = spm_MDP_VB_X(MDP,OPTIONS);
for i = 1:size(TDP,2)
    subplot(4,3,i + 6)
    spm_questions_plot(TDP(:,i))
end

% confident player tells then asks
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 9'); clf
for i = 1:length(MDP)

    if i < 7
        % first model asks and the second answers
        %------------------------------------------------------------------
        MDP(1,i).n = ones(Ng,1)*[1 2 2];
        MDP(2,i).n = ones(Ng,1)*[1 2 2];
    else
        % switch roles
        %------------------------------------------------------------------
        MDP(1,i).n = ones(Ng,1)*[1 2 1];
        MDP(2,i).n = ones(Ng,1)*[1 2 1];
    end
end

TDP   = spm_MDP_VB_X(MDP,OPTIONS);
for i = 1:size(TDP,2)
    subplot(4,3,i)
    spm_questions_plot(TDP(:,i))
end

% building fantasies
%==========================================================================
spm_figure('GetWin','Figure 10'); clf

% make both players uncertain about the scene at hand
%--------------------------------------------------------------------------
MDP(2,1).D = MDP(1,1).D;

for i = 1:length(MDP)

    if i < 3
        % first model asks and the second answers
        %------------------------------------------------------------------
        MDP(1,i).n = ones(Ng,1)*[1 2 1];
        MDP(2,i).n = ones(Ng,1)*[1 2 1];
    else
        % switch roles
        %------------------------------------------------------------------
        MDP(1,i).n = ones(Ng,1)*[2 1 2];
        MDP(2,i).n = ones(Ng,1)*[2 1 2];
    end
end

TDP   = spm_MDP_VB_X(MDP,OPTIONS);
for i = 1:size(TDP,2)
    subplot(4,3,i)
    spm_questions_plot(TDP(:,i))
end

% now repeat when precluding uncertain responses
%--------------------------------------------------------------------------
for i = 1:length(MDP)

    MDP(1,i).A{4} = MDP(1,i).A{4} > 1/2;
    MDP(2,i).A{4} = MDP(2,i).A{4} > 1/2;

    if i < 3
        % first model asks and the second answers
        %------------------------------------------------------------------
        MDP(1,i).n = ones(Ng,1)*[1 2 1];
        MDP(2,i).n = ones(Ng,1)*[1 2 1];
    else
        % switch roles
        %------------------------------------------------------------------
        MDP(1,i).n = ones(Ng,1)*[2 1 2];
        MDP(2,i).n = ones(Ng,1)*[2 1 2];
    end
end

TDP   = spm_MDP_VB_X(MDP,OPTIONS);
for i = 1:size(TDP,2)
    subplot(4,3,6 + i)
    spm_questions_plot(TDP(:,i))
end

return

function spm_questions_plot(MDP)
%% illustrate beliefs
%--------------------------------------------------------------------------

% probabilistic mapping from hidden states to outcomes: A
%--------------------------------------------------------------------------
% label.modality{1} = 'noun';   label.outcome{1}  = {'square','triangle'};
% label.modality{2} = 'adj.';   label.outcome{2}  = {'green','red'};
% label.modality{3} = 'adverb'; label.outcome{3}  = {'above','below'};
% label.modality{4} = 'syntax'; label.outcome{4}  = {'1','2','3','Y','N','Sil.'};

% plot question, answer and posterior beliefs
%==========================================================================
cla;

for m = 1:numel(MDP)

    % Assemble question-and-answer
    %----------------------------------------------------------------------
    question = MDP(m).o(4,2);
    answer   = MDP(m).o(4,3);

    try
        qstr = cell2mat(MDP(m).MDP.label.outcome{1}(MDP(m).mdp(2).o));
        astr = cell2mat(MDP(m).MDP.label.outcome{1}(MDP(m).mdp(3).o));
    catch
        noun     = {'square','triangle'};
        adj      = {'green','red'};
        adverb   = {'above','below'};
        if question == 1
            qstr = ['is there a ' noun{MDP(m).o(1,2)} ' ?'];
        elseif question == 2
            qstr = ['Is any ' noun{MDP(m).o(1,2)} ' ' adverb{MDP(m).o(3,2)} ' ?'];
        elseif question == 3
            qstr = ['Is a ' adj{MDP(m).o(2,2)} ' ' noun{MDP(m).o(1,2)} ' ' adverb{MDP(m).o(3,2)} ' ?'];
        else
            qstr = '!';
        end
        if answer == 4
            astr = 'Yes !';
        elseif answer == 5
            astr = 'No !';
        else
            astr = 'I''m not sure';
        end
    end

    % is the answer right (for a single player)?
    %----------------------------------------------------------------------
    ind      = num2cell(MDP(m).s(:,3));
    if answer == find(MDP(m).A{4}(:,ind{:}),1) || m > 1
        cor = spm_softmax(2*[0;1;0]);
    else
        cor = spm_softmax(2*[1;0;0]);
    end

    %  plot posterior beliefs
    %----------------------------------------------------------------------
    % label.factor{3}  = 'upper colour'; label.name{3}  = {'green','red'};
    % label.factor{4}  = 'lower colour'; label.name{4}  = {'green','red'};
    % label.factor{5}  = 'upper shape';  label.name{5}  = {'square','triangle'};
    % label.factor{6}  = 'lower shape';  label.name{6}  = {'square','triangle'};

    % upper and lower object
    %----------------------------------------------------------------------
    T       = 2;
    col{1}  = [MDP(m).X{3}(2,T) MDP(m).X{3}(1,T) 0];
    col{1}  = col{1}*MDP(m).X{5}(1,T) + (1 - MDP(m).X{5}(1,T));
    col{2}  = [MDP(m).X{3}(2,T) MDP(m).X{3}(1,T) 0];
    col{2}  = col{2}*MDP(m).X{5}(2,T) + (1 - MDP(m).X{5}(2,T));
    col{3}  = [MDP(m).X{4}(2,T) MDP(m).X{4}(1,T) 0];
    col{3}  = col{3}*MDP(m).X{6}(1,T) + (1 - MDP(m).X{6}(1,T));
    col{4}  = [MDP(m).X{4}(2,T) MDP(m).X{4}(1,T) 0];
    col{4}  = col{4}*MDP(m).X{6}(2,T) + (1 - MDP(m).X{6}(2,T));

    plot(m,0,'^','MarkerSize',24,'LineWidth',4,'Color',col{4}), hold on
    plot(m,0,'s','MarkerSize',24,'LineWidth',4,'Color',col{3})
    plot(m,1,'^','MarkerSize',24,'LineWidth',4,'Color',col{2})
    plot(m,1,'s','MarkerSize',24,'LineWidth',4,'Color',col{1})

end

try, nq = MDP(1).n(4,2); catch, nq = 1; end
try, na = MDP(1).n(4,3); catch, na = 1; end
if ~nq, nq = 1; end
if ~na, na = 1; end
text(nq, 2,qstr,'HorizontalAlignment','Center')
text(na,-1,astr,'HorizontalAlignment','Center','FontWeight','bold','Color',cor)
axis([0 (m + 1) -1.5 2.5]), axis off, axis square

% return if multiple agents
%--------------------------------------------------------------------------
if numel(MDP) > 1, return, end

% upper and lower object
%--------------------------------------------------------------------------
rgb     = {[0 1 0],[1 0 0]};
shape   = {'s','^'};
plot(1 + 1/2,1,shape{MDP.s(5)},'MarkerSize',8,'LineWidth',1,'Color',rgb{MDP.s(3)})
plot(1 + 1/2,0,shape{MDP.s(6)},'MarkerSize',8,'LineWidth',1,'Color',rgb{MDP.s(4)})


function sampleScatterMatrix(rxnNames, model, sample, nPoints, fontSize, dispRFlag, rxnNames2)
% Draws a scatterplot matrix with pairwise scatterplots
% for multiple reactions
%
% USAGE:
%
%    sampleScatterMatrix(rxnNames, model, sample, nPoints, fontSize, dispRFlag, rxnNames2)
%
% INPUTS:
%    rxnNames:     Cell array of reaction names to be plotted
%    model:        Model structure
%    sample:       Samples to be analyzed (`nRxns x nSamples`)
%
% OPTIONAL INPUTS:
%    nPoints:      How many sample points to plot (Default `nSamples`)
%    fontSize:     Font size for labels (Default calculated based on
%                  number of reactions)
%    dispRFlag:    Display correlation coefficients (Default false)
%    rxnNames2:    Optional second set of reaction names
%
% EXAMPLE:
%
%    %Plots the scatterplots only between the three reactions listed -
%    %histograms for each reaction will be on the diagonal
%    sampleScatterMatrix({'PFK', 'PYK', 'PGL'}, model, sample);
%    %Plots the scatterplots between each of the first set of reactions and
%    %each of the second set of reactions. No histograms will be shown.
%    sampleScatterMatrix({'PFK', 'PYK', 'PGL'}, model, sample, 100, 10, true, {'ENO','TPI'});
%
% .. Author: - Markus Herrgard 9/14/06

[isInModel, rxnInd] = ismember(rxnNames, model.rxns);
rxnNames = rxnNames(isInModel);
rxnInd = rxnInd(isInModel);
nRxns = length(rxnNames);

%no optional inputs specified
if nargin < 4
    nPoints = size(sample, 2);
    dispRFlag = false;
    nRxns2 = nRxns;
    rxnNames2 = rxnNames;
    nPanels = nRxns * nRxns2;
    fontSize = 10 + ceil(50 / sqrt(nPanels));
    twoSetsFlag = false;
%some optional inputs specified
else
    if isempty(nPoints)
        nPoints = size(sample, 2);
    end
    if isempty(dispRFlag)
        dispRFlag = false;
    end
    if nargin == 7
        [isInModel2, rxnInd2] = ismember(rxnNames2, model.rxns);

        rxnNames2 = rxnNames2(isInModel2);
        rxnInd2 = rxnInd2(isInModel2);

        nRxns2 = length(rxnNames2);
        twoSetsFlag = true;
        nRxns = nRxns + 1;
        nRxns2 = nRxns2 + 1;
    else
        nRxns2 = nRxns;
        rxnNames2 = rxnNames;
        twoSetsFlag = false;
    end
    if isempty(fontSize)
        nPanels = nRxns * nRxns2;
        fontSize = 10 + ceil(50 / sqrt(nPanels));
    end
end
height = 0.8 / nRxns;
width = 0.8 / nRxns2;

clf
showprogress(0, 'Drawing scatterplots ...');
for i = 1:nRxns

    for j = 1:nRxns2
        showprogress(((i - 1) * nRxns+j) / (nRxns * nRxns2));
        left = 0.1 + (j - 1) * width;
        bottom = 0.9 - i * height;
        if twoSetsFlag
            if i == 1 && j == 1
            else
                %subplot(nRxns,nRxns2,(i-1)*nRxns2+j);
                subplot('position', [left bottom width height]);
                if j >1 && i >1
                    sampleScatterPlot(sample, rxnInd2(j - 1), rxnInd(i - 1), nPoints, fontSize, dispRFlag);
                elseif i == 1
                    sampleHistInternal(sample, rxnInd2(j - 1), fontSize);
                elseif j == 1
                    sampleHistInternal(sample, rxnInd(i - 1), fontSize);
                end
                if i == 1
                    xlabel(rxnNames2{j - 1}, 'FontSize', fontSize);
                    set(gca, 'XAxisLocation', 'top');
                end
                if j == 1
                    ylabel(rxnNames{i - 1}, 'FontSize', fontSize);
                end
            end
        else
            if j == i
                %subplot(nRxns,nRxns2,(i-1)*nRxns2+j);
                subplot('position', [left bottom width height]);
                sampleHistInternal(sample, rxnInd(i), fontSize);

            elseif j > i
                %subplot(nRxns,nRxns2,(i-1)*nRxns2+j);
                subplot('position', [left bottom width height]);
                sampleScatterPlot(sample, rxnInd(j), rxnInd(i), nPoints, fontSize, dispRFlag);
            end
            if i == 1
                xlabel(rxnNames2{j}, 'FontSize', fontSize);
                set(gca, 'XAxisLocation', 'top');
            end
            if j == nRxns2
                set(gca, 'YAxisLocation', 'right');
                ylabel(rxnNames{i}, 'FontSize', fontSize);
            end
        end
    end

end

function sampleScatterPlot(sample, id1, id2, nPoints, fontSize, dispRFlag)

selPts = randperm(size(sample, 2));
selPts = selPts(1:nPoints);

plot(sample(id1, selPts),sample(id2, selPts), 'r.');
set(gca, 'YTickLabel', []);
set(gca, 'XTickLabel', []);
maxx = max(sample(id1, :));
maxy = max(sample(id2, :));
minx = min(sample(id1, :));
miny = min(sample(id2, :));
axis([minx maxx miny maxy]);
% Display correlation coefficients
if dispRFlag
    r = corrcoef(sample(id1, :)', sample(id2, :)');
    h = text(minx + 0.66 * (maxx-minx), miny + 0.2 * (maxy - miny), num2str(round(100 * r(1, 2)) / 100));
    set(h, 'FontSize', fontSize - 5);
end

function sampleHistInternal(sample , id, fontSize)

[n, bins] = hist(sample(id , :), 30);
if exist('smooth')
    plot(bins, smooth(bins, n') / sum(n'));
else
    plot(bins, n' / sum(n'));
end
maxx = max(bins);
minx = min(bins);
set(gca, 'XTick', linspace(minx, maxx, 4));
set(gca, 'XTickLabel', round(10 * linspace(minx, maxx, 4)) / 10);
set(gca, 'YTickLabel', []);
set(gca, 'FontSize', fontSize - 5);
axis tight


function mne_write_cov(fid,cov)
%
%
%   mne_write_cov(fid,cov)
%
%   Write a covariance matrix to an open file
%
%   fid     - an open file id
%   cov     - the covariance matrix to write
%

%
%   Author : Matti Hamalainen, MGH Martinos Center
%   License : BSD 3-clause
%
%   Revision 1.4  2007/12/10 01:02:55  msh
%   Fixed writing of a diagonal covariance matrix
%
%   Revision 1.3  2006/05/05 03:50:40  msh
%   Added routines to compute L2-norm inverse solutions.
%   Added mne_write_inverse_sol_stc to write them in stc files
%   Several bug fixes in other files
%
%   Revision 1.2  2006/05/03 18:53:06  msh
%   Approaching Matlab 6.5 backward compatibility
%
%   Revision 1.1  2006/04/29 12:44:10  msh
%   Added covariance matrix writing routines.
%
%

me='MNE:mne_write_cov';

global FIFF;
if isempty(FIFF)
   FIFF = fiff_define_constants();
end

fiff_start_block(fid,FIFF.FIFFB_MNE_COV);
%
%   Dimensions etc.
%
fiff_write_int(fid,FIFF.FIFF_MNE_COV_KIND,cov.kind);
fiff_write_int(fid,FIFF.FIFF_MNE_COV_DIM,cov.dim);
if cov.nfree > 0
    fiff_write_int(fid,FIFF.FIFF_MNE_COV_NFREE,cov.nfree);
end
%
%   Channel names
%
if ~isempty(cov.names)
    fiff_write_name_list(fid,FIFF.FIFF_MNE_ROW_NAMES,cov.names);
end
%
%   Data
%
if cov.diag
   fiff_write_double(fid,FIFF.FIFF_MNE_COV_DIAG,cov.data);
else
   if issparse(cov.data)
      fiff_write_float_sparse_rcs(fid,FIFF.FIFF_MNE_COV,cov.data);
   else
      q = 1;
      vals = zeros(cov.dim*(cov.dim+1)/2,1);
      for j = 1:cov.dim
          for k = 1:j
              vals(q) = cov.data(j,k);
              q = q + 1;
          end
      end
      fiff_write_double(fid,FIFF.FIFF_MNE_COV,vals);
   end
end
%
%   Eigenvalues and vectors if present
%
if ~isempty(cov.eig) && ~isempty(cov.eigvec)
    fiff_write_float_matrix(fid,FIFF.FIFF_MNE_COV_EIGENVECTORS,cov.eigvec);
    fiff_write_double(fid,FIFF.FIFF_MNE_COV_EIGENVALUES,cov.eig);
end
%
%   Projection operator
%
fiff_write_proj(fid,cov.projs);
%
%   Bad channels
%
if ~isempty(cov.bads)
    fiff_start_block(fid,FIFF.FIFFB_MNE_BAD_CHANNELS);
    fiff_write_name_list(fid,FIFF.FIFF_MNE_CH_NAME_LIST,cov.bads);
    fiff_end_block(fid,FIFF.FIFFB_MNE_BAD_CHANNELS);
end
%
%   Done!
%
fiff_end_block(fid,FIFF.FIFFB_MNE_COV);


function test_bug1397

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_preprocessing ft_appenddata

% the following code was obtained from http://www.fieldtriptoolbox.org/tutorial/coherence
% on Wed Mar 28 15:36:40 CEST 2012

% find the interesting epochs of data
cfg = [];
% MODIFICATION, use trialfun handle and other path to the data
cfg.trialfun                  = @trialfun_left;
cfg.dataset                   = dccnpath('/home/common/matlab/fieldtrip/data/ftp/test/ctf/SubjectCMC.ds');
cfg = ft_definetrial(cfg);

% MODIFICATION, use only 10 trials
cfg.trl = cfg.trl(1:10,:);

% MODIFICATION, the following should not affect the problem
%
% % detect EOG artifacts in the MEG data
% cfg.continuous                = 'yes';
% cfg.artfctdef.eog.padding     = 0;
% cfg.artfctdef.eog.bpfilter    = 'no';
% cfg.artfctdef.eog.detrend     = 'yes';
% cfg.artfctdef.eog.hilbert     = 'no';
% cfg.artfctdef.eog.rectify     = 'yes';
% cfg.artfctdef.eog.cutoff      = 2.5;
% cfg.artfctdef.eog.interactive = 'no';
% cfg = ft_artifact_eog(cfg);
%
% % detect jump artifacts in the MEG data
% cfg.artfctdef.jump.interactive = 'no';
% cfg.padding                    = 5;
% cfg = ft_artifact_jump(cfg);
%
% % detect muscle artifacts in the MEG data
% cfg.artfctdef.muscle.cutoff      = 8;
% cfg.artfctdef.muscle.interactive = 'no';
% cfg = ft_artifact_muscle(cfg);
%
% % reject the epochs that contain artifacts
% cfg.artfctdef.reject          = 'complete';
% cfg = ft_rejectartifact(cfg);

% preprocess the MEG data
cfg.demean                    = 'yes';
cfg.dftfilter                 = 'yes';
cfg.channel                   = {'MEG'};
cfg.continuous                = 'yes';
meg = ft_preprocessing(cfg);

cfg              = [];
cfg.dataset      = meg.cfg.dataset;
cfg.trl          = meg.cfg.trl;
cfg.continuous   = 'yes';
cfg.demean       = 'yes';
cfg.dftfilter    = 'yes';
cfg.channel      = {'EMGlft' 'EMGrgt'};
cfg.hpfilter     = 'yes';
cfg.hpfreq       = 10;
cfg.rectify      = 'yes';
emg = ft_preprocessing(cfg);

% see http://bugzilla.fieldtriptoolbox.org/show_bug.cgi?id=1397
% the reported problem in fieldtrip-20120302 was
%
% ??? Error using ==> ft_appenddata at 266
% there is a difference in the time axes of the input data

data = ft_appenddata([], meg, emg);
assert(numel(data.label)==153);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION to avoid external dependencies of this test
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function trl = trialfun_left(cfg)

% read in the triggers and create a trial-matrix
% consisting of 1-second data segments, in which
% left ECR-muscle is active.

event = ft_read_event(cfg.dataset);
trig  = [event(find(strcmp('backpanel trigger', {event.type}))).value];
indx  = [event(find(strcmp('backpanel trigger', {event.type}))).sample];

%left-condition
sel = [find(trig==1028):find(trig==1029)];

trig = trig(sel);
indx = indx(sel);

trl = [];
for j = 1:length(trig)-1
  trg1 = trig(j);
  trg2 = trig(j+1);
  if trg1<=100 && trg2==2080
    trlok      = [[indx(j)+1:1200:indx(j+1)-1200]' [indx(j)+1200:1200:indx(j+1)]'];
    trlok(:,3) = [0:-1200:-1200*(size(trlok,1)-1)]';
    trl        = [trl; trlok];
  end
end


function createVolAnatSKERI(imgPathName,outputVAnatName,outputUnfoldName, clipVals)
% createVolAnat([imgPathName],[outputVAnatName],[outputUnfoldName], ...
% [clipVals=get from UI])
%
% imgPathName is the path to one of the I-files or to an analyze-format file.
% A file browser comes up if none is given.
%
% If the optional clipVals (in the format of [minVal maxVal]) is provided,
% will clip the vAnatomy before converting to uint8 (0-255, a big
% annoyance resulting from older software constraints) for the .dat file.
% Otherwise, pops up some example slices and a histogram, and lets you
% select the cutoff.
%
% 2000.01.20 RFD (bob@white.stanford.edu): wrote it.
% 2001.02.21 RFD: renamed from createVanatomy and added improved
%            the clipping UI by checking for obvious cases, like
%            when no clipping is necessary.
% 2002.05.22 RFD: fixed a bug that would apply an incorrect upper clip
%            when the lower clip was not zero. THis probably didn't affect
%            anyone with the old (crappy) anatomy data, since the lower clip
%            was always best left at zero anyway.
%            I also now get the mmPerPix from the image header.
% 2002.07.30 RFD: now allow Analyze format files as input. Reorients standard
%            axial Analyze data to match the mrGray sagittal convention.
% 2002.08.05 RFD: fixed crash when imgPathName is passed that was introduced on 7/30.
% 2003.06.25 DHB (brainard@psych.upenn.edu): Allow passing of output names.
% 2004.01.24 Junjie: change GE_readHeader to readIfileHeader
% Get the input filename
if ~exist('imgPathName','var') | isempty(imgPathName)
    [fname, fullPath] = uigetfile({'*.img;*.nii;*.nii.gz','MRI images';'*.img','analyze';'*.nii','NIFTI';'*.nii.gz','NIFTI gz';'*.*','all files'}, 'Select analyze/NIFTI I-files...');
    if(isnumeric(fname))
        error('user canceled.');
    end
    imgPathName = fullfile(fullPath, fname);
end
if ~exist('outputVAnatName','var')
    outputVAnatName = [];
end
if ~exist('outputUnfoldName','var') | isempty(outputUnfoldName)
    outputUnfoldName = 'UnfoldParams';
end
[fullPath,fname,ext] = fileparts(imgPathName);
if(strcmp(ext,'.hdr') | strcmp(ext,'.img'))
    [img,mmPerPix] = loadAnalyze(fullfile(fullPath,[fname '.hdr']));
    % To make analyze data the same orientation as vAnatomy, we reorient the data.
    % Note that we assume Analyze orientation code '0'- transverse, unflipped.
    % If that is the case, then the following will make the data look correct in mrGray.
    % It will even get left/right correct (left will be left and right right).
    img = permute(img,[3,2,1]);
    mmPerPix = [mmPerPix(3),mmPerPix(2),mmPerPix(1)];
    % flip each slice ud (ie. flip along matlab's first dimension, which is our x-axis)
    for jj=1:size(img,3)
        img(:,:,jj) = flipud(squeeze(img(:,:,jj)));
    end
    % flip each slice lr(ie. flip along matlab's second dimension, which is our y-axis)
    for jj=1:size(img,3)
        img(:,:,jj) = fliplr(squeeze(img(:,:,jj)));
    end
elseif any(strcmpi(ext,{'.nii','.gz'}))
% 	nii = load_nii(fullfile(fullPath,[fname,'.nii']));		% old_RGB flag only matters if datatype=128 & bitpix=24
% 	if isfield(nii.hdr.hist,'rot_orient')		% already RAS?
% 		img = nii.img;
% 		mmPerPix = nii.hdr.dime.pixdim(2:4);
% 	else
% 		error('unknown orientation')
% % 		k = [? ? ?];
% % 		img = permute(nii.img,k);
% % 		mmPerPix = nii.hdr.dime.pixdim(k+1);
% 	end
% 	% now orient to IPR for writeVolAnat to yield PIR vAnatomy
% 	img = permute(img,[3 2 1]);
% 	mmPerPix =mmPerPix([3 2 1]);
% 	img = flipdim(img,1);
% 	img = flipdim(img,2);

	nii = niftiRead(imgPathName);
	mmPerPix = nii.pixdim;
	img = double(nii.data);

	% get nifti rotation matrix
	qb = nii.quatern_b;
	qc = nii.quatern_c;
	qd = nii.quatern_d;
	qa = sqrt(1-qb^2-qc^2-qd^2);
	R = [ qa^2+qb^2-qc^2-qd^2, 2*(qb*qc+qa*qd), 2*(qb*qd-qa*qc);...
			2*(qb*qc-qa*qd), qa^2-qb^2+qc^2-qd^2, 2*(qc*qd+qa*qb);...
			2*(qb*qd+qa*qc), 2*(qc*qd-qa*qb), qa^2-qb^2-qc^2+qd^2];
	R = R';	% something about the handedness??? only if positive determinant of R???
				% rows of R now [R;A;S]
	R(:,3) = nii.qfac * R(:,3);
	disp(R)

	% now orient to IPR for writeVolAnat to yield PIR vAnatomy
	IPR = [0 0 0];
	[junk,IPR(3)] = max(abs(R(1,:)));	% L-R dimension
	[junk,IPR(2)] = max(abs(R(2,:)));	% P-A dimension
	[junk,IPR(1)] = max(abs(R(3,:)));	% I-S dimension
	if numel(unique(IPR))~=3
		error('bad rotation matrix')
	end
	if any(abs([R(1,IPR(3)),R(2,IPR(2)),R(3,IPR(1))]) < 0.8)
		disp('nifti quaternion inconsistent with 90deg rotations')
	end
	img = permute(img,IPR);
	if R(3,IPR(1)) >= 0
		img = flipdim(img,1);
	end
	if R(2,IPR(2)) >= 0
		img = flipdim(img,2);
	end
	if R(1,IPR(3)) < 0
		img = flipdim(img,3);
	end

% elseif(strcmp(ext,'.gz') | strcmp(ext,'.nii'))
elseif strcmp(ext,'.gz')
    ni = niftiRead(imgPathName);
    img = permute(double(ni.data),[3,2,1]);
    mmPerPix = [ni.pixdim(3),ni.pixdim(2),ni.pixdim(1)];
    for(jj=1:size(img,3))
        img(:,:,jj) = flipud(squeeze(img(:,:,jj)));
    end
    for(jj=1:size(img,3))
        img(:,:,jj) = fliplr(squeeze(img(:,:,jj)));
    end
else
    % get some info from the I-file header
    [su_hdr,ex_hdr,se_hdr,im_hdr,pix_hdr,im_offset] = readIfileHeader(imgPathName);
    mmPerPix = [im_hdr.pixsize_Y, im_hdr.pixsize_X, im_hdr.slthick];
%    nSlices = se_hdr.se_numimages;
% Now, the DICOM format of i-files does not provide numimages info, so you
% need to count all i-files in the folder.
    nSlices = length(getIfileNames(imgPathName));
    imDimXY = [im_hdr.dim_X, im_hdr.dim_Y];

    disp('Image header says:');
    disp(['  ',num2str(nSlices),' slices']);
    disp(['  ',num2str(imDimXY),' pixels inplane (x,y)']);
    disp(['  ',num2str(mmPerPix),' mm per pixel']);
    % load ifiles
    disp('Loading I-files...');
    img = makeCubeIfiles(fullfile(fullPath,fname), imDimXY, [1:nSlices]);
end
% Get image information
volume_pix_size = 1./mmPerPix;
disp('Finding optimal clip values...');
minVal = min(img(:));
maxVal = max(img(:));

% Set the minumum to zero
if(minVal~=0)
    disp(['Image minimum is not zero (',num2str(minVal),')- adjusting...']);
    img = img-minVal;
	 maxVal = maxVal - minVal;
	 minVal = 0;
end

% clip and scale, if necessary
if (maxVal <= 255)
    disp('Image values are within 8-bit range- no clipping or scaling is necessary.');
	 lowerClip = minVal;
	 upperClip = maxVal;
elseif exist('clipVals', 'var') && ~isempty(clipVals)
    % Clip and scale image values to be 0-255
    disp('Clipping intensities and scaling to 0-255...');
    img(img > clipVals(2)) = clipVals(2);
    img(img < clipVals(1)) = clipVals(1);
    img = round((img-clipVals(1))./(clipVals(2)-clipVals(1)) * 255);
	 lowerClip = clipVals(1);
	 upperClip = clipVals(2);
else
    figure(99);
    disp('Computing histogram...');
    [clippedImg, suggestedClipVals] = mrAnatHistogramClip(img, 0.5, 0.98);
    lowerClip = suggestedClipVals(1);
    upperClip = suggestedClipVals(2);
    [n,x] = hist(img(:),100);
    histHandle = subplot(2,2,1);

%     bar(x,n,1);
%     lcLine = line([lowerClip,lowerClip], [0,max(n)]);
%     ucLine = line([upperClip,upperClip], [0,max(n)]);

% set yscale so you can see more than the largest histogram peak
	bar(x,n,1,'b');
	set(histHandle,'ylim',[-0.05 1.05]*2e5,'xlim',[-0.05 1.05]*max(x))
	cLineY = [-0.05,1.05]*max(n);
	lcLine = line([lowerClip,lowerClip], cLineY, 'color','r');
	ucLine = line([upperClip,upperClip], cLineY, 'color','r');

    % display some slices
    slice{1} = squeeze(img(round(end/2),:,:));
    slice{2} = squeeze(img(:,round(end/2),:));
    slice{3} = squeeze(img(:,:,round(end/2)));
    for(i=[1:3])
        slice{i}(slice{i}<lowerClip) = lowerClip;
        slice{i}(slice{i}>upperClip) = upperClip;
        slice{i} = slice{i}-lowerClip;
        slice{i} = round(slice{i}./upperClip*255);
        subplot(2,2,2);
        image(slice{1});colormap(gray(256));axis image;axis off;
        subplot(2,2,3);
        image(slice{2});colormap(gray(256));axis image;axis off;
        subplot(2,2,4);
        image(slice{3});colormap(gray(256));axis image;axis off;
    end
    % Loop until user says it looks good
    ok = 0;
    while(~ok)
%         answer = inputdlg({'lower clip value (min=0): ',...
%                 ['upper clip value (max=',num2str(maxVal),'):']}, ...
%                 'Set intensity clip values', 1, ...
%                 {num2str(lowerClip),num2str(upperClip)}, 'on');
%         if ~isempty(answer)
%             lowerClip = str2num(answer{1});
%             upperClip = str2num(answer{2});
%         end

        disp(['intensity clip values are: ' num2str(lowerClip) ', ' num2str(upperClip)]);
        infoStr = 'Click twice on the histogram to set clip values. Define the smallest range that keeps the two right peaks.';
        uiwait(helpdlg(infoStr, 'clip info'))
        axes(histHandle);
        [x,y] = ginput(2);
        lowerClip = min(x);
        upperClip = max(x);

%         delete(lcLine); delete(ucLine);
%         lcLine = line([lowerClip,lowerClip], [0,max(n)]);
%         ucLine = line([upperClip,upperClip], [0,max(n)]);
			set(lcLine,'xdata',[lowerClip,lowerClip])
			set(ucLine,'xdata',[upperClip,upperClip])

        % Display some slices
        slice{1} = squeeze(img(round(end/2),:,:));
        slice{2} = squeeze(img(:,round(end/2),:));
        slice{3} = squeeze(img(:,:,round(end/2)));
        for(i=[1:3])
            slice{i} = rescale2(slice{i}, [lowerClip, upperClip]);
%             slice{i}(slice{i}<lowerClip) = lowerClip;
%             slice{i}(slice{i}>upperClip) = upperClip;
%             slice{i} = slice{i}-lowerClip;
%             slice{i} = round(slice{i}./upperClip*255);
            subplot(2,2,2);
            image(slice{1});colormap(gray(256));axis image;axis off;
            subplot(2,2,3);
            image(slice{2});colormap(gray(256));axis image;axis off;
            subplot(2,2,4);
            image(slice{3});colormap(gray(256));axis image;axis off;
        end

        bn = questdlg('Is this OK?','Confirm clip values','Yes','No','Cancel','Yes');
        if(strcmp(bn,'Cancel'))
            error('Cancelled.');
        else
            ok = strcmp(bn,'Yes');
        end
    end

    % Clip and scale image values to be 0-255
    %
    % I haven't been fully satisfied by just automatically clipping off the top
    % few % of intensities (this is what mrInitRet currently does to the inplanes).
    % So, lately I've been looking at the histogram and picking intensityClip
    % by hand, then viewing the images to see how well it worked.
    disp('Clipping intensities and scaling to 0-255...');
    img(img>upperClip) = upperClip;
    img(img<lowerClip) = lowerClip;
    img = round((img-lowerClip)./(upperClip-lowerClip) * 255);
    %img = rescale2(img, [lowerClip, upperClip], [0, 255]);
end
subplot(2,2,2);
image(squeeze(img(round(end/2),:,:)));colormap(gray(256));axis image; set(gca,'xtick',[],'ytick',[]),	xlabel('right \rightarrow'),ylabel('\leftarrow posterior') %axis off;
subplot(2,2,3);
image(squeeze(img(:,round(end/2),:)));colormap(gray(256));axis image; set(gca,'xtick',[],'ytick',[]), xlabel('right \rightarrow'),ylabel('\leftarrow inferior') %axis off;
subplot(2,2,4);
image(squeeze(img(:,:,round(end/2))));colormap(gray(256));axis image; set(gca,'xtick',[],'ytick',[]), xlabel('posterior \rightarrow'),ylabel('\leftarrow inferior') %axis off;
% Crop the image cube
% (you can skip this if you want, and just do the crop in mrGray)
%figure(1);image(squeeze(img2(round(end/2),:,:))./clip.*255);colormap(gray(256));axis image;
%figure(2);image(squeeze(img2(:,round(end/2),:))./clip.*255);colormap(gray(256));axis image;
%figure(3);image(squeeze(img2(:,:,round(end/2)))./clip.*255);colormap(gray(256));axis image;
%img2 = img2(20:200,30:240,1:124);
%figure(1);image(squeeze(img(round(end/2),:,:))./clip.*255);colormap(gray(256));axis image;
%figure(2);image(squeeze(img(:,round(end/2),:))./clip.*255);colormap(gray(256));axis image;
%figure(3);image(squeeze(img(:,:,round(end/2)))./clip.*255);colormap(gray(256));axis image;
% Save
disp('Saving volume anatomy and unfolding params ...');
path = writeVolAnat(img,mmPerPix,outputVAnatName);
save([path outputUnfoldName], 'volume_pix_size', 'lowerClip', 'upperClip');
disp(['vAnatomy and UnfoldParams saved to ' path]);
return;


%%*******************************************************************
%% NTrhsfun: compute the right-hand side vector of the
%%           Schur complement equation for the NT direction.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*******************************************************************

function [rhs,EinvRc,hRd] = NTrhsfun(blk,At,par,X,Z,rp,Rd,sigmu,hRd,dX,dZ)

spdensity = par.spdensity;
m = length(rp);
if (nargin > 8)
    corrector = 1;
else
    corrector = 0;
    hRd = zeros(m,1);
end
hEinvRc = zeros(m,1);
EinvRc  = cell(size(blk,1),1);
rhsfree = [];
%%
for p = 1:size(blk,1)
    pblk = blk(p,:);
    n = sum(pblk{2});  numblk = length(pblk{2});
    if strcmp(pblk{1},'l')
        if iscell(sigmu)
            EinvRc{p} = sigmu{p}./Z{p} -X{p};
        else
            EinvRc{p} = sigmu./Z{p} -X{p};
        end
        Rq = sparse(n,1);
        if (corrector) && (norm(par.parbarrier{p})==0)
            Rq = dX{p}.*dZ{p}./Z{p};
        else
            tmp  = par.dd{p}.*Rd{p};
            tmp2 = mexMatvec(At{p},tmp,1);
            hRd = hRd + tmp2;
        end
        EinvRc{p} = EinvRc{p} - Rq;
        tmp2 = mexMatvec(At{p},EinvRc{p},1);
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'q')
        if iscell(sigmu)
            EinvRc{p} = qops(pblk,-sigmu{p}./(par.gamz{p}.*par.gamz{p}),Z{p},4) -X{p};
        else
            EinvRc{p} = qops(pblk,-sigmu./(par.gamz{p}.*par.gamz{p}),Z{p},4) -X{p};
        end
        Rq = sparse(n,1);
        if (corrector) && (norm(par.parbarrier{p})==0)
            w = sqrt(par.gamz{p}./par.gamx{p});
            hdx = qops(pblk,w,par.ff{p},5,dX{p});
            hdz = qops(pblk,w,par.ff{p},6,dZ{p});
            hdxdz = Arrow(pblk,hdx,hdz);
            vv = qops(pblk,w,par.ff{p},5,X{p});
            Vihdxdz = Arrow(pblk,vv,hdxdz,1);
            Rq = qops(pblk,w,par.ff{p},6,Vihdxdz);
        else
            tmp  = par.dd{p}.*Rd{p} + qops(pblk,qops(pblk,Rd{p},par.ee{p},1),par.ee{p},3);
            tmp2 = mexMatvec(At{p},tmp,1);
            hRd = hRd + tmp2;
        end
        EinvRc{p} = EinvRc{p} - Rq;
        tmp2 = mexMatvec(At{p},EinvRc{p},1);
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'s')
        n2 = pblk{2}.*(pblk{2}+1)/2;
        if iscell(sigmu)
            %%ss = [0,cumsum(pblk{2})];
            %%sigmuvec = zeros(n,1);
            %%for k = 1:length(pblk{2});
            %%   sigmuvec(ss(k)+1:ss(k+1)) = sigmu{p}(k)*ones(pblk{2}(k),1);
            %%end
            sigmuvec = mexexpand(pblk{2},sigmu{p});
            tmp = spdiags(sigmuvec./par.sv{p} -par.sv{p},0,n,n);
        else
            tmp = spdiags(sigmu./par.sv{p} -par.sv{p},0,n,n);
        end
        EinvRc{p} = Prod3(pblk,par.G{p}',tmp,par.G{p},1);
        Rq = sparse(n,n);
        if (corrector) && (norm(par.parbarrier{p})==0)
            hdZ = Prod3(pblk,par.G{p},dZ{p},par.G{p}',1);
            hdX = spdiags(qops(pblk,par.parbarrier{p}',1./par.sv{p},3)-par.sv{p},0,n,n)-hdZ;
            tmp = Prod2(pblk,hdX,hdZ,0);
            tmp = 0.5*(tmp+tmp');
            if (numblk == 1)
                d = par.sv{p};
                e = ones(pblk{2},1);
                Rq = 2*tmp./(d*e'+e*d');
                if (nnz(Rq) <= spdensity*n2); Rq = sparse(Rq); end
            else
                Rq = sparse(n,n);
                ss = [0, cumsum(pblk{2})];
                for i = 1:numblk
                    pos = ss(i)+1 : ss(i+1);
                    d = par.sv{p}(pos); e = ones(length(pos),1);
                    Rq(pos,pos) = 2*tmp(pos,pos)./(d*e' + e*d'); %#ok
                end
            end
            Rq = Prod3(pblk,par.G{p}',Rq,par.G{p},1);
        else
            tmp = Prod3(pblk,par.W{p},Rd{p},par.W{p},1,par.nzlistAy{p});
            tmp2 = AXfun(pblk,At(p,:),par.permA(p,:),{tmp});
            hRd = hRd + tmp2;
        end
        EinvRc{p} = EinvRc{p} - Rq;
        tmp2 = AXfun(pblk,At(p,:),par.permA(p,:),EinvRc(p));
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'u')
        rhsfree = [rhsfree; Rd{p}]; %#ok
    end
end
%%
rhs = rp + hRd - hEinvRc;
rhs = full([rhs; rhsfree]);
%%*******************************************************************


function [imref, im, mask] = histology_preprocessing(imref, im, opts)
% HISTOLOGY_PREPROCESSING  Prepare slices for intra-histology registration.
%
% HISTOLOGY_PREPROCESSING converts a list of histology images to grayscale,
% inverts and thresholds them (so that the background is black instead of
% white), extends the histograms to cover the dynamic range, and then
% matches the histograms to a reference slice. This prepares the slices for
% successful intra-histology registration.
%
% The function has two syntaxes. The reference can be provided as an image
% or histogram.
%
% -------------------------------------------------------------------------
% Reference image syntax:
% [IMREF2, IM2, MASK] = HISTOLOGY_PREPROCESSING(IMREF, IM)
%
%   IMREF is the slice whose histogram will be used as reference for the
%   others. IM is the stack of RGB colour or grayscale histology images to
%   preprocess. The images can be provided as:
%
%     * A char with a single path and filename. E.g. 'file.mha'. With this
%       format, an output directory must be provided in OPTS.outdir, or
%       else a temporal one with a random name is created (see below).
%
%     * A cell array of filenames. E.g. {'file1.mha', 'file2.mha'}. IM{I}
%       is the path and filename of the I-th slice. See note about
%       OPTS.outdir in previous item.
%
%     * A plain array IM(R, C, I, CH), R=row, C=column, I=slice,
%       CH=channel.
%
%     * A vector of scimat structs. IM(I) is a scimat struct with the I-th
%       slice. See "help scimat" for details.
%
%   IMREF2, IM2 are the preprocessed images in the same format as the
%   inputs.
%
%   MASK is a binary mask for IM2. This mask can be used to speed up
%   registration (when a mask is provided to elastix, only pixels within
%   the mask are used for the registration metric). The mask includes the
%   tissue and a bit of background around it.
%
% ... = HISTOLOGY_PREPROCESSING(..., OPTS)
%
%   OPTS is a struct with options for the algorithm.
%
%     'outdir': If IM is a list of filenames, this is the path where the
%               preprocessed images are saved to.
%
%     'Grayscale': (def false) true/false whether image will be converted
%              to grayscale.
%
% -------------------------------------------------------------------------
% Reference histogram syntax:
% [HREF2, IM2, MASK] = HISTOLOGY_PREPROCESSING(HREF, IM)
%
%   HREF is a (CH, L)-matrix with the reference histogram, where CH is the
%   number of channels, and L is the number of intensity levels. For
%   example, for an RGB image, CH=3, and for a uint8 class, L=256.
%
%   IM is the image to preprocess, as in the syntax above.
%
%   HREF2 is the histogram with the same preprocessing as the image.
%
%   IM2, MASK are as in the syntax above.
%
% ... = HISTOLOGY_PREPROCESSING(..., OPTS)
%
%   OPTS is as in the syntax above.
%
%
% See also: regmatchedfilt, transfdiffreg.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2014-2016 University of Oxford
% Version: 0.5.1
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(2, 3);
nargoutchk(0, 3);

% defaults
if (nargin < 3 || isempty(opts))
    opts = struct;
end
if (~isfield(opts, 'Grayscale') || isempty(opts.Grayscale))
    opts.Grayscale = false;
end

% check the type and number of the input images
if (ischar(im)) % single filename

    imType = 'filename';
    im = {im};
    N = 1;

elseif (iscell(im)) % list of filenames

    imType = 'filename';
    N = length(im);

elseif (isstruct(im)) % list of scimat structs

    imType = 'scimat';
    N = length(im);

else % plain array

    imType = 'array';
    N = size(im, 3);

end

% convert IMREF to a plain array if it's not already, or keep as histogram
% if it's a histogram
if (ischar(imref)) % filename

    imrefMat = scimat_load(imref);
    imrefMat = imrefMat.data;
    imrefType = 'filename';

elseif (isstruct(imref)) % scimat struct

    imrefMat = imref.data;
    imrefType = 'scimat';

elseif (isa(imref, 'double') && size(imref, 1)==3) % histogram as (3,256) matrix

    imrefType = 'histogram';

else % array

    % move channels to 5th index, so that it's the same format as IM.data
    imrefMat = reshape(imref, size(imref, 1), size(imref, 2), 1, 1, size(imref, 3));
    imrefType = 'array';

end

switch (imrefType)
    case 'histogram'

        % is the histogram has more than one channel, we don't try to
        % convert to grayscale

        % preprocessing of histogram equivalent to preprocessing the
        % reference image (invert, threshold, etc.)
        imref = histogram_preprocessing(imref);

    otherwise

        % grayscale conversion of IMREF, if requested by user
        if (opts.Grayscale)

            imrefMat = rgb2gray(squeeze(imrefMat));

        end

        % preprocessing of the reference image (invert, threshold, etc.)
        imrefMat = individual_image_preprocessing(imrefMat);

end

% if images are provided as filenames, either the user must provide an
% output directory, or we generate a temp one with a random name
if (strcmp(imType, 'filename') ...
        && (~isfield(opts, 'outdir') || isempty(opts.outdir)))
    opts.outdir = tempname;
end

% allocate memory for masks
switch (imType)

    case  'filename' % filename

        mask = cell(1, N);

    case 'scimat' % scimat struct

        mask = im;

    case 'array' % plain array

        mask = zeros(size(im, 1), size(im, 2), size(im, 3), 'uint8');

end

% loop all images
for I = 1:N

    % convert IM to a scimat struct if it's not already
    switch (imType)

        case  'filename' % filename

            im0 = scimat_load(im{I});

        case 'scimat' % scimat struct

            im0 = im(I);

        case 'array' % plain array

            im0 = scimat_im2scimat(reshape(im(:, :, I, :, :), ...
                [size(im, 1) size(im, 2) 1 1 size(im, 4)]));

    end

    switch (imrefType)
        case 'histogram'

            % preprocessing of IM
            mask0 = im0;
            [im0.data, mask0.data] = individual_image_preprocessing(im0.data);

            im0.data = match_histograms_hist(imref, im0.data);

            % grayscale conversion of IM, if requested by user
            if (opts.Grayscale)

                im0.data = rgb2gray(squeeze(im0.data));

            end

        otherwise

            % grayscale conversion of IM, if requested by user
            if (opts.Grayscale)

                im0.data = rgb2gray(squeeze(im0.data));

            end

            % preprocessing of IM
            mask0 = im0;
            [im0.data, mask0.data] = individual_image_preprocessing(im0.data);

            % match IM histogram to IMREF
            im0.data = match_histograms_im(imrefMat, im0.data);

    end

    % convert processed images to output format
    switch (imType)

        case  'filename' % filename

            [~, name, ext] = fileparts(im{I});
            im{I} = [opts.outdir filesep name ext];
            scimat_save(im{I}, im0);
            mask{I} = [opts.outdir filesep 'mask-' name ext];
            scimat_save(mask{I}, mask0);

        case 'scimat' % scimat struct

            im(I) = im0;
            mask(I) = mask0;

        case 'array' % plain array

            im(:, :, I, :) = reshape(im0.data, ...
                [size(im, 1) size(im, 2) 1 size(im, 5)]);
            mask(:, :, I, :) = mask0.data;

    end

end

% convert processed images to output format
switch (imrefType)

    case  'filename' % filename

        error('Not implemented yet')

    case 'scimat' % scimat struct

        imref.data = imrefMat;

    case 'array' % plain array

        imref = reshape(imrefMat, ...
            [size(imrefMat, 1) size(imrefMat, 2) 1 size(imrefMat, 5)]);

end

end

%% nested functions

% individual_image_preprocessing:
%
%   Preprocessing of an individual image. Invert, threshold background,
%   extend histogram to whole dynamic range. Compute a rough binary mask of
%   the object.
function [im, mask] = individual_image_preprocessing(im)

% loop image channels
for I = 1:size(im, 5)

    % select one channel
    ch = im(:, :, :, :, I);

    % the image should have two types of blackground pixels: white-ish and
    % pure black. The white-ish ones are from the original image, and the
    % black ones come from the B-spline fill-in. When we invert the image,
    % we don't want to invert the black ones, so we select non-black pixels
    idx = ch ~= 0;

    % invert image and make lowest intensity = 0
    ch(idx) = max(ch(idx)) - ch(idx);

    % remove background by zeroing anything <= mode (the background forms
    % the largest peak). We are quite conservative here in terms of keeping
    % all the tissue we can, to avoid losing detail, even if that means
    % that we are going to have a bit of background noise
    ch(ch <= mode(double(ch(idx)))) = 0;

    % extend histogram to cover whole dynamic range
    minh = double(min(ch(ch > 0)));
    maxh = double(max(ch(ch > 0)));
    ch = uint8(255 * (double(ch) - minh) / (maxh - minh));

    % replace image channel with the processed one
    im(:, :, :, :, I) = ch;

end

% compute a mask of the foreground
if (size(im, 5) == 1)

    mask = uint8(im > 2);

else

    mask = uint8(rgb2gray(squeeze(im)) > 2);

end

% remove some background noise in the mask
se = strel('disk', 1);
mask = imerode(mask, se);

% dilate mask to cover some background
se = strel('disk', 10);
mask = imdilate(mask, se);

end

% histogram_preprocessing:
%
%   Transform histogram to replicate what individual_image_preprocessing()
%   does to an image
function hgram = histogram_preprocessing(hgram)

% invert histogram, except for the pixels == 0, that continue being 0
hgram(:, 2:end) = hgram(:, end:-1:2);

% find the modes of the histogram
[~, imode] = max(hgram, [], 2);

% number of intensity levels
L = size(hgram, 2);

% loop histogram channels
for CH = 1:size(hgram, 1)

    % crop the histogram <= mode
    hgram(CH, 1) = sum(hgram(CH, 1:imode(CH)));
    hgram(CH, 2:imode(CH)) = 0;

    % extend dynamic range of histogram
    xbin = linspace(1, L-1, L-imode(CH));
    hgram(CH, 2:end) = round(interp1(xbin, hgram(CH, (imode(CH)+1):end), 1:L-1));

end

end

% match_histograms_im:
%
%   Match the histogram of each channel in IM to IMREF, ignoring background
%   voxels. IMREF is a reference image.
%
%   IMREF: reference image, array (R, C, CH)
function im = match_histograms_im(imref, im)

% match histograms to reference slice
for I = 1:size(im, 5)

    % select channels
    chref = imref(:, :, :, :, I);
    ch = im(:, :, :, :, I);

    % match histograms ignoring the background
    idxref = chref > 0;
    idx = ch > 0;
    ch(idx) = imhistmatch(ch(idx), chref(idxref));

    % replace image channel with processed ones
    %imref(:, :, I) = chref;
    im(:, :, :, :, I) = ch;

end

end

% match_histograms_hist:
%
%   Match the histogram of each channel in IM to IMREF, ignoring background
%   voxels. IMREF is a histogram with one or more channels.
%
%   IMREF: histogram, array (CH, intensity)
function im = match_histograms_hist(hgram, im)

% match image histograms to reference histogram
for I = 1:size(im, 5)

    % select image channel
    ch = im(:, :, :, :, I);

    % match histograms ignoring the background
    idx = ch > 0;
    ch(idx) = histeq(ch(idx), hgram(I, 2:end));

    % replace image channel with processed ones
    %imref(:, :, I) = chref;
    im(:, :, :, :, I) = ch;

end

end


function flattenFromROI(view, ROI)
% Initiates the mrFlatMesh GUI, filling in some fields
%
%  flattenFromROI(view, ROI)
%
% Wrapper for mrFlatMesh GUI. Opens the GUI, filling in some
% default starting values. Uses the center of mass of the specified
% ROI as the start point.
%
% GLOBALS: uses mrSESSION fields to initialize the mrFlatMesh GUI.
%
% Example:
%   flattenFromROI(VOLUME{1});    % Uses selected (current) ROI
%
% HISTORY:
% 2002.03.04 RFD (bob@white.stanford.edu): wrote it
% 7/16/02 djh, replaced mrSESSION.vAnatomyPath with global vANATOMYPATH
% 2002.07.19 RFD: cleaned up default filenames a bit
% 2006.02.05 MMS: made it work with ROIs with only one coordinate

mrGlobals

switch view.viewType
    case 'Gray'
        leftGrayPath  = view.leftPath;
        rightGrayPath = view.rightPath;
    otherwise
        error([mfilename,': unsupported view type "',view.viewType,'".']);
end

if(~exist('ROI','var'))
    if(view.selectedROI>0)
        ROI = view.ROIs(view.selectedROI);
    else
        myErrorDlg('No ROI selected');
        return;
    end
end

mmPerPixXYZ = readVolAnatHeader(vANATOMYPATH);

startXYZ = round(mean(ROI.coords',1)');
startXYZ = [startXYZ(2);startXYZ(1);startXYZ(3)];

% Now how do we find which hemisphere this coord belongs to?
hemi = viewGet(view, 'roihemi', ROI.coords);
if isempty('hemi'), hemi = 'left'; end;

if strcmpi('hemi', 'left')
    grayPath = fullfile(leftGrayPath,'');
else
    grayPath = fullfile(rightGrayPath,'');
end

[grayPathDir,grayPathFileName] = fileparts(grayPath);
if(exist(grayPath)~=2)
    if(exist(grayPathDir,'dir'))
        % at least get them close
        grayPath = grayPathDir;
    else
        grayPath = '';
        grayPathDir = '';
    end
end

% If the Gray coords are saved in the project as 'coords.mat', then use
% this to get the gray graph
coordsFile = fullfile(HOMEDIR, 'Gray', 'coords.mat');
if exist(coordsFile, 'file'),
    grayPath = coordsFile;
end

% Try to guess the mesh file path
[p f e] = fileparts(leftGrayPath);

% if the gray path is a nifti class file, then we use this and not a mesh
if strcmpi(e, '.gz')
    if strcmpi('hemi', 'left')
        meshPath = fullfile(leftGrayPath,'');
    else
        meshPath = fullfile(rightGrayPath,'');
    end
else
    % otherwise we look for a mesh
    subDirs = {'','3dMeshes','3DMeshes','3Dmeshes'};
    extensions = {'.MrM','.mrm','.Mrm','.MRm','.MRM', 'nii.gz'};
    meshPath = grayPathDir;
    for ii=1:length(subDirs)
        for jj=1:length(extensions)
            if(exist(fullfile(grayPathDir,subDirs{ii},[grayPathFileName,extensions{jj}]))==2)
                meshPath = fullfile(grayPathDir,subDirs{ii},[grayPathFileName,extensions{jj}]);
                break;
            end
        end
    end
end
ts = now;
saveFileName = ['unfold_',datestr(ts,'mm'),datestr(ts, 'dd'),datestr(ts,'yy')];
if(exist(fullfile(grayPathDir,'Unfolds'), 'dir'))
    savePath = fullfile(grayPathDir, 'Unfolds', saveFileName);
else
    savePath = fullfile(grayPathDir, saveFileName);
end

unfoldRadiusMM = 60;

disp('calling mrFlatMesh...');

mrFlatMesh({'grayPath', grayPath, ...
    'meshPath', meshPath, ...
    'savePath', savePath, ...
    'scaleXYZ', mmPerPixXYZ , ...
    'startXYZ', startXYZ, ...
    'unfoldRadiusMM', unfoldRadiusMM, ...
    'hemi', hemi});

return;


function [position0,position,messages]= delineateP3D(heasig,w1,w2,w3,timenew,position0,position,intervalo,samp,ultimo_anot,messages)
%
% [position0,position,A,V,A2,V2]=% delineateP3D(heasig,w1,w2,w3,intreg,scale,timenew,position0,position,intervalo,samp,figuresoff,sig)
%
% Input Parameters:
%   heasig: struct vector with header information
%   w1: matrix with WT scales 1 to 5 for lead 1
%   w2: matrix with WT scales 1 to 5 for lead 2
%   w3: matrix with WT scales 1 to 5 for lead 1
%  intreg: interval to be considered in fitting the optimal direction
%  scale to use in delineation
%   timenew:  QRS times in inedexes refering the interval included in the current excerpt (borders excluded)
%   position0: struct vector with the detected points for 3D multilead  step1
%   position: struct vector with the detected points for 3D multilead  step2
%   position1: struct vector with the detected points for lead1 - to be replaced for multilead marks
%   samp: samples included in the current excerpt (borders excluded)
%   intervalo: numeration of the beats processed in this segment
%
%Output Parameters:
%   actualized parameters: position, position0
% A - points of fitted lines in step 1
% A2 - points of fitted lines in step 2
%  V- vectors director to the fitted line
%  V2- vectors director to the fitted line
%
% Rute Almeida: trabalho de doutoramento
% Last update: 07FEB2012 Rute Almeida
%
% MATLAB Version 6.5.0.180913a (R13)

if nargin<11
    messages = new_empty_mesg;
elseif nargin<10
    messages.errors=[messages.errors {'Fatal error in wavedet_3D\delineate3D: not enough inputs.'}];
    warning(char(messages.errors(end)))
    messages.errors_desc=[messages.errors_desc 'Mandatory inputs not defined.'];
    messages.status=0;
    return
end
if ~isfield(messages,'setup'), messages.setup.wavedet=[]; end
if ~isfield(messages,'errors'), messages.errors=[]; end
if ~isfield(messages,'errors_desc'), messages.errors_desc=[]; end
if ~isfield(messages,'warnings'), messages.warnings=[]; end
if isfield(messages,'status')
    if messages.status~=1
        messages.warnings=[messages.warnings {['Initial status=' num2str(messages.status) '. status changed to 1']}];
    end
end
messages.status=1;

global recursioncount OPT

%Threshols for onset and offset detection
%%%%%% Constants and Thresholds  !!!!!!!!!!!!!!!!!!!!!!!!!
if ~isfield(messages.setup.wavedet,'inivent_tol_P')
    messages.setup.wavedet.inivent_tol_P  =  0.34;
end
if ~isfield(messages.setup.wavedet,'finvent_tol_P')
    messages.setup.wavedet.finvent_tol_P  =  0.1;
end
if ~isfield(messages.setup.wavedet,'P_CSE_tol')
    messages.setup.wavedet.P_CSE_tol=10.2*2/1000; % based on CSE tolerance
end
if ~isfield(messages.setup.wavedet,'Pconvergence_crit')
    messages.setup.wavedet.Pconvergence_crit=0; % 1 maks are acepted as the same if they difer by Tconvergence_crit
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

A2=[];
if ~isempty(w3)
    V=NaN*ones(size(timenew,2),3);
    A=NaN*ones(size(timenew,2),3);
else
    V=NaN*ones(size(timenew,2),2);
    A=NaN*ones(size(timenew,2),2);
end
V2=[];
intervalo=intervalo(1):intervalo(end);
for i=1:size(timenew,2)

    if ~isnan(position.QRSon(i+intervalo(1)-1))
        qrson = position.QRSon(i+intervalo(1)-1)-samp(1)+1;
    else
        qrson = timenew(i) - messages.setup.wavedet.finvent_tol_P*messages.setup.wavedet.freq;
    end

    ultimo_anot = ultimo_anot - samp(1)+1;
    % last anotation of the previous segment can overlap with the present one
    if (i==1 && ultimo_anot>=1),   % Protection in case it is the first beat
        begwin = max([1 qrson-round(messages.setup.wavedet.inivent_tol_P*messages.setup.wavedet.freq)+1 ultimo_anot+1]);
    else
        begwin = max(1, qrson-round(messages.setup.wavedet.inivent_tol_P*messages.setup.wavedet.freq)+1);
    end
    endwin = qrson- round(messages.setup.wavedet.finvent_tol_P*messages.setup.wavedet.freq)+1;

    if begwin>=endwin % empty search window: ultimo_anot should be too close to qrson
        position.Ptipoon(intervalo(i))=NaN;
        position.Ppicon(intervalo(i))=NaN;
        position.PscalePon(intervalo(i))=NaN;
        position.Pon(intervalo(i))=NaN;
        recursioncount.Pon(intervalo(i))=NaN;
        position.contadorPon(intervalo(i))=NaN;
        position.PscalePoff(intervalo(i))=NaN;
        position.Ppicoff(intervalo(i))=NaN;
        position.Poff(intervalo(i))=NaN;
        position.Ptipooff(intervalo(i))=NaN;
        recursioncount.Poff(intervalo(i))=NaN;
        position.contadorPoff(intervalo(i))=NaN; %Rute
        position.P(intervalo(i))=NaN;
    else

        scale=4;
        if ~isempty(w3)
            pontos=[w1(begwin:min(endwin,length(w1)),scale) w2(begwin:min(endwin,length(w2)),scale) w3(begwin:min(endwin,length(w3)),scale)];
        else
            pontos=[w1(begwin:min(endwin,length(w1)),scale) w2(begwin:min(endwin,length(w2)),scale)];
        end
        weight=ones(size(pontos,1),1);
        [a,v] = optimline(pontos, weight,OPT);
        A(i,:)=a;
        V(i,:)=v;
        %WT projected: from the previous QRS complex to the following QRS complex
        newleadbeat=[];
        if ~isempty(w3)
            if i>1,
                interval=timenew(i-1):timenew(i);
            else                        % if first beat of the segment
                interval=1:timenew(i);
            end

            newleadbeat(interval,3)=(w1(interval,3).*v(1)+w2(interval,3)*v(2)+w3(interval,3)*v(3))./norm(v); %#ok<AGROW>
            newleadbeat(interval,4)=(w1(interval,4).*v(1)+w2(interval,4)*v(2)+w3(interval,4)*v(3))./norm(v); %#ok<AGROW>
            newleadbeat(interval,5)=(w1(interval,5).*v(1)+w2(interval,5)*v(2)+w3(interval,5)*v(3))./norm(v); %#ok<AGROW>
            %signew(interval)=(sig(interval,1).*v(1)+sig(interval,2)*v(2)+sig(interval,3)*v(3))./norm(v);
        else
            if i>1,
                interval=timenew(i-1):timenew(i);
            else                        % if first beat of the segment
                interval=1:timenew(i);
            end
            % newleadbeat=NaN*ones(length(interval),5);
            newleadbeat(:,3)=(w1(interval,3).*v(1)+w2(timenew(i):timenew(i+1),3)*v(2))./norm(v);
            newleadbeat(:,4)=(w1(interval,4).*v(1)+w2(timenew(i):timenew(i+1),4)*v(2))./norm(v);
            newleadbeat(:,5)=(w1(interval,5).*v(1)+w2(timenew(i):timenew(i+1),5)*v(2))./norm(v);
            %signew=(sig(timenew(i):timenew(i+1),1).*v(1)+sig(timenew(i):timenew(i+1),2)*v(2))./norm(v);
        end
        if i>1,
            pos.qrs=position.qrs([i-1 i]);
            pos.QRSon=position.QRSon([i-1 i]);
            pos.QRSoff=position.QRSoff([i-1 i]);
            pos.Toff=position.Toff([i-1 i]);
            [pos,picon,picoff]= pwavef(heasig,samp,timenew([i i]),position,newleadbeat,[intervalo(i) intervalo(i)],ultimo_anot,messages);
        else
            pos.qrs=position.qrs(i);
            pos.QRSon=position.QRSon(i);
            pos.QRSoff=position.QRSoff(i);
            pos.Toff=position.Toff(i);
            [pos,picon,picoff]= pwavef(heasig,samp,timenew(i),position,newleadbeat,[intervalo(i) intervalo(i)] ,ultimo_anot,messages);
        end

        position0.Pscale(intervalo(i))=pos.Pscale(intervalo(i));
        position0.Pon(intervalo(i))=pos.Pon(intervalo(i));
        position0.Poff(intervalo(i))=pos.Poff(intervalo(i));
        position0.Ptipo(intervalo(i))=pos.Ptipo(intervalo(i));
        position0.direccionPonopt(:,intervalo(i))=NaN*ones(size(intervalo(i)));
        %position0.P(intervalo(i))=pos.P(intervalo(i));
        %position0.Pprima(intervalo(i))=pos.Pprima(intervalo(i));

        %%%%% P wave onset
        if ~isnan(picon)

            begaux=max([(position0.Pon(intervalo(i))-samp(1)+1-round(messages.setup.wavedet.P_CSE_tol*messages.setup.wavedet.freq)) 1]);
            if ~isempty(w3)
                pontos2on=[w1(begaux:picon,scale) w2(begaux:picon,scale) w3(begaux:picon,scale)];
            else
                pontos2on=[w1(begaux:picon,scale) w2(begaux:picon,scale)];
            end
            if size(pontos2on,1)<=1 % unable to do another step: empty search window
                position.contadorPon(intervalo(i))=-1;
                position.PscalePon(intervalo(i))=position0.Pscale(intervalo(i));
                position.Pon(intervalo(i))=position0.Pon(intervalo(i));
                position.Ppicon=picon+samp(1)-1;
                %position.P(intervalo(i))= position0.P(intervalo(i));
                %position.Pprima(intervalo(i))=position0.Pprima(intervalo(i));
                position.direccionPonopt(intervalo(i),:)=V(end,:)';
                position.Ptipoon(intervalo(i))=position0.Ptipo(intervalo(i));
            else
                weight2=ones(size(pontos2on,1),1);
                [a,v] = optimline(pontos2on, weight2,OPT);
                A2=[A2;a]; %#ok<AGROW>
                V2=[V2;v]; %#ok<AGROW>
                newleadbeat2on = [];
                if ~isempty(w3)
                    newleadbeat2on(interval,3)=(w1(interval,3).*v(1)+w2(interval,3)*v(2)+w3(interval,3)*v(3))./norm(v);%#ok<AGROW>
                    newleadbeat2on(interval,4)=(w1(interval,4).*v(1)+w2(interval,4)*v(2)+w3(interval,4)*v(3))./norm(v);%#ok<AGROW>
                    newleadbeat2on(interval,5)=(w1(interval,5).*v(1)+w2(interval,5)*v(2)+w3(interval,5)*v(3))./norm(v);%#ok<AGROW>
                    % signew2on(interval)=(sig(interval,1).*v(1)+sig(interval,2)*v(2)+sig(interval,3)*v(3))./norm(v);
                else
                    newleadbeat2on(interval,3)=(w1(interval,3).*v(1)+w2(timenew(i):timenew(i+1),3)*v(2))./norm(v);%#ok<AGROW>
                    newleadbeat2on(interval,4)=(w1(interval,4).*v(1)+w2(timenew(i):timenew(i+1),4)*v(2))./norm(v);%#ok<AGROW>
                    newleadbeat2on(interval,5)=(w1(interval,5).*v(1)+w2(timenew(i):timenew(i+1),5)*v(2))./norm(v);%#ok<AGROW>
                    %signew2on(interval)=(sig(timenew(i):timenew(i+1),1).*v(1)+sig(timenew(i):timenew(i+1),2)*v(2))./norm(v);
                end

                if i>1,
                    [pos2,picon2,picoff2]= pwavef(heasig,samp,timenew([i  i]),pos,newleadbeat2on,[intervalo(i) intervalo(i)],ultimo_anot,messages); %#ok<NASGU>
                else
                    [pos2,picon2,picoff2]= pwavef(heasig,samp,timenew(i),pos,newleadbeat2on,[intervalo(i) intervalo(i)] ,ultimo_anot,messages); %#ok<NASGU>
                end
                piconall=[picon picon2];
                if isnan(picon2)
                    amppiconall=[abs(newleadbeat(picon,scale)) NaN];
                else
                    amppiconall=[abs(newleadbeat(picon,scale)) abs(newleadbeat2on(picon2,scale))];
                end
                if (amppiconall(end)<amppiconall(end-1)) || isnan(amppiconall(end))% second step worse than first step
                    position.contadorPon(intervalo(i))=-1;
                    position.Ppicon(intervalo(i))=piconall(end-1)+samp(1)-1;
                    position.PscalePon(intervalo(i))=position0.Pscale(intervalo(i));
                    position.Pon(intervalo(i))=position0.Pon(intervalo(i));
                    position.direccionPonopt(:,intervalo(i))=V(end,:);
                    position.Ptipoon(intervalo(i))=position0.Ptipo(intervalo(i));
                elseif pos2.Pon(intervalo(i))==position0.Pon(intervalo(i)) % first and second steps give same mark
                    position.contadorPon(intervalo(i))=0;
                    position.Ppicon(intervalo(i))=piconall(end)+samp(1)-1;
                    position.PscalePon(intervalo(i))=pos2.Pscale(intervalo(i));
                    position.Pon(intervalo(i))=pos2.Pon(intervalo(i));
                    position.direccionPonopt(:,intervalo(i))=V2(end,:);
                    position.Ptipoon(intervalo(i))=pos2.Ptipo(intervalo(i));
                else % more steps
                    newPon=[position0.Pon(intervalo(i)) pos2.Pon(intervalo(i))];
                    position.contadorPon(intervalo(i))=0;
                    posnewon.Pscale(intervalo(i))=NaN;
                    posnewon.Pon(intervalo(i))=NaN;
                    posnewon.Ptipo(intervalo(i))=NaN;
                    position.Ppicon(intervalo(i))=piconall(end)+samp(1)-1;
                    while (~isnan(piconall(end)) &&  abs(newPon(end)-newPon(end-1))>messages.setup.wavedet.Pconvergence_crit) && (amppiconall(end)>amppiconall(end-1))
                        position.contadorPon(intervalo(i))=position.contadorPon(intervalo(i))+1;
                        begaux=max([(position0.Pon(intervalo(i))-samp(1)+1-round(messages.setup.wavedet.P_CSE_tol*messages.setup.wavedet.freq)) 1]);
                        if ~isempty(w3)
                            pontosnewon=[w1( begaux:piconall(end),scale) w2( begaux:piconall(end),scale) w3( begaux:piconall(end),scale)];
                        else
                            pontosnewon=[w1(begaux:piconall(end),scale) w2( begaux:piconall(end),scale)];
                        end
                        if size(pontosnewon,1)<=1 % unable to do another step: empty search window
                            posnewon.Pon(intervalo(i))=newPon(end-1); % previous step
                            posnewon.Pscale(intervalo(i))=pos2.Pscale(intervalo(i));
                            posnewon.Ptipo(intervalo(i))=pos2.Ptipo(intervalo(i));
                            position.contadorPon(intervalo(i))=position.contadorPon(intervalo(i))-1;
                            piconall=[piconall NaN]; %#ok<AGROW>
                            amppiconall=[amppiconall NaN]; %#ok<AGROW>
                        else
                            weight2=ones(size(pontosnewon,1),1);
                            [a,v] = optimline(pontosnewon, weight2,OPT);
                            A2=[A2;a]; %#ok<AGROW>
                            V2=[V2;v]; %#ok<AGROW>
                            newleadbeatnewon = [];
                            if ~isempty(w3)
                                % newleadbeatnewon=NaN*ones(interval(end),5);
                                newleadbeatnewon(interval,3)=(w1(interval,3).*v(1)+w2(interval,3)*v(2)+w3(interval,3)*v(3))./norm(v);%#ok<AGROW>
                                newleadbeatnewon(interval,4)=(w1(interval,4).*v(1)+w2(interval,4)*v(2)+w3(interval,4)*v(3))./norm(v);%#ok<AGROW>
                                newleadbeatnewon(interval,5)=(w1(interval,5).*v(1)+w2(interval,5)*v(2)+w3(interval,5)*v(3))./norm(v);%#ok<AGROW>
                                %signewon(interval)=(sig(interval,1).*v(1)+sig(interval,2)*v(2)+sig(interval,3)*v(3))./norm(v);
                            else
                                % newleadbeatnewon=NaN*ones(interval(end),5);
                                newleadbeatnewon(interval,3)=(w1(interval,3).*v(1)+w2(timenew(i):timenew(i+1),3)*v(2))./norm(v);%#ok<AGROW>
                                newleadbeatnewon(interval,4)=(w1(interval,4).*v(1)+w2(timenew(i):timenew(i+1),4)*v(2))./norm(v);%#ok<AGROW>
                                newleadbeatnewon(interval,5)=(w1(interval,5).*v(1)+w2(timenew(i):timenew(i+1),5)*v(2))./norm(v);%#ok<AGROW>
                                %signewon(interval)=(sig(timenew(i):timenew(i+1),1).*v(1)+sig(timenew(i):timenew(i+1),2)*v(2))./norm(v);
                            end
                            if i>1,
                                [posnewon,piconnew,picoffnew]= pwavef(heasig,samp,timenew([i  i]),pos,newleadbeatnewon,[intervalo(i) intervalo(i)],ultimo_anot,messages); %#ok<NASGU>
                            else
                                [posnewon,piconnew,picoffnew]= pwavef(heasig,samp,timenew(i),pos,newleadbeatnewon,[intervalo(i) intervalo(i)] ,ultimo_anot,messages); %#ok<NASGU>
                            end
                            newPon=[newPon posnewon.Pon(intervalo(i))]; %#ok<AGROW>
                            if isnan(piconnew)
                                amppiconall=[amppiconall NaN]; %#ok<AGROW>
                            else
                                amppiconall=[amppiconall abs(newleadbeatnewon(piconnew,scale))]; %#ok<AGROW>
                            end
                            piconall=[piconall piconnew]; %#ok<AGROW>

                            if isnan(posnewon.Pon(intervalo(i))) || (amppiconall(end)<amppiconall(end-1)) % lead got worse
                                posnewon.Pon(intervalo(i))=newPon(end-1); % previous step
                                position.Ppicon(intervalo(i))=piconall(end-1)+samp(1)-1;
                                posnewon.Pscale(intervalo(i))=pos2.Pscale(intervalo(i));
                                posnewon.Ptipo(intervalo(i))=pos2.Ptipo(intervalo(i));
                                position.contadorPon(intervalo(i))=position.contadorPon(intervalo(i))-1;
                            else
                                position.direccionPonopt(:,intervalo(i))=V2(end,:);
                                position.Ppicon(intervalo(i))=piconall(end)+samp(1)-1;
                            end
                            if  prod(piconall(1:end-1)-piconall(end))==0  % ciclo infinito no teste 14
                                piconall(end)=NaN;
                            end

                        end % if size(pontosnewon,1)>1
                    end %while
                    position.PscalePon(intervalo(i))=posnewon.Pscale(intervalo(i));
                    position.Pon(intervalo(i))=posnewon.Pon(intervalo(i));
                    position.Ptipoon(intervalo(i))=posnewon.Ptipo(intervalo(i));
                end % more steps
            end   %size(pontosnewon,1)>1
        else % isnan(picon)
            position.Ptipoon(intervalo(i))=NaN;
            position.Ppicon(intervalo(i))=NaN;
            position.PscalePon(intervalo(i))=NaN;
            position.Pon(intervalo(i))=NaN;
            recursioncount.Pon(intervalo(i))=NaN;
            position.contadorPon(intervalo(i))=NaN; %Rute
        end
        %%%%%% P wave onset
        %%%%% P wave end
        if ~isnan(picoff)

            endaux=min([(position0.Poff(intervalo(i))-samp(1)+1+round(messages.setup.wavedet.P_CSE_tol*messages.setup.wavedet.freq)) length(w1)]);
            if ~isempty(w3)
                pontos2off=[w1(picoff:endaux,scale) w2(picoff:endaux,scale) w3(picoff:endaux,scale)];
            else
                pontos2off=[w1(picoff:endaux,scale) w2(picoff:endaux,scale)];
            end
            if size(pontos2off,1)<=1 % unable to do another step: empty search window
                position.contadorPoff(intervalo(i))=-1;
                position.PscalePoff(intervalo(i))=position0.Pscale(intervalo(i));
                position.Poff(intervalo(i))=position0.Poff(intervalo(i));
                position.Ppicoff=picoff+samp(1)-1;
                %position.P(intervalo(i))= position0.P(intervalo(i));
                %position.Pprima(intervalo(i))=position0.Pprima(intervalo(i));
                position.direccionPoffopt(:,intervalo(i))=V(end,:);
                position.Ptipooff(intervalo(i))=position0.Ptipo(intervalo(i));
            else
                weight2=ones(size(pontos2off,1),1);
                [a,v] = optimline(pontos2off, weight2,OPT);
                A2=[A2;a]; %#ok<AGROW>
                V2=[V2;v]; %#ok<AGROW>
                newleadbeat2off = [];
                if ~isempty(w3)
                    %   newleadbeat2off=NaN*ones(interval(end),5);
                    newleadbeat2off(interval,3)=(w1(interval,3).*v(1)+w2(interval,3)*v(2)+w3(interval,3)*v(3))./norm(v);%#ok<AGROW>
                    newleadbeat2off(interval,4)=(w1(interval,4).*v(1)+w2(interval,4)*v(2)+w3(interval,4)*v(3))./norm(v);%#ok<AGROW>
                    newleadbeat2off(interval,5)=(w1(interval,5).*v(1)+w2(interval,5)*v(2)+w3(interval,5)*v(3))./norm(v);%#ok<AGROW>
                    %signew2off(interval)=(sig(interval,1).*v(1)+sig(interval,2)*v(2)+sig(interval,3)*v(3))./norm(v);
                else
                    % newleadbeat2off=NaN*ones(interval(end),5);
                    newleadbeat2off(interval,3)=(w1(interval,3).*v(1)+w2(timenew(i):timenew(i+1),3)*v(2))./norm(v);%#ok<AGROW>
                    newleadbeat2off(interval,4)=(w1(interval,4).*v(1)+w2(timenew(i):timenew(i+1),4)*v(2))./norm(v);%#ok<AGROW>
                    newleadbeat2off(interval,5)=(w1(interval,5).*v(1)+w2(timenew(i):timenew(i+1),5)*v(2))./norm(v);%#ok<AGROW>
                    %signew2off(interval)=(sig(timenew(i):timenew(i+1),1).*v(1)+sig(timenew(i):timenew(i+1),2)*v(2))./norm(v);
                end

                if i>1,
                    [pos2off,picon2off,picoff2]= pwavef(heasig,samp,timenew([i  i]),pos,newleadbeat2off,[intervalo(i) intervalo(i)],ultimo_anot,messages); %#ok<ASGLU>
                else
                    [pos2off,picon2off,picoff2]= pwavef(heasig,samp,timenew(i),pos,newleadbeat2off,[intervalo(i) intervalo(i)] ,ultimo_anot,messages); %#ok<ASGLU>
                end

                picoffall=[picoff picoff2];
                if isnan(picoff2)
                    amppicoffall=[abs(newleadbeat(picoff,scale)) NaN];
                else
                    amppicoffall=[abs(newleadbeat(picoff,scale)) abs(newleadbeat2off(picoff2,scale))];
                end
                if (amppicoffall(end)<amppicoffall(end-1))  || isnan(amppicoffall(end)) % second step worse than first step
                    position.contadorPoff(intervalo(i))=-1;
                    position.Ppicoff(intervalo(i))=picoffall(end-1)+samp(1)-1;
                    position.PscalePoff(intervalo(i))=position0.Pscale(intervalo(i));
                    position.Poff(intervalo(i))=position0.Poff(intervalo(i));
                    %position.P(intervalo(i))= position0.P(intervalo(i));
                    %position.Pprima(intervalo(i))=position0.Pprima(intervalo(i));
                    position.direccionPoffopt(:,intervalo(i))=V(end,:);
                    position.Ptipooff(intervalo(i))=position0.Ptipo(intervalo(i));
                elseif pos2off.Poff(intervalo(i))==position0.Poff(intervalo(i)) % first and second steps give same mark
                    position.contadorPoff(intervalo(i))=0;
                    position.Ppicoff(intervalo(i))=picoffall(end)+samp(1)-1;
                    position.PscalePoff(intervalo(i))=pos2off.Pscale(intervalo(i));
                    position.Poff(intervalo(i))=pos2off.Poff(intervalo(i));
                    %position.P(intervalo(i))= pos2off.P(intervalo(i));
                    %position.Pprima(intervalo(i))=pos2off.Pprima(intervalo(i));
                    position.direccionPoffopt(:,intervalo(i))=V2(end,:);
                    position.Ptipooff(intervalo(i))=pos2off.Ptipo(intervalo(i));
                else % more steps
                    newPoff=[position0.Poff(intervalo(i)) pos2off.Poff(intervalo(i))];
                    position.contadorPoff(intervalo(i))=0;
                    position.Ppicoff(intervalo(i))=picoffall(end)+samp(1)-1;
                    posnewoff.Pscale(intervalo(i))=NaN;
                    posnewoff.Poff(intervalo(i))=NaN;
                    posnewoff.Ptipo(intervalo(i))=NaN;
                    while (~isnan(picoffall(end)) &&  abs(newPoff(end)-newPoff(end-1))>messages.setup.wavedet.Pconvergence_crit ) && (amppicoffall(end)>amppicoffall(end-1))
                        position.contadorPoff(intervalo(i))=position.contadorPoff(intervalo(i))+1;
                        endaux=min([(position0.Poff(intervalo(i))-samp(1)+1+round(messages.setup.wavedet.P_CSE_tol*messages.setup.wavedet.freq)) length(w1)]);
                        if ~isempty(w3)
                            pontosnewoff=[w1(picoffall(end):endaux,scale) w2(picoffall(end):endaux,scale) w3(picoffall(end):endaux,scale)];
                        else
                            pontosnewoff=[w1(picoffall(end):endaux,scale) w2(picoffall(end):endaux,scale)];
                        end
                        if size(pontosnewoff,1)<=1 % unable to do another step: empty search window
                            posnewoff.Poff(intervalo(i))=newPoff(end-1); % previous step
                            posnewoff.Pscale(intervalo(i))=pos2off.Pscale(intervalo(i));
                            posnewoff.Ptipo(intervalo(i))=pos2off.Ptipo(intervalo(i));
                            %posnew.P=pos2off.P;
                            %posnew.Pprima=pos2off.Pprima;
                            position.contadorPoff(intervalo(i))=position.contadorPoff(intervalo(i))-1;
                            picoffall=[picoffall NaN]; %#ok<AGROW>
                            amppicoffall=[amppicoffall NaN]; %#ok<AGROW>
                        else
                            weight2=ones(size(pontosnewoff,1),1);
                            [a,v] = optimline(pontosnewoff, weight2,OPT);
                            A2=[A2;a]; %#ok<AGROW>
                            V2=[V2;v]; %#ok<AGROW>
                            newleadbeatnewoff = [];
                            if ~isempty(w3)
                                %newleadbeatnewoff=NaN*ones(interval(end),5);
                                newleadbeatnewoff(interval,3)=(w1(interval,3).*v(1)+w2(interval,3)*v(2)+w3(interval,3)*v(3))./norm(v);%#ok<AGROW>
                                newleadbeatnewoff(interval,4)=(w1(interval,4).*v(1)+w2(interval,4)*v(2)+w3(interval,4)*v(3))./norm(v);%#ok<AGROW>
                                newleadbeatnewoff(interval,5)=(w1(interval,5).*v(1)+w2(interval,5)*v(2)+w3(interval,5)*v(3))./norm(v);%#ok<AGROW>
                                %signewoff(interval)=(sig(interval,1).*v(1)+sig(interval,2)*v(2)+sig(interval,3)*v(3))./norm(v);
                            else
                                %newleadbeatnewoff=NaN*ones(interval(end),5);
                                newleadbeatnewoff(interval,3)=(w1(interval,3).*v(1)+w2(timenew(i):timenew(i+1),3)*v(2))./norm(v);%#ok<AGROW>
                                newleadbeatnewoff(interval,4)=(w1(interval,4).*v(1)+w2(timenew(i):timenew(i+1),4)*v(2))./norm(v);%#ok<AGROW>
                                newleadbeatnewoff(interval,5)=(w1(interval,5).*v(1)+w2(timenew(i):timenew(i+1),5)*v(2))./norm(v);%#ok<AGROW>
                                %signewoff(interval)=(sig(timenew(i):timenew(i+1),1).*v(1)+sig(timenew(i):timenew(i+1),2)*v(2))./norm(v);
                            end
                            if i>1,
                                [posnewoff,piconnew,picoffnew]= pwavef(heasig,samp,timenew([i  i]),pos,newleadbeatnewoff,[intervalo(i) intervalo(i)],ultimo_anot,messages); %#ok<ASGLU>
                            else
                                [posnewoff,piconnew,picoffnew]= pwavef(heasig,samp,timenew(i),pos,newleadbeatnewoff,[intervalo(i) intervalo(i)] ,ultimo_anot,messages); %#ok<ASGLU>
                            end
                            newPoff=[newPoff posnewoff.Poff(intervalo(i))]; %#ok<AGROW>
                            if isnan(picoffnew)
                                amppicoffall=[amppicoffall NaN]; %#ok<AGROW>
                            else
                                amppicoffall=[amppicoffall abs(newleadbeatnewoff(picoffnew,scale))]; %#ok<AGROW>
                            end
                            picoffall=[picoffall picoffnew]; %#ok<AGROW>

                            if isnan(posnewoff.Poff(intervalo(i))) || (amppicoffall(end)<amppicoffall(end-1)) % lead got worse
                                position.Ppicoff(intervalo(i))=picoffall(end-1)+samp(1)-1;
                                posnewoff.Poff(intervalo(i))=newPoff(end-1); % previous step
                                posnewoff.Pscale(intervalo(i))=pos2off.Pscale(intervalo(i));
                                posnewoff.Ptipo(intervalo(i))=pos2off.Ptipo(intervalo(i));
                                %posnewoff.P=pos2.P;
                                %posnewoff.Pprima=pos2.Pprima;
                                position.contadorPoff(intervalo(i))=position.contadorPoff(intervalo(i))-1;
                            else
                                position.direccionPoffopt(:,intervalo(i))=V2(end,:);
                                position.Ppicoff(intervalo(i))=picoffall(end)+samp(1)-1;
                            end

                            if  prod(picoffall(1:end-1)-picoffall(end))==0
                                picoffall(end)=NaN;
                            end
                        end % if size(pontosnewoff,1)>1
                    end %while
                    position.PscalePoff(intervalo(i))=posnewoff.Pscale(intervalo(i));
                    position.Poff(intervalo(i))=posnewoff.Poff(intervalo(i));
                    position.Ptipooff(intervalo(i))=posnewoff.Ptipo(intervalo(i));
                    % position.P(intervalo(i))= posnewoff.P(intervalo(i));
                    % position.Pprima(intervalo(i))=posnewoff.Pprima(intervalo(i));
                end % more steps
            end   %size(pontosnewoff,1)>1
        else % isnan(picoff)
            position.PscalePoff(intervalo(i))=NaN;
            position.Ppicoff(intervalo(i))=NaN;
            position.Poff(intervalo(i))=NaN;
            position.Ptipooff(intervalo(i))=NaN;
            %position.P(intervalo(i))=NaN;
            %position.Pprima(intervalo(i))=NaN;
            recursioncount.Poff(intervalo(i))=NaN;
            position.contadorPoff(intervalo(i))=NaN; %Rute
        end
        %%%%%% P wave end

        %P peak
        if ~isnan(position.Ppicon(intervalo(i))) && ~isnan(position.Ppicoff(intervalo(i))) && position.Ppicon(intervalo(i))<position.Ppicoff(intervalo(i))
            pontosP=[w1((position.Ppicon(intervalo(i))-samp(1)+1):(position.Ppicoff(intervalo(i))-samp(1)+1),scale) w2((position.Ppicon(intervalo(i))-samp(1)+1):(position.Ppicoff(intervalo(i))-samp(1)+1),scale) w3((position.Ppicon(intervalo(i))-samp(1)+1):(position.Ppicoff(intervalo(i))-samp(1)+1),scale)];
            [m1,m2]=min((pontosP(:,1)).^2+(pontosP(:,2)).^2+(pontosP(:,3)).^2); %#ok<ASGLU> % main P peak
            position.P(intervalo(i))=m2+position.Ppicon(intervalo(i));
        else
            position.P(intervalo(i))=position0.P(intervalo(i));
        end
        %%
        %writennot protection: at least one sample between peak and border
        if (position.P(intervalo(i))-position.Pon(intervalo(i)))<1
            position.P(intervalo(i))=NaN;
        end
        if (position.Poff(intervalo(i))-position.P(intervalo(i)))<1
            position.P(intervalo(i))=NaN;
        end
        clear pos pos2 pontos pontos2on pontos2off posnew pos2off posnewoff
    end
end % for