function [fx,tx,pv,fv]=fxpefac(s,fs,tinc,m,pp)
%FXPEFAC PEFAC pitch tracker [FX,TT,PV,FV]=(S,FS,TINC,M,PP)
%
% Input:   s(ns)      Speech signal
%          fs         Sample frequency (Hz)
%          tinc       Time increment between frames (s) [0.01]
%                     or [start increment end]
%          m          mode
%                     'g' plot graph showing waveform and pitch
%                     'G' plot spectrogram with superimposed pitch using
%                         options pp.sopt [default: 'ilcwpf']
%                     'x' use external files for algorithm parameter
%                         initialization: fxpefac_g and fxpefac_w
%          pp         structure containing algorithm parameters
%
% Outputs: fx(nframe)     Estimated pitch (Hz)
%          tx(nframe)     Time at the centre of each frame (seconds).
%          pv(nframe)     Probability of the frame of being voiced
%          fv             structure containing feature vectors
%                           fv.vuvfea(nframe,2) = voiced/unvoiced GMM features

% References
%  [1]  S.Gonzalez and M. Brookes,
%       A pitch estimation filter robust to high levels of noise (PEFAC), Proc EUSIPCO,Aug 2011.

% Bugs/Suggestions
% (1) do long files in chunks
% (2) option of n-best DP

%	   Copyright (C) Sira Gonzalez and Mike Brookes 2011
%      Version: $Id: fxpefac.m 3601 2013-10-11 15:27:30Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

persistent w_u m_u v_u w_v m_v v_v dpwtdef
% initialize persistent variables
if ~numel(w_u)

    % voiced/unvoiced decision based on 2-element feature vector
    % (a) mean power of the frame's log-freq spectrum (normalized so its short-term average is LTASS)
    % (b) sum of the power in the first three peaks
    %===== VUV
    if nargin>3 && any(m=='x')
        fxpefac_g;     % read in GMM parameters
        fxpefac_w;     % read in Weights parameters
    else
        w_u=[0.1461799 0.3269458 0.2632178 0.02331986 0.06360947 0.1767271 ]';

        m_u=[13.38533 0.4199435 ;
             12.23505 0.1496836 ;
             12.76646 0.2581733 ;
             13.69822 0.6893078 ;
             9.804372 0.02786567 ;
             11.03848 0.07711229 ];

        v_u=reshape([0.4575519 0.002619074 0.002619074 0.01262138 ;
             0.7547719 0.008568089 0.008568089 0.001933864 ;
             0.5770533 0.003561592 0.003561592 0.00527957 ;
             0.3576287 0.01388739 0.01388739 0.04742106 ;
             0.9049906 0.01033191 0.01033191 0.0001887114 ;
             0.637969 0.009936445 0.009936445 0.0007082946 ]',[2 2 6]);

        w_v=[0.1391365 0.221577 0.2214025 0.1375109 0.1995124 0.08086066 ]';

        m_v=[15.36667 0.8961554 ;
             13.52718 0.4809653 ;
             13.95531 0.8901121 ;
             14.56318 0.6767258 ;
             14.59449 1.190709 ;
             13.11096 0.2861982 ];

        v_v=reshape([0.196497 -0.002605404 -0.002605404 0.05495016 ;
             0.6054919 0.007776652 0.007776652 0.01899244 ;
             0.5944617 0.0485788 0.0485788 0.03511229 ;
             0.3871268 0.0292966 0.0292966 0.02046839 ;
             0.3377683 0.02839657 0.02839657 0.04756354 ;
             1.00439 0.03595795 0.03595795 0.006737475 ]',[2 2 6]);
    end
    %===== PDP
    %     dfm = -0.4238; % df mean
    %     dfv = 3.8968; % df variance (although treated as std dev here)
    %     delta = 0.15;
    %     dflpso=[dfm 0.5/(log(10)*dfv^2) -log(2*delta/(dfv*sqrt(2*pi)))/log(10)]; % scale factor & offset for df pdf
    %     dpwtdef=[1.0000, 0.8250, 1.3064, 1.9863]; % default DP weights
    dpwtdef=[1.0000, 0.8250, 0.01868, 0.006773, 98.9, -0.4238]; % default DP weights
    %===== END

end
% Algorithm parameter defaults

p.fstep=5;              % frequency resolution of initial spectrogram (Hz)
p.fmax=4000;            % maximum frequency of initial spectrogram (Hz)
p.fres = 20;            % bandwidth of initial spectrogram (Hz)
p.fbanklo = 10;         % low frequency limit of log filterbank (Hz)
p.mpsmooth = 21;       % width of smoothing filter for mean power
% p.maxtranf = 1000;      % maximum value of tranf cost term
p.shortut = 7;          % max utterance length to average power of entire utterance
p.pefact = 1.8;         % shape factor in PEFAC filter
p.numopt = 3;           % number of possible frequencies per frame
p.flim = [60 400];      % range of feasible fundamental frequencies (Hz)
p.w = dpwtdef;          % DP weights
% p.rampk = 1.1;          % constant for relative-amplitude cost term
% p.rampcz = 100;         % relative amplitude cost for missing peak
p.tmf = 2;              % median frequency smoothing interval (s)
p.tinc = 0.01;          % default frame increment (s)
p.sopt = 'ilcwpf';      % spectrogram options

% update parameters from pp argument

if nargin>=5 && isstruct(pp)
    fnq=fieldnames(pp);
    for i=1:length(fnq)
        if isfield(p,fnq{i})
            p.(fnq{i})=pp.(fnq{i});
        end
    end
end

% Sort out input arguments
if nargin>=3  && numel(tinc)>0
    p.tinc = tinc;   % 0.01 s between consecutive time frames
end
if nargin<4
    m='';
end

% Spectrogram of the mixture
fmin = 0; fstep = p.fstep; fmax = p.fmax;
fres = p.fres;  % Frequency resolution (Hz)
[tx,f,MIX]=spgrambw(s,fs,fres,[fmin fstep fmax],[],p.tinc);
nframes=length(tx);
txinc=tx(2)-tx(1);  % actual frame increment
%  ==== we could combine spgrambw and filtbankm into a single call to spgrambw or use fft directly ====
% Log-frequency scale
[trans,cf]=filtbankm(length(f),2*length(f)-1,2*f(end),p.fbanklo,f(end),'usl');
O = MIX*trans'; % Original spectrum in Log-frequency scale

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Amplitude Compression

% Calculate alpha based on LTASS ratios
ltass = stdspectrum(6,'p',cf);
auxf = [cf(1),(cf(1:end-1)+cf(2:end))./2,cf(end)];
ltass = ltass.*diff(auxf);                  % weight by bin width

% estimated ltass
O = O.*repmat(diff(auxf),nframes,1);     % weight spectrum by bin width
O1 = O;

if tx(end)<p.shortut                        % if it is a short utterance
    eltass = mean(O,1);                     % mean power per each frequency band
    eltass = smooth(eltass,p.mpsmooth);     % smooth in log frequency
    eltass= eltass(:).';                    % force a row vector

    % Linear AC
    alpha = (ltass)./(eltass);
    alpha = alpha(:).';
    alpha = repmat(alpha,nframes,1);
    O = O.*alpha;                           % force O to have an average LTASS spectrum

    % ==== should perhaps exclude the silent portions ***
else                                        % long utterance

    tsmo = 3; % time smoothing over 3 sec
    stt = round(tsmo/txinc);
    eltass = timesm(O,stt);
    eltass = smooth(eltass,p.mpsmooth);     % filter in time and log frequency

    % Linear AC
    alpha = repmat(ltass,nframes,1)./(eltass);
    O = O.*alpha;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create the filter to detect the harmonics
ini = find(cf>3*cf(1));
sca = cf/cf(ini(1)); % bin frequencies start at approximately 0.33 with sca(ini(1))=1 exactly

% Middle
sca = sca(sca<10.5 & sca>0.5);  % restrict to 0.5 - 10.5 times fundamental

sca1 = sca;
filh = 1./(p.pefact-cos(2*pi*sca1));
filh = filh - sum(filh(1:end).*diff([sca1(1),(sca1(1:end-1)+sca1(2:end))./2,sca1(end)]))/sum(diff([sca1(1),(sca1(1:end-1)+sca1(2:end))./2,sca1(end)]));

posit = find(sca>=1);  % ==== this should just equal ini(1) ====
negat = find(sca<1);
numz = length(posit)-1-length(negat);
filh = filh./max(filh);
filh = [zeros(1,numz) filh]; % length is always odd with central value = 1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Filter the log-frequency scaled spectrogram
B = imfilter(O,filh);  % does a convolution with zero lag at centre of filh

% Feasible frequency range
numopt = p.numopt; % Number of possible fundamental frequencies per frame
flim = p.flim;
pfreq = find(cf>flim(1) & cf<flim(2)); % flim = permitted fx range = [60 400]
ff = zeros(nframes,numopt);
amp = zeros(nframes,numopt);
for i=1:nframes
    [pos,peak]=v_findpeaks(B(i,pfreq),[],5/(cf(pfreq(2))-cf(pfreq(1)))); % min separation = 5Hz @ fx=flim(1) (could pre-calculate) ====
    if numel(pos)
        [peak,ind]=sort(peak,'descend');
        pos = pos(ind);                     % indices of peaks in the B array
        posff = cf(pfreq(pos));             % frequencies of peaks
        fin = min(numopt,length(posff));
        ff(i,1:fin)=posff(1:fin);           % save both frequency and amplitudes
        amp(i,1:fin)=peak(1:fin);
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Probabilitly of the frame of being voiced

% voiced/unvoiced decision based on 2-element feature vector
% (a) mean power of the frame's log-freq spectrum (normalized so its short-term average is LTASS)
% (b) sum of the power in the first three peaks

pow = mean(O,2);

vuvfea = [log(pow) 1e-3*sum(amp,2)./(pow+1.75*1e5)];

% %%%%%%%%%%%%%%%%%%%%%

pru=gaussmixp(vuvfea,m_u,v_u,w_u);  % Probability of being unvoiced
prv=gaussmixp(vuvfea,m_v,v_v,w_v);  % Probability of being voiced

pv=(1+exp(pru-prv)).^(-1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Dynamic programming

% w(1): relative amp, voiced local cost
% w(2): median pitch deviation cost
% w(3): df cost weight
% w(4): max df cost
% w(5): relative amp cost for missing peaks (very high)
% w(6): df mean

w = p.w;

% Relative amplitude
camp = -amp./repmat(max(amp,[],2),1,numopt);  % relative amplitude used as cost
camp(amp==0)=w(5); % If no frequency found

% Time interval for the median frequency
tmf = p.tmf; % in sec
inmf = round(tmf/txinc);

%--------------------------------------------------------------------------
% FORWARDS
% Initialize values
cost = zeros(nframes,numopt);
prev = zeros(nframes,numopt);
medfx = zeros(nframes,1);
dffact=2/txinc;

% First time frame
% cost(1,:) = w(1)*ramp(1,:);
cost(1,:) = w(1)*camp(1,:);  % only one cost term for first frame
fpos = ff(1:min(inmf,end),1);
mf=median(fpos(pv(1:min(inmf,end))>0.6));   % calculate median frequency of first 2 seconds
if isnan(mf)
    mf=median(fpos(pv(1:min(inmf,end))>0.5));
    if isnan(mf)
        mf=median(fpos(pv(1:min(inmf,end))>0.4));
        if isnan(mf)
            mf=median(fpos(pv(1:min(inmf,end))>0.3)); % ==== clumsy way of ensuring that we take the best frames ====
            if isnan(mf)
                mf=0;
            end
        end
    end
end
medfx(1)=mf;

for i=2:nframes              % main dynamic programming loop
    if i>inmf
        fpos = ff(i-inmf:i,1);  % fpos is the highest peak in each frame
        mf=median(fpos(pv(1:inmf)>0.6));  % find median frequency over past 2 seconds
        if isnan(mf)
            mf=median(fpos(pv(1:inmf)>0.5));
            if isnan(mf)
                mf=median(fpos(pv(1:inmf)>0.4));
                if isnan(mf)
                    mf=median(fpos(pv(1:inmf)>0.3));% ==== clumsy way of ensuring that we take the best frames ====
                    if isnan(mf)
                        mf=0;
                    end
                end
            end
        end
    end
    medfx(i)=mf;
    % Frequency difference between candidates and cost
    df = dffact*(repmat(ff(i,:).',1,numopt) - repmat(ff(i-1,:),numopt,1))./(repmat(ff(i,:).',1,numopt) + repmat(ff(i-1,:),numopt,1));
    costdf=w(3)*min((df-w(6)).^2,w(4));

    % Cost related to the median pitch
    if mf==0                                   % this test was inverted in the original version
        costf = zeros(1,numopt);
    else
        costf = abs(ff(i,:) - mf)./mf;
    end
    [cost(i,:),prev(i,:)]=min(costdf + repmat(cost(i-1,:),numopt,1),[],2); % ==== should we allow the possibility of skipping frames ? ====
    cost(i,:)=cost(i,:)+w(2)*costf + w(1)*camp(i,:);  % add on costs that are independent of previous path

end

% Traceback

fx=zeros(nframes,1);
ax=zeros(nframes,1);
best = zeros(nframes,1);

nose=find(cost(end,:)==min(cost(end,:))); % ==== bad method (dangerous) ===
best(end)=nose(1);
fx(end)=ff(end,best(end));
ax(end)=amp(end,best(end));
for i=nframes:-1:2
    best(i-1)=prev(i,best(i));
    fx(i-1)=ff(i-1,best(i-1));
    ax(i-1)=amp(i-1,best(i-1));
end

if nargout>=4
    fv.vuvfea=vuvfea;  % voiced-unvoiced features
    fv.best=best;  % selected path
    fv.ff=ff;  % pitch candidates
    fv.amp=amp;  % pitch candidate amplitudes
    fv.medfx=medfx;  % median pitch
    fv.w=w;  % DP weights
    fv.dffact=dffact;  % df scale factor
    fv.hist = [log(mean(O,2)) sum(amp,2)./((mean(O,2)))];
end

if ~nargout || any(m=='g') || any(m=='G')
    nax=0;  % number of axes sets to link
    msk=pv>0.5; % find voiced frames as a mask
    fxg=fx;
    fxg(~msk)=NaN; % allow only good frames
    fxb=fx;
    fxb(msk)=NaN; % allow only bad frames
    if any(m=='G') || ~nargout && ~any(m=='g')
        clf;
        spgrambw(s,fs,p.sopt); % draw spectrogram with log axes
        hold on
        plot(tx,log10(fxg),'-b',tx,log10(fxb),'-r'); % fx track
        yy=get(gca,'ylim');
        plot(tx,yy(1)+yy*[-1;1]*(0.02+0.05*pv),'-k'); % P(V) track
        hold off
        nax=nax+1;
        axh(nax)=gca;
        if any(m=='g')
            figure;   % need a new figure if plotting two graphs
        end
    end
    if any(m=='g')
        ns=length(s);
        [tsr,ix]=sort([(1:ns)/fs 0.5*(tx(1:end-1)+tx(2:end))']); % intermingle speech and frame boundaries
        jx(ix)=1:length(ix); % create inverse index
        sp2fr=jx(1:ns)-(0:ns-1);  % speech sample to frame number
        spmsk=msk(sp2fr);   % speech sample voiced mask
        sg=s;
        sg(~spmsk)=NaN;   % good speech samples only
        sb=s;
        sb(spmsk)=NaN;    % bad speech samples only
        clf;
        subplot(5,1,1);
        plot(tx,pv,'-b',(1:ns)/fs,0.5*mod(cumsum(fx(sp2fr)/fs),1)-0.6,'-b');
        nax=nax+1;
        axh(nax)=gca;
        ylabel('\phi(t), P(V)');
        set(gca,'ylim',[-0.65 1.05]);
        subplot(5,1,2:3);
        plot((1:ns)/fs,sg,'-b',(1:ns)/fs,sb,'-r');
        nax=nax+1;
        axh(nax)=gca;
        subplot(5,1,4:5);
        plot(tx,fxg,'-b',tx,fxb,'-r');
        ylabel('Pitch (Hz)');
        %         semilogy(tx,fxg,'-b',tx,fxb,'-r');
        %         ylabel(['Pitch (' yticksi 'Hz)']);
        set(gca,'ylim',[min(fxg)-30 max(fxg)+30]);
        nax=nax+1;
        axh(nax)=gca;
    end
    if nax>1
        linkaxes(axh,'x');
    end
end

function y=smooth(x,n)
nx=size(x,2);
nf=size(x,1);
c=cumsum(x,2);
y=[c(:,1:2:n)./repmat(1:2:n,nf,1) (c(:,n+1:end)-c(:,1:end-n))/n (repmat(c(:,end),1,floor(n/2))-c(:,end-n+2:2:end-1))./repmat(n-2:-2:1,nf,1)];

function y=timesm(x,n)
if ~mod(n,2)
    n = n+1;
end
nx=size(x,2);
nf=size(x,1);
c=cumsum(x,1);
mid = round(n/2);
y=[c(mid:n,:)./repmat((mid:n).',1,nx); ...
    (c(n+1:end,:)-c(1:end-n,:))/n; ...
    (repmat(c(end,:),mid-1,1) - c(end-n+1:end-mid,:))./repmat((n-1:-1:mid).',1,nx)];


%TEST_bezierFunction.m
%
% This script tests the use of a bezier curve to represent a vector
% function, rather than an arbitrary space curve
%

order = 2;

p = rand(1,order+1);
w = 0.05 + 0.95*rand(1,order+1);

tSpan = [1,4];
t = linspace(tSpan(1),tSpan(2),100);
pGrid = linspace(tSpan(1),tSpan(2),order+1);

tic
x = rationalBezierCurve(p,w,t,tSpan);
toc

figure(1); clf;
plot(t,x); hold on;
scatter(pGrid,p,50*w,'filled');


function FEP_fluctuations
% This demonstration  uses an ensemble of particles with intrinsic (Lorentz
% attractor) dynamics and (Newtonian) short-range coupling.  The focus of
% this routine is to unpack the Bayesian perspective. We first simulate
% dynamics  to nonequilibrium steady-state, identify the Markov blanket and
% then examine the encoding of external states by internal states; in terms
% of their expected values.
%
% The crucial aspect of this implicit inference (and the basis of the free
% energy principle) is the existence of a conditional synchronisation
% manifold, when conditioning internal and external states on the Markov
% blanket. This provides the basis for a mapping between internal and
% external states that can be interpreted in terms of a probabilistic
% representation or inference.
%
% This Bayesian perspective is illustrated in terms of a mapping between
% the canonical modes of internal and external states (as approximated
% with a polynomial expansion). The canonical modes her are evaluated
% using an estimate of the conditional expectations  based upon the
% Euclidean proximity of Markov blanket states. The ensuing posterior over
% external states is than illustrated, in relation to the actual external
% states. We also  simulate event related potentials by identifying
% several points in time when the Markov blankets revisit the same
% neighbourhood. Finally, to illustrate the underlying dynamics, the
% Jacobians or coupling among internal and external states are
% presented; using different orders of coupling (i.e., degrees of
% separation)
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: FEP_fluctuations.m 7163 2017-09-04 09:12:50Z karl $

% default settings (GRAPHICS sets movies)
%--------------------------------------------------------------------------
rng('default')
GRAPHICS = 1;

% Demo of synchronization manifold using coupled Lorenz attractors
%==========================================================================
N    = 128;                         % number of (Lorenz) oscillators
T    = 2048;                        % number of time bins
dt   = 1/32;                        % time interval

% parameters
%--------------------------------------------------------------------------
P.k  = 1 - exp(-rand(1,N)*4);       % variations in temporal scale
P.d  = 1/8;                         % amplitude of random fluctuations

% states
%--------------------------------------------------------------------------
x.p  = randn(2,N)*4;                % microstates (position)
x.v  = zeros(2,N);                  % microstates (velocity)
x.q  = randn(3,N)/32;               % microstates (states)
u    = zeros(1,T);                  % exogenous fluctuations

% generate an dynamics from initial conditions
%==========================================================================
spm_figure('GetWin','Markov blanket');clf
if GRAPHICS
    subplot(2,2,1)
else
    subplot(2,1,1)
end
[Q,X,V,A,x] = spm_soup(x,u,P,T,dt,1);

% States
%--------------------------------------------------------------------------
% Q    - history of microstates (states)
% X    - history of microstates (position)
% V    - history of microstates (velocity)

for i = 1:size(X,3)
    S(:,:,i) = [Q(:,:,i);X(:,:,i);V(:,:,i)];
end

% Markov blanket - parents, children, and parents of children
%==========================================================================

% Adjacency matrix
%--------------------------------------------------------------------------
t     = (T - 256):T;                              % final time indices
L     = sparse(double(any(A(:,:,t),3)))';

% internal states (defined by principle eigenvector of Markov blanket)
%--------------------------------------------------------------------------
B     = double((L + L' + L'*L));
B     = B - diag(diag(B));
v     = spm_svd(B*B',1);
[v,j] = sort(abs(v(:,1)),'descend');

% get Markov blanket and divide into sensory and active states
%--------------------------------------------------------------------------
m     = j(1:8);                                   % internal cluster
mm    = sparse(m,1,1,N,1);                        % internal states
bb    = B*mm & (1 - mm);                          % Markov blanket
ee    = 1 - bb - mm;                              % external states
b     = find(bb);
e     = find(ee);
m     = find(mm);
s     = b(find( any(L(b,e),2)));
a     = b(find(~any(L(b,e),2)));

% adjacency matrix - with partition underneath (LL)
%--------------------------------------------------------------------------
k       = [e; s; a; m];
LL      = L;
LL(e,e) = LL(e,e) + 1/8;
LL(s,s) = LL(s,s) + 1/8;
LL(a,a) = LL(a,a) + 1/8;
LL(m,m) = LL(m,m) + 1/8;
LL      = LL(k,k);

% plot dynamics for the initial and subsequent time periods
%--------------------------------------------------------------------------
subplot(4,1,3)
r    = 1:512;
plot(r,squeeze(Q(1,e,r)),':c'), hold on
plot(r,squeeze(Q(1,m,r)),' b'), hold off
axis([r(1) r(end) -32 32])
xlabel('Time','FontSize',12)
title('Electrochemical dynamics','FontSize',16)

subplot(4,1,4)
r    = 1:T;
plot(r,squeeze(V(1,e,r)),':c'), hold on
plot(r,squeeze(V(1,m,r)),' b'), hold off
axis([r(1) r(end) -32 32])
xlabel('Time','FontSize',12)
title('Newtonian dymanics','FontSize',16)

% Markov blanket - self-assembly
%==========================================================================
subplot(2,2,1)
imagesc(1 - LL)
axis square
xlabel('Element','FontSize',12)
xlabel('Element','FontSize',12)
title('Adjacency matrix','FontSize',16)

% follow self-assembly
%--------------------------------------------------------------------------
clear M
for i = (T - 512):T

    % plot positions
    %----------------------------------------------------------------------
    subplot(2,2,2),set(gca,'color','w')

    px = ones(3,1)*X(1,:,i) + Q([1 2 3],:,i)/16;
    py = ones(3,1)*X(2,:,i) + Q([2 3 1],:,i)/16;
    plot(px,py,'.b','MarkerSize',8), hold on
    px = X(1,e,i); py = X(2,e,i);
    plot(px,py,'.c','MarkerSize',24)
    px = X(1,m,i); py = X(2,m,i);
    plot(px,py,'.b','MarkerSize',24)
    px = X(1,s,i); py = X(2,s,i);
    plot(px,py,'.m','MarkerSize',24)
    px = X(1,a,i); py = X(2,a,i);
    plot(px,py,'.r','MarkerSize',24)

    xlabel('Position','FontSize',12)
    ylabel('Position','FontSize',12)
    title('Markov Blanket','FontSize',16)
    axis([-1 1 -1 1]*8)
    axis square, hold off, drawnow

    % save
    %----------------------------------------------------------------------
    if i > (T - 128) && GRAPHICS
        M(i - T + 128) = getframe(gca);
    end

end

% set ButtonDownFcn
%--------------------------------------------------------------------------
if GRAPHICS
    h   = findobj(gca);
    set(h(1),'Userdata',{M,16})
    set(h(1),'ButtonDownFcn','spm_DEM_ButtonDownFcn')
    xlabel('Click for Movie','Color','r')
end

% illustrate the Bayesian perspective (predictability of external states)
%==========================================================================
spm_figure('GetWin','Bayesian perspective');clf

% establish a statistical dependency between internal (dynamic) states (XQ)
%--------------------------------------------------------------------------
T     = 512;                                   % length of timeseries
t     = size(X,3) - T - 2;
for i = 1:T
    Xe(i,:) = spm_vec(V(:,e,i + t));           % external states
    Xb(i,:) = spm_vec(S(:,[a;s],i + t));       % Markov blanket
    Xm(i,:) = spm_vec(Q(:,m,i + t));           % internal states
end
xe    = zeros(size(Xe));
xm    = zeros(size(Xm));
iC    = inv(cov(Xb));

% probabilistic proximity in the space of the Markov blanket
%--------------------------------------------------------------------------
for i = 1:T
    for j = 1:T
        r      = Xb(i,:) - Xb(j,:);
        w(i,j) = exp(-(r*iC*r')/128);
    end
end

% convert into proper probability distribution
%--------------------------------------------------------------------------
w = diag(sum(w,2))\w;

% mean
%--------------------------------------------------------------------------
for i = 1:T
    for j = 1:T
        xe(i,:) = xe(i,:) + w(i,j)*Xe(j,:);
        xm(i,:) = xm(i,:) + w(i,j)*Xm(j,:);
    end
end

% covariance (not used)
%--------------------------------------------------------------------------
% ce    = zeros(size(Xe,1),size(Xe,2),size(Xe,2));
% for i = 1:T
%     for j = 1:T
%         ce(i,:,:) = squeeze(ce(i,:,:)) + w(i,j)*(Xe(i,:) - xe(i,:))'*(Xe(j,:) - xe(j,:));
%     end
% end

% normalise and identify canonical eigenvariates
%--------------------------------------------------------------------------
xe    = spm_detrend(xe);
xm    = spm_detrend(xm);
CVA   = spm_cva(xe,xm);

% show results - canonical vectors over elements (mode M)
%--------------------------------------------------------------------------
subplot(3,2,1)

M     = 1;
Ve    = CVA.V(:,M);
Ve    = spm_unvec(Ve,V(:,e,1));
ve    = sum(Ve.^2);
ve    = ve/max(ve);
for k = 1:length(Ve)
    c = [0 1 1]*ve(k) + [1 1 1]*(1 - ve(k));
    plot(X(1,e(k),end),X(2,e(k),end),'.','MarkerSize',32,'Color',c), hold on
end

% overplot mode of motion
%--------------------------------------------------------------------------
quiver(X(1,e,end),X(2,e,end),Ve(1,:),Ve(2,:))

Vm    = CVA.W(:,M);
Vm    = spm_unvec(Vm,Q(:,m,1));
vm    = sum(Vm.^2);
vm    = vm/max(vm);
for k = 1:length(Vm)
    c = [0 0 1]*vm(k) + [1 1 1]*(1 - vm(k));
    plot(X(1,m(k),end),X(2,m(k),end),'.','MarkerSize',32,'Color',c), hold on
end
xlabel('Position', 'FontSize',12)
ylabel('Position','FontSize',12)
title('Canonical mode','FontSize',16)

% conditional synchronisation manifold (polynomial approximation)
%==========================================================================

% polynomial approximation
%--------------------------------------------------------------------------
xX    = xm*CVA.W(:,M);
XX    = [xX.^0 xX.^1 xX.^2 xX.^3 xX.^4 xX.^5];
bE    = pinv(XX)*Xe*CVA.V(:,M);
qE    = XX*bE;

% conditional expectation and variance
%--------------------------------------------------------------------------
% for i = 1:T
%     qC(i) = CVA.V(:,1)'*squeeze(ce(i,:,:))*CVA.V(:,1);
% end
qC    = ones(size(qE));
qC    = abs(var(Xe*CVA.V(:,1) - qE)*qC/mean(qC));

% show results - conditional synchronisation manifold
%--------------------------------------------------------------------------
subplot(3,2,2)
plot(CVA.w(:,1),Xe*CVA.V(:,1),'.c' ), hold on
plot(CVA.w(:,1),qE,'.b' ), hold off
xlabel('Internal mode', 'FontSize',12)
ylabel('External mode','FontSize',12)
title('Synchronisation manifold','FontSize',16), spm_axis tight

% show results - conditional distributions as a function of time
%--------------------------------------------------------------------------
subplot(3,1,2)
plot(Xe*CVA.V(:,1),'c' ), hold on
spm_plot_ci(qE',qC(:)'),  hold off
xlabel('Time', 'FontSize',12)
ylabel('External states','FontSize',12)
title('Inferred and real motion','FontSize',16), spm_axis tight

%  event related potentials
%==========================================================================

%  identify points of interest using the external canonical variate
%--------------------------------------------------------------------------
u     = CVA.v(:,1);
ue    = [];
for i = 1:8
    [d,j]   = max(u(33:end - 65));
    j       = j + 32;
    % j     = fix(rand*T);              % random times
    k       = (j - 32):(j + 64);        % perstimulus time (around j)
    u((j - 8):(j + 8)) = -Inf;          % eliminate from next max(u(:,1))
    ue(:,i) = Xe(k,:)*CVA.V(:,1);
    um(:,i) = Xm(k,:)*CVA.W(:,1);
    us(i)   = j;

end
j    = any(ue);
us   = us(j);
ue   = spm_detrend(ue(:,j));
um   = spm_detrend(um(:,j));

% plot points of interest on conditional density
%--------------------------------------------------------------------------
subplot(3,1,2)
uy    = get(gca,'Ylim'); hold on
for i = 1:length(us),plot([1 1]*us(i),uy,':'), end, hold off

%  show time locked (internal and external) fluctuations and their mean
%--------------------------------------------------------------------------
subplot(3,2,5)
pst   = (-32:64)*8;
plot(pst,ue,'c:',pst,um,'b:'),               hold on
plot(pst,mean(ue,2),'c',pst,mean(um,2),'b'), hold on
plot([0 0],get(gca,'YLim'),'--'),            hold off, axis square
xlabel('Time (milliseconds)', 'FontSize',12)
ylabel('Electrochemical response','FontSize',12)
title('Simulated ERP','FontSize',16)

% canonical correlations
%--------------------------------------------------------------------------
subplot(3,2,6)
bar(CVA.r,1/2,'c')
xlabel('Mode', 'FontSize',12)
ylabel('Correlation','FontSize',12)
title('Canonical correlations','FontSize',16), axis square

% Jacobian's and generalised synchronisation
%==========================================================================
spm_figure('GetWin','Jacobians');clf

% get Markov blanket indices the Jacobian
%--------------------------------------------------------------------------
xi    = spm_zeros(x); xi.v(:,e) = 1; iXe = find(spm_vec(xi));
xi    = spm_zeros(x); xi.q(:,m) = 1; iXm = find(spm_vec(xi));

% show results - Jacobians (of increasing order: 1 to n)
%--------------------------------------------------------------------------
[d,j] = max(CVA.v(:,1));
j     = j + (-8:8);
J     = spm_soup(Q(:,:,j),X(:,:,j),V(:,:,j),P);
J     = mean(J,3);
j     = [iXe;iXm];
% q   = 8;
% U   = blkdiag(CVA.V(:,1:q),CVA.W(:,1:q));  % eigenmodes (not used)

n     = 4;
for i = 1:n

    % all states
    %----------------------------------------------------------------------
    JJ    = J^i;
    subplot(n,2,(i - 1)*2 + 1)
    spy(abs(JJ) > 1e-2,'k')
    title(sprintf('%i-order coupling',i),'FontSize',16)
    xlabel('All states','FontSize',12)
    ylabel('All states','FontSize',12)

    % Internal and external states
    %----------------------------------------------------------------------
    JJ    = JJ(j,j);  % JJ     = pinv(U)*JJ(j,j)*U;
    subplot(n,2,(i - 1)*2 + 2)
    spy(abs(JJ) > 1e-2,'k')
    title(sprintf('%i-order coupling',i),'FontSize',16)
    xlabel('External and internal','FontSize',12)
    ylabel('External and internal','FontSize',12)

end

return


function varargout = lap(varargin)
%LAP   Laplacian of a DISKFUN.
%   L = LAP(F) returns a DISKFUN representing the Laplacian of F.
%
%   This is shorthand for LAPLACIAN(F).
%
% See also DISKFUN/LAPLACIAN.

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

[varargout{1:nargout}] = laplacian(varargin{:});

end


function val = checkFuncs(obj, x, derivative_level)
    % Evaluates the external functions (or function handles) with a given
    % input argument.
    %
    % Parameters:
    % x: the value of dependent variables @type colvec
    % derivative_level: determines the level of derivatives to be exported
    %  (1, 2) @type double
    %
    % Return values:
    % val: a structure of computed values @type struct

    % test function
    if nargin < 2
        x = vertcat([obj.DepVariables.InitialValue]);
    end

    if nargin < 3
        derivative_level = 1;
    end

    if isempty(obj.AuxData)
        f_value = feval(obj.Funcs.Func, x);
        j_value = feval(obj.Funcs.Jac, x);
        j_struct = feval(obj.Funcs.JacStruct, 0);
        if derivative_level == 2
            h_value = feval(obj.Funcs.Hess, x, ones(obj.Dimension,1));
            h_struct = feval(obj.Funcs.Hess, 0, ones(obj.Dimension,1));
        else
            h_value = [];
            h_struct = [];
        end
    else
        f_value = feval(obj.Funcs.Func, x, obj.AuxData);
        j_value = feval(obj.Funcs.Jac, x, obj.AuxData);
        j_struct = feval(obj.Funcs.JacStruct, 0);
        if derivative_level == 2
            h_value = feval(obj.Funcs.Hess, x, ones(obj.Dimension,1), obj.AuxData);
            h_struct = feval(obj.Funcs.Hess, 0, ones(obj.Dimension,1));
        else
            h_value = [];
            h_struct = [];
            h_mat = [];
        end
    end

    % validate computed values
    assert(length(f_value) == obj.Dimension,...
        'The dimension of the output value does not match the dimension specified.');

    assert(~isempty(j_value),...
        'All-zero Jacobian entries found.');
    if derivative_level == 2
        if obj.Type == NlpFunction.LINEAR
            assert(isempty(h_value),...
                'Non-zero Hessian entries found for the Linear constraints.');
        elseif obj.Type == NlpFunction.NONLINEAR
            assert(~isempty(h_value),...
                'All-zero Hessian entries found for the Non-Linear constraints.');
        end
        h_mat = sparse(h_struct(:,1), h_struct(:,2), h_value, length(x), length(x));
    end

    j_mat = sparse(j_struct(:,1), j_struct(:,2), j_value, length(f_value), length(x));

    val = struct(...
        'f', f_value, ...
        'J', j_mat, ...
        'H', h_mat);

end


classdef SymmetricFourthOrder3DTensor < FourthOrder3DTensor

    methods (Access = public)

        function obj = SymmetricFourthOrder3DTensor()
        end

        function createRandomTensor(obj)
            obj.createRandomTensor@FourthOrder3DTensor();
            obj.MakeMajorAndMinorSymmetrization();
        end

        function MakeMajorAndMinorSymmetrization(obj)
            Symmetrizer = FourthOrderSymmetrizer;
            obj.tensorValue = Symmetrizer.symmetrize(obj.tensorValue);
        end

    end

end


function [xm, ym, xfit,yfit] = companal(icapix, rs)

sica = size(icapix,3);
fprintf('maxima: \n')
for ii=1:sica
    aim = abs(imresize(icapix(:,:,ii),1/rs));
    m(ii) = max(max(aim));
    [ym(ii), xm(ii)] = find(m(ii)==aim);
    [ycog(ii), xcog(ii)] = cog(imresize(icapix(:,:,ii),1/rs)); %center of gravity
    [xfit(ii), yfit(ii), sig(ii)] = fitgauss2d(imresize(icapix(:,:,ii),1/rs));
    fprintf('%g . maximum: %g %g \tcog: %g %g \tfit: %g %g \tsig: %g\n',ii , [xm(ii), ym(ii)], [xcog(ii) ycog(ii)], [xfit(ii), yfit(ii), sig(ii)] );
end


function p = prior_sqrtunif(varargin)
%PRIOR_SQRTUNIF  Uniform prior structure for the square root of the parameter
%
%  Description
%    P = PRIOR_SQRTUNIF creates uniform prior structure for the
%    square root of the parameter.
%
%  See also
%    PRIOR_*
%
% Copyright (c) 2009 Jarno Vanhatalo
% Copyright (c) 2010 Jaakko Riihimäki
% Copyright (c) 2010 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'PRIOR_SQRTUNIFORM';
  ip.addOptional('p', [], @isstruct);
  ip.parse(varargin{:});
  p=ip.Results.p;

  if isempty(p)
    init=true;
    p.type = 'Sqrt-Uniform';
  else
    if ~isfield(p,'type') && ~isequal(p.type,'Sqrt-Uniform')
      error('First argument does not seem to be a valid prior structure')
    end
    init=false;
  end

  if init
    % set functions
    p.fh.pak = @prior_sqrtunif_pak;
    p.fh.unpak = @prior_sqrtunif_unpak;
    p.fh.lp = @prior_sqrtunif_lp;
    p.fh.lpg = @prior_sqrtunif_lpg;
    p.fh.recappend = @prior_sqrtunif_recappend;
  end

end

function [w, s, h] = prior_sqrtunif_pak(p)
  w=[];
  s={};
  h=[];
end

function [p, w] = prior_sqrtunif_unpak(p, w)
  w = w;
  p = p;
end

function lp = prior_sqrtunif_lp(x, p)
  lJ  = -log(2*sqrt(x));  % log(1/(2*sqrt(x))) log(|J|) of transformation
  lp  = sum(lJ);
end

function lpg = prior_sqrtunif_lpg(x, p)
  lJg = -1./(2*x);        % gradient of log(|J|) of transformation
  lpg = lJg;
end

function rec = prior_sqrtunif_recappend(rec, ri, p)
% The parameters are not sampled in any case.
  rec = rec;
end


function [growthRate, minProd, maxProd] = testOptKnockSol(model, targetRxn, deletions)
% Tests an `OptKnock` knockout strain
%
% USAGE:
%
%    [growthRate, minProd, maxProd] = testOptKnockSol(model, targetRxn, deletions)
%
% INPUTS:
%    model:         COBRA model structure
%    targetRxn:     Target reaction (e.g. 'EX_etoh(e)')
%
% OPTIONAL INPUT:
%    deletions:     Set of reaction deletions (e.g. {'PGI', 'TPI'})
%                   (Default = [])
%
% OUTPUTS:
%    growthRate:    Maximim growth rate of the strain
%    minProd:       Minimum production rate at max growth rate
%    maxProd:       Maximum production rate at max growth rate
%
% .. Author - Markus Herrgard 5/23/07

if (nargin < 3)
    deletions = [];
end

tol = 1e-7;

% Number of deletions
nDel = length(deletions);

modelKO = model;
for i = 1:nDel
    modelKO = changeRxnBounds(modelKO,deletions{i},0,'b');
end
% Calculate optimal growth rate
solKO = optimizeCbModel(modelKO);
growthRate = solKO.f;
if (solKO.stat == 1)
    % Max & min production of the metabolite at the optimal growth rate
    grRounded = floor(solKO.f/tol)*tol;
    modelKO = changeRxnBounds(modelKO,modelKO.rxns(modelKO.c==1),grRounded,'l');
    modelKO = changeObjective(modelKO,targetRxn);
    solMax = optimizeCbModel(modelKO,'max');
    solMin = optimizeCbModel(modelKO,'min');
    maxProd = solMax.f;
    minProd = solMin.f;
else
    maxProd = 0;
    minProd = 0;
end


function [u,res,iter] = deflationshifted(elem,A,b,M,isfreeNode,tol)
global ZH AH;
[HB, NL, level]      = HBstructure(elem,10);
% isfreeNode           = false(size(node,1),1);
% isfreeNode(freeNode) = 1;
[Pro]                = transferoperator(HB,NL,isfreeNode);
ZH                   = Pro{level-1};
AH                   = ZH'*A*ZH;
%invAH = inv(AH);
%P = (speye(Ni)-AD(freeNode,freeNode)*ZH*invAH*ZH');
u1 = ZH*(AH\(ZH'*b));
[u,~,res,iter]  = Pgmres_deflation(A,b,[],tol,size(b,1),M);
iter = max(iter);
u2 = u - ZH*(AH\(ZH'*(A*u)));
u0 = u1+u2;
u  = u0;
end


classdef testHorizontalTensorRotatedVsVHP < ...
        testHorizontalTensorRotatedVsSequentialLaminate

    methods (Access = public)

        function obj = testHorizontalTensorRotatedVsVHP()
            obj.computeTest()
        end

        function hasPassed = hasPassed(obj)
            rotHor = obj.rotHorTensor.getValue();
            lTens  = obj.lamTensor.getValue();
            lb = norm(rotHor - lTens) > 1e-12;
            ub = norm(rotHor - lTens) < 1e-2;
            hasPassed = lb & ub;
        end

    end

    methods (Access = protected)

        function computeLaminateDirectly(obj)
            c0       = obj.C0;
            c1       = obj.C1;
            dir{1}   = obj.lamDir;
            m1       = obj.lamPar;
            frac     = obj.theta;
            lam      = VoigtHomogPlaneStressHomogenizer(c0,c1,dir,m1,frac);
            obj.lamTensor = lam.getPlaneStressHomogenizedTensor();
        end

    end

end


classdef ParadigmSSRCSP < ParadigmBase
    % Selected-Subjects Regularized CSP (SSRCSP)
    %
    % This paradigm implements SSRCSP [1], which is a generalization of the Common Spatial Patterns
    % algorithms to calibration data comprising multiple subjects (or recordings). To train a model
    % for a particular "goal" (or target) subject using auxiliary data from other subjects, this
    % algorithm attempts to find a subset of other subjects such that, when data from those subjects
    % is combined with data from the goal subject, the performance on the goal subject's data is
    % optimal. The combination of data from multiple subjects and from the goal subject is done by
    % shrinking the goal-subject covariance matrix towards the average covariance matrix of the
    % other subjects (within each class), using a regularization parameter. The subset selection
    % algorithm employed is Sequential Floating Forward Selection (SFFS) [2].
    %
    % References:
    % [1] Lotte, F., & Guan, C.
    %     "Regularizing common spatial patterns to improve BCI designs: unified theory and new algorithms."
    %     Biomedical Engineering, IEEE Transactions on, 58(2), 355-362, 2011.
    %
    % [2] Pudil, P., Ferri, F. J., Novovicova, J., & Kittler, J.
    %     "Floating search methods for feature selection with nonmonotonic criterion functions."
    %     In Pattern Recognition, Proceedings of the 12th IAPR International. Conference on (Vol. 2, pp. 279-283). (1994)
    %
    % Name:
    %   Selected-Subjects Regularized Common Spatial Patterns
    %
    %                            Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
    %                            2014-02-06

    methods

        function defaults = preprocessing_defaults(self)
            defaults = {'FIRFilter',{'Frequencies',[6 8 28 32],'Type','minimum-phase'}, 'EpochExtraction',[0.5 3.5], 'Resampling',100};
        end

        function defaults = machine_learning_defaults(self)
            % set up the default parameters for machine learning
            defaults = {'lda',0.01,'regularization','shrinkage'};
        end

        function model = calibrate(self,varargin)
            % calibrate an SSRCSP model from a corpus of training sets
            args = arg_define(varargin, ...
                arg_norep({'collection','Collection'}), ...
                arg_norep({'goal_identifier','GoalIdentifier'}), ...
                arg({'patterns','PatternPairs'},3,uint32([1 1 64 10000]),'Number of CSP patterns (times two).'),...
                arg({'beta','CovarianceShrinkage'},0.5,[0 1],'Covariance shrinkage. This is the degree to which data from the goal subject is shrunken towards that of other subjects.'),...
                arg_sub({'flt','SignalProcessing'}, self.preprocessing_defaults(), @flt_pipeline, 'Signal processing stages. These parameters control filter stages that run on the signal level; they can be enabled, disabled and configured for the given paradigm. The prediction operates on the outputs of this stage.'), ...
                arg_sub({'ml','MachineLearning'},{'Learner',self.machine_learning_defaults()},@ml_train,'Machine learning stage of the paradigm. Operates on the feature vectors that are produced by the feature-extraction stage.'),...
                arg({'arg_dialogsel','ConfigLayout'},self.dialog_layout_defaults(),[],'Parameters displayed in the config dialog. Cell array of parameter names to display (dot-notation allowed); blanks are translated into empty rows in the dialog. Referring to a structure argument lists all parameters of that struture, except if it is a switchable structure - in this case, a pulldown menu with switch options is displayed.','type','cellstr','shape','row'));

            % get the data of the reference subject
            [reference,remaining] = utl_collection_closest(args.collection,args.goal_identifier);
            % preprocess each recording in the reference collection and concatenate them across epochs into a single set
            for r=1:length(reference)
                refsets{r} = exp_eval_optimized(flt_pipeline('signal',reference{r}.streams{1}, args.flt)); end
            refdata = exp_eval(set_joinepos(refsets{:}));

            % pre-process data of all other subjects
            otherdata = {};
            for s=1:length(remaining)
                if length(remaining{s}.streams) > 1
                    disp_once('Note: ParadigmMKLCSP will use only the first data stream of a recording (no support for multi-modal data).'); end
                % preprocess
                otherdata{s} = exp_eval_optimized(flt_pipeline('signal',remaining{s}.streams{1}, args.flt)); %#ok<*NODEF>
                if otherdata{s}.nbchan < args.patterns
                    error('CSP requires at least as many channels as you request output patterns. Please reduce the number of pattern pairs.'); end
            end

            % get the best subjects
            model.best_subjects = hlp_diskcache('featuremodels',@self.find_best_subjects,otherdata,refdata,args.patterns,args.ml);

            % calculate composite CSP
            covar = self.class_covariances(refdata);
            other_covar = self.class_covariances(exp_eval(set_joinepos(otherdata{model.best_subjects})));
            for k=1:2
                covar{k} = (1-args.beta)*covar{k} + args.beta*other_covar{k}; end
            [V,D] = eig(covar{1},covar{1}+covar{2}); P = inv(V); %#ok<NASGU>
            model.featuremodel.filters = V(:,[1:args.patterns end-args.patterns+1:end]);
            model.featuremodel.patterns = P([1:args.patterns end-args.patterns+1:end],:);

            % train predictive model
            model.predictivemodel = ml_train('data',{self.feature_extract(refdata,model.featuremodel),set_gettarget(refdata)}, args.ml);
            % set the filter graph based on the reference data
            model.tracking.filter_graph = refsets{end};
            % also store channel locations for model visualization
            model.chanlocs = refdata.chanlocs;
        end

        function predictions = predict(self,bundle,model)
            % extract features
            features = self.feature_extract(bundle.streams{1},model.featuremodel);
            % apply classifier
            predictions = ml_predict(features,model.predictivemodel);
        end

        function best_subjects = find_best_subjects(self,otherdata,refdata,patterns,ml)
            % find set of best subjects to include
            selected = {[]};
            remaining = {1:length(otherdata)};
            accuracy = {-Inf};
            n = 0;
            while n < length(otherdata)
                % find best subject to add
                best_accuracy = -Inf;
                best_index = NaN;
                for k = remaining{1+n}
                    acc = self.evaluate_subset(otherdata([selected{1+n} k]),refdata,patterns,ml);
                    if acc > best_accuracy
                        best_accuracy = acc;
                        best_index = k;
                    end
                end
                selected{1+n+1} = [selected{1+n} best_index];
                remaining{1+n+1} = setdiff(remaining{1+n},best_index);
                accuracy{1+n+1} = best_accuracy;
                n = n+1;
                % remove subjects
                while n > 2
                    % find best subject to remove
                    best_accuracy = -Inf;
                    best_index = NaN;
                    for k=selected{1+n}
                        acc = self.evaluate_subset(otherdata(setdiff(selected{1+n},k)),refdata,patterns,ml);
                        if acc > best_accuracy
                            best_accuracy = acc;
                            best_index = k;
                        end
                    end
                    if best_accuracy > accuracy{1+n-1}
                        selected{1+n-1} = setdiff(selected{1+n},best_index);
                        remaining{1+n-1} = [remaining{1+n} best_index];
                        accuracy{1+n-1} = best_accuracy;
                        n = n-1;
                    else
                        break;
                    end
                end
            end
            best_n = argmax([accuracy{:}]);
            best_subjects = selected{best_n};
        end

        function accuracy = evaluate_subset(self,trainset,testset,patterns,ml)
            % note: we are here implicitly weighting by the amount of training data per subject
            if iscell(trainset)
                trainset = exp_eval(set_joinepos(trainset{:})); end
            % train CSP on training set
            covar = self.class_covariances(trainset);
            [V,D] = eig(covar{1},covar{1}+covar{2}); %#ok<NASGU>
            featuremodel.filters = V(:,[1:patterns end-patterns+1:end]);
            % train classifier
            classifier = ml_train('data',{self.feature_extract(trainset,featuremodel),set_gettarget(trainset)},ml);
            % test on test set
            accuracy = -ml_calcloss('auto',set_gettarget(testset),ml_predict(self.feature_extract(testset,featuremodel),classifier));
        end

        function covar = class_covariances(self,dataset)
            % calculate per-class covariance matrices
            for k=1:2
                classdata = exp_eval(set_picktrials(dataset,'rank',k));
                covar{k} = (classdata.data(:,:) * classdata.data(:,:)') / (size(classdata.data,2)*size(classdata.data,3));
                covar{k}(~isfinite(covar{k})) = 0;
            end
        end

        function features = feature_extract(self,signal,featuremodel)
            % extract log-variance features from an epoched and preprocessed recording
            features = zeros(size(signal.data,3),size(featuremodel.filters,2));
            for t=1:size(signal.data,3)
                features(t,:) = sum((signal.data(:,:,t)'*featuremodel.filters).^2,1); end
            features = log(features/size(signal.data,2));
        end

        function visualize(self,varargin) %#ok<*INUSD>
            % visualize an mklCSP model
            args = arg_define(varargin, ...
                arg_norep({'model','Model'},[],[],'BCI Model to visualize.'), ...
                arg({'patterns','PlotPatterns'},true,[],'Plot patterns instead of filters. Whether to plot spatial patterns (forward projections) rather than spatial filters.'), ...
                arg({'paper','PaperFigure'},false,[],'Use paper-style font sizes. Whether to generate a plot with font sizes etc. adjusted for paper.'));

            f = figure;
            % get number of pairs, and index of pattern per subplot
            np = size(args.model.featuremodel.patterns,1)/2;
            idx = [1:np 2*np:-1:np+1];
            % for each CSP pattern...
            for p=1:np*2
                subplot(2,np,p,'Parent',f);
                if args.patterns
                    topoplot(args.model.featuremodel.patterns(idx(p),:),args.model.featuremodel.chanlocs);
                else
                    topoplot(args.model.featuremodel.filters(:,idx(p)),args.model.featuremodel.chanlocs);
                end
                t = title(['CSP Pattern ' num2str(idx(p))]);
                if args.paper
                    set(t,'FontUnits','normalized');
                    set(t,'FontSize',0.1);
                end
            end
        end

        function layout = dialog_layout_defaults(self)
            % define the default configuration dialog layout
            layout = {'SignalProcessing.Resampling.SamplingRate', 'SignalProcessing.FIRFilter.Frequencies', ...
                'SignalProcessing.FIRFilter.Type', 'SignalProcessing.EpochExtraction', '', ...
                'PatternPairs', 'CovarianceShrinkage', '', 'MachineLearning.Learner'};
        end

    end
end

% (turn off a few editor warnings because some actual implementations are missing in this file)
%#ok<*INUSD,*STOUT,*MANU>


function PLOT_Map(States,Params, Trajectory)

%This function plots an empty version of the map.

x = States(1,:);
y = States(2,:);
th = States(3,:);
phi = States(4,:);

Lt = Params.Dyn.Lt;
Lc = Params.Dyn.Lc;

Npts = Params.Traj.Npts;

%[dStates, A, B, Positions] = Derive_EoM()

Pa = [...   %Position of the back of the trailer
    x;
    y];

Pb = [...    %Position of the back of the cab
 x - Lt*sin(th);
 y + Lt*cos(th)];

Pc = [...   %Position of the front of the cab
 x - Lc*(cos(phi).*sin(th) + cos(th).*sin(phi)) - Lt*sin(th);
 y + Lc*(cos(phi).*cos(th) - sin(phi).*sin(th)) + Lt*cos(th)];

if strcmp(Params.Sim.Direction,'reverse')
    Dir = 2;
else
    Dir = 1;
end
Road = zeros(2,Npts);
for i=1:Npts
    Road(:,i) = Trajectory.States{Dir,i}(1:2);
end

figure(1)
clf; hold on;
plot(Pa(1,1),Pa(2,1),'r','LineWidth',5)
plot(Pb(1,1),Pb(2,1),'b','LineWidth',3)
plot(Pc(1,1),Pc(2,1),'g','LineWidth',3)
legend('Back of Trailer','Back of Cab', 'Front of Cab')
plot(Road(1,:),Road(2,:),'k','LineWidth',Params.Sim.RoadWidth)
title('Map of the Road and Traces')
axis equal

end


function f_x = ParFor10(in1)
%PARFOR10
%    F_X = PARFOR10(IN1)

%    This function was generated by the Symbolic Math Toolbox version 8.2.
%    31-Mar-2020 17:03:12

r = in1(:,1);
s = in1(:,13);
u = in1(:,19);
ut = in1(:,20);
ux = in1(:,21);
uxx = in1(:,22);
uy = in1(:,23);
uyy = in1(:,24);
z = in1(:,7);
t2 = u.^2;
f_x = r.*-4.845016406907234+s.*6.96612441301113+ut.*5.457984690783633e-1-ux.*1.530748504418966e-2+uxx.*2.135644249305187-uy.*3.570878181824355e-2+uyy.*5.02823932467436-z.*1.323504040227272e2+r.*u.*6.191151455437648-s.*u.*9.105578580027213-t2.*u.*1.077295023277402e2+u.*ut.*5.65024831920482e-2+u.*ux.*1.830355084143775e-2-u.*uxx.*4.132096419489244+u.*uy.*4.442896435307375e-2-u.*uyy.*6.867807677976089+u.*z.*1.83605552916415e2+4.028421339788474e1;


function params = crazyflie()
% crazyflie: physical parameters for the Crazyflie 2.0
%
% 2016 Bernd Pfrommer
%
% This function creates a struct with the basic parameters for the
% Crazyflie 2.0 quad rotor (without camera, but with about 5 vicon
% markers)
%
% Model assumptions based on physical measurements:
%
% motor + mount + vicon marker = mass point of 3g
% arm length of mass point: 0.046m from center
% battery pack + main board are combined into cuboid (mass 18g) of
% dimensions:
%
%   width  = 0.03m
%   depth  = 0.03m
%   height = 0.012m
%

m = 0.030;  % weight (in kg) with 5 vicon markers (each is about 0.25g)
g = 9.81;   % gravitational constant
I = [1.43e-5,   0,          0; % inertial tensor in m^2 kg
     0,         1.43e-5,    0;
     0,         0,          2.89e-5];
L = 0.046; % arm length in m

params.mass = m;
params.I    = I;
params.invI = inv(I);
params.grav = g;
params.arm_length = L;

params.maxangle = 40*pi/180; % you can specify the maximum commanded angle here
params.maxF     = 2.5*m*g;   % left these untouched from the nano plus
params.minF     = 0.05*m*g;  % left these untouched from the nano plus

% You can add any fields you want in params
% for example you can add your controller gains by
% params.k = 0, and they will be passed into controller.m

end


function sr_parameterstudyNvsTw

disp('~/zmap/src/thomas/seismicrates/sr_parameterstudyNvsTw.m')

% reset random number generator
rand('state',sum(100*clock));
% set default sample size

fYrs=[10]';
fYr1=1980;
fYr2=1980+fYrs;

% create range for N
vN=[25 50 100 150 200]';
% vN=[50 75 100]';

% create range for Tw
vTw=[0.1:0.1:0.9]'.*fYrs;

% fix Tbin by 1/100 of whole period
nTbin1=1/100;
% round time bins
vTbin=ceil((fYr2-fYr1)*nTbin1*365)';

% set default catalog size
nCatSize=5000;

% prepare input parameter matrix  [ vN   vTw   vTbin ]
mVar(:,1)=repmat(vN,size(vTw,1)*size(vTbin,1),1);
mVar(:,2)=reshape(repmat(vTw',size(vN,1)*size(vTbin,1),1),...
    size(vTw,1)*size(vN,1)*size(vTbin,1),1);
mVar(:,3)=repmat(reshape(repmat(vTbin',size(vN,1),1),...
    size(vTbin,1)*size(vN,1),1),size(vTw,1),1);

mCat20=[];mCat00=[];

% bRchange=logical(0)
% rate change?
fRate2=100  ; %  percent of original rate
% nInit=logical(1);
for iCat=1:500
    iCat
    for j=1:size(mVar)
        while isempty(mCat20) | isempty(mCat00)
            vRand=rand(mVar(j,1),1);

            if (fRate2~=100)
                % Fraction of period with 100% rate and fRchange % rate
                fRYr1=1-mVar(j,2)/(fYr2-fYr1);
                fRYr2=mVar(j,2)/(fYr2-fYr1);
                vRand=rand(mVar(j,1),1);
                fRYr1+(fRYr2*fRate2/100);
                vRand1=vRand*(fRYr1+(fRYr2*fRate2/100));
                vSel=(vRand1>fRYr1);
                vRand=vRand1;
                vRand(vSel)=(vRand1(vSel)-fRYr1)*100/fRate2+fRYr1;
                clear vRand1
            else
                vRand=rand(mVar(j,1),1);
            end

            mCat00=vRand*(fYr2-fYr1)+fYr1;
            mCat=rand(nCatSize,1)*(fYr2-fYr1)+fYr1;
            mCat20=mCat00(mCat00>(fYr2-mVar(j,2)));
            if isempty(mCat20) disp('looooop'); end
        end
%         [mLTA(j,iCat), mLTAprob(j,iCat)] =calc_zlta(mCat,mCat00,mCat20,fYr1, fYr2,mVar(j,2),mVar(j,3),mVar(j,1));
%         [mBeta(j,iCat), mBetaprob(j,iCat)] =calc_beta(mCat,mCat00,mCat20,fYr1, fYr2,mVar(j,2),mVar(j,3),mVar(j,1));
        [mResult_(iCat,1,j), mResult_(iCat,2,j)] =calc_zlta(mCat,mCat00,mCat20,fYr1, fYr2,mVar(j,2),mVar(j,3),mVar(j,1));
        [mResult_(iCat,3,j), mResult_(iCat,4,j)] =calc_beta(mCat,mCat00,mCat20,fYr1, fYr2,mVar(j,2),mVar(j,3),mVar(j,1));
        mCat20=[];
        clear mCat00 mCat;
    end
end
%         disp(n);
%     subplot(2,2,1)
%     hold on;plot(1./nTbin1,mLTA);
%     subplot(2,2,2)
%     hold on;plot(1./nTbin1,mLTAprob);
%     subplot(2,2,3)
%     hold on;plot(1./nTbin1,mBeta);
%     subplot(2,2,4)
%     hold on;plot(1./nTbin1,mBetaprob);

% for i=1:size(mLTA,1)
%     mLTA1(:,i)=reshape(mLTA(i,:,:),size(mLTA,2)*size(mLTA,3),1);
%     mLTAprob1(:,i)=reshape(mLTAprob(i,:,:),size(mLTAprob,2)*size(mLTAprob,3),1);
%     mBeta1(:,i)=reshape(mBeta(i,:,:),size(mBeta,2)*size(mBeta,3),1);
%     mBetaprob1(:,i)=reshape(mBetaprob(i,:,:),size(mBetaprob,2)*size(mBetaprob,3),1);
% end

params.mResult_=mResult_;
params.mVar=mVar;
params.vN=vN;
params.vTw=vTw;
params.vTbin=vTbin;

sPrint=sprintf('save NvsTw-Sim%04.0f-R%03.0f.mat params -mat',iCat, fRate2);
eval(sPrint);
disp(sPrint);
% figure;
% subplot(2,1,1)
% errorbar(1./nTbin1,mean(mLTA1),std(mLTA1),'b--x','LineWidth',3);
% hold on;
% errorbar(1./nTbin1,mean(mBeta1),std(mBeta1),'r:o','LineWidth',2);
% xlabel('Bin Size [1/X]');
% ylabel('Z and \beta [ ]');
% legend('z','\beta');
% subplot(2,1,2);
% errorbar(1./nTbin1,mean(mLTAprob1),std(mLTAprob1),'b--x','LineWidth',3);
% YLim([0,1]);
% hold on;
% errorbar(1./nTbin1,mean(mBetaprob1),std(mBetaprob1),'r:o','LineWidth',2);
% xlabel('Bin Size [1/X]');
% ylabel('Probability of Z and \beta [ ]');
% legend('p(z)','p(\beta)');
%


function ehmm = hsupdate_ehmm(Xi,Gamma,T,ehmm)
%
% updates hidden state parameters of an HMM
%
% INPUT:
%
% Xi     probability of past and future state cond. on data
% Gamma  probability of current state cond. on data
% T      length of observation sequences
% hmm    single hmm data structure
%
% OUTPUT
% hmm    single hmm data structure with updated state model probs.
%
% Author: Diego Vidaurre, OHBA, University of Oxford

K = ehmm.K;

if isempty(Xi)  % non-exact estimation
    order = (sum(T)-size(Gamma,1)) / length(T);
    Xi = zeros(sum(T-1-order),K,2,2);
    for j = 1:length(T)
        indG = (1:(T(j)-order)) + sum(T(1:j-1)) - (j-1)*order;
        indXi =  (1:(T(j)-order-1)) + sum(T(1:j-1)) - (j-1)*(order+1);
        for k = 1:K
            g = [Gamma(indG,k) (1-Gamma(indG,k))];
            for t = 1:length(indXi)
                xi = g(t,:)' * g(t+1,:);
                xi = xi / sum(xi(:));
                Xi(indXi(t),k,:,:) = xi;
            end
        end
    end
end
if length(size(Xi))==4
    Xi = permute(sum(Xi),[2 3 4 1]);
end

% transitions
for k = 1:K
    ehmm.state(k).Dir2d_alpha = permute(Xi(k,:,:),[2 3 1]) ...
        + ehmm.state(k).prior.Dir2d_alpha;
    ehmm.state(k).P = zeros(2);
    for j = 1:2
        PsiSum = psi(sum(ehmm.state(k).Dir2d_alpha(j,:)));
        for j2 = 1:2
            ehmm.state(k).P(j,j2) = ...
                exp(psi(ehmm.state(k).Dir2d_alpha(j,j2))-PsiSum);
        end
        ehmm.state(k).P(j,:) = ehmm.state(k).P(j,:) ./ sum(ehmm.state(k).P(j,:));
    end
end

% initial state is always OFF for all chains
end


function Mi = get_block_row(M, C, row_range)
	%% ================== File info ==========================
	% Author		: Tiep Vu (http://www.personal.psu.edu/thv102/)
	% Time created	: 1/27/2016 2:44:26 AM
	% Last modified	: 1/27/2016 2:44:30 AM
	% Description	: Get a row block of a big matrix X.
	% 	INPUT
	%		M        : the big matrix. M = [M1 ; M2 ; ... ; MC]
	%		C        : block index
	%		row_range: a vector store the last index of each block. row_range(1) = 0.
	%					i-th block is indexed by col_range(i)+1: col_range(i+1).
	% 	OUTPUT
	%		Mi: output block matrix
	%
	%% ================== end File info ==========================
	id_sel = [];
	for i = 1: numel(C)
		c = C(i);
		id_sel = [id_sel, row_range(c) + 1: row_range(c+1)];
	end
	Mi = M(id_sel, : , :);
end


function diagnostic = bisection(varargin)
%BISECTION Solve simple quasi-convex MAXIMIZATION problem by bisection
%
%   DIAGNOSTIC = BISECTION(F,h,options,tolerance)
%
%    min   t
%    subject to
%            F(x,t) >=0
%
%   NOTES
%    It is assumed that the problem is quasi-convex in the scalar simple
%    variable t.
%
%    Lower and upper bounds are automatically derived
%    (you are adviced not to add simple bounds on the t-variable)
%
%    Default bisection tolerance 1e-5.
%
%    The algorithm simply solves a series of feasibility problems where the
%    parameter t is fixed, and hones in on an optimal value using bisection
%
%    It is recommended to explicitly set a solver. Otherwise YALMIP will
%    have to try to figure out a suitable solver for the feasibility
%    problems

solvertime = tic;
Constraints = varargin{1};
Objective = varargin{2};
if isequal(getbase(Objective),[0 -1])
    % User wants to maximize something, so we can reuse old code format
    varargin{2} = -Objective;
    options = varargin{3};
    options.bisection.switchedsign = 0;
    options.solver = options.bisection.solver;
    varargin{3} = options;
    diagnostic = bisection_core(varargin{:});
elseif isequal(getbase(Objective),[0 1])
    % User wants to minimize something. Rewrite as old max code
    options = varargin{3};
    options.bisection.switchedsign = 1;
    options.solver = options.bisection.solver;
    varargin{3} = options;
    Constraints = replace(Constraints, Objective, -Objective);
    varargin{1} = Constraints;
    diagnostic = bisection_core(varargin{:});
end
diagnostic.yalmiptime = toc(solvertime)-diagnostic.solvertime;
diagnostic.info = yalmiperror(diagnostic.problem,'bisection');


% rejstatepoch() - reject bad eeg trials based a statistical measure. Can
%                 be applied either to the raw eeg data or the ICA
%                 component activity. This is an interactive function.
%
% Usage:
%   >> [ Irej, Irejdetails, n, threshold, thresholdg] = ...
%                         rejstatepoch( signal, rej, 'key1', value1...);
%
% Inputs:
%   signal     - 3 dimensional data array channel x points x trials
%                (instead of channels, one might also use independent
%                components).
%   rej        - rejection array, one value per trial and per channel or
%                component (size channel x trials).
%                By default, values are normalized by this
%                function and trehshold is expressed in term of standard
%                deviation of the mean.
%
% Optional inputs:
%   'plot'       - ['on'|'off'] interactive mode or just rejection.
%                  In the interactive mode, it plots the normalized
%                  entropy of original signal (blue) and the limits
%                  (red) (default:'on')
%   'threshold'  - percentage error threshold (default 1-0.25/nb_trials)
%                  for individual trials of individual channel/component.
%                  This treshold is expressed in term of standart
%                  deviation from the mean (default 5).
%   'global'     - ['on'|'off'], also perform threshold on the global
%                  measure (by default, the mean over all channel or
%                  electrodes).
%   'rejglob'    - rejection array, one value per trials. Use this
%                  argument when the global measure for all channels or
%                  does not correspond to their mean.
%   'thresholdg' - global threshold for the reunion of all channels
%                  or components. By default, it is equal to 'threshold'.
%   'normalize'  - ['on'|'off'], normalize values before applying the
%                  treshold. Default is 'on'.
%   'plotcom'    - sting command to plot single trials. Default
%                  is none.
%   'title'      - title of the graph. Default is none.
%   'labels'     - labels for electrodes (array not cell).
%
% Outputs:
%   Irej        - indexes of trials to be rejected
%   Irejdetails - array for rejected components or channel (nb_rejected x
%                 nb_channel or nb_rejected x nb_components)
%   n           - number of trials rejected
%   threshold   - percentage error threshold
%   thresholdg  - percentage error threshold for global rejection
%
% See also: eeglab()

% Algorithm:
%   normalise the measure given as input and reject trials based on
%   an uniform distribution of the data

% Copyright (C) 2001 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% userdata
% gcf : plotsig - signal to plot in the pop_out window
%       pnts - number of points per epoch
%       Irej - rejection of trials
%       Irejelec - rejection of trials and electrodes
%       labels - labels for curves
% plotwin : rej1 - rejection array (with all electrodes x trials)
%           rej2 - global rejection array (size trials)
%           thr1 - threshold (electrodes)
%           thr2 - threshold global

function [ Irej, Irejdetails, n, threshold, thresholdg] = rejstatepoch( signal, ...
		rej, varargin); % pnts, th_E, th_rejg, command, commandplot, typetitle, E, rejg);

if nargin < 1
	help rejstatepoch;
	return;
end;

if ~isstr( signal )

	if nargin < 2
		help rejstatepoch;
		return;
	end;

	if ~isempty( varargin ), g=struct(varargin{:});
	else g= []; end;
	try, g.plot; 			catch, g.plot='on'; end;
	try, g.threshold; 		catch, g.threshold=5; end;
	try, g.thresholdg; 		catch, g.thresholdg=5; end;
	try, g.global; 			catch, g.global='on'; end;
	try, g.rejglob; 		catch, g.rejglob=[]; end;
	try, g.normalize; 		catch, g.normalize='on'; end;
	try, g.plotcom; 		catch, g.plotcom=''; end;
	try, g.title;	 		catch, g.title=''; end;
	try, g.labels;	 		catch, g.labels=''; end;

	g.rej = rej;
	clear rej
	switch lower(g.plot)
		case {'on', 'off'} ;
		otherwise disp('Error: Plot must be either ''on'' or ''off'''); return;
	end;
	switch lower(g.global)
		case {'on', 'off'} ;
		otherwise disp('Error: Global must be either ''on'' or ''off'''); return;
	end;
	switch lower(g.normalize)
		case {'on', 'off'} ;
		otherwise disp('Error: Normalize must be either ''on'' or ''off'''); return;
	end;
	if ~isstr(g.plotcom)
		disp('Error: Plotcom must be a string to evaluate'); return;
	end;
	if ~isstr(g.title)
		disp('Error: Title must be a string'); return;
	end;
	try, g.threshold*2;
		catch, disp('Error: Threhsold must be a number'); return;
	end;
	try, g.thresholdg*2;
		catch, disp('Error: Threhsoldg must be a number'); return;
	end;
	if length(g.threshold(:)) > 1
		disp('Error: Threhsold must be a single number'); return;
	end;
	if length(g.thresholdg(:)) > 1
		disp('Error: Threhsoldg must be a single number'); return;
	end;
	if ~isempty(g.rejglob)
		if length(g.rejglob) ~= size(g.rej,2)
			disp('Error: Rejglob must be have the same length as rej columns'); return;
		end;
	else
		switch lower(g.global), case 'on', g.rejglob = sum(g.rej,1); end;
	end;
	if size(signal,3) ~= size(g.rej,2)
		disp('Error: Signal must be have the same number of element in 3rd dimension as rej have columns'); return;
	end;
	if isempty(g.labels)
		for index = 1:size(g.rej,1)
			g.labels(index,:) = sprintf('%3d', index);
		end;
		if ~isempty(g.rejglob)
			g.labels(index+2,:) = 'g. ';
		end;
	end;

	switch lower(g.normalize),
		case 'on',
			g.rej = (g.rej-mean(g.rej,2)*ones(1, size(g.rej,2)))./ (std(g.rej, 0, 2)*ones(1, size(g.rej,2)));
			switch lower(g.global), case 'on', g.rejglob = (g.rejglob(:)-mean(g.rejglob(:)))./ std(g.rejglob(:)); end;
	end;
	switch lower(g.global), case 'off',g.rejglob = []; end;

	% plot the buttons
	% ----------------
	try, icadefs; catch, GUIBUTTONCOLOR = [0.8 0.8 0.8]; BACKCOLOR = [0.8 0.8 0.8]; end;
	figure('color', BACKCOLOR);
	set(gcf, 'name', 'Rejectrials');
	pos = get(gca,'position'); % plot relative to current axes
	set( gca, 'tag', 'mainaxis');
	q = [pos(1) pos(2) 0 0];
	s = [pos(3) pos(4) pos(3) pos(4)]./100; % allow to use normalized position [0 100] for x and y
	axis('off');

	plotsig = sum(abs(signal(:,:)),1);
	set(gcf, 'userdata', { plotsig, size(signal, 2), [], [] }); % the two last arguments are the rejection

	% Create axis
	% -----------
	h6 = axes('Units','Normalized', 'tag',  'Plotwin', 'Position',[-10 12 120 84].*s+q);
	title(g.title);
	set( h6, 'userdata', { g.rej g.rejglob g.threshold g.thresholdg g.labels }); % g.rej was put twice because it is used to compute the global entropy

	% CANCEL button
	% -------------
	h  = uicontrol(gcf, 'Style', 'pushbutton', 'string', 'Cancel', 'Units','Normalized','Position',...
			[-15 -6 15 6].*s+q, 'callback', 'close(gcf);', 'backgroundcolor', GUIBUTTONCOLOR);

	% Entropy component text and threshold
	% ------------------------------------
    makebutton( 'Single-channel', 'Plotwin'  ,  [3 -6 27 6].*s+q, [30 -6 15 6].*s+q, 3, g.threshold, GUIBUTTONCOLOR);
    makebutton( 'All-channel', 'Plotwin'     ,  [50 -6  27 6].*s+q, [77 -6  15 6].*s+q, 4, g.thresholdg, GUIBUTTONCOLOR);

	% EEGPLOT button
	% --------------
	if ~isempty(g.plotcom)
			h  = uicontrol(gcf, 'backgroundcolor', GUIBUTTONCOLOR, 'Style', 'pushbutton', 'string', 'EEGPLOT', 'Units','Normalized','Position',[95 -2 15 6].*s+q, ...
					'callback',['TMPEEG = get(gcbf, ''userdata'');' ...
								'TMPREJ = TMPEEG{3};' ...
								'TMPREJELEC = TMPEEG{4};' ...
								g.plotcom ] );
			posaccept = [95 -10 15 6];
	else	posaccept = [95 -6 15 6];
	end;

	% ACCEPT button
	% -------------
	command = 'fprintf(''Rejection indices has been put in the matrix TMPREJ\n'')';
	haccept  = uicontrol(gcf, 'backgroundcolor', GUIBUTTONCOLOR, 'Style', 'pushbutton', 'string', 'UPDATE', 'Units','Normalized','Position', posaccept.*s+q, ...
					'callback', [	'set(gcbo, ''userdata'', 1);' ... %signal to signify termination
									'TMPEEG = get(gcbf, ''userdata'');' ...
									'TMPREJ = TMPEEG{3};' ...
									'TMPREJELEC = TMPEEG{4};' ...
									command ] );

	command = [ 'entwin = get(gcbf, ''currentobject'');' ...
				'tmptype = get(entwin,''type'');' ...
				'if tmptype(1) == ''l'' entwin = get(entwin, ''parent''); end;' ...
				'tagwin = get(entwin,''tag'');' ... % either entropy or kurtosis
				'switch tagwin,' ...
				' case ''Plotwin'',' ... % check if the user clicked on the right window
				'   alldata = get(gcf, ''userdata'');' ...
				'   plotsig = alldata{1};' ...
				'   pnts = alldata{2};' ...
				'   fig = figure(''position'', [100 300 600 400],''color'', [1 1 1]);' ...
				'   I = alldata{3};' ...
				'   sweeps = size(plotsig,2) /pnts;' ...
				'   h1 = axes(''parent'', fig, ''Units'',''Normalized'', ''Position'',[0.6 0.11 0.27 0.815]);' ...
				'   pos = get(entwin, ''currentpoint'');' ...
				'   component = round(pos(1) / sweeps + 0.5);' ... % determine the component number
				'   alldata = get(entwin, ''userdata'');' ...
		    	'   rej = alldata{1};' ...
				'   if component <= size(rej,1)' ... % component
				'        rej_threshold = alldata{3};' ...
				'        component = max(1,min(component, size(rej,1))); ' ...
		    	'   	 rej = rej(component, :);' ...
				'        titlegraph = sprintf(''' g.title ' #%d'', component);' ...
				'        colorgraph = ''b'';' ...
				'   else' ...                        % global
		    	'   	 rej = alldata{2};' ...
				'        rej_threshold = alldata{4};' ...
				'        titlegraph = sprintf(''' g.title ' global'');' ...
				'        colorgraph = ''g'';' ...
				'   end;' ...
				'   plot([1:length(rej)], rej, ''color'', colorgraph);' ...
				'   ss = get(h1, ''xlim'');' ...
				'   set(h1, ''view'', [90 90]);' ...
				'   set(h1, ''xdir'', ''reverse'');' ...
				'   set(h1, ''XLim'', ss);' ...
				'   hold on;' ...  % plot component
				'   yl = get(h1, ''ylim'');' ...
				'   set(h1, ''xtickmode'', ''manual'', ''xtick'', sweeps/2, ''xticklabel'', component, ''xlim'', [ 0 sweeps ]);' ...
				'   title( titlegraph );' ...
				'   plot( get(h1, ''xlim''), [rej_threshold rej_threshold], ''r'');' ... % plot limit
				'   plot( get(h1, ''xlim''), [-rej_threshold -rej_threshold], ''r'');' ... % plot limit
				'   set(h1, ''xticklabel'', []);' ...
				'   hold on;' ...
				'   h2 = axes(''parent'', fig,''Units'',''Normalized'', ''Position'',[0.13 0.11 0.27 0.815]);' ...
				'   erpimage( plotsig, ones(1,size(plotsig,2)/pnts), [0:1/(pnts-1):1], '''', 3, 1, ''nosort'', ''noplot'');' ...
				'   title(''Currentset all chans''); xlabel(''''); ylabel(''''); ' ...
				'   set(gca, ''xticklabel'', []);' ...
				'   hold on;' ...
				'   h3 = axes(''parent'', fig,''Units'',''Normalized'', ''Position'',[0.45 0.11 0.1 0.815]);' ...
				'   if any(I ~= 0)' ...
				'      rejImage = (I'' * ones(1, 10))'';' ...
				'      imagesc( rejImage'' );' ...
				'      set(gca, ''ydir'', ''normal'');' ...
				'   end;' ...
				'   title(''Rejected (all)''); xlabel(''''); ylabel('''');' ...
				'   set(gca, ''xticklabel'', [], ''yticklabel'', []);' ...
				'end;' ...
				'clear fig tmptype tagwin alldata rej rejImage plotsig sweeps pnts rej_threshold ss q s h1 h2 h3 pos component yl;' ];

%				'      erpimage( rejImage(:)'', ones(1,size(I,2)), [0:1/(10-1):1], '''', 1, 0, ''nosort'', ''noplot'');' ...
	set(gcf, 'WindowButtonDownFcn', command);

	rejstatepoch('draw');
	switch g.plot,
		case 'on', waitfor( haccept, 'userdata'); drawnow;
	end;

	threshold  = g.threshold;
	thresholdg = g.thresholdg;
	Irej = [];
	Irejdetails = [];
	n = 0;
	try
		TMPEEG = get(gcf, 'userdata');
		Irej = TMPEEG{3};
		Irejdetails = TMPEEG{4};
		n = length(find(Irej == 1));

		plothandler = findobj( 'parent', gcf, 'tag', 'Plotwin');
		TMPEEG = get(plothandler, 'userdata');
		threshold = TMPEEG{3};
		thresholdg = TMPEEG{4};
		close(gcf);
	catch, end;
else %if signal is a string draw everything

	% retreive data
	% -------------
	gcfdata = get(gcf, 'userdata');
	plotsig = gcfdata {1};
	pnts    = gcfdata {2};
    sweeps  = size(plotsig,2)/pnts;

	h6 = findobj('parent', gcf, 'tag', 'Plotwin');
	alldata = get(h6, 'userdata');
	g.rej       = alldata {1};
	g.rejg      = alldata {2};
	g.threshold   = alldata {3};
	g.thresholdg  = alldata {4};
	set(h6, 'userdata', alldata);

	nbchans = size(g.rej,1);

	% reject trials
	% -------------
	rejelec = abs(g.rej) > g.threshold;
	rej  = max(rejelec,[],1);
	n1 = sum(rej(:));
	if ~isempty( g.rejg )
		rej2 = abs(g.rejg) > g.thresholdg;
		n2 = sum(rej2(:));
		rej = rej | rej2(:)';
	end;
	fprintf('%d trials rejected (single:%d, all:%d)\n', sum(rej(:)), n1, n2);
	gcfdata {3} = rej;
	gcfdata {4} = rejelec;
	set(gcf, 'userdata', gcfdata);

	% plot the sorted entropy curve
	% -----------------------------
	plotstat( 'Plotwin');

end;
return;

function plotstat( id );

	% plot the sorted entropy curve
	% -----------------------------
	h6 = findobj('parent', gcf, 'tag', id);
	axes(h6); cla;
	ttmp = get(gca, 'title');
	oldtitle = get(ttmp, 'string');

	% get datas
    % ---------
   	alldata = get(gca, 'userdata');
	g.rej       = alldata {1};
	g.rejg      = alldata {2};
	g.threshold   = alldata {3};
	g.thresholdg  = alldata {4};
	g.labels      = alldata {5};
	nbchans = size(g.rej,1);
	sweeps  = size(g.rej,2);

	% plot datas
    % ----------
	g.rej = g.rej'; plot(g.rej(:)); g.rej = g.rej';
	hold on;
	yl = get(gca, 'ylim');

	% plot vertival bars to separate components and the trehsold
	% ----------------------------------------------------------
	set( gca, 'tag',  id, 'ylimmode', 'manual');
	set(gca, 'xtickmode', 'manual', 'xtick', [0:sweeps:(size(g.rej(:),1)-1+2*sweeps)] + sweeps/2, ...
			 'xticklabel', g.labels, 'xlim', [ 0 (size(g.rej(:),1)-1+2*sweeps)]);
	plot( [1 size(g.rej(:),1)], [-g.threshold -g.threshold], 'r');	% plot threshold
	plot( [1 size(g.rej(:),1)], [g.threshold g.threshold], 'r');	% plot threshold

	if ~isempty(g.rejg) % plot global ?
		plot([size(g.rej(:),1)+sweeps:size(g.rej(:),1)+2*sweeps-1],  g.rejg(:), 'g');
		pp = patch([size(g.rej(:),1) size(g.rej(:),1) size(g.rej(:),1)+sweeps size(g.rej(:),1)+sweeps], [yl(1)-1 yl(2)+1 yl(2)+1 yl(1)-1], get(gcf, 'color'), 'clipping', 'off');
		set(pp, 'EdgeColor',  get(gcf, 'color'));
		plot( [size(g.rej(:),1)+sweeps length(g.rejg)+size(g.rej(:),1)+sweeps], [-g.thresholdg -g.thresholdg], 'r');	% plot threshold
		plot( [size(g.rej(:),1)+sweeps length(g.rejg)+size(g.rej(:),1)+sweeps], [g.thresholdg g.thresholdg], 'r');	% plot threshold
		plot([size(g.rej(:),1)+sweeps size(g.rej(:),1)+sweeps], yl, 'k');
	else
		pp = patch([size(g.rej(:),1) size(g.rej(:),1) size(g.rej(:),1)+2*sweeps size(g.rej(:),1)+2*sweeps], [yl(1)-1 yl(2)+1 yl(2)+1 yl(1)-1], get(gcf, 'color'), 'clipping', 'off');
		set(pp, 'EdgeColor',  get(gcf, 'color'));
	end;
	for index = 0:sweeps:size(g.rej(:),1);
		plot([index index], yl, 'k');
	end;

	% restore properties
	title(oldtitle);
	set(h6, 'userdata', alldata);

return;

function makebutton( string, ax, pos1, pos2, userindex, init, GUIBUTTONCOLOR );
	h  = uicontrol(gcf , 'backgroundcolor', GUIBUTTONCOLOR, 'Style', 'radiobutton', 'string', string, 'value', fastif(init == 0, 0, 1), 'Units','Normalized', 'Position', pos1, ...
					'callback', [ 'textresh = findobj(''parent'', gcbf, ''tag'', ''' string ''');' ...
								  'checkstatus = get(gcbo, ''value'');' ...
								  'ax = findobj(''parent'', gcbf, ''tag'', ''' ax ''');' ...
								  'tmpdat = get(ax, ''userdata'');' ...
								  'if checkstatus' ... % change status of the textbox
								  '   set(textresh, ''enable'', ''on'');' ...
								  '   tmpdat{' int2str(userindex) '} = str2num(get(textresh, ''string''));' ...
								  'else' ...
								  '   set(textresh, ''enable'', ''off'');' ...
								  '   tmpdat{' int2str(userindex) '} = 0;' ...
								  'end;' ...
								  'set(ax, ''userdata'' , tmpdat);' ...
								  'rejstatepoch(''draw'');' ...
								  'clear tmpdat ax checkstatus textresh;'  ] );
	h  = uicontrol(gcf, 'Style', 'edit', 'backgroundcolor', [1 1 1], 'tag', string, 'string', num2str(init), 'enable', fastif(init == 0, 'off', 'on'), 'Units','Normalized', 'Position', pos2, ...
					'callback', [ 	'ax = findobj(''parent'', gcbf, ''tag'', ''' ax ''');' ...
								    'tmpdat = get(ax, ''userdata'');' ...
									'tmpdat{' int2str(userindex) '} = str2num(get(gcbo, ''string''));' ...
									'set(ax, ''userdata'' , tmpdat);' ...
								    'rejstatepoch(''draw'');' ...
									'clear tmpdat ax;' ] );
return;


function [C, Ceq] = multipleShooting2(z,pack,dyn)
%
% This function enforces the dynamics of the cart-pole system
% using 4th-order runge kutta:
%
%
% INPUTS:
% 	z = [N,1] = vector of decision variables
% 	pack = struct with matrix dimensions
% 	dyn = struct with physical parameters
%
% OUTPUTS:
% 	C = []
% 	Ceq = column vector of defects. Trajectory is feasible iff norm(Ceq)->0
%
% NOTES:
%  	[t,x,u] = unPackDecVar(z,pack);
%   size(t) == [1,1];
% 	size(x) == [nState, nSegment+1]
% 	size(u) == [nControl, nSubStep*nSegment+1]
%
% * The trajectory is represented with piecewise-linear control
% 	and piece-wise quartic state (implicityly).
%

[t,x,u] = unPackDecVar(z,pack);
nx = size(x,2);
nu = size(u,2);

nSubStep = (nu-1)/(nx-1);
dt = t/(nu-1);

xLow = x(:,1:(nx-1));  %Each segment starts here
xUpp = x(:,2:nx);   %And goes here

% Time-step along each trajectory segment in parallel
xStar = xLow;
uIdx = 0:nSubStep:(nu-1);
for i=1:nSubStep
	uStarLow = u(:,i + uIdx);
	uStarUpp = u(:,i+1 + uIdx);
	uStarMid = 0.5*(uStarLow+uStarUpp);
    k1 = cartPoleDynamics(xStar,uStarLow,dyn);
    k2 = cartPoleDynamics(xStar+0.5*k1,uStarMid,dyn);
    k3 = cartPoleDynamics(xStar+0.5*k2,uStarMid,dyn);
    k4 = cartPoleDynamics(xStar+k3,uStarUpp,dyn);
    xStar = xStar + (dt/6)*(k1 + 2*k2 + 2*k3 + k4);
end

% Defect constraint:
defect = xStar-xUpp;

% Pack up and return:
C = [];
Ceq = reshape(defect,numel(defect),1);

end


function [um] = au2um(au)
% Convert length from astronomical units to microns.
% Chad A. Greene 2012
um = au*1.49597870691e+17;


clc; clear all; close all;

%% plot scalp topographies of ERDs
%% get converted data

dd='dir';
filelist={'subj1','subj2'};

Result=zeros(length(filelist),1);
Result_Std=zeros(length(filelist),1);

for i=1:length(filelist)
    [cnt,mrk,mnt]=eegfile_loadMatlab([dd filelist{i}]);

    band=[8 25];
    %% Define channel layout

    grd=sprintf('C3,Cz,C4');
    mnt=mnt_setGrid(mnt,grd);

    %% band pass filter to the continuous EEG
    [b,a]=butter(5, band/cnt.fs*2);
    cnt_flt=proc_channelwise(cnt,'filtfilt',b,a);

    %% cutout segments (short-time windows) from the continuous signals
    epo=makeEpochs(cnt_flt,mrk,[-500 4000]);
    epo=proc_rectifyChannels(epo);
    epo=proc_movingAverage(epo,200,'centered');
    epo=proc_baseline(epo, [-500 0]);
    erd=proc_average(epo);

    scalpEvolutionPlusChannel(erd,mnt,'C4',1000:750:4000);
end


filename='Cantilever_quad_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'PROJECTED GRADIENT';
incrementFactor = 1;
designVariable = 'Density';
filterType = 'P1';

nsteps = 5;
Vfrac_final = 0.4;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function [anybad,wh] = iimg_check_volinfo(maskInfo,imgInfo)
% Checks a series of image .mat files and dims against a reference
% (maskInfo)
%
% :Usage:
% ::
%
%     anybad = iimg_check_volinfo(maskInfo,imgInfo)
%
% :Inputs:
%
%   **maskInfo** and **volInfo :**
%        are spm-style volume info structures
%
% see spm_vol.m

wh = [];

n = length(imgInfo);
notok = zeros(1,n);

tol = .01;

for i=1:n
    chk = abs(maskInfo.mat - imgInfo(i).mat) > tol;

    chk = any(diag(chk(1:3, 1:3)));

% %     chk = chk(:);
% %     chk = chk(1:end-1);             % eliminate SPM scale factor and translation
% %     chk1 = any(chk);

    chk2 = any(maskInfo.dim(1:3) - imgInfo(i).dim(1:3));

    notok(i) = chk | chk2;
end

anybad = any(notok);

if anybad
    wh = find(notok);

    disp('The following images'' mat files or dims differed from the first:')
    disp(num2str(wh));

    disp('First mat:');
    disp(maskInfo.mat);
    disp(maskInfo.dim);

    disp('Bad mats:');
    for i = wh
        disp(imgInfo(i).fname);
        disp(imgInfo(i).mat);
        disp(imgInfo(i).dim);
    end
end

return


function M_hat = run_mc(params)
  M = params.M;
  Idx = params.Idx;
  [numr,numc] = size(M);
  rank = 10;
  Known = find(Idx);
  data = M(Known);
  [X,Y] = lmafit_mc_adp(numr,numc,rank,Known,data,[]);
  M_hat = X*Y;
end


function [ip, deltaGPn, iter] = point_scene_to_image(S, sicd_meta, varargin)
%POINT_SCENE_TO_IMAGE Transforms a 3D ECF point to pixel row, col
% This function implements the SICD Image Projections Description Document:
% http://www.gwg.nga.mil/ntb/baseline/docs/SICD/index.html
%
% [ip, deltaGPn, iter] = point_scene_to_image(S, sicd_meta, 'PropertyName', PropertyValue, ...)
%
% Inputs:
%    S            - [3xN] N Ground Points in ECF coordinates
%    sicd_meta    - SICD meta data structure
%
%       Property name     Description
%       delta_gp_max      Ground plane displacement tol (m), default = quarter pixel
%       delta_arp         ARP position adjustable parameter (ECF, m).  Default 0.
%       delta_varp        VARP position adjustable parameter (ECF, m/s).  Default 0.
%       range_bias        Range bias adjustable parameter (m).  Default 0.
%       adj_params_frame  Coordinate frame used for expressing delta_arp
%                         and delta_varp adjustable parameters.  Allowed
%                         values: 'ECF', 'RIC_ECF', 'RIC_ECI'. Default ECF.
%
% Outputs:
%    ip           - [2xN] (row; column) coordinates of N points in image
%                   (or subimage if FirstRow/FirstCol are nonzero).
%                   Zero-based, following SICD convention (rather than
%                   MATLAB convention, which is one-based); that is,
%                   upper-left pixel is [0;0].
%    deltaGPn     - Residual ground plane displacement (m)
%    iter         - # iterations required
%
% Authors: Thomas McDowall, Harris Corporation
%          Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

%% Extract the relevant SICD info
% Convert ground / scene point to image coordinates
SCP_Row = double(sicd_meta.ImageData.SCPPixel.Row);
SCP_Col = double(sicd_meta.ImageData.SCPPixel.Col);

First_Row = double(sicd_meta.ImageData.FirstRow);
First_Col = double(sicd_meta.ImageData.FirstCol);
% We override this so that point_image_to_ground will work in coordinate
% space of full image. We will add it back in at the end.
sicd_meta.ImageData.FirstRow = 0;
sicd_meta.ImageData.FirstCol = 0;

Row_SS = sicd_meta.Grid.Row.SS;
Col_SS = sicd_meta.Grid.Col.SS;

% Parse input parameters
p = inputParser;
p.addParamValue('delta_gp_max',0.25 * sqrt(Row_SS^2 + Col_SS^2), @isscalar); % meters
% Adjustable parameters
p.addParamValue('delta_arp',[0 0 0], @(x) numel(x)==3); % ECF? (meters)
p.addParamValue('delta_varp',[0 0 0], @(x) numel(x)==3); % ECF? (meters/s)
p.addParamValue('range_bias',0, @isscalar); % meters
p.addParamValue('adj_params_frame','ECF', @(x) any(strcmpi(x,{'ECF','RIC_ECF','RIC_ECI'})));
p.FunctionName = mfilename;
p.parse(varargin{:});

uRow = [sicd_meta.Grid.Row.UVectECF.X;...
    sicd_meta.Grid.Row.UVectECF.Y;...
    sicd_meta.Grid.Row.UVectECF.Z];

uCol = [sicd_meta.Grid.Col.UVectECF.X;...
    sicd_meta.Grid.Col.UVectECF.Y;...
    sicd_meta.Grid.Col.UVectECF.Z];

%% 3.1 SCP Projection Equations
SCP = [sicd_meta.GeoData.SCP.ECF.X; sicd_meta.GeoData.SCP.ECF.Y; sicd_meta.GeoData.SCP.ECF.Z];

ARP_SCP_COA = [sicd_meta.SCPCOA.ARPPos.X; sicd_meta.SCPCOA.ARPPos.Y; sicd_meta.SCPCOA.ARPPos.Z];
VARP_SCP_COA = [sicd_meta.SCPCOA.ARPVel.X; sicd_meta.SCPCOA.ARPVel.Y; sicd_meta.SCPCOA.ARPVel.Z];

ARP_SCP_COAminusSCP = ARP_SCP_COA - SCP;
%R_SCP_COA = norm(ARP_SCP_COAminusSCP);
%Rdot_SCP_COA = (1/R_SCP_COA) * dot(VARP_SCP_COA, ARP_SCP_COAminusSCP);

% Normal to instantaneous slant plane that contains SCP at SCP COA is
% tangent to R/Rdot contour at SCP. Points away from center of Earth. Use
% LOOK to establish sign.
if strcmp(sicd_meta.SCPCOA.SideOfTrack, 'L')
    LOOK = 1;
else
    LOOK = -1;
end
SPN_SCP_COA = LOOK * cross(VARP_SCP_COA, -ARP_SCP_COAminusSCP);
uSPN = SPN_SCP_COA./repmat(sqrt(sum(SPN_SCP_COA.^2)),3,1);

%% 6.1 Scene To Image: Single Scene Point

% Spherical earth ground plane normal (exact orientation of plane is not
% critical, as long as it contains S)
if isvector(S), S = S(:); end; % Assure orientation
uGPN = S./repmat(sqrt(sum(S.^2)),3,1);

% Ground plane points are projected along straight lines to the image plane. The
% GP to IP direction is along the SCP COA slant plane normal. Also, compute
% image plane unit normal, uIPN. Compute projection scale factor SF.
uPROJ = uSPN;
IPN = cross(uRow, uCol); % should match SICD.PFA.IPN for PFA data
uIPN = IPN./repmat(sqrt(sum(IPN.^2)),3,1); % pointing away from center of earth
SF = dot(uPROJ, uIPN);

% Initialize
Gn = S;

% 2.4 - Image Plane parameters
% The following section is for ground to image with non-orthogonal axes.
% theta col is angle between uRow and uCol if not 0.
cos_theta_col = dot(uRow, uCol);
sin_theta_col = sqrt(1 - cos_theta_col.^2);
GI = (1 / sin_theta_col^2) * [1 -cos_theta_col; -cos_theta_col 1];

done = false;

% Iterate the ground to image transform
iter = ones(1,size(S,2));
to_iter = true(1,size(S,2));
ip = zeros(2,size(S,2));
deltaPn = zeros(3,size(S,2));
deltaGPn = zeros(1,size(S,2));
while(~done && all(iter < 6))
    % Project ground plane point Gn to image plane point In. The projection
    % distance is DISTn. Compute image coordinates xrow and ycol.
    DISTn = (1/SF) * dot((repmat(SCP,1,size(Gn,2)) - Gn), repmat(uIPN,1,size(Gn,2)));
    In = Gn + repmat(DISTn,3,1) .* repmat(uPROJ,1,size(DISTn,2));

    % For a point at IPP, corresponding SCP pixel-centered coord is
    deltaIPP = In - repmat(SCP,1,size(In,2));
    ip_iter = GI * ...
        [dot(deltaIPP, repmat(uRow,1,size(In,2))); ...
        dot(deltaIPP, repmat(uCol,1,size(In,2)))];

    xrow = ip_iter(1,:);
    ycol = ip_iter(2,:);

    irow = xrow / Row_SS;
    icol = ycol / Col_SS;

    row = irow + SCP_Row;
    col = icol + SCP_Col;

    ip(:,to_iter) = [row; col];

    % Transform to ground plane containing the scene point(s) S
    Pn = point_image_to_ground(ip(:,to_iter), sicd_meta, ...
        'projection_type', 'plane', 'gref', S(:,to_iter), 'ugpn', uGPN(:,to_iter), ...
        ... % Pass through adjustable parameters
        'delta_arp', p.Results.delta_arp, ...
        'delta_varp', p.Results.delta_varp, ...
        'range_bias', p.Results.range_bias, ...
        'adj_params_frame', p.Results.adj_params_frame);

    %   gndPt = ecfToGeodetic(Pn');
    %   fprintf('Ground Point: %f %f %f\n', gndPt(1), gndPt(2), gndPt(3));

    % Compute displacement between Pn and S
    deltaPn(:,to_iter) = S(:,to_iter) - Pn;
    deltaGPn(to_iter) = sqrt(sum(deltaPn(:,to_iter).^2));

    old_iter = to_iter;
    to_iter = deltaGPn > p.Results.delta_gp_max; % Need to iterate further on these points
    if any(to_iter)
        Gn = Gn(:,to_iter(old_iter)) + deltaPn(:,to_iter);
        iter(to_iter) = iter(to_iter) + 1;
    else
        done = true;
    end

end
if iter > 5
    warning('point_scene_to_image:SolutionDidNotConverge',...
        'Solution for image coordinate did not converge.');
end

ip(1,:) = ip(1,:) - First_Row;
ip(2,:) = ip(2,:) - First_Col;

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


function test_issue1632

% MEM 4gb
% WALLTIME 00:10:00
% DEPENDENCY ft_compute_leadfield

% https://github.com/fieldtrip/fieldtrip/issues/1632
% This script will demonstrate that with ft_prepare leadfields, the
% leadfields are calculated for every coil and not every sensor.
% Subsequently, it will show that ft_sourceanalysis lines 437-443 truncate
% the leadfield matrix, removing all lines beyond the number of sensors.

% The mat-file includes:
% data (emptied except for grad and label) from ftp://ftp.fieldtriptoolbox.org/pub/fieldtrip/tutorial/beamformer_lcmv/
% headmodel created with ft_prepare_mesh and ft_prepare_headmodel with
% method 'openmeeg', based on segmentedmri.mat from ftp://ftp.fieldtriptoolbox.org/pub/fieldtrip/tutorial/beamformer_lcmv/
% Author: B Knipscheer 13-01-2021

load(dccnpath('/home/common/matlab/fieldtrip/data/test/issue1632.mat'));

cfg                  = [];
cfg.grad             = data.grad;
cfg.headmodel        = headmodel;
cfg.sourcemodel.pos  = [0 0 0.07];
cfg.channel          = 'MEG';
cfg.normalize        = 'column';
sourcemodel          = ft_prepare_leadfield(cfg);

fprintf('Number of good MEG sensors = %d\n', size(data.label,1));  % There are 274 MEG sensors
fprintf('Number of coils matching the MEG sensors = %d\n', size(data.grad.tra,2)); % There are 548 MEG coils for the 274 MEG sensors.
fprintf('Number of leadfield rows for point 1 before ft_sourceanalysis = %d\n', size(sourcemodel.leadfield{1},1));

assert(isequal(size(sourcemodel.leadfield{1},1), numel(data.label)));


function out = draw_arrow(startpoint,endpoint,headsize)
%by Ryan Molecke
% accepts two [x y] coords and one double headsize

   v1 = headsize*(startpoint-endpoint)/2.5;

   theta = 22.5*pi/180;
   theta1 = -1*22.5*pi/180;
   rotMatrix = [cos(theta)  -sin(theta) ; sin(theta)  cos(theta)];
   rotMatrix1 = [cos(theta1)  -sin(theta1) ; sin(theta1)  cos(theta1)];

   v2 = v1*rotMatrix;
   v3 = v1*rotMatrix1;
   x1 = endpoint;
   x2 = x1 + v2;
   x3 = x1 + v3;
hold on;
   fill([x1(1) x2(1) x3(1)],[x1(2) x2(2) x3(2)],[0 0 0]);     % this fills the arrowhead (black)
   plot([startpoint(1) endpoint(1)],[startpoint(2) endpoint(2)],'linewidth',2,'color',[0 0 0]);


function [x, y, model, pca_model] = train(total)
% X = features
% Y = label
x = [];
y = {};

for n = 1:length(total)-1
    display(['Reading img',num2str(n)])
    for i = 1:8
        for j = 1:8
            y_temp = total{n+1,3}{i,j};
            if strcmp(y_temp,'')
                continue;
            end
            fields = fieldnames(total{n+1,2}{i,j});
            x_temp = zeros(1,length(fields));
            for f = 1:length(fields)
                x_temp(f) = [getfield(total{n+1,2}{i,j},fields{f})];
            end

            x = [ x ; x_temp ];
            y = [ y ; y_temp ];
        end
    end
end

[x, pca_x,pca_coeff] = preprocess(x);
save('pca_coeff.mat','pca_coeff')

model = fitcknn(x,y,'NumNeighbors',15);
save('model.mat','model')

pca_model = fitcknn(pca_x,y,'NumNeighbors',15);
save('pca_model.mat','pca_model')


function varargout = flipdim(varargin)
%FLIPDIM   Flip/reverse a CHEBFUN2 in a chosen direction.
%   G = FLIPDIM(F, DIM) returns a CHEBFUN2 G with the same domain as F but
%   reversed in a direction, i.e., G(x,y)=F(x, c+d-y). If DIM = 2 (default) then
%   G(x,y) = F(x, c+d-y).  Otherwise DIM = 1 and G(x,y) = F(a+b-x, y). The
%   domain of F is [a, b, c, d].
%
% See also FLIPLR, FLIPUD.

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

[varargout{1:nargout}] = flipdim@separableApprox(varargin{:});

end


% Author: Ricardo Baptista and Matthias Poloczek
% Date:   June 2018
%
% See LICENSE.md for copyright information
%

function particle_system = smc_resample(particle_system)
% SMC_RESAMPLE: Function resamples binary particles (or models) based on
% the effective sample size at the current iteration.

% Extract inputs from particle_system
models     = particle_system.models;
model_val  = particle_system.model_val;
weights    = particle_system.weights;

% Generate new particles and data
[new_models, new_idx] = binary_resampling(models, weights);
n_models = size(new_models,1);

% Assign uniform weights
new_weights  = 1/n_models*ones(n_models,1);

% Re-order model data
model_val  = model_val(new_idx);

% Save models and weights
particle_system.models     = new_models;
particle_system.model_val  = model_val;
particle_system.weights    = new_weights;

end


%DIAGNOSTIC TOOLS (in the diag-folder):
%
% Convergence diagnostics
%   PSRF     - Potential Scale Reduction Factor
%   CPSRF    - Cumulative Potential Scale Reduction Factor
%   MPSRF    - Multivariate Potential Scale Reduction Factor
%   CMPSRF   - Cumulative Multivariate Potential Scale Reduction Factor
%   IPSRF    - Interval-based Potential Scale Reduction Factor
%   CIPSRF   - Cumulative Interval-based Potential Scale Reduction Factor
%   KSSTAT   - Kolmogorov-Smirnov goodness-of-fit hypothesis test
%   HAIR     - Brooks' hairiness convergence diagnostic
%   CUSUM    - Yu-Mykland convergence diagnostic for MCMC
%   SCORE    - Calculate score-function convergence diagnostic
%   GBINIT   - Initial iterations for Gibbs iteration diagnostic
%   GBITER   - Estimate number of additional Gibbs iterations
%
% Time series analysis
%   ACORR      - Estimate autocorrelation function of time series using xcorr
%   ACORR2     - Estimate autocorrelation function of time series using fft
%   ACORRTIME  - Estimate autocorrelation evolution of time series (simple)
%   GEYER_ICSE - Compute autocorrelation time tau using Geyer's
%                initial convex sequence estimator
%                (requires Optimization toolbox)
%   GEYER_IMSE - Compute autocorrelation time tau using Geyer's
%                initial monotone sequence estimator
%
% Survival model criteria
%   AUCS       - Compute area under curve for survival model
%   AUCT       - Compute area under curve for survival model at given time
%   EXT_AUC    - Compute Extended AUC proposed by Chambless et al (2011)
%   HCS        - Compute Harrell's C for survival model at given time
%   HCT        - Compute Harrel's C for survival model at several time points
%   IDIS       - Integrated Discrimination Improvement between two models
%   RSQR       - R^2 statistic given probabilities at time point T
%
% Kernel density estimation etc.:
%   KERNEL1  - 1D Kernel density estimation of data
%   KERNELS  - Kernel density estimation of independent components of data
%   KERNELP  - 1D Kernel density estimation, with automatic kernel width
%   NDHIST   - Normalized histogram of N-dimensional data
%   HPDI     - Estimates the Bayesian HPD intervals
%
% Misc:
%   CUSTATS   - Calculate cumulative statistics of data
%   GRADCHEK  - Checks a user-defined gradient function using finite
%               differences.
%   DERIVATIVECHECK - Compare user-supplied derivatives to
%                     finite-differencing derivatives.
%