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


function obj = Jacob_pTop_func_new(in1,in2,in3,in4,in5)
coef_f0_q_sym1 = in1(:,1);
coef_f0_q_sym2 = in1(:,2);
coef_f0_q_sym3 = in1(:,3);
coef_f0_q_sym4 = in1(:,4);
coef_f0_q_sym5 = in1(:,5);
coef_f0_q_sym6 = in1(:,6);
coef_f0_q_sym7 = in1(:,7);
coef_f0_q_sym8 = in1(:,8);
coef_f0_q_sym9 = in1(:,9);
coef_f0_q_sym10 = in1(:,10);
coef_f0_q_sym11 = in1(:,11);
coef_f1_q_sym1 = in2(:,1);
coef_f0_q_sym12 = in1(:,12);
coef_f1_q_sym2 = in2(:,2);
coef_f0_q_sym13 = in1(:,13);
coef_f1_q_sym3 = in2(:,3);
coef_f0_q_sym14 = in1(:,14);
coef_f1_q_sym4 = in2(:,4);
coef_f0_q_sym15 = in1(:,15);
coef_f1_q_sym5 = in2(:,5);
coef_f0_q_sym16 = in1(:,16);
coef_f1_q_sym6 = in2(:,6);
coef_f0_q_sym17 = in1(:,17);
coef_f1_q_sym7 = in2(:,7);
coef_f0_q_sym18 = in1(:,18);
coef_f1_q_sym8 = in2(:,8);
coef_f0_q_sym19 = in1(:,19);
coef_f1_q_sym9 = in2(:,9);
coef_f0_q_sym20 = in1(:,20);
coef_f0_q_sym21 = in1(:,21);
coef_f2_q_sym1 = in3(:,1);
coef_f0_q_sym22 = in1(:,22);
coef_f2_q_sym2 = in3(:,2);
coef_f0_q_sym23 = in1(:,23);
coef_f2_q_sym3 = in3(:,3);
coef_f0_q_sym24 = in1(:,24);
coef_f2_q_sym4 = in3(:,4);
coef_f2_q_sym5 = in3(:,5);
coef_f2_q_sym6 = in3(:,6);
coef_f2_q_sym7 = in3(:,7);
coef_f2_q_sym8 = in3(:,8);
coef_f2_q_sym9 = in3(:,9);
coef_f3_q_sym1 = in4(:,1);
coef_f3_q_sym2 = in4(:,2);
coef_f3_q_sym3 = in4(:,3);
coef_f3_q_sym4 = in4(:,4);
coef_f3_q_sym5 = in4(:,5);
coef_f3_q_sym6 = in4(:,6);
coef_f3_q_sym7 = in4(:,7);
coef_f3_q_sym8 = in4(:,8);
coef_f3_q_sym9 = in4(:,9);
coef_f1_q_sym10 = in2(:,10);
coef_f1_q_sym11 = in2(:,11);
coef_f1_q_sym12 = in2(:,12);
coef_f1_q_sym13 = in2(:,13);
coef_f1_q_sym14 = in2(:,14);
coef_f1_q_sym15 = in2(:,15);
coef_f1_q_sym16 = in2(:,16);
coef_f1_q_sym17 = in2(:,17);
coef_f1_q_sym18 = in2(:,18);
coef_f1_q_sym19 = in2(:,19);
coef_f1_q_sym20 = in2(:,20);
coef_f1_q_sym21 = in2(:,21);
coef_f1_q_sym22 = in2(:,22);
coef_f1_q_sym23 = in2(:,23);
coef_f1_q_sym24 = in2(:,24);
coef_f2_q_sym10 = in3(:,10);
coef_f2_q_sym11 = in3(:,11);
coef_f2_q_sym12 = in3(:,12);
coef_f2_q_sym13 = in3(:,13);
coef_f2_q_sym14 = in3(:,14);
coef_f2_q_sym15 = in3(:,15);
coef_f2_q_sym16 = in3(:,16);
coef_f2_q_sym17 = in3(:,17);
coef_f2_q_sym18 = in3(:,18);
coef_f2_q_sym19 = in3(:,19);
coef_f2_q_sym20 = in3(:,20);
coef_f2_q_sym21 = in3(:,21);
coef_f2_q_sym22 = in3(:,22);
coef_f2_q_sym23 = in3(:,23);
coef_f2_q_sym24 = in3(:,24);
coef_f3_q_sym10 = in4(:,10);
coef_f3_q_sym11 = in4(:,11);
coef_f3_q_sym12 = in4(:,12);
coef_f3_q_sym13 = in4(:,13);
coef_f3_q_sym14 = in4(:,14);
coef_f3_q_sym15 = in4(:,15);
coef_f3_q_sym16 = in4(:,16);
coef_f3_q_sym17 = in4(:,17);
coef_f3_q_sym18 = in4(:,18);
coef_f3_q_sym19 = in4(:,19);
coef_f3_q_sym20 = in4(:,20);
coef_f3_q_sym21 = in4(:,21);
coef_f3_q_sym22 = in4(:,22);
coef_f3_q_sym23 = in4(:,23);
coef_f3_q_sym24 = in4(:,24);
q0 = in5(1,:);
q1 = in5(2,:);
q2 = in5(3,:);
q3 = in5(4,:);
t2 = coef_f0_q_sym11.*q0;
t3 = coef_f0_q_sym18.*q1;
t4 = coef_f0_q_sym22.*q2;
t5 = coef_f0_q_sym24.*q3;
t6 = q0.^2;
t7 = q0.^3;
t8 = q1.^2;
t9 = q1.^3;
t10 = q2.^2;
t11 = q2.^3;
t12 = q3.^2;
t13 = q3.^3;
t24 = coef_f0_q_sym6.*q0.*q1.*q2.*2.0;
t25 = coef_f0_q_sym7.*q0.*q1.*q3.*2.0;
t26 = coef_f0_q_sym9.*q0.*q2.*q3.*2.0;
t27 = coef_f0_q_sym16.*q1.*q2.*q3.*2.0;
t14 = coef_f0_q_sym1.*t7;
t15 = coef_f0_q_sym12.*t9;
t16 = coef_f0_q_sym19.*t11;
t17 = coef_f0_q_sym23.*t13;
t18 = coef_f0_q_sym2.*q1.*t6;
t19 = coef_f0_q_sym3.*q2.*t6;
t20 = coef_f0_q_sym5.*q0.*t8;
t21 = coef_f0_q_sym4.*q3.*t6;
t22 = coef_f0_q_sym8.*q0.*t10;
t23 = coef_f0_q_sym10.*q0.*t12;
obj = reshape([coef_f0_q_sym11.*q1-coef_f1_q_sym11.*q0.*2.0-coef_f1_q_sym18.*q1-coef_f1_q_sym22.*q2-coef_f1_q_sym24.*q3+coef_f0_q_sym5.*t9-coef_f1_q_sym1.*t7.*4.0-coef_f1_q_sym12.*t9-coef_f1_q_sym19.*t11-coef_f1_q_sym23.*t13+coef_f0_q_sym1.*q1.*t6.*3.0+coef_f0_q_sym2.*q0.*t8.*2.0+coef_f0_q_sym6.*q2.*t8+coef_f0_q_sym7.*q3.*t8+coef_f0_q_sym8.*q1.*t10-coef_f1_q_sym2.*q1.*t6.*3.0-coef_f1_q_sym3.*q2.*t6.*3.0+coef_f0_q_sym10.*q1.*t12-coef_f1_q_sym4.*q3.*t6.*3.0-coef_f1_q_sym5.*q0.*t8.*2.0-coef_f1_q_sym8.*q0.*t10.*2.0-coef_f1_q_sym10.*q0.*t12.*2.0-coef_f1_q_sym13.*q2.*t8-coef_f1_q_sym14.*q3.*t8-coef_f1_q_sym15.*q1.*t10-coef_f1_q_sym17.*q1.*t12-coef_f1_q_sym20.*q3.*t10-coef_f1_q_sym21.*q2.*t12+coef_f0_q_sym3.*q0.*q1.*q2.*2.0+coef_f0_q_sym4.*q0.*q1.*q3.*2.0+coef_f0_q_sym9.*q1.*q2.*q3-coef_f1_q_sym6.*q0.*q1.*q2.*2.0-coef_f1_q_sym7.*q0.*q1.*q3.*2.0-coef_f1_q_sym9.*q0.*q2.*q3.*2.0-coef_f1_q_sym16.*q1.*q2.*q3,coef_f0_q_sym11.*q2-coef_f2_q_sym11.*q0.*2.0-coef_f2_q_sym18.*q1-coef_f2_q_sym22.*q2-coef_f2_q_sym24.*q3+coef_f0_q_sym8.*t11-coef_f2_q_sym1.*t7.*4.0-coef_f2_q_sym12.*t9-coef_f2_q_sym19.*t11-coef_f2_q_sym23.*t13+coef_f0_q_sym1.*q2.*t6.*3.0+coef_f0_q_sym3.*q0.*t10.*2.0+coef_f0_q_sym5.*q2.*t8+coef_f0_q_sym6.*q1.*t10+coef_f0_q_sym9.*q3.*t10+coef_f0_q_sym10.*q2.*t12-coef_f2_q_sym2.*q1.*t6.*3.0-coef_f2_q_sym3.*q2.*t6.*3.0-coef_f2_q_sym4.*q3.*t6.*3.0-coef_f2_q_sym5.*q0.*t8.*2.0-coef_f2_q_sym8.*q0.*t10.*2.0-coef_f2_q_sym10.*q0.*t12.*2.0-coef_f2_q_sym13.*q2.*t8-coef_f2_q_sym14.*q3.*t8-coef_f2_q_sym15.*q1.*t10-coef_f2_q_sym17.*q1.*t12-coef_f2_q_sym20.*q3.*t10-coef_f2_q_sym21.*q2.*t12+coef_f0_q_sym2.*q0.*q1.*q2.*2.0+coef_f0_q_sym4.*q0.*q2.*q3.*2.0+coef_f0_q_sym7.*q1.*q2.*q3-coef_f2_q_sym6.*q0.*q1.*q2.*2.0-coef_f2_q_sym7.*q0.*q1.*q3.*2.0-coef_f2_q_sym9.*q0.*q2.*q3.*2.0-coef_f2_q_sym16.*q1.*q2.*q3,coef_f0_q_sym11.*q3-coef_f3_q_sym11.*q0.*2.0-coef_f3_q_sym18.*q1-coef_f3_q_sym22.*q2-coef_f3_q_sym24.*q3+coef_f0_q_sym10.*t13-coef_f3_q_sym1.*t7.*4.0-coef_f3_q_sym12.*t9-coef_f3_q_sym19.*t11-coef_f3_q_sym23.*t13+coef_f0_q_sym1.*q3.*t6.*3.0+coef_f0_q_sym4.*q0.*t12.*2.0+coef_f0_q_sym5.*q3.*t8+coef_f0_q_sym7.*q1.*t12+coef_f0_q_sym8.*q3.*t10+coef_f0_q_sym9.*q2.*t12-coef_f3_q_sym2.*q1.*t6.*3.0-coef_f3_q_sym3.*q2.*t6.*3.0-coef_f3_q_sym4.*q3.*t6.*3.0-coef_f3_q_sym5.*q0.*t8.*2.0-coef_f3_q_sym8.*q0.*t10.*2.0-coef_f3_q_sym10.*q0.*t12.*2.0-coef_f3_q_sym13.*q2.*t8-coef_f3_q_sym14.*q3.*t8-coef_f3_q_sym15.*q1.*t10-coef_f3_q_sym17.*q1.*t12-coef_f3_q_sym20.*q3.*t10-coef_f3_q_sym21.*q2.*t12+coef_f0_q_sym2.*q0.*q1.*q3.*2.0+coef_f0_q_sym3.*q0.*q2.*q3.*2.0+coef_f0_q_sym6.*q1.*q2.*q3-coef_f3_q_sym6.*q0.*q1.*q2.*2.0-coef_f3_q_sym7.*q0.*q1.*q3.*2.0-coef_f3_q_sym9.*q0.*q2.*q3.*2.0-coef_f3_q_sym16.*q1.*q2.*q3,q0.*2.0,t2+t3.*2.0+t4+t5+t14+t15.*4.0+t16+t17+t18.*2.0+t19+t20.*3.0+t21+t22+t23+t24+t25+t27-coef_f1_q_sym18.*q0-coef_f1_q_sym2.*t7-coef_f1_q_sym5.*q1.*t6.*2.0+coef_f0_q_sym13.*q2.*t8.*3.0-coef_f1_q_sym6.*q2.*t6+coef_f0_q_sym14.*q3.*t8.*3.0+coef_f0_q_sym15.*q1.*t10.*2.0-coef_f1_q_sym7.*q3.*t6+coef_f0_q_sym17.*q1.*t12.*2.0+coef_f0_q_sym20.*q3.*t10+coef_f0_q_sym21.*q2.*t12-coef_f1_q_sym12.*q0.*t8.*3.0-coef_f1_q_sym15.*q0.*t10-coef_f1_q_sym17.*q0.*t12+coef_f0_q_sym9.*q0.*q2.*q3-coef_f1_q_sym13.*q0.*q1.*q2.*2.0-coef_f1_q_sym14.*q0.*q1.*q3.*2.0-coef_f1_q_sym16.*q0.*q2.*q3,coef_f0_q_sym18.*q2-coef_f2_q_sym18.*q0+coef_f0_q_sym15.*t11-coef_f2_q_sym2.*t7+coef_f0_q_sym2.*q2.*t6+coef_f0_q_sym6.*q0.*t10+coef_f0_q_sym12.*q2.*t8.*3.0+coef_f0_q_sym13.*q1.*t10.*2.0+coef_f0_q_sym16.*q3.*t10+coef_f0_q_sym17.*q2.*t12-coef_f2_q_sym5.*q1.*t6.*2.0-coef_f2_q_sym6.*q2.*t6-coef_f2_q_sym7.*q3.*t6-coef_f2_q_sym12.*q0.*t8.*3.0-coef_f2_q_sym15.*q0.*t10-coef_f2_q_sym17.*q0.*t12+coef_f0_q_sym5.*q0.*q1.*q2.*2.0+coef_f0_q_sym7.*q0.*q2.*q3+coef_f0_q_sym14.*q1.*q2.*q3.*2.0-coef_f2_q_sym13.*q0.*q1.*q2.*2.0-coef_f2_q_sym14.*q0.*q1.*q3.*2.0-coef_f2_q_sym16.*q0.*q2.*q3,coef_f0_q_sym18.*q3-coef_f3_q_sym18.*q0+coef_f0_q_sym17.*t13-coef_f3_q_sym2.*t7+coef_f0_q_sym2.*q3.*t6+coef_f0_q_sym7.*q0.*t12+coef_f0_q_sym12.*q3.*t8.*3.0+coef_f0_q_sym14.*q1.*t12.*2.0+coef_f0_q_sym15.*q3.*t10+coef_f0_q_sym16.*q2.*t12-coef_f3_q_sym5.*q1.*t6.*2.0-coef_f3_q_sym6.*q2.*t6-coef_f3_q_sym7.*q3.*t6-coef_f3_q_sym12.*q0.*t8.*3.0-coef_f3_q_sym15.*q0.*t10-coef_f3_q_sym17.*q0.*t12+coef_f0_q_sym5.*q0.*q1.*q3.*2.0+coef_f0_q_sym6.*q0.*q2.*q3+coef_f0_q_sym13.*q1.*q2.*q3.*2.0-coef_f3_q_sym13.*q0.*q1.*q2.*2.0-coef_f3_q_sym14.*q0.*q1.*q3.*2.0-coef_f3_q_sym16.*q0.*q2.*q3,q1.*2.0,coef_f0_q_sym22.*q1-coef_f1_q_sym22.*q0-coef_f1_q_sym3.*t7+coef_f0_q_sym13.*t9+coef_f0_q_sym3.*q1.*t6+coef_f0_q_sym6.*q0.*t8-coef_f1_q_sym6.*q1.*t6+coef_f0_q_sym15.*q2.*t8.*2.0-coef_f1_q_sym8.*q2.*t6.*2.0+coef_f0_q_sym16.*q3.*t8-coef_f1_q_sym9.*q3.*t6+coef_f0_q_sym19.*q1.*t10.*3.0+coef_f0_q_sym21.*q1.*t12-coef_f1_q_sym13.*q0.*t8-coef_f1_q_sym19.*q0.*t10.*3.0-coef_f1_q_sym21.*q0.*t12+coef_f0_q_sym8.*q0.*q1.*q2.*2.0+coef_f0_q_sym9.*q0.*q1.*q3+coef_f0_q_sym20.*q1.*q2.*q3.*2.0-coef_f1_q_sym15.*q0.*q1.*q2.*2.0-coef_f1_q_sym16.*q0.*q1.*q3-coef_f1_q_sym20.*q0.*q2.*q3.*2.0,t2+t3+t4.*2.0+t5+t14+t15+t16.*4.0+t17+t18+t19.*2.0+t20+t21+t22.*3.0+t23+t24+t26+t27-coef_f2_q_sym22.*q0-coef_f2_q_sym3.*t7+coef_f0_q_sym13.*q2.*t8.*2.0+coef_f0_q_sym14.*q3.*t8+coef_f0_q_sym15.*q1.*t10.*3.0+coef_f0_q_sym17.*q1.*t12+coef_f0_q_sym20.*q3.*t10.*3.0-coef_f2_q_sym6.*q1.*t6+coef_f0_q_sym21.*q2.*t12.*2.0-coef_f2_q_sym8.*q2.*t6.*2.0-coef_f2_q_sym9.*q3.*t6-coef_f2_q_sym13.*q0.*t8-coef_f2_q_sym19.*q0.*t10.*3.0-coef_f2_q_sym21.*q0.*t12+coef_f0_q_sym7.*q0.*q1.*q3-coef_f2_q_sym15.*q0.*q1.*q2.*2.0-coef_f2_q_sym16.*q0.*q1.*q3-coef_f2_q_sym20.*q0.*q2.*q3.*2.0,coef_f0_q_sym22.*q3-coef_f3_q_sym22.*q0+coef_f0_q_sym21.*t13-coef_f3_q_sym3.*t7+coef_f0_q_sym3.*q3.*t6+coef_f0_q_sym9.*q0.*t12+coef_f0_q_sym13.*q3.*t8+coef_f0_q_sym16.*q1.*t12+coef_f0_q_sym19.*q3.*t10.*3.0+coef_f0_q_sym20.*q2.*t12.*2.0-coef_f3_q_sym6.*q1.*t6-coef_f3_q_sym8.*q2.*t6.*2.0-coef_f3_q_sym9.*q3.*t6-coef_f3_q_sym13.*q0.*t8-coef_f3_q_sym19.*q0.*t10.*3.0-coef_f3_q_sym21.*q0.*t12+coef_f0_q_sym6.*q0.*q1.*q3+coef_f0_q_sym8.*q0.*q2.*q3.*2.0+coef_f0_q_sym15.*q1.*q2.*q3.*2.0-coef_f3_q_sym15.*q0.*q1.*q2.*2.0-coef_f3_q_sym16.*q0.*q1.*q3-coef_f3_q_sym20.*q0.*q2.*q3.*2.0,q2.*2.0,coef_f0_q_sym24.*q1-coef_f1_q_sym24.*q0-coef_f1_q_sym4.*t7+coef_f0_q_sym14.*t9+coef_f0_q_sym4.*q1.*t6+coef_f0_q_sym7.*q0.*t8-coef_f1_q_sym7.*q1.*t6+coef_f0_q_sym16.*q2.*t8-coef_f1_q_sym9.*q2.*t6+coef_f0_q_sym17.*q3.*t8.*2.0+coef_f0_q_sym20.*q1.*t10+coef_f0_q_sym23.*q1.*t12.*3.0-coef_f1_q_sym10.*q3.*t6.*2.0-coef_f1_q_sym14.*q0.*t8-coef_f1_q_sym20.*q0.*t10-coef_f1_q_sym23.*q0.*t12.*3.0+coef_f0_q_sym9.*q0.*q1.*q2+coef_f0_q_sym10.*q0.*q1.*q3.*2.0+coef_f0_q_sym21.*q1.*q2.*q3.*2.0-coef_f1_q_sym16.*q0.*q1.*q2-coef_f1_q_sym17.*q0.*q1.*q3.*2.0-coef_f1_q_sym21.*q0.*q2.*q3.*2.0,coef_f0_q_sym24.*q2-coef_f2_q_sym24.*q0+coef_f0_q_sym20.*t11-coef_f2_q_sym4.*t7+coef_f0_q_sym4.*q2.*t6+coef_f0_q_sym9.*q0.*t10+coef_f0_q_sym14.*q2.*t8+coef_f0_q_sym16.*q1.*t10+coef_f0_q_sym21.*q3.*t10.*2.0-coef_f2_q_sym7.*q1.*t6+coef_f0_q_sym23.*q2.*t12.*3.0-coef_f2_q_sym9.*q2.*t6-coef_f2_q_sym10.*q3.*t6.*2.0-coef_f2_q_sym14.*q0.*t8-coef_f2_q_sym20.*q0.*t10-coef_f2_q_sym23.*q0.*t12.*3.0+coef_f0_q_sym7.*q0.*q1.*q2+coef_f0_q_sym10.*q0.*q2.*q3.*2.0+coef_f0_q_sym17.*q1.*q2.*q3.*2.0-coef_f2_q_sym16.*q0.*q1.*q2-coef_f2_q_sym17.*q0.*q1.*q3.*2.0-coef_f2_q_sym21.*q0.*q2.*q3.*2.0,t2+t3+t4+t5.*2.0+t14+t15+t16+t17.*4.0+t18+t19+t20+t21.*2.0+t22+t23.*3.0+t25+t26+t27-coef_f3_q_sym24.*q0-coef_f3_q_sym4.*t7+coef_f0_q_sym13.*q2.*t8+coef_f0_q_sym14.*q3.*t8.*2.0+coef_f0_q_sym15.*q1.*t10+coef_f0_q_sym17.*q1.*t12.*3.0+coef_f0_q_sym20.*q3.*t10.*2.0+coef_f0_q_sym21.*q2.*t12.*3.0-coef_f3_q_sym7.*q1.*t6-coef_f3_q_sym9.*q2.*t6-coef_f3_q_sym10.*q3.*t6.*2.0-coef_f3_q_sym14.*q0.*t8-coef_f3_q_sym20.*q0.*t10-coef_f3_q_sym23.*q0.*t12.*3.0+coef_f0_q_sym6.*q0.*q1.*q2-coef_f3_q_sym16.*q0.*q1.*q2-coef_f3_q_sym17.*q0.*q1.*q3.*2.0-coef_f3_q_sym21.*q0.*q2.*q3.*2.0,q3.*2.0],[4,4]);


function [s, cfg] = ft_statfun_actvsblT(cfg, dat, design)

% FT_STATFUN_ACTVSBLT calculates the activation-versus-baseline T-statistic on the
% biological data (the dependent variable), using the information on the independent
% variable (ivar) in design.
%
% Note that it does not make sense to use this test statistic when baseline
% correction was performed by subtracting the mean of the baseline period from the
% whole data (for ERP data) or by dividing by the mean (for TFR data). If baseline
% correction is desired, you should subtract the full baseline and activation period.
%
% Use this function by calling one of the high-level statistics functions as
%   [stat] = ft_timelockstatistics(cfg, timelock1, timelock2, ...)
%   [stat] = ft_freqstatistics(cfg, freq1, freq2, ...)
%   [stat] = ft_sourcestatistics(cfg, source1, source2, ...)
% with the following configuration option
%   cfg.statistic = 'ft_statfun_actvsblT'
%
% Configuration options
%   cfg.computestat    = 'yes' or 'no', calculate the statistic (default='yes')
%   cfg.computecritval = 'yes' or 'no', calculate the critical values of the test statistics (default='no')
%   cfg.computeprob    = 'yes' or 'no', calculate the p-values (default='no')
%
% The following options are relevant if cfg.computecritval='yes' and/or
% cfg.computeprob='yes'.
%   cfg.alpha = critical alpha-level of the statistical test (default=0.05)
%   cfg.tail  = -1, 0, or 1, left, two-sided, or right (default=1)
%               cfg.tail in combination with cfg.computecritval='yes'
%               determines whether the critical value is computed at
%               quantile cfg.alpha (with cfg.tail=-1), at quantiles
%               cfg.alpha/2 and (1-cfg.alpha/2) (with cfg.tail=0), or at
%               quantile (1-cfg.alpha) (with cfg.tail=1).
%
% Design specification
%   cfg.ivar  = row number of the design that contains the labels of the conditions that must be
%               compared (default=1). The first condition, indicated by 1, corresponds to the
%               activation period and the second, indicated by 2, corresponds to the baseline period.
%   cfg.uvar  = row number of design that contains the labels of the units-of-observation (subjects or trials)
%               (default=2). The labels are assumed to be integers ranging from 1 to
%               the number of units-of-observation.
%
% See also FT_TIMELOCKSTATISTICS, FT_FREQSTATISTICS or FT_SOURCESTATISTICS

% Copyright (C) 2006, Eric Maris
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% set defaults
if ~isfield(cfg, 'computestat'),       cfg.computestat='yes';     end
if ~isfield(cfg, 'computecritval'),    cfg.computecritval='no';   end
if ~isfield(cfg, 'computeprob'),       cfg.computeprob='no';      end
if ~isfield(cfg, 'alpha'),             cfg.alpha=0.05;            end
if ~isfield(cfg, 'tail'),              cfg.tail=1;                end

% perform some checks on the configuration
if strcmp(cfg.computeprob,'yes') && strcmp(cfg.computestat,'no')
  ft_error('P-values can only be calculated if the test statistics are calculated.');
end

% calculate the number of time samples
switch cfg.dimord
  case 'chan_freq_time'
    nchan = cfg.dim(1);
    nfreq = cfg.dim(2);
    ntime = cfg.dim(3);
    [nsmpls,nrepl] = size(dat);
  otherwise
    ft_error('Inappropriate dimord for the statistics function FT_STATFUN_ACTVSBLT.');
end

sel1 = find(design(cfg.ivar,:)==1);
sel2 = find(design(cfg.ivar,:)==2);
n1  = length(sel1);
n2  = length(sel2);
if (n1+n2)<size(design,2) || (n1~=n2)
  ft_error('Invalid specification of the design array.');
end
nunits = max(design(cfg.uvar,:));
df = nunits - 1;
if nunits<2
  ft_error('The data must contain at least two units (trials or subjects).')
end
if (nunits*2)~=(n1+n2)
  ft_error('Invalid specification of the design array.');
end

if strcmp(cfg.computestat,'yes')
  % compute the statistic
  % calculate the time averages of the activation and the baseline period
  % for all units-of-observation.
  meanreshapeddat=nanmean(reshape(dat,nchan,nfreq,ntime,nrepl),3);
  timeavgdat=repmat(reshape(meanreshapeddat,(nchan*nfreq),nrepl),ntime,1);

  % store the positions of the 1-labels and the 2-labels in a nunits-by-2 array
  poslabelsperunit=zeros(nunits,2);
  poslabel1=find(design(cfg.ivar,:)==1);
  poslabel2=find(design(cfg.ivar,:)==2);
  [dum,i]=sort(design(cfg.uvar,poslabel1),'ascend');
  poslabelsperunit(:,1)=poslabel1(i);
  [dum,i]=sort(design(cfg.uvar,poslabel2),'ascend');
  poslabelsperunit(:,2)=poslabel2(i);

  % calculate the differences between the conditions
  diffmat=dat(:,poslabelsperunit(:,1))-timeavgdat(:,poslabelsperunit(:,2));

  % calculate the dependent samples t-statistics
  avgdiff=nanmean(diffmat,2);
  vardiff=nanvar(diffmat,0,2);
  s.stat=sqrt(nunits)*avgdiff./sqrt(vardiff);
end

if strcmp(cfg.computecritval,'yes')
  % also compute the critical values
  s.df      = df;
  if cfg.tail==-1
    s.critval = tinv(cfg.alpha,df);
  elseif  cfg.tail==0
    s.critval = [tinv(cfg.alpha/2,df),tinv(1-cfg.alpha/2,df)];
  elseif cfg.tail==1
    s.critval = tinv(1-cfg.alpha,df);
  end
end

if strcmp(cfg.computeprob,'yes')
  % also compute the p-values
  s.df      = df;
  if cfg.tail==-1
    s.prob = tcdf(s.stat,s.df);
  elseif  cfg.tail==0
    s.prob = 2*tcdf(-abs(s.stat),s.df);
  elseif cfg.tail==1
    s.prob = 1-tcdf(s.stat,s.df);
  end
end


function [planar] = constructplanargrad(cfg, grad)

% CONSTRUCTPLANARGRAD constructs a planar gradiometer array from an axial gradiometer
% definition. This can be used to compute the planar field gradient for a known
% (estimated) source configuration.
%
% Use as
%   [grad_planar] = constructplanargrad(cfg, grad_axial)
%
% Where cfg contains the following configuration details
%   cfg.baseline_axial   = number (default is 5)
%   cfg.baseline_planar  = number (default is 0.5)
%   cfg.planaraxial      = 'no' or 'yes' (default)
%
% The option planaraxial='yes' specifies that the planar gradiometers
% should consist of axial gradiometers, to make them comparable with
% Ole Jensens planar gradient computation. If planaraxial='no', the
% planar gradiometers will be more or less similar to the Neuromag
% system.
%
% The input grad can be a CTF type axial gradiometer definition, but
% just as well be a magnetometer definition. This function only assumes
% that
%   grad.coilpos
%   grad.coilori
%   grad.label
% exist and that the first Nlabel channels in pnt and ori should be
% used to compute the position of the coils in the planar gradiometer
% channels.

% Copyright (C) 2004, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if ~isfield(cfg, 'planaraxial'),     cfg.planaraxial = 'yes';   end
if ~isfield(cfg, 'baseline_axial'),  cfg.baseline_axial  = 5;   end
if ~isfield(cfg, 'baseline_planar'), cfg.baseline_planar = 0.5; end

Nchan = length(grad.label);

% these will hold all the coil positions
lo_posx = zeros(Nchan,3);
lo_negx = zeros(Nchan,3);
lo_posy = zeros(Nchan,3);
lo_negy = zeros(Nchan,3);
hi_posx = zeros(Nchan,3);
hi_negx = zeros(Nchan,3);
hi_posy = zeros(Nchan,3);
hi_negy = zeros(Nchan,3);

for chan=1:Nchan
  % Attach a local coordinate system to this gradiometer:
  %   the origin at the location of its bottom coil
  %   the z-axis pointing outwards from the head
  %   the x-axis pointing horizontal w.r.t. the head
  %   the y-axis pointing vertical, i.e. approximately towards the vertex
  this_o = grad.chanpos(chan,:);
  this_z = grad.chanori(chan,:);
  this_z = this_z / norm(this_z);
  this_x = cross([0 0 1], this_z);
  if all(this_x==0)
    this_x = [1 0 0];
  else
    this_x = this_x / norm(this_x);
  end
  this_y = cross(this_z, this_x);

  % compute the position of all the 8 coils per channel
  lo_posx(chan,:) = this_o + (cfg.baseline_planar/2) * this_x;
  lo_negx(chan,:) = this_o - (cfg.baseline_planar/2) * this_x;
  lo_posy(chan,:) = this_o + (cfg.baseline_planar/2) * this_y;
  lo_negy(chan,:) = this_o - (cfg.baseline_planar/2) * this_y;
  hi_posx(chan,:) = lo_posx(chan,:) + cfg.baseline_axial * this_z;
  hi_negx(chan,:) = lo_negx(chan,:) + cfg.baseline_axial * this_z;
  hi_posy(chan,:) = lo_posy(chan,:) + cfg.baseline_axial * this_z;
  hi_negy(chan,:) = lo_negy(chan,:) + cfg.baseline_axial * this_z;
end

% start with an empty planar gradiometer definition
planar = [];

if strcmp(cfg.planaraxial, 'yes')
  % combine all the 8 coils into a single sensor
  planar.coilpos = [
    lo_posx
    lo_negx
    lo_posy
    lo_negy
    hi_posx
    hi_negx
    hi_posy
    hi_negy
  ];

  % the orientation of all the coils of a single sensor should be the same
  planar.coilori = [
    grad.coilori(1:Nchan,:)
    grad.coilori(1:Nchan,:)
    grad.coilori(1:Nchan,:)
    grad.coilori(1:Nchan,:)
    grad.coilori(1:Nchan,:)
    grad.coilori(1:Nchan,:)
    grad.coilori(1:Nchan,:)
    grad.coilori(1:Nchan,:)
  ];

  e = eye(Nchan);
  z = zeros(Nchan);

  % the linear combination matrix should be 2*Nchan x 8*Nchan
  planar.tra = [
    e -e  z  z -e  e  z  z    % this is for the horizontal gradients
    z  z  e -e  z  z -e  e    % this is for the vertical gradients
  ];

else
  % combine only the 4 lower coils into a single sensor
  planar.coilpos = [
    posx
    negx
    posy
    negy
  ];

  % the orientation of all the coils of a single gradiometer should be the same
  planar.coilori = [
    grad.coilori(1:Nchan,:)
    grad.coilori(1:Nchan,:)
    grad.coilori(1:Nchan,:)
    grad.coilori(1:Nchan,:)
  ];

  e = eye(Nchan);
  z = zeros(Nchan);

  % the linear combination matrix should be 2*Nchan x 4*Nchan
  planar.tra = [
    e -e  z  z    % this is for the horizontal gradients
    z  z  e -e    % this is for the vertical gradients
  ];

end

for chan=1:Nchan
  planar.label{chan      } = [grad.label{chan} '_dH'];
  planar.label{chan+Nchan} = [grad.label{chan} '_dV'];
end

planar.label = planar.label(:);
planar.tra   = planar.tra / cfg.baseline_planar;
planar.chanpos = [grad.chanpos; grad.chanpos];
planar.chanori = [grad.chanori; grad.chanori];

try
  planar.unit  = grad.unit;
end

% add information about the version of this function to the configuration
cfg.version.name = mfilename('fullpath');
cfg.version.id   = '$Id$';

% rememember the exact configuration details in the output
planar.cfg = cfg;


% 2021 EDIT: This function is now deprecated. The current version of the shaped PRESS
% simulation is run_simPressShaped.m. The current version employs coherence selection,
% resulting in a 4x simulation speed increase.
%
% run_simPressShaped_fast_phCyc.m
% Jamie Near, McGill University 2018.
%
% USAGE:
% out=run_simPressShaped_fast_phCyc(spinSys);
%
% DESCRIPTION:
% This script simulates a PRESS experiment with fully shaped refocusing
% pulses.  Phase cycling of refocusing pulses is performed.  Furthermore,
% simulations are run at various locations in space to account for the
% within-voxel spatial variation of the metabolite signal.  Summation
% across phase cycles and spatial positions is performed.  To achieve
% faster perfomance compared to the original 'run_simPressShaped.m' function,
% this code uses the method described by Yan Zhang et al. Med Phys 2017;44(8):
% 4169-78.  Some additional acceleration is currently performed using parfor
% loops in both x and y directions.  To enable the use of the MATLAB
% parallel computing toolbox, initialize the multiple worked nodes using
% "matlabpool size X" where "X" is the number of available processing
% nodes.  If the parallel processing toolbox is not available, then replace
% the "parfor" loop with a "for" loop.
%
% INPUTS:
% To run this script, there is technically only one input argument:
% spinSys           = spin system to simulate
%
% However, the user should also edit the following parameters as
% desired before running the function:
% refocWaveform     = name of refocusing pulse waveform.
% refTp             = duration of refocusing pulses[ms]
% Bfield            = Magnetic field strength in [T]
% Npts              = number of spectral points
% sw                = spectral width [Hz]
% Bfield            = magnetic field strength [Tesla]
% lw                = linewidth of the output spectrum [Hz]
% thkX              = slice thickness of x refocusing pulse [cm]
% thkY              = slice thickness of y refocusing pulse [cm]
% fovX              = full simulation FOV in the x direction [cm]
% fovY              = full simulation FOV in the y direction [cm]
% nX                = number of spatial grid points to simulate in x-direction
% nY                = number of spatial grid points to simulate in y-direction
% taus              = vector of pulse sequence timings  [ms]
% refPhCyc1         = vector of phase cycling steps for 1st refocusing pulse [degrees]
% refPhCyc2         = vector of phase cycling steps for 2nd refocusing pulse [degrees]
%
% OUTPUTS:
% out               = Simulation results, summed over all space.

function out=run_simPressShaped_fast_phCyc(spinSys)

% ************INPUT PARAMETERS**********************************
refocWaveform='sampleRefocPulse.pta'; %name of refocusing pulse waveform.
refTp=3.5; %duration of refocusing pulses[ms]
flipAngle=137;  %Flip Angle of the refocusing pulses [degrees] (e.g. Use 180 for Siemens PRESS.  Use 137 for GE PRESS).
Npts=2048; %number of spectral points
sw=2000; %spectral width [Hz]
Bfield=3; %magnetic field strength [Tesla]
lw=2; %linewidth of the output spectrum [Hz]
thkX=1.66; %slice thickness of x refocusing pulse [cm]
thkY=1.66; %slice thickness of y refocusing pulse [cm]
fovX=2.4; %size of the full simulation Field of View in the x-direction [cm]
fovY=2.4; %size of the full simulation Field of View in the y-direction [cm]
nX=32; %Number of grid points to simulate in the x-direction
nY=32; %Number of grid points to simulate in the y-direction
tau1=30; %TE1 for first spin echo [ms]
tau2=105; %TE2 for second spin echo [ms]
refPhCyc1=[0,90]; %phase cycling steps for 1st refocusing pulse [degrees]
refPhCyc2=[0,90]; %phase cycling steps for 2nd refocusing pulse [degrees]
% ************END OF INPUT PARAMETERS**********************************

%set up spatial grid
x=linspace(-fovX/2,fovX/2,nX); %X positions to simulate [cm]
y=linspace(-fovY/2,fovY/2,nY); %y positions to simulate [cm]

%Load RF waveform
refRF=io_loadRFwaveform(refocWaveform,'ref',0);

gamma=42577000; %gyromagnetic ratio

%Load spin systems
load spinSystems
sys=eval(['sys' spinSys]);

%Resample refocusing RF pulse from 400 pts to 100 pts to reduce
%computational workload
refRF=rf_resample(refRF,100);

Gx=(refRF.tbw/(refTp/1000))/(gamma*thkX/10000); %[G/cm]
Gy=(refRF.tbw/(refTp/1000))/(gamma*thkY/10000); %[G/cm]

%Initialize structures:
d_temp=cell(length(x),length(refPhCyc1));
d=cell(length(refPhCyc1));

%loop through space: If you are using the parfor loops below, and you are
%using an older version of MATLAB (e.g.R2012), don't forget to initialize
%the parallel processing toolbox workers using 'matlabpool open N' (for N
%workers, 12 max).  I don't think this is necessary for newer version of
%MATLAB.

%First loop through all x-positions, simulating only the first refocusing
%pulse.
%First loop through x direction (first refoc pulse only);

%for X=1:length(x)  %Use this if you don't have the MATLAB parallel processing toolbox
parfor X=1:length(x)  %Use this if you have the MATLAB parallel processing toolbox
    for RP1=1:length(refPhCyc1)
        disp(['Executing X-position ' num2str(X) ' of ' num2str(length(x)) ', '...
            'First Refoc phase cycle ' num2str(RP1) ' of ' num2str(length(refPhCyc1)) '!!!']);
        d_temp{X}{RP1}=sim_press_shaped_fastRef1(Bfield,sys,tau1,tau2,refRF,refTp,x(X),Gx,refPhCyc1(RP1),flipAngle);
    end
end

%calculate the average density matrix (Doing this inside a separate for
%loop because I couldn't figure out how to do this inside the parfor loop):
for X=1:length(x)
    for RP1=1:length(refPhCyc1)
        d{RP1}=sim_dAdd(d{RP1},d_temp{X}{RP1});
    end
end

% %Initialize structures:
out_temp=cell(length(y),length(refPhCyc1),length(refPhCyc2));
out=struct([]);

%Now loop through y direction (second refoc pulse only);
%for Y=1:length(y) %Use this if you don't have the MATLAB parallel processing toolbox
parfor Y=1:length(y) %Use this if you do have the MATLAB parallel processing toolbox
    for RP1=1:length(refPhCyc1)
        for RP2=1:length(refPhCyc2)
            disp(['Executing Y-position ' num2str(Y) ' of ' num2str(length(y)) ', '...
                'First Refoc phase cycle ' num2str(RP1) ' of ' num2str(length(refPhCyc1)) ', '...
                'Second Refoc phase cycle ' num2str(RP2) ' of ' num2str(length(refPhCyc2)) '!!!']);
            out_temp{Y}{RP1}{RP2}=sim_press_shaped_fastRef2(d{RP1},Npts,sw,Bfield,lw,sys,tau1,tau2,...
                refRF,refTp,y(Y),Gy,refPhCyc2(RP2),flipAngle);
        end
    end
end

%Now combine the outputs;  Again, doing this inside a separate for loop
%becuase I can't figure out how to do this inside the parfor loop:
for Y=1:length(y)
    for RP1=1:length(refPhCyc1)
        for RP2=1:length(refPhCyc2)
            out=op_addScans(out,out_temp{Y}{RP1}{RP2},xor(RP1-1,RP2-1));
        end
    end
end

%For consistent scaling across different shaped simulations, we need to :
%1.  Scale down by the total number of simulations run (since these were
%    all added together.
numSims=(nX*nY*length(refPhCyc1)*length(refPhCyc2));
out=op_ampScale(out,1/numSims);

%2.  Scale by the total size of the simulated region, relative to the size
%    of the voxel.
voxRatio=(thkX*thkY)/(fovX*fovY);
out=op_ampScale(out,1/voxRatio);

end

%Nested Function #1
function d = sim_press_shaped_fastRef1(Bfield,sys,tau1,tau2,RF,tp,dx,Gx,phCyc1,flipAngle)
%
% USAGE:
% d = sim_press_shaped_fastRef1(n,sw,Bfield,linewidth,sys,tau1,tau2,RF,tp,dx,Gx,phCyc1,flipAngle)
%
% DESCRIPTION:
% This function simulates only the first bit of the PRESS experiment, up to
% the beginning of the second refocusing pulse.  The excitation is
% simulated as an instantaneous rotation, and the refocusing pulse is
% simulated as a shaped rotation.
%
% This code is designed to be used in highly-accelerated shaped simulations,
% using the method described by Yan Zhang et al. Med Phys 2017;44(8):
% 4169-78.
%
% This code enables the choice of the phase of the refocusing pulse.  This
% enables phase cycling of the refocusing pulses by repeating simulations
% with different editing pulse phases, which is necessary to remove phase
% artefacts from the editing pulses.  A four step phase cycling scheme is typically
% sufficient, where both refocusing pulses are phase cycled by 0 and 90 degrees, and
% the phase are combined in the following way:
%
% signal = ([0 90] - [0 0]) + ([90 0] - [90 90]);
%
% where, in [X Y], X is the phase of the first refocusing pulse and Y is
% the phase of the second refocusing pulse
%
% Finally, this code simulates the spectrum at a given point in space (x),
% given the values of the slice selection gradient (Gx).  In order
% to fully simulate the MEGA-PRESS experiment, you have to run this
% simulation many times at various points in space (x), followed by
% sim_press_shaped_fastRef2.m, at all points in space (y).
%
% INPUTS:
% n         = number of points in fid/spectrum
% sw        = desired spectral width in [Hz]
% Bfield    = main magnetic field strength in [T]
% linewidth = linewidth in [Hz]
% sys       = spin system definition structure
% tau1      = echo time 1 in [ms].
% tau2      = echo time 2 in [ms].
% RF        = RF pulse definition structure for refoc pulses (obtain using 'io_loadRFwaveform.m')
% tp        = RF pulse duration in [ms]
% dx        = position offset in x-direction (corresponding to first refocusing pulse) [cm]
% dy        = position offset in y-direction (corresponding to second refocusing pulse) [cm]
% Gx        = gradient strength for first selective refocusing pulse [G/cm]
% Gy        = gradient strength for second selective refocusing pulse [G/cm]
% phCycl    = initial phase of the first refocusing pulse in [degrees];
% phCycl2   = initial phase of the second refocusing pulse in [degrees];
% flipAngle = flip angle of refocusing pulses [degrees] (Optional.  Default = 180 deg)
%
% OUTPUTS:
% out       = simulated spectrum, in FID-A structure format, using PRESS
%             sequence.

if nargin<10
    flipAngle=180;
end

if tau1<tp/1000
    error('ERROR:  Echo-time 1 cannot be less than duration of refocusing pulse! ABORTING!!');
end
if tau2<tp/1000
    error('ERROR:  Echo-time 2 cannot be less than duration of refocusing pulse! ABORTING!!');
end

%Set water to centre
centreFreq=2.3;
for k=1:length(sys)
    sys(k).shifts=sys(k).shifts-centreFreq;
end

%Calculate Hamiltonian matrices and starting density matrix.
[H,d]=sim_Hamiltonian(sys,Bfield);

%Calculate new delays by subtracting the pulse duration from tau1 and tau2;
delays=zeros(2);
delays(1)=tau1-tp;
delays(2)=tau2-tp;
if sum(delays<0)
    error(['ERROR! The following taus are too short: ' num2str(find(delays<0)) '.']);
end

%BEGIN PULSE SEQUENCE************
d=sim_excite(d,H,'x');                                    %EXCITE
d=sim_evolve(d,H,delays(1)/2000);                            %Evolve by delays(1)/2
d=sim_shapedRF(d,H,RF,tp,flipAngle,90+phCyc1,dx,Gx);          %1st shaped 180 degree refocusing pulse
d=sim_evolve(d,H,(delays(1)+delays(2))/2000);                     %Evolve by (delays(1)+delays(2))/2
%END PULSE SEQUENCE**************

%After running this many times along x, the density matrices should be
%averaged, and then the average density matrix should be passed through
%'sim_press_shaped_fastRef2' at various different y-positions.

end

%Nested Function #2
function out = sim_press_shaped_fastRef2(d,n,sw,Bfield,linewidth,sys,tau1,tau2,RF,tp,dy,Gy,phCyc2,flipAngle)
%
% USAGE:
% out = sim_press_shaped_fastRef2(d,n,sw,Bfield,linewidth,sys,tau2,RF,tp,dy,Gy,phCyc2,flipAngle)
%
% DESCRIPTION:
% This function simulates only the last bit of the PRESS experiment, from the
% the beginning of the second refocusing pulse, to the end.  The refocusing
%pulse is simulated as a shaped rotation.
%
% This code is designed to be used in highly-accelerated shaped simulations,
% using the method described by Yan Zhang et al. Med Phys 2017;44(8):
% 4169-78.
%
% This code enables the choice of the phase of the refocusing pulse.  This
% enables phase cycling of the refocusing pulses by repeating simulations
% with different editing pulse phases, which is necessary to remove phase
% artefacts from the editing pulses.  A four step phase cycling scheme is typically
% sufficient, where both refocusing pulses are phase cycled by 0 and 90 degrees, and
% the phase are combined in the following way:
%
% signal = ([0 90] - [0 0]) + ([90 0] - [90 90]);
%
% where, in [X Y], X is the phase of the first refocusing pulse and Y is
% the phase of the second refocusing pulse
%
% Finally, this code simulates the spectrum at a given point in space (y),
% given the values of the slice selection gradient (Gy).  In order
% to fully simulate the MEGA-PRESS experiment, you have to first run
% sim_press_shaped_fastRef1.m at all points in space (x), followed by
% this code, at all points in space (y).
%
% INPUTS:
% d         = starting density matrix (obtained using 'sim_press_shaped_fastRef1.m')
% n         = number of points in fid/spectrum
% sw        = desired spectral width in [Hz]
% Bfield    = main magnetic field strength in [T]
% linewidth = linewidth in [Hz]
% sys       = spin system definition structure
% tau1      = echo time 1 in [ms].
% tau2      = echo time 2 in [ms].
% RF        = RF pulse definition structure for refoc pulses (obtain using 'io_loadRFwaveform.m')
% tp        = RF pulse duration in [ms]
% dx        = position offset in x-direction (corresponding to first refocusing pulse) [cm]
% dy        = position offset in y-direction (corresponding to second refocusing pulse) [cm]
% Gx        = gradient strength for first selective refocusing pulse [G/cm]
% Gy        = gradient strength for second selective refocusing pulse [G/cm]
% phCycl    = initial phase of the first refocusing pulse in [degrees];
% phCycl2   = initial phase of the second refocusing pulse in [degrees];
% flipAngle = flip angle of refocusing pulses [degrees] (Optional.  Default = 180 deg)
%
% OUTPUTS:
% out       = simulated spectrum, in FID-A structure format, using PRESS
%             sequence.

if nargin<14
    flipAngle=180;
end

if tau1<tp/1000
    error('ERROR:  Echo-time 1 cannot be less than duration of refocusing pulse! ABORTING!!');
end
if tau2<tp/1000
    error('ERROR:  Echo-time 2 cannot be less than duration of refocusing pulse! ABORTING!!');
end

%Set water to centre
centreFreq=2.3;
for k=1:length(sys)
    sys(k).shifts=sys(k).shifts-centreFreq;
end

%Calculate Hamiltonian matrices and starting density matrix.
[H]=sim_Hamiltonian(sys,Bfield);

%Calculate new delays by subtracting the pulse duration from tau1 and tau2;
delays=zeros(2);
delays(1)=tau1-tp;
delays(2)=tau2-tp;
if sum(delays<0)
    error(['ERROR! The following taus are too short: ' num2str(find(delays<0)) '.']);
end

%BEGIN PULSE SEQUENCE************
d=sim_shapedRF(d,H,RF,tp,flipAngle,90+phCyc2,dy,Gy);          %2nd shaped 180 degree refocusing pulse
d=sim_evolve(d,H,delays(2)/2000);                            %Evolve by delays(2)/2
[out,~]=sim_readout(d,H,n,sw,linewidth,90);      %Readout along y (90 degree phase);
%END PULSE SEQUENCE**************

%Correct the ppm scale:
out.ppm=out.ppm-(4.65-centreFreq);

%Fill in structure header fields:
out.seq='press';
out.te=tau1+tau2;
out.sim='shaped';

%Additional fields for compatibility with FID-A processing tools.
out.sz=size(out.specs);
out.date=date;
out.dims.t=1;
out.dims.coils=0;
out.dims.averages=0;
out.dims.subSpecs=0;
out.dims.extras=0;
out.averages=1;
out.rawAverages=1;
out.subspecs=1;
out.rawSubspecs=1;
out.flags.writtentostruct=1;
out.flags.gotparams=1;
out.flags.leftshifted=0;
out.flags.filtered=0;
out.flags.zeropadded=0;
out.flags.freqcorrected=0;
out.flags.phasecorrected=0;
out.flags.averaged=1;
out.flags.addedrcvrs=1;
out.flags.subtracted=1;
out.flags.writtentotext=0;
out.flags.downsampled=0;
out.flags.isFourSteps=0;

end


function varargout = classify_naive_bayes(meth, varargin)
% Naive Bayes classifier
%
% :Usage:
% ::
%
%     % set up model structure
%     bayes_model = classify_naive_bayes('setup', Y, Xi, [activation_cutoff, selectivity_cutoff]);
%
% :Inputs:
%
%   **Y:**
%        is full (not sparse); empty features will be eliminated
%
%   **Xi:**
%        is obs x classes, an indicator matrix of 1's and 0's
%
% :TEST: test classifier; make a prediction about classes from data
% ::
%
%    [class_est log_joint best_log map p_obs_act_given_class] = classify_naive_bayes('test', Y, bayes_model);
%
% :EVAL: evaluate classification accuracy
% ::
%
%    [prop_correct, confusion_mtx, misclass, prop_correct_by_class, chance, chance_95_ci] = classify_naive_bayes('eval', true_class, class_est, wh_obs);
%
% :APPARENT: apparent classification; with full dataset
% ::
%
%     bayes_model = classify_naive_bayes('apparent', Y, bayes_model);
%
% :XVAL: crossvalidate
% ::
%
%    xval = classify_naive_bayes('xval', Y, Xi);
%
%    bayes_model = classify_naive_bayes('write', bayes_model, Y, volInfo, conditionnames)
%    bayes_model = classify_naive_bayes('write', bayes_model, Y, MC_Setup.volInfo, MC_Setup.Xinms);
%
% To add feature abstraction step within xval:
% ::
%
%    xval = classify_naive_bayes('xval', Y, Xi, 0, .9, .05, 1, volInfo );
%
% :PLOT:
% ::
%
%    classify_naive_bayes('plot', bayes_model, ['pa|t', 'lr', 'lr surface',  'map plot', or 'class plot']);
%
% :Optional Inputs: (any order)
%
% Threshold (abs. value), and colors in cell array
% ::
%
%    classify_naive_bayes('plot', bayes_model, 'lr', .10, {[1 .7 0] [0 0 1]});
%
%
% :Examples:
% ::
%
%    [bayes_model, Y] = classify_naive_bayes('setup', Y, Xi);
%
%    % Test obs. 2
%    tic, [class_est log_joint best_log map] = classify_naive_bayes('test',Y(2,:), bayes_model); toc
%
%    % Get apparent classification rate and look at confusion matrix
%    tic, bayes_model = classify_naive_bayes('apparent', Y, bayes_model); toc
%    bayes_model.apparent.confusion_mtx
%
%    % Cross-validate
%    bayes_model.xval = classify_naive_bayes('xval', Y, Xi);
%    bayes_model.xval.confusion_mtx
%
% :Example 2:
% ::
%
%     Select features, and do apparent and cross-validated classification
%    Y = MC_Setup.unweighted_study_data';
%    wh = sum(Y) > 5;
%    Y = Y(:, wh);
%    whos Y
%    [bayes_model, Y] = classify_naive_bayes('setup', Y, Xi);
%    bayes_model = classify_naive_bayes('apparent', Y, bayes_model);
%    bayes_model.apparent.prop_correct, bayes_model.apparent.confusion_mtx
%    bayes_model.xval = classify_naive_bayes('xval', Y, Xi);
%    bayes_model.xval.prop_correct, bayes_model.xval.confusion_mtx
%
% :Example 3:
%    create_figure('hist'); hist(bayes_model.pa1_given_t, 100);
%    xval = classify_naive_bayes('xval', Y, Xi, .05, .3);
%
% Get results from key regions and run classifier only on those regions:
% ::
%
%     cl = classify_naive_bayes('plot', bayes_model, 'lr', .10, {[1 .7 0] [0 0 1]});
%    [studybyroi,studybyset] = Meta_cluster_tools('getdata',cl{1},dat',volInfo);
%    bayes_model_regions = classify_naive_bayes('setup', studybyroi, Xi);
%    bayes_model_regions = classify_naive_bayes('apparent', studybyroi,bayes_model_regions);
%    disp('Apparent confusion')
%    disp(bayes_model_regions.apparent.confusion_mtx)
%
%    bayes_model_regions.xval = classify_naive_bayes('xval', studybyroi, Xi);
%    disp('Cross-validated confusion')
%    disp(bayes_model_regions.xval.confusion_mtx)
%
%    fprintf('Proportion correct: Apparent: %3.0f%%  Xval: %3.0f%%\n', 100*bayes_model_regions.apparent.prop_correct, 100*bayes_model_regions.xval.prop_correct);
%    fprintf('Proportion correct by class: \t'); fprintf('%3.0f%%\t', 100*bayes_model_regions.xval.prop_correct_by_class);
%    fprintf('\n');

%    sz = cat(1,cl{1}(:).numVox);
%    cl{1}(sz < 10) = [];
%
%    subcl = subclusters_from_local_max(cl{1}, 10);
%
% ..
%    Tor Wager, Sept 2007
% ..

switch meth

    case 'setup'
        Y = full(varargin{1});
        Xi = varargin{2};

        activation_cutoff = [];
        selectivity_cutoff = [];
        return_full_ps = 1;         % for faster processing if not all fields are needed (i.e., in xval)

        if length(varargin) > 2

            activation_cutoff = varargin{3};
            selectivity_cutoff = varargin{4};

        end

        if length(varargin) > 4
            return_full_ps = varargin{5};
        end

        k = 1; % default k regularization param
        if length(varargin) > 5
            k = varargin{6};
        end

        % setup
        [Y, whkeep] = eliminate_empty_Y(Y);

        [bayes_model, Y] = classify_naive_bayes_setup(Y, Xi, activation_cutoff, selectivity_cutoff, whkeep, return_full_ps, k);

        gam = 1; % default gam prior weighting param
        if length(varargin) > 6
            gam = varargin{7};
        end
        bayes_model.params.gam = gam;

        varargout{1} = bayes_model;
        varargout{2} = Y;

    case 'apparent'

        Y = full(varargin{1});
        bayes_model = varargin{2};

        bayes_model.true_class = indic2condf(bayes_model.Xi);

        % test apparent classification rate
        [bayes_model.apparent.class_est bayes_model.apparent.log_joint bayes_model.apparent.best_log bayes_model.apparent.map bayes_model.apparent.pact_given_task] = ...
            classify_naive_bayes_test(Y, bayes_model);

        [bayes_model.apparent.prop_correct, bayes_model.apparent.confusion_mtx, bayes_model.apparent.misclass, ...
            bayes_model.apparent.prop_correct_by_class, bayes_model.apparent.chance, bayes_model.apparent.chance_95_ci] = ...
            classify_naive_bayes_eval(bayes_model.true_class, bayes_model.apparent.class_est);

        varargout{1} = bayes_model;

    case 'test'
        Y = full(varargin{1});
        bayes_model = varargin{2};

        [class_est log_joint best_log map p_obs_act_given_class] = classify_naive_bayes_test(Y, bayes_model);

        varargout{1} = class_est;
        varargout{2} = log_joint;
        varargout{3} = best_log;
        varargout{4} = map;
        varargout{5} = p_obs_act_given_class;

    case 'eval'
        bayes_model = varargin{1};
        class_est = varargin{2};
        wh_obs = [];
        if length(varargin) > 2, wh_obs = varargin{3}; end

        [prop_correct, confusion_mtx, misclass, prop_correct_by_class, chance, chance_95_ci] = classify_naive_bayes_eval(bayes_model, class_est, wh_obs);

        varargout{1} = prop_correct;
        varargout{2} = confusion_mtx;
        varargout{3} = misclass;
        varargout{4} = prop_correct_by_class;
        varargout{5} = chance;
        varargout{6} = chance_95_ci;

        case 'abstract'
          %bayes_model = classify_naive_bayes('abstract', Y, thresh, bayes_model, volInfo);

        Y = full(varargin{1});
        thresh = varargin{2};

        bayes_model = varargin{3};
        volInfo = varargin{4};

        reducedY = bayes_meta_feature_abstract(Y, thresh, bayes_model, volInfo);

        [bayes_model, Y] = classify_naive_bayes('setup', reducedY, bayes_model.Xi, bayes_model.params.activation_cutoff, bayes_model.params.selectivity_cutoff, 1, bayes_model.params.k);

        varargout{1} = bayes_model;
        varargout{2} = Y;

    case 'xval'
        Y = full(varargin{1});
        Xi = varargin{2};

        activation_cutoff = [];
        selectivity_cutoff = [];
        if length(varargin) > 2

            activation_cutoff = varargin{3};
            selectivity_cutoff = varargin{4};

        end

        k = .5;
        gam = 1;
        if length(varargin) > 4

            k = varargin{5};
            gam = varargin{6};

        end

        if length(varargin) > 6
            volInfo = varargin{7};
            do_feature_abstract = 1;
        end

        % initialize
        if do_feature_abstract, Yorig = Y; end

        [Y, whkeep] = eliminate_empty_Y(Y);

        Y = sparse(Y);

        nobs = size(Y, 1);
        fprintf('\nCross-validating %3.0f observations: 00000', nobs);

        log_joint = zeros(nobs, size(Xi, 2));
        map = zeros(nobs, size(Xi, 2));
        class_est = zeros(nobs, 1);
        best_log = zeros(nobs, 1);

        true_class = indic2condf(Xi);

        % Update volinfo for feature abstraction
% %         volInfo.wh_inmask = double(whkeep');
% %         volInfo.n_inmask = sum(whkeep);
% %         volInfo.image_indx(volInfo.image_indx) = whkeep';
% %         volInfo.xyzlist(~whkeep',:) = [];

        % run
        for i = 1:nobs

            if mod(i,10) == 0, fprintf(1,'\b\b\b\b\b%05d',i); end

            include_in_training = true(nobs, 1);
            include_in_training(i) = 0;

            % setup: does feature-selection as well, if requested; no
            % need to return Y, because ...
            bayes_model = classify_naive_bayes_setup(Y(include_in_training, :), Xi(include_in_training, :), activation_cutoff, selectivity_cutoff, whkeep, 0, k);

            bayes_model.params.k = k;
            bayes_model.params.gam = gam;

            testdata = Y(i, bayes_model.whkeep_from_notempty);

            % feature abstraction step
            if do_feature_abstract
                [reducedY, cl, cluster_indx] = bayes_meta_feature_abstract(Yorig(include_in_training, :), .1, .1, bayes_model, volInfo, 0);
                cluster_indx = cluster_indx(bayes_model.whkeep);

                bayes_model = classify_naive_bayes('setup', reducedY, Xi(include_in_training, :), 0, 0, gam, k);

                wh_cl = cluster_indx(testdata);
                wh_cl = unique(wh_cl); wh_cl(wh_cl == 0) = [];
                testdata = false(1, bayes_model.nfeatures);
                testdata(wh_cl) = 1;

            end

            [class_est(i) log_joint(i,:) best_log(i) map(i,:)] = classify_naive_bayes_test(testdata, bayes_model);
        end

        fprintf(1, '\n');

        [prop_correct, confusion_mtx, misclass, prop_correct_by_class, chance, chance_95_ci] = classify_naive_bayes_eval(true_class, class_est);

        varargout{1} = struct('class_est', class_est, 'log_joint', log_joint, 'best_log', best_log, 'map', map, ...
            'prop_correct', prop_correct, 'prop_correct_by_class', prop_correct_by_class, 'confusion_mtx', confusion_mtx, ...
            'misclass', misclass, 'chance', chance, 'chance_95_ci', chance_95_ci, 'params', bayes_model.params);

    case {'write', 'images', 'write_images'}
        bayes_model = varargin{1};
        Y = varargin{2};
        volInfo = varargin{3};
        conditionnames = varargin{4};

        bayes_model = write_images(bayes_model, Y, volInfo, conditionnames);
        varargout{1} = bayes_model;

    case {'plot', 'output'}
        bayes_model = varargin{1};
        disptype = varargin{2};

        cl = display_output(bayes_model, disptype, varargin{3:end});

        varargout{1} = cl;

    otherwise
        error('Unknown method (invalid first argument).');
end

end   % end main function

function [bayes_model, Y] = classify_naive_bayes_setup(Y, Xi, activation_cutoff, selectivity_cutoff, whkeep, return_full_ps, k)

% Y is data matrix, nobs observations x nfeatures variables
% in a meta-analysis, e.g., this is studies x voxels
%
% Each row of Y is an observation; if Y has one row, we're classifying an
% observation (e.g., one brain map)
%
% Xi are indicators for tasks (classes), ntasks columns of nobs elements
% (i.e., an nobs x ntasks matrix)

% eliminate features with no 'on' values (activations)

% k is regularization param, biases towards 0.5
if nargin < 7, k = 1; end

whkeep_from_notempty = whkeep;  % indicator of features after feature-selection, in index of not-empties

% feature selection, if requested
if ~isempty(selectivity_cutoff) && ~isempty(activation_cutoff)

    [Y, whkeep, whkeep_from_notempty] = select_features(Y, Xi, activation_cutoff, selectivity_cutoff, whkeep);

end

[nobs, nfeatures] = size(Y);
nclasses = size(Xi, 2);

if return_full_ps
    [priors, pa1_given_t, pa0_given_t, pt_given_act1, pt_given_act0, pa1_given_not_t] = ...
        bayes_get_probabilities(Y, Xi, k);

else
    % key results (not all) only; saves computation time in
    % crossvalidation.
    [priors, pa1_given_t, pa0_given_t] = bayes_get_probabilities(Y, Xi, k);

end

true_class = indic2condf(Xi);

bayes_model = struct('nobs', nobs, 'nfeatures', nfeatures, 'nclasses', nclasses, ...
    'whkeep', whkeep, 'whkeep_from_notempty', whkeep_from_notempty, ...
    'params', [], ...
    'priors', priors, 'pa1_given_t', pa1_given_t, 'pa0_given_t', pa0_given_t, ...
    'Xi', Xi, 'true_class', true_class);

bayes_model.params.gam = 1;  % gamma, weighting on prior vs. joint evidence
bayes_model.params.k = k;      % k, regularization of pa|t, bias towards 0.5.
bayes_model.params.activation_cutoff = activation_cutoff;
bayes_model.params.selectivity_cutoff = selectivity_cutoff;

if return_full_ps
    bayes_model.pt_given_act1 = pt_given_act1;
    bayes_model.pt_given_act0 = pt_given_act0;
    bayes_model.pa1_given_not_t = pa1_given_not_t;

end

end

function [class_est log_joint best_log map p_obs_act_given_class] = classify_naive_bayes_test(Y, bayes_model)

% Estimate classes, log joint p, and MAP estimates for a test data set

% P(task | act)
% The var. below is all that is needed to choose the best class.
% argmax log_joint
% do this for each observation

[nobs, nfeatures] = size(Y);

if nfeatures ~= bayes_model.nfeatures
    %  try eliminating zero voxels
    Y = Y(:, bayes_model.whkeep);
    [nobs, nfeatures] = size(Y);
end

if nfeatures ~= bayes_model.nfeatures
    error('Number of features (i.e., voxels/regions) in Y must equal that in bayes_model structure or that of original training data.');
end

log_joint = zeros(nobs, bayes_model.nclasses);
map = zeros(nobs, bayes_model.nclasses);

class_est = zeros(nobs, 1);
best_log = zeros(nobs, 1);

gam = bayes_model.params.gam;    % prior weighting factor; from 0 to nfeatures.

logpriors = log(bayes_model.priors);
%logpriors = logpriors(ones(nfeatures, bayes_model.nclasses));

for i = 1:nobs

    wh_active = Y(i,:)' > 0;

    % sum of log prob. of activation at each active voxel, plus sum of
    % log prob. of NOT activation at each inactive voxel (1 - p(activation))
    % Same as ln of product of all p(act state = observed state |
    % class) for all features

    % prob. that activity state is the observed one in the test vector,
    % given each class.
    % p(Fi = fi | C = c)
    % p(ACTi = acti | T = t)

    % % % %     p_obs_act_given_class(wh_active, :) = bayes_model.pt_given_act1(wh_active, :);         % p(active | t, obs active)
    % % % %     p_obs_act_given_class(~wh_active, :) = bayes_model.pt_given_act0(~wh_active, :);

    p_obs_act_given_class = zeros(nfeatures, bayes_model.nclasses);
    p_obs_act_given_class(wh_active, :) = bayes_model.pa1_given_t(wh_active, :);         % p(active | t, obs active)
    p_obs_act_given_class(~wh_active, :) = bayes_model.pa0_given_t(~wh_active, :);   % p(not active | t, obs not active)

    loglikelihood = sum(log(p_obs_act_given_class));
    log_joint(i,:) = gam * log(bayes_model.priors) + loglikelihood;      % log of joint prob. p(T, Y), p(task, act1, act2, act3, etc.)

    %log_joint(i,:) = sum(log(p_obs_act_given_class)); % proportional to the posterior.

    [best_log(i) class_est(i)] = max(log_joint(i,:));

    % Divide joint by evidence
    % get scaled estimate of p(task | activation)
    % These are Max. a posteriori estimates, and values may be > 1 because
    % we're assuming independence when that assumption is not likely to be true
    % * something still wrong with this
    % % % %
    % % % %         % evidence
    % % % %         log_evidence = sum(bayes_model.log_evidence_act(wh_active)) + sum(bayes_model.log_evidence_notact(~wh_active));
    % % % %         % because something is wrong, exp() gives Inf values a lot, so isn't
    % very sensible.
    % % % %         map(i,:) = log_joint(i,:) - log_evidence;  %exp(log_joint(i,:) - log_evidence);

    map(i,:) = exp(log_joint(i,:) ./ (gam + nfeatures) );  %exp(log_joint(i, :)); % will be very small
    %sum(log_joint) + nfeatures * logpriors;

    %map = map ./ sum(map);

end

end

function [prop_correct, m, misclass, prop_correct_by_class, chance, chance_95_ci] = classify_naive_bayes_eval(true_class, class_est, wh_obs)

if nargin < 3 || isempty(wh_obs)
    % assume we have all observations.
    wh_obs = 1:length(true_class);
end

[m,dp,corr,far,misclass] = confusion_matrix(true_class(wh_obs),class_est);

totaln = sum(m(:));
prop_correct = trace(m) ./ totaln;

nclasses = length(unique(true_class(true_class ~= 0)));

prop_correct_by_class = diag(m) ./ sum(m,2);

chance = 1 ./ nclasses;
chance_95_ci = [chance - sqrt(chance * (1 - chance) ./ totaln)   chance + sqrt(chance * (1 - chance) ./ totaln)];

end

function [Y, whkeep] = eliminate_empty_Y(Y)

fprintf('Eliminating any empty features. \n');

% using whkeep helps avoid out of memory errors.
whzero = (sum(Y) < eps);
whkeep = ~whzero;
Y = Y(:, whkeep);

end

function [Y, whkeep, whomsave] = select_features(Y, Xi, activation_cutoff, selectivity_cutoff, whkeep)

% select features
% -----------------------------------------------------------------

[nobs, nfeatures] = size(Y);
ntasks = size(Xi,2);

sumtasks = sum(Y); sumtasks(sumtasks==0) = 1;  % avoid warning for empty tasks

% p(task | activation) estimates
ptask = (Xi' * Y) ./ repmat(sumtasks,ntasks,1);

maxp = max(ptask);  % max prob of task across vars

pt = (Xi' * Y) ./ repmat(sum(Xi)',1,nfeatures);  % proportion of each task that activated in an area
ptmax = max(pt);
whomsave = maxp > (selectivity_cutoff) & ptmax > activation_cutoff;

if sum(whomsave) == 0
    disp('Warning: No variables meet feature selection criteria.');
    whomsave = (maxp == max(maxp));
end

whkeep(whkeep) = whomsave;  % indices of saved voxels in whole-brain (original) index vector
%whkeep = whkeep(whomsave);  % overall indices of saved voxels
Y = Y(:,whomsave);

%fprintf(1,'Selected: %3.0f ',nvars);
% get weights on voxels based on n activations.... ******

end

function bayes_model = write_images(bayes_model, Y, volInfo, conditionnames)
% bayes_model = write_images(bayes_model, MC_Setup.volInfo, MC_Setup.Xinms)

% Xi = bayes_model.Xi;

%[priors, pa1_given_t, pa0_given_t, varargout] = bayes_get_probabilities(Y, Xi, bayes_model.params.k);

nfeatures_orig = length(bayes_model.whkeep);

pa1_given_t = NaN * zeros(nfeatures_orig, bayes_model.nclasses);
pa1_given_not_t = NaN * zeros(nfeatures_orig, bayes_model.nclasses);

pt_given_act1 = NaN * zeros(nfeatures_orig, bayes_model.nclasses);
pt_given_act0 = NaN * zeros(nfeatures_orig, bayes_model.nclasses);

pa1_given_t(bayes_model.whkeep,:) = bayes_model.pa1_given_t;
pa1_given_not_t(bayes_model.whkeep,:) = bayes_model.pa1_given_not_t;

pt_given_act1(bayes_model.whkeep,:) = bayes_model.pt_given_act1;
pt_given_act0(bayes_model.whkeep,:) = bayes_model.pt_given_act0;

% now done in bayes_get_probabilities
% % pa1_given_not_t = (double(~(Xi)') * Y)' ./ repmat(sum(~Xi), size(Y, 2), 1);
% % pa1_given_not_t(pa1_given_not_t < eps) = 1 ./ bayes_model.nobs;
% % pa1_given_not_t(pa1_given_not_t == 1) = 1 - (1 ./ bayes_model.nobs);

%%%%pa1_given_not_t(~bayes_model.whkeep, :) = NaN;

disp('Writing images to disk in the current directory.');

for i = 1:bayes_model.nclasses
    pa1_given_tname{i} = ['pa|t_' conditionnames{i} '.img'];
    iimg_reconstruct_3dvol(pa1_given_t(:,i), volInfo, 'outname', pa1_given_tname{i});

    disp(pa1_given_tname{i})

    pa_given_nottname{i} = ['pa|nott_' conditionnames{i} '.img'];
    iimg_reconstruct_3dvol(pa1_given_not_t(:,i), volInfo, 'outname', pa_given_nottname{i});

    disp(pa_given_nottname{i})

    pt_given_aname{i} = ['pt|a_' conditionnames{i} '.img'];
    iimg_reconstruct_3dvol(pt_given_act1(:,i), volInfo, 'outname', pt_given_aname{i});

    disp(pt_given_aname{i})

    pt_given_notaname{i} = ['pt|nota_' conditionnames{i} '.img'];
    iimg_reconstruct_3dvol(pt_given_act0(:,i), volInfo, 'outname', pt_given_notaname{i});

    disp(pt_given_notaname{i})
end

% likelihood ratio at each voxel
% should penalize this in some way for low activations...
%pa1_given_t(pa1_given_t == 0) = 1;
%pa1_given_not_t(pa1_given_not_t == 0) = 1;

% higher numbers mean more association between activation and class.
% lr for 2 classes can both be positive IF not all obs. fall within class 1
% or class 2!!!
evidence_yes = (sum(Y) ./ bayes_model.nobs)';
evidence_yes = evidence_yes(:, ones(1, bayes_model.nclasses));

% % priors = NaN * zeros(nfeatures_orig, bayes_model.nclasses);
% % priors(bayes_model.whkeep,:) = bayes_model.priors(ones(bayes_model.nfeatures, bayes_model.nclasses));

% add .1 to regularize...this could be k param
lr = ( (pa1_given_t + .1) ./ (evidence_yes + .1) ) - 1;
%pt_given_act1 ./ pt_given_act0;
lr(lr < .05 & lr > -.05) = NaN;
%lr = log(lr);

for i = 1:bayes_model.nclasses
    lrname{i} = ['likeratio_' conditionnames{i} '.img'];
    iimg_reconstruct_3dvol(lr(:,i), volInfo, 'outname', lrname{i});

    disp(lrname{i})
end

bayes_model.image_output.volInfo = volInfo;
bayes_model.image_output.pa1_given_tname = pa1_given_tname;
bayes_model.image_output.pa_given_nottname = pa_given_nottname;

bayes_model.image_output.pt_given_aname = pt_given_aname;
bayes_model.image_output.pt_given_notaname = pt_given_notaname;

bayes_model.image_output.likerationame = lrname;

bayes_model.image_output.pa1_given_t = pa1_given_t;
bayes_model.image_output.pa1_given_not_t = pa1_given_not_t;
bayes_model.image_output.likeratio = lr;

end

function cl = display_output(bayes_model, disptype, varargin)
% display_output(bayes_model, disptype, varargin)

switch spm('Ver')
    case 'SPM2'
        % spm_defaults is a script
        disp('WARNING: spm defaults not set for spm2. Make sure your defaults are set correctly');

    case 'SPM5'
        % spm_defaults is a function
        spm_defaults()

    case 'SPM8'
        % spm_defaults is a function
        spm_defaults()

    otherwise
        % unknown SPM
        disp('Unknown version of SPM!');
        spm_defaults()
end

cl = [];
colors1 = [1 .7 0];
colors2 = [0 0 1];

for i = 1:length(varargin)
    if iscell(varargin{i})
        colors1 = varargin{i}{1};
        colors2 = varargin{i}{2};
    elseif length(varargin{i}) == 1
        thresh = varargin{i};
    end
end

switch disptype

    case {'pa|t', 'lr', 'pt|a'}

        overlay = which('spm2_single_subj_T1_scalped.img');
        spm_check_registration(repmat(overlay, bayes_model.nclasses, 1));

        for i = 1:bayes_model.nclasses

            switch disptype
                case 'pa|t'
                    cl{i} = mask2clusters(bayes_model.image_output.pa1_given_tname{i});
                    fprintf('Displayed: p(a | t) for each task.\n');

                    spm_orthviews_name_axis(bayes_model.image_output.pa1_given_tname{i}, i);

                case 'lr'
                    cl{i} = mask2clusters(bayes_model.image_output.likerationame{i});
                    fprintf('Displayed: regions with positive likelihood ratio for each task.\n');

                    spm_orthviews_name_axis(bayes_model.image_output.likerationame{i}, i);

                case 'pt|a'
                    cl{i} = mask2clusters(bayes_model.image_output.pt_given_aname{i});
                    fprintf('Displayed: p(t | a) for each task.\n');

                    spm_orthviews_name_axis(bayes_model.image_output.pt_given_aname{i}, i);

            end

            % threshold
            if exist('thresh', 'var')
                for j = 1:length(cl{i})
                    wh_omit = abs( cat(2, cl{i}(j).Z) ) < thresh;
                    cl{i}(j).XYZ(:, wh_omit) = [];
                    cl{i}(j).XYZmm(:, wh_omit) = [];
                    cl{i}(j).Z(:, wh_omit) = [];
                end

                clu = clusters2CLU(cl{i});
                cl{i} = tor_extract_rois([], clu, clu);

            end

            cluster_orthviews(cl{i}, 'handle', i, 'add');

        end

        switch disptype
            case 'pa|t'
                %spm_orthviews_change_colormap([.2 .2 .4], [1 1 0], [.9 .6 .1]);  %slate to orange to yellow
                spm_orthviews_change_colormap([.5 0 0], [1 1 0], [1 .5 0]);  %blue to yellow

            case  'lr'
                spm_orthviews_change_colormap(colors2, colors1);  %blue to white to yellow

            case 'pt|a'
                spm_orthviews_change_colormap(colors2, colors1);

        end

    case 'lr surface'

        poscm = colormap_tor([.2 .2 .4], [1 1 0], [.9 .6 .1]);  %slate to orange to yellow
        negcm = colormap_tor([0 0 1], [0 .3 1]);  % light blue to dark blue (shouldn't be needed!)

        for i = 1:bayes_model.nclasses

            cl{i} = mask2clusters(bayes_model.image_output.likerationame{i});

            fprintf('Displayed: regions with positive likelihood ratio for each task.\n');

            % Left medial and lateral surface
            % ----------------------------------------------------------
            create_figure('Brain Surface'); scn_export_papersetup(800);

            cluster_surf(cl{i}, 4, 'heatmap', 'colormaps', poscm, negcm, 'hires left');
            sh = cluster_surf(cl{i}, 4, 'heatmap', 'colormaps', poscm, negcm, 'brainstem');
            saveas(gcf, [bayes_model.image_output.likerationame{i}(1:end-4) '_left_medial'], 'png');

            view(270,0); lightRestoreSingle;
            saveas(gcf, [bayes_model.image_output.likerationame{i}(1:end-4) '_left_lateral'], 'png');

            % Right medial and lateral surface
            % ----------------------------------------------------------
            create_figure('Brain Surface'); cluster_surf(cl{i}, 4, 'heatmap', 'colormaps', poscm, negcm, 'hires right');
            sh = cluster_surf(cl{i}, 4, 'heatmap', 'colormaps', poscm, negcm, 'brainstem');
            scn_export_papersetup(800); saveas(gcf, [bayes_model.image_output.likerationame{i}(1:end-4) '_right_lateral'], 'png');

            view(270,0); lightRestoreSingle;
            saveas(gcf, [bayes_model.image_output.likerationame{i}(1:end-4) '_right_medial'], 'png');

            % Limbic
            % ---------------------------------------------------------
            create_figure('Brain Surface'); cluster_surf(cl{i}, 4, 'heatmap', 'colormaps', poscm, negcm, 'limbic');
            saveas(gcf, [bayes_model.image_output.likerationame{i}(1:end-4) '_limbic'], 'png');
            saveas(gcf, [bayes_model.image_output.likerationame{i}(1:end-4) '_limbic'], 'fig');
        end

    case 'map plot'

        % MAP plot

        colors = {'b' 'r' 'k' 'g' 'm'};
        create_figure('MAP Plot across observations');
        for i = 1:bayes_model.nclasses

            trueclassi = find(bayes_model.Xi(:,i) == 1);
            myclassmap = bayes_model.apparent.map(trueclassi, i);

            plot(bayes_model.apparent.map(:, i), 'color', colors{i});
            plot(trueclassi, bayes_model.apparent.map(find(bayes_model.Xi(:,i) == 1), i), 'o', 'Color', colors{i},'MarkerFaceColor',colors{i});

            incorrect_obs = ~(myclassmap == max(bayes_model.apparent.map(trueclassi, :), [], 2));
            incorrect_obs_vec = zeros(bayes_model.nobs, 1);
            incorrect_obs_vec(trueclassi(incorrect_obs)) = 1;

            plot(find(incorrect_obs_vec), myclassmap(incorrect_obs), 'kx', 'MarkerSize', 12, 'LineWidth', 2);
        end
        xlabel('Observations'); ylabel('log(MAP) (up to scaling constant)');

    case 'class plot'

        pr = bayes_model.priors;
        fr = [linspace(1, 0, 10)' linspace(0, 1, 10)'];   % frequencies of guesses, from always A to always B

        xfr = linspace(1, 0, 10);

        %hr_byclass = fr .* repmat(pr, size(fr, 1), 1)
        colors = {'b' 'r' 'k' 'g' 'm'};

        hr = fr * pr';

        create_figure('Summary Line Plot');
        %plot(xfr, hr_byclass);
        plot(xfr, hr, 'k', 'LineWidth', 2);

        phighestfreq = sum(bayes_model.xval.class_est == wh) ./ length(bayes_model.xval.class_est);  % p chosen for highest-frequency class
        plot(phighestfreq, bayes_model.xval.prop_correct, 'ko', 'LineWidth', 2, 'MarkerFaceColor', [.5 .5 .5], 'MarkerSize', 12);

        xx = repmat(phighestfreq, 2, bayes_model.nclasses);
        yy = [bayes_model.xval.prop_correct_by_class'; bayes_model.xval.prop_correct_by_class'];

        for i = 1:size(yy, 2)
            plot(xx(:,i), yy(:,i), 'o', 'Color', colors{i}, 'LineWidth', 2, 'MarkerFaceColor', colors{i}, 'MarkerSize', 12);
        end

        % Standard error of hit rate
        % var(hits for each category) = p*(1-p)*N for that category
        % where p is the prior p(correct) for that category, and N is the number of
        % choices in that category given the frequency of sampling that category.

        se_hr = sqrt( hr .* (1 - hr) ./ bayes_model.nobs );
        ci95_hr = 1.96 * se_hr;

        plot(xfr, hr + ci95_hr, 'k--', 'LineWidth', 1);
        plot(xfr, hr - ci95_hr, 'k--', 'LineWidth', 1);
        fill_around_line(hr, ci95_hr, 'k', xfr);

        ylabel('Correct classification rate');
        xlabel('Prop. choices from highest class');

        % N = fr .* bayes_model.nobs;     % for each of k categories, in a row

        % pq = repmat(pr .* (1 - pr), size(fr,1), 1);
        % varsum = sum(pq .* N, 2)
        %
        %
        % se_hr = sqrt( sum(repmat(pr .* (1 - pr), size(fr,1), 1) ./ (bayes_model.nobs .* fr), 2) )
        % se_hr(isinf(se_hr)) = 0;

    otherwise
        error('Unknown display option.')
end

end