function [pos,picon,picoff,janelas,messages]=twave3D(w,timenew,i,freq,S,QRSoff,samp,rrmed,messages)
%
%[pos,picon,picoff,janelas]=twave3D(w,timenew,i,freq,S,QRSoff,samp,rrmed)
%
% multilead T wave delineation
%
%Input Parameters:
%   w: matrix with WT scales 1 to 5
%   timenew:  QRS times in inedexes refering the interval included in the current excerpt (borders excluded)
%   i: beat number
%   freq: sampling frequency (heasig.freq)
%   S: lattest S wave position found in any lead
%   QRSoff: lattest QRS end position found in any lead
%   samp: samples included in the current excerpt (borders excluded)
%   rrmed: Exponentially averaged RR
%
%Output Parameters:
%   pos: fiducial marks structure (position)
%   picon: position of the first relevant modulos maximum in the wavelet
%   picoff: position of the last relevant modulos maximum in the wavelet
%   janelas: T wave seach windows
%
% Rute Almeida
% Last update: Rute Almeida  05AGO2011
%
% Designed for MATLAB Version R12; tested with MATLAB Version R13
%

%%%%%% Constants and Thresholds  !!!!!!!!!!!!!!!!!!!!!!!!!
if ~isfield(messages.setup.wavedet,'umbraldetT')
    messages.setup.wavedet.umbraldetT = 0.25;  % We use umbraldet*sqrt(mean(w(time(i):time(i+1),4).^2))
end
if ~isfield(messages.setup.wavedet,'umbralsig')
    messages.setup.wavedet.umbralsig  =  1/8;
end
if ~isfield(messages.setup.wavedet,'Kton')
    messages.setup.wavedet.Kton = 4;      % 2 4!
end
if ~isfield(messages.setup.wavedet,'inivent_tol')
    messages.setup.wavedet.inivent_tol  =  0.1;
end
if ~isfield(messages.setup.wavedet,'inivent_tol_S')
    messages.setup.wavedet.inivent_tol_S=0.05; % sec
end
if ~isfield(messages.setup.wavedet,'finvent_tol')
    messages.setup.wavedet.finvent_tol=0.240;% sec
end
if ~isfield(messages.setup.wavedet,'finvent_max')
    messages.setup.wavedet.finvent_max=0.6;% sec
end
if ~isfield(messages.setup.wavedet,'min_vent')
    messages.setup.wavedet.min_vent=0.1;%sec
end
if ~isfield(messages.setup.wavedet,'Tmax_Tmin_time_min')
    messages.setup.wavedet.Tmax_Tmin_time_min=0.15;%sec
end
if ~isfield(messages.setup.wavedet,'Tmax_Tmin_bifasic')
    messages.setup.wavedet.Tmax_Tmin_bifasic=2.5;
end

Kton=messages.setup.wavedet.Kton;
Ktoff=messages.setup.wavedet.Kton;
messages.warnings=[messages.warnings {'Recall that in twave3D ktoff=kton.'}];
umbraldetT = messages.setup.wavedet.umbraldetT;
umbralsig = messages.setup.wavedet.umbralsig;
inivent_tol= messages.setup.wavedet.inivent_tol;
inivent_tol_S=messages.setup.wavedet.inivent_tol_S;
finvent_tol=messages.setup.wavedet.finvent_tol;
finvent_max= messages.setup.wavedet.finvent_max;
min_vent=messages.setup.wavedet.min_vent;
Tmax_Tmin_time_min= messages.setup.wavedet.Tmax_Tmin_time_min;%sec
Tmax_Tmin_bifasic=messages.setup.wavedet.Tmax_Tmin_bifasic;% extra criteria
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
janelas=[];%%31May05
% Initialization of auxiliary variables
T = []; Tprima=[]; picon=[]; picoff=[]; Ton=[]; Toff=[]; tipoT=[];
% minapos = []; minppos=[]; maxapos=[]; maxppos=[]; mina=[]; minp=[]; maxa=[];
% maxp=[];
picon_keep=[];%multileadchange
picoff_keep=[];%multileadchange
lead_keep=[];%multileadchange

inivent = round(inivent_tol*freq);   % Begining of window
if ~isempty(S),              % If there is an S wave
    inivent = max(inivent, S-timenew(i)+round(inivent_tol_S*freq));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% changed 13/06/02 Rute
if rrmed <= freq,
    %if rrmed >= freq,
    finvent = round(finvent_max*freq);     % End of window
else
    finvent = round(rrmed*finvent_max);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  %% changed 13/06/02 Rute

% using timenew from diffferent leads ty ca dist less tahn 0.24 sec!!!
if i~=length(timenew),                 % For last beat in the segment
    finvent = min(finvent,timenew(i+1)-timenew(i)-round(finvent_tol*freq));
else
    finvent = min(finvent,timenew(i)-timenew(i-1)-round(finvent_tol*freq));
end

% We work at scale scale (in general).
if isempty(QRSoff),   % Should never happen, but...
    % begwin = inivent + timenew(i);
    begwin = inivent;
else
    %  begwin = max(inivent + timenew(i), QRSoff+1);
    begwin = max(inivent , QRSoff+1-timenew(i));
end
%endwin = min(finvent + timenew(i),length(w)); %% Rute 20/05/02
endwin = min(finvent ,length(w)); %% Rute 20/05/02
janelas=[janelas; i begwin endwin];%31May05
if ~isempty(begwin+1:endwin ) % 22.04.05

    % Positive maxima and negative minima in the window
    maxpos = begwin + modmax(w(begwin+1:endwin ),2,0,+1);
    minpos = begwin + modmax(w(begwin+1:endwin ),2,0,-1);
    [maxim ind] = max(w(maxpos ));   % The biggest of the positive
    maxpos = maxpos(ind);
    [minim ind] = min(w(minpos ));   % The biggest of the negative
    minpos = minpos(ind);

    if isempty(maxpos),               % If no local positive maximum
        % The maximum will be the first
        % or the last sample
        if (w(begwin )>=w(endwin )) && w(begwin )>0,
            maxpos = begwin; maxim = w(maxpos );
        elseif (w(endwin )>=w(begwin )) && w(endwin )>0,
            maxpos = endwin; maxim = w(maxpos );
        end
    end
    if isempty(minpos),               % if no local negative minimum
        % the minimum will be the first
        % or the last sample
        if (w(begwin )<=w(endwin )) && w(begwin )<0,
            minpos = begwin; minim = w(minpos );
        elseif (w(endwin )<=w(begwin )) && w(endwin )<0,
            minpos = endwin; minim = w(minpos );
        end
    end

    absmax = abs(maxim);
    absmin = abs(minim);

    if i<length(timenew),
        veficaz = sqrt(mean(w .^2)); % all w 03.Dec.04 interval was restricted before
    else                        % if last beat of the segment
        veficaz = sqrt(nanmean(w.^2)); % all w 03.Dec.04 interval was restricted before
    end

    hay_onda = ((absmax>umbraldetT*veficaz)|(absmin>umbraldetT*veficaz));

    % Rute 18/06/02
    if endwin-begwin<(min_vent*freq) % se a janela tem amplitude menor do que min_vent seg enato nao existe onda T
        hay_onda =0;
    end

    % Is there a wave?
    if hay_onda,
        if absmax >= absmin,    % the greatest modulus maximum is the maximum
            % Now we search the two minima nearest to maxpos, one before and one after
            minapos = max(modmax(w(begwin+1:maxpos-1 ),2,0,-1));
            minapos = begwin + minapos; % Position of the negative minimum before the maximum
            if isempty(minapos) && (maxpos ~= begwin) && (w(begwin )<0),
                minapos = begwin;% If no local minimum before the maximum, take the first sample
            end
            minppos = min(modmax(w(maxpos+1:endwin ),2,0,-1));
            minppos = maxpos + minppos; % Position of the positive maximum after the minimum
            if isempty(minppos) && (maxpos ~= endwin) && (w(endwin )<0),
                minppos = endwin;        % If no local minimum after the maximum, take the last sample
            end

            mina = abs(w(minapos ));     % Amplitude of minimum before maximum
            minp = abs(w(minppos ));     % Amplitude of minimum after maximum

            if (mina < umbralsig*absmax), % If mina is not big enough
                mina =[];                  % forget it
            elseif (maxpos-minapos>Tmax_Tmin_time_min*freq), %or if ther are more than 150 ms to maxpos
                mina = [];
            end
            if (minp < umbralsig*absmax), % If minp is not big enough
                minp =[];                  % forget it
            elseif (minppos-maxpos>Tmax_Tmin_time_min*freq), % and also if there are more than 150 ms to maxpos
                minp =[];
            end

            if ~isnan(mina)&~isnan(minp),      %#ok<AND2> %%% NUEVO JP
                if (mina >= minp)&&(minp < umbralsig*absmax*Tmax_Tmin_bifasic),
                    minp = [];
                elseif (minp> mina)&&(mina < umbralsig*absmax*Tmax_Tmin_bifasic),
                    mina = [];
                end
            end

            % Test which modulus maxima are significative and find zero crossings
            if isempty(mina),
                if isempty(minp),
                    tipoT = 2;      % only upwards T wave
                    if maxpos - minapos > 2,         % if not !!!!!!!!!!!
                        ind = zerocros(flipud(w(minapos:maxpos )));   %Scale 3 !!! % also in scale 4!!!!!!! 03.Dec.04
                        T = maxpos - ind +1;                           % Zero crossing = T wave position
                        picoff = maxpos;                               %wavelet  peak to detect offset
                    elseif isempty(minapos);  % If there were no minimum, there is no zero crossing
                        T = picant (w(begwin:maxpos ),maxpos);       % Take the minimum at scale 4
                        if ~isempty(T) %%%%%%%%%%%% 14/06/02 Rute
                            picoff = maxpos;
                        else
                            picoff = []; % if did not exist a peak in scale 4 there is no T
                        end %%%%%%%%%%%% 14/06/02 Rute
                    else %%%%%%%%%%%%%%%%%%%%% nao faz nada!!!!!!! % 12/06/02  Rute
                        messages.warnings=[messages.warnings {'unknown case: unable to classify T wave.'}];
                    end
                else      % minp exists (is significative) but mina not
                    tipoT = 0;  		%normal T wave
                    if minppos -maxpos >2,       % if not!!!!!!???
                        ind = zerocros(w(maxpos:minppos ));  %% 07/06/02 Rute % also in scale 4!!!!!!! 03.Dec.04
                        %                         if isempty(ind) %%%%%%%%%%%%%%Rute 03/09/02
                        %                             ind = zerocros(w(maxpos:minppos));
                        %                         end %%%%%%%%%%%%%%Rute 03/09/02
                        T = maxpos + ind -1;
                        picon = maxpos;		% For determining onset and offset
                        picoff = minppos;
                    else %%%%%%%%%%%%%%%%%%%%% nao faz nada!!!!!!! % 12/06/02  Rute
                        messages.warnings=[messages.warnings {'unknown case: unable to classify T wave.'}];
                    end
                end
            else
                if isempty(minp),   %  mina exists (is significative) but minp not
                    tipoT = 1;  	%inverted T wave
                    if maxpos -minapos >2,  % if not !!!!!!!!!
                        ind = zerocros(w(minapos:maxpos));  % wavelet zero crossing is T wave peak  %% 07/06/02 Rute % also in scale 4!!!!!!! 03.Dec.04
                        %                         if isempty(ind)%%%%%%%%%%%%%%Rute 03/09/02
                        %                             ind = zerocros(w(minapos:maxpos ));  % wavelet zero crossing is T wave peak  %% 03/09/02 Rute
                        %                         end %%%%%%%%%%%%%%Rute 03/09/02
                        T = minapos + ind -1;
                        picon = minapos;
                        picoff = maxpos;
                    else %%%%%%%%%%%%%%%%%%%%% nao faz nada!!!!!!! % 12/06/02  Rute
                        messages.warnings=[messages.warnings {'unknown case: unable to classify T wave.'}];
                    end
                else    % both mina and minp are significative.  Biphasic wave.
                    tipoT = 5;	% biphasic neg-pos T wave
                    if maxpos - minapos > 2,    %!!!!!!!!!!!!
                        ind = zerocros(flipud(w(minapos:maxpos ))); %% 07/06/02 Rute % also in scale 4!!!!!!! 03.Dec.04
                        %                         if isempty(ind) %%%%%%%%%%%%%%Rute 03/09/02
                        %                             ind = zerocros(flipud(w(minapos:maxpos ))); %% 03/09/02 Rute
                        %                         end%%%%%%%%%%%%%%Rute 03/09/02
                        T = maxpos - ind +1;
                        picon = minapos;
                    else %%%%%%%%%%%%%%%%%%%%% nao faz nada!!!!!!! % 12/06/02  Rute
                        messages.warnings=[messages.warnings {'unknown case: unable to classify T wave.'}];
                    end
                    if minppos - maxpos > 2,    %!!!!!!!!!!!!
                        ind = zerocros(flipud(w(maxpos:minppos ))); %% 07/06/02 Rute % also in scale 4!!!!!!! 03.Dec.04
                        %                         if isempty(ind)%%%%%%%%%%%%%%Rute 03/09/02
                        %                             ind = zerocros(flipud(w(maxpos:minppos ))); %% 03/09/02 Rute
                        %                         end %%%%%%%%%%%%%%Rute 03/09/02
                        %Tprima = maxpos + ind -1; %18.11.05
                        Tprima = minppos- ind +1;
                        picoff = minppos;
                    else %%%%%%%%%%%%%%%%%%%%% nao faz nada!!!!!!! % 12/06/02  Rute
                        messages.warnings=[messages.warnings {'unknown case: unable to classify T wave.'}];
                    end
                end
            end
        else        % If the greatest modulus maximum is the minimum
            % Search two maxima, one before and one after the minimum
            maxapos = max(modmax(w(begwin+1:minpos-1 ),2,0,1));
            maxapos = begwin + maxapos;
            if isempty(maxapos) && (minpos ~= begwin) && (w(begwin )>0),
                maxapos = begwin;
            end
            maxppos = min(modmax(w(minpos+1:endwin ),2,0,1));
            maxppos = minpos + maxppos;
            if isempty(maxppos) && (minpos ~= endwin) && (w(endwin )>0),
                maxppos = endwin;
            end
            maxa = abs(w(maxapos ));
            maxp = abs(w(maxppos )) ;                                   % See if they are significative
            if (maxa < umbralsig*absmin)
                maxa =[];
            elseif (minpos-maxapos>Tmax_Tmin_time_min*freq),
                maxa = [];
            end
            if (maxp < umbralsig*absmin),
                maxp =[];
            elseif (maxppos-minpos>Tmax_Tmin_time_min*freq),
                maxp = [];
            end
            if ~isnan(maxa)&~isnan(maxp),      %#ok<AND2> %%% NUEVO JP
                if (maxa >= maxp)&&(maxp < umbralsig*absmin*Tmax_Tmin_bifasic),
                    maxp = [];
                elseif (maxp> maxa)&&(maxa < umbralsig*absmin*Tmax_Tmin_bifasic),
                    maxa = [];
                end
            end
            % Test which modulus maxima are significative and find zero crossings
            if isempty(maxa),
                if isempty(maxp),
                    tipoT = 3;      % only downwards T wave
                    if minpos - maxapos > 2, %!!!!!!!!
                        ind = zerocros(flipud(w(maxapos:minpos )));   %Scale 3 % also in scale 4!!!!!!! 03.Dec.04
                        %                         if isempty(ind) %%%%%%%%%%%%%%Rute 03/09/02
                        %                             ind = zerocros(flipud(w(maxapos:minpos )));
                        %                         end %%%%%%%%%%%%%%Rute 03/09/02
                        T = minpos - ind +1;
                        picoff = minpos;
                    elseif isempty(maxapos);  % If there were no maximum, there is no zero crossing.
                        T = picant (w(begwin:minpos ),minpos);  % menimo en escala 4.
                        if ~isempty(T) %%%%%%%%%%%% 14/06/02 Rute
                            picoff = minpos;
                        else
                            picoff = []; % if did not exist a peak in scale 4 there is no T
                        end %%%%%%%%%%%% 14/06/02 Rute
                    else %%%%%%%%%%%%%%%%%%%%% nao faz nada!!!!!!! % 12/06/02  Rute
                        messages.warnings=[messages.warnings {'unknown case: unable to classify T wave.'}];
                    end
                else      % maxp is signficative, but not maxa
                    tipoT = 1;  %inverted T wave
                    if maxppos -minpos >2,  % !!!!!!
                        ind = zerocros(w(minpos:maxppos ));  %% 07/06/02 Rute % also in scale 4!!!!!!! 03.Dec.04
                        %                         if isempty(ind) %%%%%%%%%%%%%%Rute 03/09/02
                        %                             ind = zerocros(w(minpos:maxppos ));  %% 03/09/02 Rute
                        %                         end %%%%%%%%%%%%%%Rute 03/09/02
                        T = minpos + ind -1;
                        picon = minpos;		% For calculating onset and offset
                        picoff = maxppos;
                    else %%%%%%%%%%%%%%%%%%%%% nao faz nada!!!!!!! % 12/06/02  Rute
                        messages.warnings=[messages.warnings {'unknown case: unable to classify T wave.'}];
                    end
                end
            else
                if isempty(maxp),   % maxa is significative, but not maxp
                    tipoT = 0;       %normal T wave
                    if minpos -maxapos >2,
                        ind = zerocros(w(maxapos:minpos )); %% 07/06/02 Rute % also in scale 4!!!!!!! 03.Dec.04
                        %                         if isempty(ind) %%%%%%%%%%%%%%%%%%%% 03/09/02 Rute
                        %                             ind = zerocros(w(maxapos:minpos )); %% 03/09/02 Rute
                        %end %%%%%%%%%%%%%%%%%%%% 03/09/02 Rute
                        T = maxapos + ind -1;
                        picon = maxapos;
                        picoff = minpos;
                    else %%%%%%%%%%%%%%%%%%%%% nao faz nada!!!!!!! % 12/06/02  Rute
                        messages.warnings=[messages.warnings {'unknown case: unable to classify T wave.'}];
                    end
                else    % both maxa and maxp are significative.  Biphasic wave.
                    tipoT = 4;	% biphasic pos-neg T wave
                    if minpos - maxapos > 2,  %!!!!!!!!!!!
                        ind = zerocros(flipud(w(maxapos:minpos ))); %% 07/06/02 Rute % also in scale 4!!!!!!! 03.Dec.04
                        %                         if isempty(ind) %%%%%%%%%%%%%%%%%%%% 03/09/02 Rute
                        %                             ind = zerocros(flipud(w(maxapos:minpos ))); %% 03/09/02 Rute
                        %                         end %%%%%%%%%%%%%%%%%%%% 03/09/02 Rute
                        T = minpos - ind +1;
                        picon = maxapos;
                    else %%%%%%%%%%%%%%%%%%%%% nao faz nada!!!!!!! % 12/06/02  Rute
                        %                         [i tipoT]
                        disp('caso nao previsto; onda T nao marcada!')
                    end
                    if maxppos - minpos > 2,    %!!!!!!!!!!!!
                        ind = zerocros(flipud(w(minpos:maxppos ))); %% 07/06/02 Rute % also in scale 4!!!!!!! 03.Dec.04
                        %                         if isempty(ind) %%%%%%%%%%%%%%%%%%%% 03/09/02 Rute
                        %                             ind = zerocros(flipud(w(minapos:maxpos ))); %% 03/09/02 Rute
                        %                         end %%%%%%%%%%%%%%%%%%%% 03/09/02 Rute
                        %Tprima = minpos + ind -1; %18.11.05 DUVIDA
                        Tprima = maxppos- ind +1;
                        picoff = maxppos;
                    else %%%%%%%%%%%%%%%%%%%%% nao faz nada!!!!!!! % 12/06/02  Rute
                        messages.warnings=[messages.warnings {'unknown case: unable to classify T wave.'}];
                    end
                end
            end
        end

        % T wave onset and offset detection
        %if isempty(T), picon=[]; picoff=[]; end

        picon_keep=[picon_keep picon]; %multileadchange
        picoff_keep=[picoff_keep picoff];%multileadchange
        lead_keep=[lead_keep 4];%multileadchange

        if ~isempty(picon),
            Ton = searchon (picon, w(max(begwin,picon-0.12*freq):picon ), Kton);
        end
        if ~isempty(picoff),
            Toff=searchoff(picoff, w(picoff:min([size(w,1) picoff+0.12*freq ])  ) , Ktoff);
            if (Toff > endwin),
                Toff = endwin;
            end
        end
    else
        tipoT=9;
        messages.warnings=[messages.warnings {'unknown case: unable to find T wave in multilead approach using scale 4.'}];
    end       %% utilizar a escala 5 07/06/02 Rute %%%%%%%%%%%%%%%%%%%%%%%%%%%

end
% Filling the structure with positions
if isempty(Ton), Ton = NaN; end;
if isempty(Toff), Toff = NaN;  end;
if isempty(T), T=NaN; end;
if isnan(T),
    picon_keep=[picon_keep NaN]; %multileadchange
    picoff_keep=[picoff_keep NaN];%multileadchange
    lead_keep=[lead_keep NaN];%#ok<NASGU> %multileadchange
end
if isempty(Tprima), Tprima=NaN; end;
if isempty(tipoT), tipoT=NaN; end;
pos.Ton= Ton+samp(1)-1+timenew(i)-1;
pos.Toff= Toff+samp(1)-1+timenew(i)-1;
pos.T= T+samp(1)-1+timenew(i)-1;
pos.Tprima= Tprima+samp(1)-1+timenew(i)-1;
pos.Ttipo= tipoT;
% T = []; Tprima=[]; picon=[]; picoff=[]; Ton=[]; Toff=[]; tipoT=[];
% minapos = []; minppos=[]; maxapos=[]; maxppos=[]; mina=[]; minp=[]; maxa=[];
% maxp=[];
%end
% position.Ton(intervalo(1):intervalo(2)) = pos.Ton+samp(1)-1;
% position.Toff(intervalo(1):intervalo(2))= pos.Toff+samp(1)-1;
% position.T(intervalo(1):intervalo(2)) = pos.T+samp(1)-1;
% position.Tprima(intervalo(1):intervalo(2)) = pos.Tprima+samp(1)-1;
% position.Ttipo(intervalo(1):intervalo(2)) = pos.Ttipo;
% pos.Ton
% Ton = pos.Ton(i)
% Toff= pos.Toff(i)
% T= pos.T(i)
% Tprima = pos.Tprima(i)
% Ttipo= pos.Ttipo(i);
%timenew(i)
%picon_keep
picoff=picoff_keep+timenew(i)-1;
picon=picon_keep+timenew(i)-1;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% MRiLab auto generated file: DO NOT EDIT!     %
% Generated by MRiLab "DoWriteXML2m" Generator %
% MRiLab Version 1.3                           %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [rfAmp,rfPhase,rfFreq,rfCoil,GzAmp,GyAmp,GxAmp,ADC,Ext,uts,ts,flags]=PSD_GRE3DSpiral
global VCtl
global VVar
CV1=10e-3;
CV10=0;
CV11=0;
CV12=0;
CV13=0;
CV14=0;
CV2=20e-3;
CV3=0;
CV4=0;
CV5=0;
CV6=0;
CV7=0;
CV8=0;
CV9=0;
rfAmpAll=[];
rfPhaseAll=[];
rfFreqAll=[];
rfCoilAll=[];
GzAmpAll=[];
GyAmpAll=[];
GxAmpAll=[];
ADCAll=[];
ExtAll=[];
rfTimeAll=[];
GzTimeAll=[];
GyTimeAll=[];
GxTimeAll=[];
ADCTimeAll=[];
ExtTimeAll=[];
SEtAll=[];
uts=[];
ts=[];
flags=[];
if VCtl.PlotSeq == 1
rfAmp=[];
rfPhase=[];
rfFreq=[];
rfCoil=[];
GzAmp=[];
GyAmp=[];
GxAmp=[];
ADC=[];
Ext=[];
Freq=1;
Notes='regular TR section';
AttributeOpt={'on','off'};
Switch=AttributeOpt{1};
TREnd=Inf;
TRStart=1;
tE=VCtl.TR;
tS=0;
if VVar.TRCount<TRStart | VVar.TRCount>TREnd | mod(VVar.TRCount-TRStart,Freq)~=0 | strcmp(Switch,'off')
% do nothing
else
ts = [ts tS tE];
end
ts = [0 max(ts)-min(ts)];
return;
end
%==============Pulses 1==============
rfAmp=[];
rfPhase=[];
rfFreq=[];
rfCoil=[];
GzAmp=[];
GyAmp=[];
GxAmp=[];
ADC=[];
Ext=[];
rfTime=[];
GzTime=[];
GyTime=[];
GxTime=[];
ADCTime=[];
ExtTime=[];
Freq=1;
Notes='regular TR section';
AttributeOpt={'on','off'};
Switch=AttributeOpt{1};
TREnd=Inf;
TRStart=1;
tE=VCtl.TR;
tS=0;
if isempty(tS) | isempty(tE) | (tS>=tE)
error('SE setting is incorrect for Pulses 1!');
end
if VVar.TRCount<TRStart | VVar.TRCount>TREnd | mod(VVar.TRCount-TRStart,Freq)~=0 | strcmp(Switch,'off')
% do nothing
else
%--------------------
AttributeOpt={'on','off'};
p.AnchorTE=AttributeOpt{1};
AttributeOpt={'Non','Hamming','Hanning'};
p.Apod=AttributeOpt{1};
p.CoilID=1;
p.DupSpacing=0;
p.Duplicates=1;
p.FA=VCtl.FlipAng;
p.Notes='excitation rf';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.TBP=4;
p.dt=20e-6;
p.rfFreq=0;
p.rfPhase=0;
p.tEnd=1e-3;
p.tStart=0;
if strcmp(p.Switch,'on')
if strcmp(p.AnchorTE,'on')
switch VCtl.TEAnchor
case 'Start'
VCtl.TEAnchorTime=p.tStart;
case 'Middle'
VCtl.TEAnchorTime=(p.tStart+p.tEnd)/2;
case 'End'
VCtl.TEAnchorTime=p.tEnd;
end
end
[rfAmp1,rfPhase1,rfFreq1,rfCoil1,rfTime1]=rfSinc(p);
if strcmp(VCtl.MultiTransmit,'off')
if VCtl.MasterTxCoil==rfCoil1(1)
rfAmp=[rfAmp rfAmp1];
rfPhase=[rfPhase rfPhase1];
rfFreq=[rfFreq rfFreq1];
rfCoil=[rfCoil rfCoil1];
rfTime=[rfTime rfTime1];
end
else
rfAmp=[rfAmp rfAmp1];
rfPhase=[rfPhase rfPhase1];
rfFreq=[rfFreq rfFreq1];
rfCoil=[rfCoil rfCoil1];
rfTime=[rfTime rfTime1];
end
end
p=[];
%--------------------
p.DupSpacing=0;
p.Duplicates=1;
p.Gz1Sign=1;
p.Gz2Sign=0;
p.Notes='cartesian phase';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.t1End=CV2;
p.t1Start=CV1;
p.t2End=VCtl.TE+CV2;
p.t2Start=VCtl.TE+CV1;
p.tRamp=100e-6;
if strcmp(p.Switch,'on')
[GzAmp1,GzTime1]=GzCartesian(p);
GzAmp=[GzAmp GzAmp1];
GzTime=[GzTime GzTime1];
end
p=[];
%--------------------
p.Notes='spiral encoding';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.dt=0.004e-3;
p.tStart=VCtl.TE;
if strcmp(p.Switch,'on')
[GyAmp1,GyTime1]=GySpiral(p);
GyAmp=[GyAmp GyAmp1];
GyTime=[GyTime GyTime1];
end
p=[];
%--------------------
p.Notes='spiral encoding';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.dt=0.004e-3;
p.tStart=VCtl.TE;
if strcmp(p.Switch,'on')
[GxAmp1,GxTime1]=GxSpiral(p);
GxAmp=[GxAmp GxAmp1];
GxTime=[GxTime GxTime1];
end
p=[];
%--------------------
p.Notes='spiral readout';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.tStart=VCtl.TE;
if strcmp(p.Switch,'on')
[ADC1,ADCTime1]=ADCSpiral(p);
ADC=[ADC ADC1];
ADCTime=[ADCTime ADCTime1];
end
p=[];
%--------------------
p.DupSpacing=0;
p.Duplicates=1;
p.Ext=1;
p.Notes='reset K space location';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.tStart=1e-3;
if strcmp(p.Switch,'on')
[Ext1,ExtTime1]=ExtBit(p);
Ext=[Ext Ext1];
ExtTime=[ExtTime ExtTime1];
end
p=[];
%--------------------
p.DupSpacing=0;
p.Duplicates=1;
p.Ext=5;
p.Notes='calculate remaining scan time';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.tStart=0;
if strcmp(p.Switch,'on')
[Ext2,ExtTime2]=ExtBit(p);
Ext=[Ext Ext2];
ExtTime=[ExtTime ExtTime2];
end
p=[];
%--------------------
p.DupSpacing=0;
p.Duplicates=1;
p.Ext=9;
p.Notes='real time recon';
AttributeOpt={'on','off'};
p.Switch=AttributeOpt{1};
p.tStart=VCtl.TE+CV2;
if strcmp(p.Switch,'on')
[Ext3,ExtTime3]=ExtBit(p);
Ext=[Ext Ext3];
ExtTime=[ExtTime ExtTime3];
end
p=[];
%--------------------
SEt=[tS tE];
rfAmp(rfTime<0 | rfTime>SEt(2)-SEt(1)) = [];
rfPhase(rfTime<0 | rfTime>SEt(2)-SEt(1)) = [];
rfFreq(rfTime<0 | rfTime>SEt(2)-SEt(1)) = [];
rfCoil(rfTime<0 | rfTime>SEt(2)-SEt(1)) = [];
GzAmp(GzTime<0 | GzTime>SEt(2)-SEt(1)) = [];
GyAmp(GyTime<0 | GyTime>SEt(2)-SEt(1)) = [];
GxAmp(GxTime<0 | GxTime>SEt(2)-SEt(1)) = [];
ADC(ADCTime<0 | ADCTime>SEt(2)-SEt(1)) = [];
Ext(ExtTime<0 | ExtTime>SEt(2)-SEt(1)) = [];
rfTime(rfTime<0 | rfTime>SEt(2)-SEt(1)) = [];
GzTime(GzTime<0 | GzTime>SEt(2)-SEt(1)) = [];
GyTime(GyTime<0 | GyTime>SEt(2)-SEt(1)) = [];
GxTime(GxTime<0 | GxTime>SEt(2)-SEt(1)) = [];
ADCTime(ADCTime<0 | ADCTime>SEt(2)-SEt(1)) = [];
ExtTime(ExtTime<0 | ExtTime>SEt(2)-SEt(1)) = [];
rfAmp(abs(rfAmp)<eps) = 0;
rfTime = rfTime + SEt(1);
GzTime = GzTime + SEt(1);
GyTime = GyTime + SEt(1);
GxTime = GxTime + SEt(1);
ADCTime = ADCTime + SEt(1);
ExtTime = ExtTime + SEt(1);
rfAmpAll=[rfAmpAll rfAmp];
rfPhaseAll=[rfPhaseAll rfPhase];
rfFreqAll=[rfFreqAll rfFreq];
rfCoilAll=[rfCoilAll rfCoil];
GzAmpAll=[GzAmpAll GzAmp];
GyAmpAll=[GyAmpAll GyAmp];
GxAmpAll=[GxAmpAll GxAmp];
ADCAll=[ADCAll ADC];
ExtAll=[ExtAll Ext];
rfTimeAll=[rfTimeAll rfTime];
GzTimeAll=[GzTimeAll GzTime];
GyTimeAll=[GyTimeAll GyTime];
GxTimeAll=[GxTimeAll GxTime];
ADCTimeAll=[ADCTimeAll ADCTime];
ExtTimeAll=[ExtTimeAll ExtTime];
SEtAll=[SEtAll SEt];
end
%====================================
if isempty(rfTimeAll)
error('rf sequence line can not be empty! Master Tx coil element must be used.');
end
if isempty(GzTimeAll)
error('GzSS sequence line can not be empty!');
end
if isempty(GyTimeAll)
error('GyPE sequence line can not be empty!');
end
if isempty(GxTimeAll)
error('GxR sequence line can not be empty!');
end
if isempty(ADCTimeAll)
error('ADC sequence line can not be empty!');
end
if isempty(ExtTimeAll)
error('Ext sequence line can not be empty!');
end
SEflag=repmat([0 0 0 0 0 0]',[1 2]);
rfflag=repmat([1 0 0 0 0 0]',[1 max(size(rfTimeAll))]);
Gzflag=repmat([0 1 0 0 0 0]',[1 max(size(GzTimeAll))]);
Gyflag=repmat([0 0 1 0 0 0]',[1 max(size(GyTimeAll))]);
Gxflag=repmat([0 0 0 1 0 0]',[1 max(size(GxTimeAll))]);
ADCflag=repmat([0 0 0 0 1 0]',[1 max(size(ADCTimeAll))]);
Extflag=repmat([0 0 0 0 0 1]',[1 max(size(ExtTimeAll))]);
ts=[[min(SEtAll) max(SEtAll)] rfTimeAll GzTimeAll GyTimeAll GxTimeAll ADCTimeAll ExtTimeAll]-min(SEtAll);
flags=[SEflag rfflag Gzflag Gyflag Gxflag ADCflag Extflag];
[ts,ind]=sort(ts);
uts=unique(ts);
flags=flags(:,ind);
[rfTime,ind]=sort(rfTimeAll-min(SEtAll));
rfAmp=rfAmpAll(:,ind);
rfPhase=rfPhaseAll(:,ind);
rfFreq=rfFreqAll(:,ind);
rfCoil=rfCoilAll(:,ind);
[GzTime,ind]=sort(GzTimeAll-min(SEtAll));
GzAmp=GzAmpAll(:,ind);
[GyTime,ind]=sort(GyTimeAll-min(SEtAll));
GyAmp=GyAmpAll(:,ind);
[GxTime,ind]=sort(GxTimeAll-min(SEtAll));
GxAmp=GxAmpAll(:,ind);
[ADCTime,ind]=sort(ADCTimeAll-min(SEtAll));
ADC=ADCAll(:,ind);
[ExtTime,ind]=sort(ExtTimeAll-min(SEtAll));
Ext=ExtAll(:,ind);
rfAmp(1) = 0;
rfPhase(1) = 0;
rfFreq(1) = 0;
GzAmp(1) = 0;
GyAmp(1) = 0;
GxAmp(1) = 0;
ADC(1) = 0;
Ext(1) = 0;
rfAmp(end) = 0;
rfPhase(end) = 0;
rfFreq(end) = 0;
GzAmp(end) = 0;
GyAmp(end) = 0;
GxAmp(end) = 0;
ADC(end) = 0;
Ext(end) = 0;
end


%%%%%%%%%%%%%%%%%%%%%%%
% Compute features for a set of video files from datasets
%
close all;
clear;
warning('off','all');
addpath(genpath('./lib/iqm/FRIQUEE_feat_sel_release'));

%%
% parameters
algo_name = 'FRIQUEE_feat_sel';
data_name = 'KONVID_1K';

%% *You need to customize here*
if strcmp(data_name, 'TEST_VIDEOS')
    data_path = 'videos'; % dataset video path
elseif strcmp(data_name, 'KONVID_1K')
    data_path = '/media/ztu/Seagate-ztu-ugc/KONVID_1K/KoNViD_1k_videos';
elseif strcmp(data_name, 'LIVE_VQC')
    data_path = '/media/ztu/Seagate-ztu-ugc/LIVE_VQC/VideoDatabase';
elseif strcmp(data_name, 'YOUTUBE_UGC')
    data_path = '/media/ztu/Seagate-ztu-ugc/YT_UGC/original_videos';
elseif strcmp(data_name, 'LIVE_VQA')
    data_path = '/media/ztu/Seagate-ztu/LIVE_VQA/videos';
end

video_tmp = 'video_tmp';
if ~exist(video_tmp, 'dir'), mkdir(video_tmp); end
feat_path = '../features';
filelist_csv = fullfile(feat_path, [data_name,'_metadata.csv']);
filelist = readtable(filelist_csv);
num_videos = size(filelist,1);
out_path = './feat_sel_mats';
if ~exist(out_path, 'dir'), mkdir(out_path); end
out_feat_name = fullfile(out_path, [data_name,'_',algo_name,'_feats.mat']);
out_feat_frames_name = fullfile(out_path, [data_name,'_',algo_name,'_frames_feats.mat']);
feats_mat = [];
feats_mat_frames = cell( num_videos, 1 );
%===================================================

tic
for i = 1:num_videos

    % get video full path and decoded video name
	if strcmp(data_name, 'TEST_VIDEOS')
	    video_name = fullfile(data_path,  filelist.video_name{i});
	    yuv_name = fullfile(video_tmp, [filelist.video_name{i}, '.yuv']);
	elseif strcmp(data_name, 'KONVID_1K')
	    video_name = fullfile(data_path,  [num2str(filelist.flickr_id(i)),'.mp4']);
	    yuv_name = fullfile(video_tmp, [num2str(filelist.flickr_id(i)), '.yuv']);
	elseif strcmp(data_name, 'LIVE_VQC')
	    video_name = fullfile(data_path, filelist.File{i});
	    yuv_name = fullfile(video_tmp, [filelist.File{i}, '.yuv']);
	elseif strcmp(data_name, 'YOUTUBE_UGC')
	    video_name = fullfile(data_path, filelist.category{i},...
		[num2str(filelist.resolution(i)),'P'],[filelist.vid{i},'.mkv']);
	    yuv_name = fullfile(video_tmp, [filelist.vid{i}, '.yuv']);
	elseif strcmp(data_name, 'LIVE_VQA')
	    strs = strsplit(filelist.filename{i}, '_');
	    video_name = fullfile(data_path, [strs{1}(1:2), '_Folder'], filelist.filename{i});
	    yuv_name = video_name;
	end
	fprintf('\n---\nComputing features for %d-th sequence: %s\n', i, video_name);
    if ~strcmp(video_name, yuv_name)
    cmd = ['ffmpeg -loglevel error -y -i ', video_name, ' -pix_fmt yuv420p -vsync 0 ', yuv_name];
    system(cmd);
    end

    % get video meta data
    width = filelist.width(i);
    height = filelist.height(i);
    framerate = round(filelist.framerate(i));
    nb_frames = filelist.nb_frames(i);

    % read YUV frame (credit: Dae Yeol Lee)
    fp_input = fopen(yuv_name, 'r');
    uv_width = width/2;
    uv_height = height/2;
    feats_frames = [];

    tic
    for fr = 1:2:nb_frames-5
%         fr % frame No. printer

        try
            %% Start a file pointer
            fseek(fp_input,(fr-1)*1.5*width*height, 'bof'); % Frame read for 8 bit
            %% Y component
            %1) read y stream
            y_stream = fread(fp_input, width * height, 'uchar'); % for 8 bit
            % 2) reshape into a plane
            y_plane= reshape(y_stream, width, height).';
            %% U component
            %1) read u stream
            u_stream = fread(fp_input, uv_width * uv_height, 'uchar'); % for 8 bit
            % 2) reshape into a plane
            u_plane= reshape(u_stream, uv_width, uv_height).';
            %% V component
            %1) read v stream
            v_stream = fread(fp_input, uv_width * uv_height, 'uchar'); % for 8 bit
            % 2) reshape into a plane
            v_plane= reshape(v_stream, uv_width, uv_height).';
            % yuv2rgb
            u_plane = imresize(u_plane, size(y_plane), 'bicubic');
            v_plane = imresize(v_plane, size(y_plane), 'bicubic');
            rgb_plane = ycbcr2rgb(uint8(cat(3, y_plane, u_plane, v_plane)));

            % extract features
%             tic
            feat_tmp = extractFRIQUEEFeatures(rgb_plane);
%             toc
            feats_frames(end+1,:) = feat_tmp.friqueeALL;

        catch
            continue
        end

    end
    fclose(fp_input);
    delete(yuv_name)
    feats_mat_frames{i} = feats_frames;

        % compute brisque mean and consistency within each 1-sec chunk!!
    cons_feats = [];
    mean_feats = [];
    n_temp_vecs = length(feats_frames(:,1));
    half_blk_len = floor(framerate/2);

    fprintf('Pooling mean and consistency features\n');

    for j = 1:half_blk_len:n_temp_vecs - half_blk_len

        j_start = j;
        j_end = j + half_blk_len;

        % compute consistency features
        cons_feats = [cons_feats; std(feats_frames(j_start:j_end, :))];
        mean_feats = [mean_feats; mean(feats_frames(j_start:j_end, :))];
    end

    feats = [mean(mean_feats) ...
             mean(cons_feats) ];

    feats_mat(i,:) = feats;
    toc
end
toc
save(out_feat_name, 'feats_mat');
save(out_feat_frames_name, 'feats_mat_frames');


classdef OrientationUpdater < handle

    properties (GetAccess = public, SetAccess = protected)
        alpha
    end

    properties (Access = protected)
        eigenVectors
        eigenValues
        optimalIndexOrientation
    end

    methods (Access = public, Static)

        function obj = create(cParams)
           f = OrientationUpdaterFactory();
           obj = f.create(cParams);
        end

    end

    methods (Access = public)

        function compute(obj,cParams)
            obj.eigenVectors = cParams.pD;
            obj.eigenValues  = cParams.pS;
            obj.computeOptimalIndexOrientation();
            obj.computeOrientation();
        end

    end

    methods (Access = private)

        function computeOrientation(obj)
            pD  = obj.eigenVectors;
            ind = obj.optimalIndexOrientation;
            isFirstOptimal  = ind == 1;
            isSecondOptimal = ind == 2;
            dir(:,isFirstOptimal)  = squeeze(pD(:,1,isFirstOptimal));
            dir(:,isSecondOptimal) = squeeze(pD(:,2,isSecondOptimal));
            obj.alpha = dir;
        end

    end

    methods (Access = protected, Abstract)
        computeOptimalIndexOrientation(obj)
    end

end


function [GAH] = dicomrt_gahcal(VOI,gamma,gamma_xmesh,gamma_ymesh,gamma_zmesh,gahselect,xgrid)
% dicomrt_gahcal(VOI,gamma,gamma_xmesh,gamma_ymesh,gamma_zmesh,gahselect,xgrid)
%
% Calculate GAMMA-AREA-HISTOGRAMS for VOIs and cell_case_study.
% This function calculate GAHs for all the available VOIs or for a selected number of them.
%
% gamma contains the 3D gamma map
% gamma_xmesh,gamma_ymesh,gamma_zmesh are x-y-z coordinates of the center of the gamma-voxels
% VOIs is a cell array and contains all the Volumes of Interest
% gahselect is an OPTIONAL parameter which contains the number of the VOI to calculate GAHs for.
% xgrid is an OPTIONAL parameter that sets the resolution for GAH calculation (default = 30)
%
% GAHs are stored in a cell array with the following structure:
%
%  -------------------------------
%  | [GAH-name] | [3D gamma mask] |
%  |            -------------------
%  |            | [gah section 1] |
%  |            | [gah section 2] |
%  |            |     ...         |
%  |            | [gah section m] |
%  |            -------------------
%  |            | [info area 1]   |
%  |            | [info area 2]   |
%  |            |     ...         |
%  |            | [info area m]   |
%  |            -------------------
%  |            | Pixel area      |
%  --------------------------------
%
% [gamma data] is a 2 columns vector with following structure:
%
% -----------------
% | gamma | count |
% -----------------
% |       |       |
% |       |       |
% |       |       |
% |       |       |
% -----------------
%
% [info area] is a 2 columns vector with following structure:
%
% ---------------------------------
% | area sec | z location section |
% ---------------------------------
%
% Example:
%
% gah=dicomrt_gahcal(VOI,gamma,gamma_xmesh,gamma_ymesh,gamma_zmesh,4) returns
% the GAH for the VOI number 4 in VOI and the gamma distribution stored in the
% 3D matrix 'gamma'.
%
% See also dicomrt_gvhcal, dicomrt_gvhplot
%
% Copyright (C) 2002 Emiliano Spezi (emiliano.spezi@physics.org)

% Check number of argument and set-up some parameters and variables
error(nargchk(5,7,nargin))

% Check input parameters and set default parameter
list_variables=who;
match=0;

% Check voi
[VOI_temp]=dicomrt_checkinput(VOI);
VOI=dicomrt_varfilter(VOI_temp);

if nargin==5 & (exist('gahselect')==1 | exist('xgrid')==1)
    error('dicomrt_gahcal: Not enough input paramenters. Exit now!');
end

if exist('gahselect')~=1
    gahselect=[1:1:size(VOI,1)];
end

% Check consistency for z values
if gamma_zmesh(1)~=VOI{gahselect,2}{1}(1,3)
    error(['dicomrt_gahcal: GAMMA map was not calculated for the selected voi (',VOI{gahselect,1},'). Exit now!']);
end

% Define xgrid
if exist('xgrid')~=1
    xgrid=[0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0 1.2 1.4 1.6 1.8 2.0 ...
            3.0 4.0 5.0 6.0 7.0 8.0 9.0 10.0 Inf];
end

% Define cell array
GAH=cell(1,2);
GAH{1,1}=['GAH-',VOI{gahselect,1}];

% Notes on pixel area calculation
% Although slice thickness may vary the pixel area is supposed to be the
% same through all the volume.
pixelarea=(abs(gamma_xmesh(2)-gamma_xmesh(1)).*(abs(gamma_ymesh(2)-gamma_ymesh(1))));

% start calculating time
disp(['GAH calculation for VOI: ',VOI{gahselect,1}]);
% Initialise parameters
count=0; % count dimensions of gamma_section
count_empty=0; % count number of times gamma_section is not defined
% mask gamma matrix with current VOI
[mask_VOI,volume_VOI,mask4VOI]=dicomrt_mask(VOI_temp,gamma,gamma_xmesh,gamma_ymesh,gamma_zmesh,gahselect,'nan','n');
% slice by slice select contents of mask_VOI ~=nan and put it into a vector for GAH calculation

for kk=1:size(mask_VOI{2,1},3)
    xgrid_temp=xgrid;
    agrid=histc(reshape(mask_VOI{2,1}(:,:,kk),1,size(mask_VOI{2,1},1)*size(mask_VOI{2,1},2)),xgrid_temp);
    % get back voxels with dose<min(xgrid) cause histc does not
    % account for it
    temp_min=find(mask_VOI{2,1}(:,:,kk)<xgrid_temp(1) & mask_VOI{2,1}(:,:,kk)>0);
    if isempty(temp_min)~=1
        agrid(1)=agrid(1)+length(temp_min);
    end
    % Inf value will won't be plotted in the x grid. Therefore we
    % collect all the Inf values in the last slot of xgrid/vgrid
    agrid(end-1)=agrid(end-1)+agrid(end);
    agrid(end)=[];
    xgrid_temp(end)=[];
    temp2=cumsum(agrid);
    agrid=agrid.*pixelarea;
    info_area=[temp2(end)*pixelarea VOI{gahselect,2}{kk}(1,3)];
    gahdata=[xgrid_temp' agrid'];
    % store data in cell array
    GAH{1,2}{2,1}{kk}=gahdata;
    GAH{1,2}{3,1}{kk}=info_area;
    % plot frequency GAH
    if isempty(agrid)~=1
        figure;
        if isempty(inputname(2))==1
            set(gcf,'Name',['dicomrt_gahcal: ','fGAH for ',inputname(1)]);
        else
            set(gcf,'Name',['dicomrt_gahcal: ','fGAH for ',inputname(2)]);
        end
        agrid(end-1)=agrid(end-1)+agrid(end);
        agrid(end)=[];
        xgrid_temp(end)=[];
        bar(xgrid_temp,agrid);
        title([VOI{gahselect,1},' section: ',num2str(kk)],'Fontsize', 18,'Interpreter','none');
        xlabel('\gamma','Fontsize', 14);
        ylabel('Area [cm^2]','Fontsize', 14);
    else
        warning(['dicomrt_gahcal: GAH is null for VOI: ',GAH{k,1},' section: ',num2str(kk)]);
        gahdata=[0,0];
        GAH{1,2}{2,1}{kk}=gahdata;
    end
    % store other data in cell array
    GAH{1,2}{1,1}=mask_VOI{2,1};
    GAH{1,2}{4,1}=pixelarea;
end


function [gKern, gVarmeans, gVarcovars] = rbfard2VardistPsi0Gradient(rbfard2Kern, vardist, covGrad)

% RBFARD2VARDISTPSI0GRADIENT Description

% VARGPLVM
gKern = zeros(1,rbfard2Kern.nParams);
gKern(1) = covGrad*vardist.numData;

gVarmeans = zeros(1,prod(size(vardist.means)));
gVarcovars = zeros(1,prod(size(vardist.means)));


classdef PTKAirwayGrowing < PTKPlugin
    % PTKAirwayGrowing. Plugin for generating an artificial airway tree
    %
    %     This is a plugin for the Pulmonary Toolkit. Plugins can be run using
    %     the gui, or through the interfaces provided by the Pulmonary Toolkit.
    %     See PTKPlugin.m for more information on how to run plugins.
    %
    %     Plugins should not be run directly from your code.
    %
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    properties
        ButtonText = 'Volume filling<br> airways'
        ToolTip = 'Grow the airways into the lobes'
        Category = 'Airways'

        AllowResultsToBeCached = true
        AlwaysRunPlugin = false
        PluginType = 'ReplaceOverlay'
        HidePluginInDisplay = false
        FlattenPreviewImage = true
        PTKVersion = '1'
        ButtonWidth = 6
        ButtonHeight = 2
        GeneratePreview = false
        Visibility = 'Developer'
    end

    methods (Static)
        function results = RunPlugin(dataset, reporting)

            left_and_right_lungs = dataset.GetResult('PTKLeftAndRightLungs');
            lobes = dataset.GetResult('PTKLobes');

            airways_by_lobe = dataset.GetResult('PTKReallocateAirwaysLabelledByLobe');

            upper_right_start_segment = airways_by_lobe.StartBranches.RightUpper;
            middle_right_start_segment = airways_by_lobe.StartBranches.RightMid;
            lower_right_start_segment = airways_by_lobe.StartBranches.RightLower;
            upper_left_start_segment = airways_by_lobe.StartBranches.LeftUpper;
            lower_left_start_segment = airways_by_lobe.StartBranches.LeftLower;

            right_lung = left_and_right_lungs.Copy;
            right_lung.ChangeRawImage(left_and_right_lungs.RawImage == 1);

            left_lung = left_and_right_lungs.Copy;
            left_lung.ChangeRawImage(left_and_right_lungs.RawImage == 2);

            template = left_and_right_lungs.BlankCopy;
            approx_number_points = 31000;
            airway_generator = PTKAirwayGenerator(left_and_right_lungs, airways_by_lobe.StartBranches.Trachea, approx_number_points, reporting);

            reporting.ShowProgress('RIGHT - upper');
            reporting.UpdateProgressStage(0, 5);
            lobes_fill = lobes.Copy;
            lobes_fill.ChangeRawImage(lobes.RawImage == PTKColormapLabels.RightUpperLobe);
            lobes_fill.CropToFit;
            lobes_fill.AddBorder(2);
            airway_generator.GrowTree(lobes_fill, upper_right_start_segment, reporting)

            reporting.ShowProgress('RIGHT - middle');
            reporting.UpdateProgressStage(1, 5);
            lobes_fill = lobes.Copy;
            lobes_fill.ChangeRawImage(lobes.RawImage == PTKColormapLabels.RightMiddleLobe);
            lobes_fill.CropToFit;
            lobes_fill.AddBorder(2);
            airway_generator.GrowTree(lobes_fill, middle_right_start_segment, reporting)

            reporting.ShowProgress('RIGHT - lower');
            reporting.UpdateProgressStage(2, 5);
            lobes_fill = lobes.Copy;
            lobes_fill.ChangeRawImage(lobes.RawImage == PTKColormapLabels.RightLowerLobe);
            lobes_fill.CropToFit;
            lobes_fill.AddBorder(2);
            airway_generator.GrowTree(lobes_fill, lower_right_start_segment, reporting)

            reporting.ShowProgress('LEFT - upper');
            reporting.UpdateProgressStage(3, 5);
            lobes_fill = lobes.Copy;
            lobes_fill.ChangeRawImage(lobes.RawImage == PTKColormapLabels.LeftUpperLobe);
            lobes_fill.CropToFit;
            lobes_fill.AddBorder(2);
            airway_generator.GrowTree(lobes_fill, upper_left_start_segment, reporting)

            reporting.ShowProgress('LEFT - lower');
            reporting.UpdateProgressStage(4, 5);
            lobes_fill = lobes.Copy;
            lobes_fill.ChangeRawImage(lobes.RawImage == PTKColormapLabels.LeftLowerLobe);
            lobes_fill.CropToFit;
            lobes_fill.AddBorder(2);
            airway_generator.GrowTree(lobes_fill, lower_left_start_segment, reporting)

            % Compute radius values based on Strahler orders
            airway_generator.AirwayTree.ComputeStrahlerOrders;

%             airway_generator.AirwayTree.GenerateBranchParameters;

            % Add values of tissue density
            density = dataset.GetResult('PTKDensityAverage');
            airway_generator.AirwayTree.AddDensityValues(density.DensityAverage);

            results = [];
            results.Airways = airway_generator.AirwayTree;

            count_terminal_points = results.Airways.CountTerminalBranches;
            disp(['Number of terminal points: ' int2str(count_terminal_points)]);

            count_branches = results.Airways.CountBranches;
            disp(['Number of branches: ' int2str(count_branches)]);

            results.InitialImage = airway_generator.InitialApexImage;
            if isempty(results.InitialImage)
                results.InitialImage = zeros(template.ImageSize, 'uint8');
            end

        end

        function results = GenerateImageFromResults(airway_results, image_templates, reporting)
            template_image = image_templates.GetTemplateImage(PTKContext.LungROI);

            % Visualising the entire airway tree is Matlab is slow if the number
            % of branches is greater than a few thousand
            % PTKVisualiseAirwayGrowingTree(airway_results.Airways, reporting);

            results = template_image;
            results.ChangeRawImage(zeros(results.ImageSize, 'uint8'));
            results = PTKDrawAirwayGrowingBranchesAsSegmentation(airway_results.Airways, template_image, reporting);
        end

    end
end


function computeAllModelChoice_STS(pathWORK,fSetNameType,outcome,freedomMat,maxOrder,nBoot)
% -------------------------------------------------------------------------
% function computeAllModelChoice_STS(pathWORK,fSetNameType,outcome,freedomMat,maxOrder,nBoot)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes feature set selection for a given feature set type
% and for all experiments with different degrees of freedom. See ref. [1]
% for more details.
% -------------------------------------------------------------------------
% REFERENCE:
% [1] Vallieres, M. et al. (2015). A radiomics model from joint FDG-PET and
%     MRI texture features for the prediction of lung metastases in soft-tissue
%     sarcomas of the extremities. Physics in Medicine and Biology, 60(14),
%     5471-5496. doi:10.1088/0031-9155/60/14/5471
% -------------------------------------------------------------------------
% INPUTS:
% - pathWORK: Full path to the STS WORKSPACE directory.
% - fSetNameType: String specifying the name of the type of feature set
%                 (e.g., 'PET', 'SEPARATE', 'FUSED', etc.)
% - outcome: Column vector of size [nInst X 1] specifying the outcome status
%            (1 or 0) for all instances.
% - freedomMat:  Matrix of row vectors of 1's and 0's to specify the degree
%                of freedom on texture extraction parameters for all
%                experiments. For example, for an ith experiment where
%                extraction parameters 1, 2 and 4 in paramAll are allowed
%                to vary, use freedomMat(i,:) = [1,1,0,1].
% - maxOrder: Integer specifying the maximal model order to construct.
% - nBoot: Number of bootstrap samples to use.
% -------------------------------------------------------------------------
% OUTPUTS: Mutivariable models are saved in a folder named 'MODELS' in the
%          STS WORKSPACE.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: May 2015
%--------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
%
%    This package is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    This package is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this package.  If not, see <http://www.gnu.org/licenses/>.
% -------------------------------------------------------------------------

startpath = pwd;
cd([pathWORK,'/FSET']), pathFSET = pwd;
cd([pathWORK,'/MODELS']), pathModels = pwd;

nParamType = size(freedomMat,2);
nFreedom = size(freedomMat,1);
tStart = tic;
for i = 1:nFreedom
    cd(pathFSET)
    nameOpen=['FSET_',fSetNameType,'_'];
    for j = 1:nParamType
        nameOpen = [nameOpen,num2str(freedomMat(i,j))];
    end
    fSet = load(nameOpen); fSet = struct2cell(fSet); fSet = fSet{1};
    fprintf(['SELECTING FEATURES (MODEL ORDERS OF 1 to % u) FOR ',nameOpen,' ... '],maxOrder)
    tic
    [models] = featureSelection_STS(fSet.Data,outcome,maxOrder,nBoot,fSet.Info,'IABR');
    toc
    cd(pathModels)
    save(['MODELS',nameOpen(5:end)],'models')
end
time = toc(tStart);
fprintf('TOTAL TIME: %.2f seconds\n',time)

cd(startpath)
end


classdef PTKWavefront < handle
    % PTKWavefront. A data structure representing a segmented airway tree
    %
    %     PTKWavefront is used as part of the airway region growing process in
    %     PTKAirwayRegionGrowingWithExplosionControl.
    %
    %     A root PTKTreeSegment is returned by
    %     PTKAirwayRegionGrowingWithExplosionControl. From this you
    %     can extract and analyse the resulting airway tree.
    %
    %     PTKTreeSegment is used in the construction and storage of
    %     the airway trees. A PTKTreeSegment stores an individual
    %     segment of the centreline tree, with references to the parent and child
    %     PTKTreeSegments, so that it is possible to reconstruct the entire
    %     tree from a single segment.
    %
    %     The way the airway voxels are stored in each segment is as follows:
    %         The wavefront only exists during the region growing. It is a thick
    %         layer of voxels which exists at the end of each segment
    %         which is currently growing. If the wavefront forms more than one
    %         connected component, the segment is ended and new child segments
    %         formed from the wavefront components. When new voxels are added to
    %         the front of the wavefront, old voxels from the back of the
    %         wavefront are pushed out into the Pending voxels of the segment.
    %         Pending voxels are 'accepted' or 'rejected' according to whether
    %         the heuristics have determined an explosion has occurred.
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    properties (SetAccess = private)
        CurrentBranch = []
    end

    properties (Access = private)

        % The wavefront is a thick layer of voxels which is used to detect
        % and process bifurcations in the airway tree before these voxels are
        % added to the segement's list of pending indices
        WavefrontVoxelIndices

        % Additional voxels that were not originally part of the segment but
        % were added later after a morphological closing operation
        ClosedPoints

        NumberOfVoxelsSkipped = 0

        WavefrontSize
        PermittedVoxelSkips = 0

        MinimumDistanceBeforeBifurcatingMm
        MinimumChildDistanceBeforeBifurcatingMm = 5

        FirstSegmentWavefrontSizeMm = 10
        ChildWavefrontSizeMm = 5
        VoxelSizeMm
        MinCoords
        MaxCoords

        % Generations greater than this are automatically terminated
        MaximumNumberOfGenerations

        MinimumNumberOfPointsThresholdMm3 = 6

        ExplosionMultiplier = 7

    end

    methods
        function obj = PTKWavefront(segment_parent, min_distance_before_bifurcating_mm, voxel_size_mm, maximum_generations, explosion_multiplier)
            if nargin > 0
                obj.WavefrontVoxelIndices = int32([]);

                obj.MinimumDistanceBeforeBifurcatingMm = min_distance_before_bifurcating_mm;
                max_voxel_size_mm = max(voxel_size_mm);
                obj.VoxelSizeMm = voxel_size_mm;
                obj.MaximumNumberOfGenerations = maximum_generations;
                obj.ExplosionMultiplier = explosion_multiplier;

                voxel_volume = voxel_size_mm(1)*voxel_size_mm(2)*voxel_size_mm(3);
                min_number_of_points_threshold = max(3, round(obj.MinimumNumberOfPointsThresholdMm3/voxel_volume));

                if ~isempty(segment_parent)
                    obj.WavefrontSize = ceil(obj.ChildWavefrontSizeMm/max_voxel_size_mm);
                    obj.CurrentBranch = PTKTreeSegment(segment_parent, min_number_of_points_threshold, explosion_multiplier);
                else
                    obj.WavefrontSize = ceil(obj.FirstSegmentWavefrontSizeMm/max_voxel_size_mm);
                    obj.CurrentBranch = PTKTreeSegment([], min_number_of_points_threshold, explosion_multiplier);
                end
            end
        end

        % Returns the very front layer of voxels at the wavefront
        function frontmost_points = GetFrontmostWavefrontVoxels(obj)
            frontmost_points = obj.WavefrontVoxelIndices{end};
        end

        % Returns the wavefront for this segment, which includes voxels that may
        % be separated into child segments
        function wavefront_voxels = GetWavefrontVoxels(obj)
            wavefront_voxels = obj.ConcatenateVoxels(obj.WavefrontVoxelIndices);
        end

        % Add new voxels to this segment, and returns a list of all segments
        % that require further processing (including this one, and any child
        % segments which have been created as a result of bifurcations)
        function segments_to_do = AddNewVoxelsAndGetNewSegments(obj, indices_of_new_points, image_size, reporting)

            % Check that indices are unique
            if (numel(indices_of_new_points) ~= numel(unique(indices_of_new_points)))
                reporting.Error('PTKWavefront:Duplicates', 'Algorithm error - some points have been duplicated');
            end

            if any(ismember(indices_of_new_points, obj.CurrentBranch.GetAcceptedVoxels))
                reporting.Error('PTKWavefront:Duplicates', 'Algorithm error - some points have been duplicated');
            end

            % First we move voxels at the rear of the wavefront into the
            % PendingVoxels
            if ~isempty(obj.WavefrontVoxelIndices)
                while length(obj.WavefrontVoxelIndices) > obj.WavefrontSize
                    obj.MoveVoxelsFromRearOfWavefrontToPendingVoxels;
                end
            end

            % Next add the new points to the front of the wavefront
            obj.WavefrontVoxelIndices{end + 1} = indices_of_new_points;

            % If an explosion has been detected then do not continue
            if obj.CurrentBranch.MarkedExplosion
                obj.MoveAllWavefrontVoxelsToPendingVoxels;
%                 obj.DeleteSegmentIfNoAcceptedVoxels;
                segments_to_do = PTKWavefront.empty; % This segment has been terminated
                return
            end

            obj.AdjustMaxAndMinForVoxels(indices_of_new_points, image_size);

            % Do not allow the segment to bifurcate until it is above a minimum
            % length
            if ~obj.MinimumLengthPassed
                segments_to_do = obj; % This segment is to continue growing
                return
            end

            % Do not allow the segment to bifurcate until it is above a minimum size
            if ~obj.WavefrontIsMinimumSize
                segments_to_do = obj; % This segment is to continue growing
                return
            end

            % Determine whether to continue growing the current segment, or to
            % split it into a new set of child segments

            % Find connected components from the wavefront (which is several voxels thick)
            [offset, reduced_image, reduced_image_size] = MimImageCoordinateUtilities.GetMinimalImageForIndices(int32(obj.GetWavefrontVoxels)', image_size);
            wavefront_connected_components = bwconncomp(reduced_image, 26);
            number_of_components = wavefront_connected_components.NumObjects;

            % If there is only one component, it will be growing, so there is no
            % need to do any further component analysis
            if number_of_components == 1
                segments_to_do = obj;
                return;
            end

            segments_to_do = PTKWavefront.empty;
            growing_branches = [];
            points_by_branches = [];

            % Iterate over the components and separate the wavefront voxels of
            % the current segment into a new branch for each growing component
            for component_number = 1 : wavefront_connected_components.NumObjects

                % Get voxel list, and adjust the indices to match those for the full image
                indices_of_component_points = wavefront_connected_components.PixelIdxList{component_number};
                indices_of_component_points = MimImageCoordinateUtilities.OffsetIndices(int32(indices_of_component_points), offset, reduced_image_size, image_size);
                points_by_branches{component_number} = indices_of_component_points;

                still_growing = obj.IsThisComponentStillGrowing(indices_of_component_points);
                if (still_growing)
                    growing_branches(end + 1) = component_number;
                end
            end

            if length(growing_branches) < 1
                reporting.Error('PTKWavefront:NoGrowingBranches', 'Algorithm error - no growing branches');
            end

            if length(growing_branches) == 1
                segments_to_do = obj;
                return
            end

            if length(growing_branches) > 1

                if ~isempty(obj.MaximumNumberOfGenerations)
                    % If the maximum permitted number of generations is exceeded
                    % then terminate this segment. This will discard any
                    % remaining wavefront voxels and mark the segment as
                    % incomplete (unless it is marked as exploded)
                    if obj.CurrentBranch.GenerationNumber >= obj.MaximumNumberOfGenerations
                        obj.CurrentBranch.EarlyTerminateBranch;
                        return;
                    end
                end

                for index = 1 : length(growing_branches)
                    segments_to_do(end + 1) = obj.SpawnChildFromWavefrontVoxels(points_by_branches{growing_branches(index)});
                end

                % If the branch has divided, there may be some unaccepted points
                % left over
                obj.CompleteThisSegment;

                if isempty(obj.CurrentBranch.GetAcceptedVoxels)
                    reporting.ShowWarning('PTKWavefront:EmptyBranch', 'Algorithm error - no points in final branch');
                end

            end

        end

        function CompleteThisSegment(obj)
            obj.MoveAllWavefrontVoxelsToPendingVoxels;
            obj.CurrentBranch.CompleteThisSegment
        end
    end

    methods (Access = private)

        function MoveAllWavefrontVoxelsToPendingVoxels(obj)
            while ~isempty(obj.WavefrontVoxelIndices)
                obj.MoveVoxelsFromRearOfWavefrontToPendingVoxels;
            end
        end

        function MoveVoxelsFromRearOfWavefrontToPendingVoxels(obj)
            % The wavefront may be empty after voxels have been divided
            % amongst child branches
            if ~isempty(obj.WavefrontVoxelIndices{1})
                obj.CurrentBranch.AddPendingVoxels(obj.WavefrontVoxelIndices{1});
            end
            obj.WavefrontVoxelIndices(1) = [];
        end

        function AdjustMaxAndMinForVoxels(obj, voxel_indices, image_size)
            [x, y, z] = ind2sub(image_size, voxel_indices);
            mins = [min(x), min(y), min(z)];
            maxs = [max(x), max(y), max(z)];
            if isempty(obj.MinCoords)
                obj.MinCoords = mins;
                obj.MaxCoords = maxs;
            else
                obj.MinCoords = min(mins, obj.MinCoords);
                obj.MaxCoords = max(maxs, obj.MaxCoords);
            end
        end

        function is_minimum_size = WavefrontIsMinimumSize(obj)
            is_minimum_size = (length(obj.WavefrontVoxelIndices) >= obj.WavefrontSize);
        end

        function passed_minimum_lengths = MinimumLengthPassed(obj)
            lengths = obj.MaxCoords - obj.MinCoords;
            lengths = single(lengths).*obj.VoxelSizeMm;
            max_length = max(lengths);

            passed_minimum_lengths = max_length >= obj.MinimumDistanceBeforeBifurcatingMm;
        end

        function new_segment = SpawnChildFromWavefrontVoxels(obj, voxel_indices)
            wavefront_voxels = [];
            for index = 1 : length(obj.WavefrontVoxelIndices)
                wavefront_voxels{index} = intersect(int32(voxel_indices), obj.WavefrontVoxelIndices{index});
                obj.WavefrontVoxelIndices{index} = setxor(wavefront_voxels{index}, obj.WavefrontVoxelIndices{index});
            end
            new_segment = PTKWavefront(obj.CurrentBranch, obj.MinimumChildDistanceBeforeBifurcatingMm, obj.VoxelSizeMm, obj.MaximumNumberOfGenerations, obj.ExplosionMultiplier);
            new_segment.WavefrontVoxelIndices = wavefront_voxels;
        end

        function still_growing = IsThisComponentStillGrowing(obj, voxel_indices)
            wavefront_voxels_end = intersect(int32(voxel_indices), obj.WavefrontVoxelIndices{end});
            still_growing = ~isempty(wavefront_voxels_end);
        end

    end

    methods (Static, Access = private)
        function concatenated_voxels = ConcatenateVoxels(voxels)
            concatenated_voxels = [];
            number_layers = length(voxels);
            for index = 1 : number_layers
                next_voxels = voxels{index};
                if ~isempty(next_voxels)
                    concatenated_voxels = cat(1, concatenated_voxels, next_voxels);
                end
            end
        end
    end
end


%RRT Class for rapidly-exploring random tree navigation
%
% A concrete subclass of the abstract Navigation class that implements the rapidly
% exploring random tree (RRT) algorithm.  This is a kinodynamic planner
% that takes into account the motion constraints of the vehicle.
%
% Methods::
%  RRT          Constructor
%  plan         Compute the tree
%  query        Compute a path
%  plot         Display the tree
%  display      Display the parameters in human readable form
%  char         Convert to string
%
% Properties (read only)::
%  graph        A PGraph object describign the tree
%
% Example::
%        goal = [0,0,0];
%        start = [0,2,0];
%        veh = Bicycle('steermax', 1.2);
%        rrt = RRT(veh, 'goal', goal, 'range', 5);
%        rrt.plan()             % create navigation tree
%        rrt.query(start, goal)  % animate path from this start location
%
% References::
% - Randomized kinodynamic planning,
%   S. LaValle and J. Kuffner,
%   International Journal of Robotics Research vol. 20, pp. 378-400, May 2001.
% - Probabilistic roadmaps for path planning in high dimensional configuration spaces,
%   L. Kavraki, P. Svestka, J. Latombe, and M. Overmars,
%   IEEE Transactions on Robotics and Automation, vol. 12, pp. 566-580, Aug 1996.
% - Robotics, Vision & Control, Section 5.2.5,
%   P. Corke, Springer 2011.
%
% See also Navigation, PRM, DXform, Dstar, PGraph.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

% Peter Corke 8/2009.

%TODO
%   more info to the display method
%   distance metric choice or weightings
%   pass time and model options to the simulation

classdef RRT < Navigation

    properties
        npoints         % number of points to find
        graph           % graph Object representing random nodes

        simtime         % path simulation time

        xrange          % range of x coordinates
        yrange          % range of y coordinates

        speed           % speed of vehicle
        vehicle         % Vehicle class object describes kinematics

        revcost         % penalty for going backwards

        root            % coordinate of the root of the tree (3x1)
    end

    methods

        function rrt = RRT(vehicle, varargin)
        %RRT.RRT Create an RRT navigation object
        %
        % R = RRT.RRT(VEH, OPTIONS) is a rapidly exploring tree navigation
        % object  for a vehicle kinematic model given by a Vehicle subclass object VEH.
        %
        % R = RRT.RRT(VEH, MAP, OPTIONS) as above but for a region with obstacles
        % defined by the occupancy grid MAP.
        %
        % Options::
        % 'npoints',N    Number of nodes in the tree (default 500)
        % 'simtime',T    Interval over which to simulate kinematic model toward
        %                random point (default 0.5s)
        % 'goal',P       Goal position (1x2) or pose (1x3) in workspace
        % 'speed',S      Speed of vehicle [m/s] (default 1)
        % 'root',R       Configuration of tree root (3x1) (default [0,0,0])
        % 'revcost',C    Cost penalty for going backwards (default 1)
        % 'range',R      Specify rectangular bounds of robot's workspace:
        %                - R scalar; X: -R to +R, Y: -R to +R
        %                - R (1x2); X: -R(1) to +R(1), Y: -R(2) to +R(2)
        %                - R (1x4); X: R(1) to R(2), Y: R(3) to R(4)
        %
        % Other options are provided by the Navigation superclass.
        %
        % Notes::
        % - 'range' option is ignored if an occupacy grid is provided.
        %
        % Reference::
        % - Robotics, Vision & Control
        %   Peter Corke, Springer 2011.  p102.
        %
        % See also Vehicle, Bicycle, Unicycle.

            % invoke the superclass constructor, it handles some options
            rrt = rrt@Navigation(varargin{:});

            rrt.vehicle = vehicle;

            % handle the options not done by Navigation superclass
            opt.npoints = 500;
            opt.simtime = 0.5;
            opt.speed = vehicle.speedmax;
            opt.revcost = 1;
            opt.root = [0 0 0];

            [rrt,args] = tb_optparse(opt, varargin, rrt);

            if isempty(rrt.occgrid)
                opt = [];
                opt.range = 5;
                [opt,args] = tb_optparse(opt, args);

                % range can be specified as scalar, min/max, different min/max per
                % direction
                switch length(opt.range)
                    case 1
                        rrt.xrange = [-opt.range opt.range];
                        rrt.yrange = [-opt.range opt.range];
                    case 2
                        rrt.xrange = [-opt.range(1) opt.range(1)];
                        rrt.yrange = [-opt.range(2) opt.range(2)];
                    case 4
                        rrt.xrange = [opt.range(1) opt.range(2)];
                        rrt.yrange = [opt.range(3) opt.range(4)];
                    otherwise
                        error('bad range specified');
                end
            else
                rrt.xrange = [1 numcols(rrt.occgrid)];
                rrt.yrange = [1 numrows(rrt.occgrid)];
            end

            rrt.graph = PGraph(3, 'distance', 'SE2', ...
                'dweight', 2*pi/norm(sum([rrt.xrange; rrt.yrange])) );  % graph of points in SE(2)

        end

        function plan(rrt, varargin)
        %RRT.plan Create a rapidly exploring tree
        %
        % R.plan(OPTIONS) creates the tree roadmap by driving the vehicle
        % model toward random goal points.  The resulting graph is kept
        % within the object.
        %
        % Options::
        % 'goal',P        Goal pose (1x3)
        % 'ntrials',N     Number of path trials (default 50)
        % 'noprogress'    Don't show the progress bar
        % 'samples'       Show progress in a plot of the workspace
        %                 - '.' for each random point x_rand
        %                 - 'o' for the nearest point which is added to the tree
        %                 - red line for the best path
        %
        % Notes::
        % - At each iteration we need to find a vehicle path/control that moves it
        %   from a random point towards a point on the graph.  We sample ntrials of
        %   random steer angles and velocities and choose the one that gets us
        %   closest (computationally slow, since each path has to be integrated
        %   over time).

            opt.progress = true;
            opt.samples = false;
            opt.goal = [];
            opt.ntrials = 50;

            opt = tb_optparse(opt, varargin);

            if ~isempty(opt.goal)
                rrt.goal = opt.goal;
            end

            % build a graph over the free space
            rrt.message('create the graph');
            rrt.graph.clear();

            if rrt.verbose
                clf
                %idisp(1-rrt.occgrid, 'ynormal', 'nogui');
                hold on
            end

            % check root node sanity
            if isempty(rrt.root)
                error('no root node specified');
            end
            if ~isvec(rrt.root, 3)
                error('root must be 3-vector');
            end
            assert( ~rrt.isoccupied(rrt.root(1:2)), 'root node cell is occupied')

            % add the goal point as the first node
            vroot = rrt.graph.add_node(rrt.root);
            data.vel = 0;
            data.path = [];
            rrt.graph.setvdata(vroot, data);

            % graphics setup
            if opt.progress
                h = Navigation.progress_init('RRT planning...');
            end
            if opt.samples
                clf
                hold on
                xlabel('x'); ylabel('y');
            end

            npoints = 0;
            while npoints < rrt.npoints       % build the tree

                % Step 3
                % find random state x,y

                % pick a point not in obstacle
                while true
                    xy = rrt.randxy();  % get random coordinate (x,y)

                    if isempty(rrt.occgrid)
                        break
                    else
                        % we have an occgrid
                        xy = round( xy );  % round it to a grid cell coordinate

                        % test if lies in the obstacle map
                        try
                            if ~rrt.isoccupied(xy)
                                break;
                            end
                        catch
                            % index error, point must be off the map
                            continue;
                        end
                    end
                end
                theta = rrt.rand*2*pi;
                xrand = [xy, theta]';
                if opt.samples
                    plot(xy(1), xy(2), '.')
                end

                % Step 4
                % find the existing node closest in state space

                vnear = rrt.graph.closest(xrand);   % nearest vertex
                xnear = rrt.graph.coord(vnear);     % coord of nearest vertex
%                 if rrt.graph.distance_metric(xnear, xrand) < 0.25
%                     continue;
%                 end

                rrt.message('xrand (%g, %g) node %d', xy, vnear);

                % Step 5
                % figure how to drive the robot from xnear to xrand

                best = rrt.bestpath(xnear, xrand, opt.ntrials);

                xnew = best.path(:,best.k);
                if opt.samples
                    plot(xnew(1), xnew(2), 'o');
                    plot2(best.path', 'r');
                    drawnow
                end

%                 % ensure that the path is collision free
%                 if ~rrt.clearpath(y(:,1:2))
%                     disp('path collision');
%                     continue;
%                 end

                % Step 7,8
                % add xnew to the graph, with an edge from xnear
                vnew = rrt.graph.add_node(xnew);

                if rrt.graph.vdata(vnear).vel * best.vel < 0
                    % we changed direction, penalise that
                    cost = rrt.revcost;
                else
                    cost = 1;
                end
                rrt.graph.add_edge(vnear, vnew, cost);

                rrt.graph.setvdata(vnew, best);

                npoints = npoints + 1;
                if opt.progress
                    Navigation.progress(h, npoints / rrt.npoints);
                end

            end

            if opt.progress
                Navigation.progress_delete(h)
            end
            rrt.message('graph create done');
        end

        function p_ = query(rrt, xstart, xgoal)
        %RRT.query Find a path between two points
        %
        % X = R.path(START, GOAL) finds a path (Nx3) from pose START (1x3)
        % to pose GOAL (1x3).  The pose is expressed as [X,Y,THETA].
        %
        % R.path(START, GOAL) as above but plots the path in 3D, where the vertical
        % axis is vehicle heading angle.  The nodes are shown as circles and the
        % line segments are blue for forward motion and red for backward motion.
        %
        % Notes::
        % - The path starts at the vertex closest to the START state, and ends
        %   at the vertex closest to the GOAL state.  If the tree is sparse this
        %   might be a poor approximation to the desired start and end.
        %
        % See also RRT.plot.

            assert(rrt.graph.n > 0, 'RTB:RRT: there is no plan');
            rrt.checkquery(xstart, xgoal);

            g = rrt.graph;
            vstart = g.closest(xstart);
            vgoal = g.closest(xgoal);

            % find path through the graph using A* search
            [path,cost] = g.Astar(vstart, vgoal);

            fprintf('A* path cost %g\n', cost);

            % concatenate the vehicle motion segments
            cpath = [];
            for i = 1:length(path)
                p = path(i);
                data = g.vdata(p);
                if ~isempty(data)
                    if i >= length(path) || g.edgedir(p, path(i+1)) > 0
                        cpath = [cpath data.path];
                    else
                        cpath = [cpath data.path(:,end:-1:1)];
                    end
                end
            end

            if nargout == 0
                % plot the path
                clf; hold on

                plot2(g.coord(path)', 'o');     % plot the node coordinates

                for i = 1:length(path)
                    p = path(i);
                    b = g.vdata(p);            % get path data for segment

                    % draw segment with direction dependent color
                    if ~isempty(b)
                        % if the vertex has a path leading to it

                        if i >= length(path) || g.edgedir(p, path(i+1)) > 0
                            % positive edge
                            %  draw from prev vertex to end of path
                            seg = [g.coord(path(i-1)) b.path]';
                        else
                            % negative edge
                            %  draw reverse path to next next vertex
                            seg = [  b.path(:,end:-1:1)  g.coord(path(i+1))]';
                        end

                        if b.vel > 0
                            plot2(seg, 'b');
                        else
                            plot2(seg, 'r');
                        end
                    end
                end

                xlabel('x'); ylabel('y'); zlabel('\theta');
                grid
            else
                p_ = cpath';
            end
        end

        function plot(rrt, varargin)
        %RRT.plot Visualize navigation environment
        %
        % R.plot() displays the navigation tree in 3D, where the vertical axis is
        % vehicle heading angle.  If an occupancy grid was provided this is also
        % displayed.

            % display the occgrid background
            rrt.plot_bg(varargin{:});

            % display the graph
            %rrt.graph.plot('noedges', 'NodeSize', 3, 'NodeFaceColor', 'm', 'NodeEdgeColor', 'm', 'edges');

            rrt.graph.plot('noedges', 'nocomponentcolor', 'NodeSize', 3, 'NodeFaceColor', 'b', 'NodeEdgeColor', 'b', 'edges');
hold on

            % display the occgrid background
            rrt.plot_fg(varargin{:});
            axis([rrt.xrange rrt.yrange])
            xlabel('x'); ylabel('y'); zlabel('\theta');
            grid on; hold off
            view(0,90);
            axis equal
            rotate3d
        end

        % required by abstract superclass
        function next(rrt)
        end

        function s = char(rrt)
        %RRT.char  Convert to string
        %
        % R.char() is a string representing the state of the RRT
        % object in human-readable form.
        %

            % invoke the superclass char() method
            s = char@Navigation(rrt);

            % add RRT specific stuff information
            s = char(s, sprintf('  region: X %f : %f; Y %f : %f', rrt.xrange, rrt.yrange));
            s = char(s, sprintf('  sim time: %f', rrt.simtime));
            s = char(s, sprintf('  speed: %f', rrt.speed));
            s = char(s, sprintf(' Graph:'));
            s = char(s, char(rrt.graph) );
            if ~isempty(rrt.vehicle)
                s = char(s, char(rrt.vehicle) );
            end
        end

    end % methods

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%    P R I V A T E    M E T H O D S
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    methods (Access='protected')

        function best = bestpath(rrt, x0, xg, N)

            % initial and final state as column vectors
            x0 = x0(:); xg = xg(:);

            best.d = Inf;
            for i=1:N   % for multiple trials

                %choose random direction of motion and random steer angle
                if rand > 0.5
                    vel = rrt.speed;
                else
                    vel = -rrt.speed;
                end
                steer = (2*rrt.rand - 1) * rrt.vehicle.steermax;    % uniformly distributed

                % simulate motion of vehicle for this speed and steer angle which
                % results in a path
                x = rrt.vehicle.run2(rrt.simtime, x0, vel, steer)';

                %% find point on the path closest to xg
                % distance of all path points from goal
                d = colnorm( [bsxfun(@minus, x(1:2,:), xg(1:2)); angdiff(x(3,:), xg(3))] );
                % the closest one
                [dmin,k] = min(d);

                % is it the best so far?
                if dmin < best.d
                    % yes it is!  save it and the inputs that led to it
                    best.d = dmin;
                    best.path = x;
                    best.steer = steer;
                    best.vel = vel;
                    best.k = k;
                end
            end
        end

        % generate a random coordinate within the working region
        function xy = randxy(rrt)
            xy = rrt.rand(1,2) .* [rrt.xrange(2)-rrt.xrange(1) rrt.yrange(2)-rrt.yrange(1)] + ...
                [rrt.xrange(1) rrt.yrange(1)];
        end

        % test if a path is obstacle free
        function c = clearpath(rrt, xy)
            if isempty(rrt.occgrid)
                c = true;
                return;
            end

            xy = round(xy);
            try
                % test that all points along the path do not lie within an obstacle
                for pp=xy'
                    if rrt.isoccupied(pp) > 0
                        c = false;
                        return;
                    end
                end
                c = true;
            catch
                % come here if we index out of bounds
                c = false;
                return;
            end
        end

    end % private methods
end % class


laststream_range = 1+mod(laststream.smax:laststream.smax+size(laststream_chunk_clr,2)-1,laststream.buffer_len);
laststream.marker_pos(:,laststream_range) = 0;
laststream.buffer(:,laststream_range) = laststream_chunk_clr;
laststream.smax = laststream.smax + size(laststream_chunk_clr,2);


function res= experiment_run(exp,net)

net.cnn_id = exp.model;
switch net.cnn_mode
case 0 % matconvnet
    exp.opts.normalize = get_cnn_normalize(net.normalization) ;
    exp.opts.denormalize = get_cnn_denormalize(net.normalization) ;
case 1
    switch exp.model
    case {'alex','nin','gnet'} % alexnet, nin, inception
        d = load('data/ilsvrc_2012_mean.mat');
        mm = d.image_mean;
    case 'vgg16' % vgg
        mm = repmat(reshape([103.939, 116.779, 123.68],[1,1,3]),[256 256]);
    case 'alexWeb'
        mm = repmat(reshape([0.8309, 0.8310, 0.8328],[1,1,3]),[256 256]);
    end
    net.normalization=struct('imageSize',[net.im_sz net.im_sz 3],'averageImage',mm);
    exp.opts.normalize = @(x) U_prepare_image(x, mm,exp.model,-2);
    exp.opts.denormalize = @(x) U_prepare_image(x, mm,exp.model,-1);
end

net.task = exp.opts.task;
switch exp.opts.task
case {1,2}
    % 1: neuron inversion
    % 2: neuron inpainting
    res = invert_nn_dw(net, exp.opts.mask, exp.opts) ;
case 0
    % feature inversion
    % same as deep-goggle
    if ischar(exp.opts.feats)
        im = single(imread(exp.opts.feats));
        if size(im,3) == 1, im = cat(3,im,im,im) ; end
        exp.opts.feats = compute_features(net, exp.opts.normalize(im),0);
        %keyboard
    end
    % gradient-descent
    net.feats = exp.opts.feats;
    res = invert_nn_dw(net, exp.opts.feats, exp.opts) ;
end

end
function args = expandOpts(opts)
% -------------------------------------------------------------------------
args = horzcat(fieldnames(opts), struct2cell(opts))' ;
end


%% =======================================================================
%  Start Here Script
%  =======================================================================
%
%  This script guides the user through examples of both simulations and
%  Wi-Fi control models for the Parrot ARDrone.
%  Authors:
%       David Escobar Sanabria -> descobar@aem.umn.edu
%       Pieter J. Mosterman -> pieter.mosterman@mathworks.com
%  =======================================================================

%%
%  Cleaning workspace
bdclose all;
clear all;
bdclose all;
clc

%%
disp('This script guides the user through examples of both simulations and');
disp('Wi-Fi control Simulink models for the Parrot ARDrone ');
disp(' ');

disp('Swlect one of the following options:');
disp('    (1) Simulation');
disp('    (2) Wi-Fi control. The computer shoud be connected to the drone');

option = input('');

%%
switch option
    case 1

        disp('Swlect one of the following options for simulation:');
        disp('    (1) Baseline simulation: The ARDrone block with inputs and scopes to visualize outputs');
        disp('    (2) Hover: Vehicle is held at constant position');
        disp('    (3) Waypoint tracking: Vehicle tracks a list of waypoints');
        option2 = input(' ');

        switch option2
            case 1
                cd simulation;
                setupBaseModel;
            case 2
                cd simulation;
                setupHoverSim;
            case 3
                cd simulation;
                setupWPTrackingSim;
            otherwise
                disp('An incorrect option was selected')

        end

    case 2
        disp('Swlect one of the following options for Wi-Fi control:');
        disp('    (1) Hover: Vehicle is held at constant position');
        disp('    (2) Waypoint tracking: Vehicle tracks a list of waypoints');
        option2 = input(' ');

        switch option2
            case 1
                cd wifiControl;
                setupHover;
                % Building model using RTWT. Install rtwt if not installed
                % using 'rtwintgt -setup'
                rtwbuild('ARDroneHover');
            case 2
                cd wifiControl;
                setupWPTracking;
                % Building model using RTWT. Install rtwt if not installed
                % using 'rtwintgt -setup'
                rtwbuild('ARDroneWPTracking');
            otherwise
                disp('An incorrect option was selected')
        end

    otherwise
       disp('An incorrect option was selected')

end


function info = cnn_imagenet_evaluate(varargin)
% CNN_IMAGENET_EVALUATE   Evauate MatConvNet models on ImageNet

run(fullfile(fileparts(mfilename('fullpath')), ...
  '..', '..', 'matlab', 'vl_setupnn.m')) ;

opts.dataDir = fullfile('data', 'ILSVRC2012') ;
opts.expDir = fullfile('data', 'imagenet12-eval-vgg-f') ;
opts.modelPath = fullfile('data', 'models', 'imagenet-vgg-f.mat') ;
[opts, varargin] = vl_argparse(opts, varargin) ;

opts.imdbPath = fullfile(opts.expDir, 'imdb.mat');
opts.networkType = [] ;
opts.lite = false ;
opts.numFetchThreads = 12 ;
opts.train.batchSize = 128 ;
opts.train.numEpochs = 1 ;
opts.train.gpus = [] ;
opts.train.prefetch = true ;
opts.train.expDir = opts.expDir ;

opts = vl_argparse(opts, varargin) ;
display(opts);

% -------------------------------------------------------------------------
%                                                   Database initialization
% -------------------------------------------------------------------------

if exist(opts.imdbPath)
  imdb = load(opts.imdbPath) ;
else
  imdb = cnn_imagenet_setup_data('dataDir', opts.dataDir, 'lite', opts.lite) ;
  mkdir(opts.expDir) ;
  save(opts.imdbPath, '-struct', 'imdb') ;
end

% -------------------------------------------------------------------------
%                                                    Network initialization
% -------------------------------------------------------------------------

net = load(opts.modelPath) ;
if isfield(net, 'net') ;
  net = net.net ;
end
% Cannot use isa('dagnn.DagNN') because it is not an object yet
isDag = isfield(net, 'params') ;

if isDag
  opts.networkType = 'dagnn' ;
  net = dagnn.DagNN.loadobj(net) ;
  trainfn = @cnn_train_dag ;

  % Drop existing loss layers
  drop = arrayfun(@(x) isa(x.block,'dagnn.Loss'), net.layers) ;
  for n = {net.layers(drop).name}
    net.removeLayer(n) ;
  end

  % Extract raw predictions from softmax
  sftmx = arrayfun(@(x) isa(x.block,'dagnn.SoftMax'), net.layers) ;
  predVar = 'prediction' ;
  for n = {net.layers(sftmx).name}
    % check if output
    l = net.getLayerIndex(n) ;
    v = net.getVarIndex(net.layers(l).outputs{1}) ;
    if net.vars(v).fanout == 0
      % remove this layer and update prediction variable
      predVar = net.layers(l).inputs{1} ;
      net.removeLayer(n) ;
    end
  end

  % Add custom objective and loss layers on top of raw predictions
  net.addLayer('objective', dagnn.Loss('loss', 'softmaxlog'), ...
               {predVar,'label'}, 'objective') ;
  net.addLayer('top1err', dagnn.Loss('loss', 'classerror'), ...
               {predVar,'label'}, 'top1err') ;
  net.addLayer('top5err', dagnn.Loss('loss', 'topkerror', ...
                                     'opts', {'topK',5}), ...
               {predVar,'label'}, 'top5err') ;

  % Make sure that the input is called 'input'
  v = net.getVarIndex('data') ;
  if ~isnan(v)
    net.renameVar('data', 'input') ;
  end

  % Swtich to test mode
  net.mode = 'test' ;
else
  opts.networkType = 'simplenn' ;
  net = vl_simplenn_tidy(net) ;
  trainfn = @cnn_train ;
  net.layers{end}.type = 'softmaxloss' ; % softmax -> softmaxloss
end

% Synchronize label indexes used in IMDB with the ones used in NET
imdb = cnn_imagenet_sync_labels(imdb, net);

% Run evaluation
[net, info] = trainfn(net, imdb, getBatchFn(opts, net.meta), ...
                      opts.train, ...
                      'train', NaN, ...
                      'val', find(imdb.images.set==2)) ;

% -------------------------------------------------------------------------
function fn = getBatchFn(opts, meta)
% -------------------------------------------------------------------------
useGpu = numel(opts.train.gpus) > 0 ;
bopts = meta.normalization ;
bopts.numThreads = opts.numFetchThreads ;

% Most networks are trained by resizing images to 256 pixels and then
% cropping a slightly smaller subarea. Reproduce this effect (center
% crop) for a more accurate evaluation (it also avoids resizing the
% images if these have been pre-processed to be of this size, which
% accelerates everything).

bopts.border = 256 - meta.normalization.imageSize ;

switch lower(opts.networkType)
  case 'simplenn'
    fn = @(x,y) getSimpleNNBatch(bopts,x,y) ;
  case 'dagnn'
    fn = @(x,y) getDagNNBatch(bopts,useGpu,x,y) ;
end

% -------------------------------------------------------------------------
function [im,labels] = getSimpleNNBatch(opts, imdb, batch)
% -------------------------------------------------------------------------
images = strcat([imdb.imageDir filesep], imdb.images.name(batch)) ;
im = cnn_imagenet_get_batch(images, opts, ...
                            'prefetch', nargout == 0) ;
labels = imdb.images.label(batch) ;

% -------------------------------------------------------------------------
function inputs = getDagNNBatch(opts, useGpu, imdb, batch)
% -------------------------------------------------------------------------
images = strcat([imdb.imageDir filesep], imdb.images.name(batch)) ;
im = cnn_imagenet_get_batch(images, opts, ...
                            'prefetch', nargout == 0) ;
if nargout > 0
  if useGpu
    im = gpuArray(im) ;
  end
  inputs = {'input', im, 'label', imdb.images.label(batch)} ;
end


function subaperturedemo(varargin)
%SUBAPERTUREDEMO Read image, generate, and view subaperture-processed image using MATLAB GUIs.
%    subaperturedemo('PropertyName',PropertyValue,...)
%
% Calculates the subaperture-processed image of a complex image (selected
% through a MATLAB dialog box) using the properties specified. The AOI over
% which to compute the subaperture-processed image is selected through a MATLAB
% GUI. This version of subaperture processing does NOT require that the complete
% data fit into memory. It processes from any format handled by OPEN_READER and
% outputs to files in SIO format.
%
%       Property name     Description
%       frames            number of frames (default = 7)
%       apfraction        fraction of aperture for each subaperture
%                            (default = .25)
%       method            'normal' (default), 'fullpixel', or 'minimal'
%       platformdir       platform direction, 'right' (default) or 'left'
%       dim               dimension over which to split subaperture
%                            (default = 1)
%       fill              fill factor
%
% Output frames are stored in one frame per file, where the first frame has
% the filename OUTFILE, and each consecutive frame has the frame number
% appended onto the filename.
%
% Written by: Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

demo_core(@subaperturefile, varargin{:});

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////