function [SPM] = spm_fMRI_design(SPM,save_SPM)
% Assembles a design for fMRI studies
% FORMAT [SPM] = spm_fMRI_design(SPM)
%
% 1st level
%--------------------------------------------------------------------------
% SPM.
%       xY: [1x1 struct] - data structure
%    nscan: [1xs double] - nscan(s) = number of scans in session s
%      xBF: [1x1 struct] - Basis function structure
%     Sess: [1xs struct] - Session structure array
%       xX: [1x1 struct] - Design matrix structure
%
%
%    2nd level
%    ----------------------------------------------------------------------
%    SPM.xY
%           RT: - repetition time {seconds)
%
%    SPM.xBF
%            T: - microtime resolution (number of time bins per scan)
%           T0: - microtime onset (reference time bin, see slice timing)
%        UNITS: - 'scans'|'secs' - units in which onsets are specified
%     Volterra: - 1|2 - order of [Volterra] convolution
%           dt: - length of time bin {seconds}
%         name: - name of basis set
%       length: - support of basis set {seconds}
%        order: - order of basis set
%           bf: - basis set matrix
%
%    SPM.Sess(s)
%            U: - Input structure array
%            C: - User specified covariate structure
%          row: - scan   indices for session s
%          col: - effect indices for session s
%           Fc: - F Contrast information for input-specific effects
%
%    SPM.xX
%            X: - design matrix
%           iH: - vector of H partition (indicator variables) indices
%           iC: - vector of C partition (covariates)          indices
%           iB: - vector of B partition (block effects)       indices
%           iG: - vector of G partition (nuisance variables)  indices
%         name: - cellstr of names for design matrix columns
%
%
%        3rd level
%        ------------------------------------------------------------------
%        SPM.Sess(s).U
%               dt: - time bin length {seconds}
%             name: - {1 x j} cell of names for each input or cause
%              ons: - (q x 1) onsets for q  trials {in UNITS}
%              dur: - (q x 1) durations for trials {in UNITS}
%                P: - Parameter stucture
%                u: - (t x j) inputs or stimulus function matrix
%              pst: - (1 x k) peristimulus times (seconds)
%             orth: - boolean: orthogonalise inputs?
%
%
%        SPM.Sess(s).C
%
%                C: - [kx1 double] of user specified regressors
%             name: - {1xk} cellstr of regressor names
%
%
%        SPM.Sess(s).Fc
%
%                i: - indices pertaining to each input
%             name: - names pertaining to each input
%                p: - grouping of regressors per parameter
%
%
%            4th level
%            --------------------------------------------------------------
%            SPM.Sess(s).U(i).P(p)
%
%                 name: - parameter name
%                    P: - (q x 1) parameter matrix
%                    h: - order of polynomial expansion (0 = none)
%                    i: - sub-indices of U(i).u for plotting
%
%
% saves SPM.mat if save_SPM==1 (this is the default)
%__________________________________________________________________________
%
% spm_fMRI_design allows you to build design matrices with separable
% session-specific partitions.  Each partition may be the same (in which
% case it is only necessary to specify it once) or different.  Responses
% can be either event- or epoch related, where the latter model prolonged
% and possibly time-varying responses to state-related changes in
% experimental conditions.  Event-related response are modelled in terms
% of responses to instantaneous events.  Mathematically they are both
% modelled by convolving a series of delta (stick) or box-car functions,
% encoding the input or stimulus function. with a set of hemodynamic
% basis functions.
%
% spm_fMRI_design allows you to combine both event- and epoch-related
% responses in the same model and/or regressor. You specify the number
% of trial (event or epoch) types.  Epoch and event-related
% responses are modeled in exactly the same way by first specifying their
% onsets [in terms of onset times] and then their durations.  Events are
% specified with a duration of 0.  If you enter a single number for the
% durations it will be assumed that all trials conform to this duration.
%
% Interactions or response modulations can enter at two levels.  Firstly
% the stick function itself can be modulated by some parametric variate
% (this can be time or some trial-specific variate like reaction time)
% modeling the interaction between the trial and the variate or, secondly
% interactions among the trials themselves can be modeled using a Volterra
% series formulation that accommodates interactions over time (and therefore
% within and between trial types).  The first sort of interaction is
% specified by extra (modulated) stick functions in Sess(s).u.  If
% a polynomial expansion of the specified variate is requested there will
% be more than one column.  The corresponding name of the explanatory
% variables in X.name is Sn(s) trial(u)xparam(p)^q*bf(i) for the qth
% order expansion of the parameter convolved with the ith basis function
% for the uth trial in the sth session.  If no parametric variate is
% specified the name is simply Sn(s) trial(u)*bf(i).  Interactions among
% and within trials enter as new trial types but do not have .pst or .ons
% fields.  These interactions can be characterized later, in results, in
% terms of the corresponding second order Volterra Kernels.
%
% The design matrix is assembled on a much finer time scale (xBF.dt) than the
% TR and is then sub-sampled at the acquisition times.  After down-sampling
% the regressors for each input are othogonalised.  This ensures that
% components due to the canonical hrf are not explained away by other basis
% functions or parametric modulators.
%
% Sess(s).ons(u) contains onset times in seconds or scans relative to the
% timing of the first scan
%
% Notes on spm_get_ons, spm_get_bf and spm_Volterra are included below
% for convenience.
%
%                           ----------------
%
% spm_get_ons contructs a struct array containing sparse input
% functions U(i).u specifying occurrence events or epochs (or both).
% These are convolved with a basis set at a later stage to give
% regressors that enter into the design matrix. Interactions of evoked
% responses with some parameter (time or a specified  variate P) enter at
% this stage as additional columns in U(u).u with each trial multiplied
% by the [expansion of the] trial-specific parameter. If parametric
% modulation is modeled, P(p).P contains the original variate and
% P(p).name is its name. The 0th order expansion of this is simply the main
% effect in the first column of U(u).u
%
%                           ----------------
%
% spm_get_bf prompts for basis functions to model hemodynamic
% responses.  The basis functions returned are orthogonalized
% and defined as a function of peri-stimulus time in time-bins.
%
%                           ----------------
%
% For first order expansions spm_Volterra simply convolves the causes
% (e.g. stick functions) in U(u).u by the basis functions in Sess(s).bf
% to create design matrix X.  For second order expansions new entries appear
% in the design matrix that correspond to the hemodynamic interaction among the
% orginal causes (if the events are sufficiently close in time).
% The basis functions for these are two dimensional and are used to
% assemble the second order kernel in spm_graph.m.  Second order effects
% are computed for only the first column of U(u).u.
%
%__________________________________________________________________________
% Copyright (C) 1999-2012 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_fMRI_design.m 7739 2019-12-02 14:00:18Z guillaume $

SVNid = '$Rev: 7739 $';

%-Say Hello
%--------------------------------------------------------------------------
spm('FnBanner',mfilename,SVNid);

%-Construct Design matrix {X}
%==========================================================================

%-Microtime onset and microtime resolution
%--------------------------------------------------------------------------
try
    fMRI_T     = SPM.xBF.T;
    fMRI_T0    = SPM.xBF.T0;
catch
    fMRI_T     = spm_get_defaults('stats.fmri.t');
    fMRI_T0    = spm_get_defaults('stats.fmri.t0');
    SPM.xBF.T  = fMRI_T;
    SPM.xBF.T0 = fMRI_T0;
end

%-Time units, dt = time bin {secs}
%--------------------------------------------------------------------------
SPM.xBF.dt     = SPM.xY.RT/SPM.xBF.T;

%-Get basis functions
%--------------------------------------------------------------------------
SPM.xBF        = spm_get_bf(SPM.xBF);

%-Get session specific design parameters
%==========================================================================
Xx    = [];
Xb    = [];
Xname = {};
Bname = {};
for s = 1:length(SPM.nscan)

    %-Number of scans for this session
    %----------------------------------------------------------------------
    k = SPM.nscan(s);

    %-Create convolved stimulus functions or inputs
    %======================================================================

    %-Get inputs, neuronal causes or stimulus functions U
    %----------------------------------------------------------------------
    U = spm_get_ons(SPM,s);

    %-Convolve stimulus functions with basis functions
    %----------------------------------------------------------------------
    [X,Xn,Fc] = spm_Volterra(U, SPM.xBF.bf, SPM.xBF.Volterra);

    %-Resample regressors at acquisition times (32 bin offset)
    %----------------------------------------------------------------------
    if ~isempty(X)
        X = X((0:(k - 1))*fMRI_T + fMRI_T0 + 32,:);
    end

    %-Orthogonalise (within trial type)
    %----------------------------------------------------------------------
    for i = 1:length(Fc)
        if i<= numel(U) && ... % for Volterra kernels
                (~isfield(U(i),'orth') || U(i).orth)
            p = ones(size(Fc(i).i));
        else
            p = Fc(i).p;
        end
        for j = 1:max(p)
            X(:,Fc(i).i(p==j)) = spm_orth(X(:,Fc(i).i(p==j)));
        end
    end

    %-Get user specified regressors
    %======================================================================
    C     = SPM.Sess(s).C.C;
    Cname = SPM.Sess(s).C.name;

    %-Append mean-corrected regressors and names
    %----------------------------------------------------------------------
    X     = [X spm_detrend(C)];
    Xn    = {Xn{:} Cname{:}};

    %-Confounds: Session effects
    %======================================================================
    B     = ones(k,1);
    Bn    = {'constant'};

    %-Session structure array
    %======================================================================
    SPM.Sess(s).U      = U;
    SPM.Sess(s).C.C    = C;
    SPM.Sess(s).C.name = Cname;
    SPM.Sess(s).row    = size(Xb,1) + (1:k);
    SPM.Sess(s).col    = size(Xx,2) + (1:size(X,2));
    SPM.Sess(s).Fc     = Fc;

    %-Append into Xx and Xb
    %======================================================================
    Xx      = blkdiag(Xx,X);
    Xb      = blkdiag(Xb,B);

    %-Append names
    %----------------------------------------------------------------------
    for i = 1:length(Xn)
        Xname{end + 1} = [sprintf('Sn(%i) ',s) Xn{i}];
    end
    for i = 1:length(Bn)
        Bname{end + 1} = [sprintf('Sn(%i) ',s) Bn{i}];
    end

end

%-Place design matrix structure in xX
%==========================================================================
SPM.xX.X    = [Xx Xb];
SPM.xX.iH   = [];
SPM.xX.iC   = 1:size(Xx,2);
SPM.xX.iB   = (1:size(Xb,2)) + size(Xx,2);
SPM.xX.iG   = [];
SPM.xX.name = {Xname{:} Bname{:}};

if nargin < 2 || save_SPM
    %-Save SPM.mat
    %----------------------------------------------------------------------
    fprintf('%-40s: ','Saving fMRI design')                             %-#
    fmt = spm_get_defaults('mat.format');
    s = whos('SPM');
    if s.bytes > 2147483647, fmt = '-v7.3'; end
    save('SPM.mat', 'SPM', fmt);
    fprintf('%30s\n','...SPM.mat saved');
end


function c=countif(vector,x)
%COUNTIF adalah fungsi untuk menghitung jumlah kehadiran bilangan dalam
%suatu array
[m,n]=size(vector);
d=0;
for a=1:m
    if vector(a)==x
    d=d+1;
    c=d;
    else
    c=d;
    end
end
c;


% poolType = 'mean' or 'max'
% poolLayer = integer layer number after which we do the pooling
function layer = genNetworkTemporalConvLSTM(para)

layer = genNetworkTemporalConv2(para);
useFirstNLayer = length(para.nFilter)*3+1;
layer = layer(1:useFirstNLayer);

paraLSTM = para;
paraLSTM.inputDim = layer{end}.dim(1);
layerLSTM = genNetworkLSTM(paraLSTM);

layer = [layer layerLSTM(2:end)];

end


function [errorsA, errorsLUB, stuckPoints] = verifyPoints(sampleStruct)
% Verify that a set of points are in the solutoin space of `sampleStruct`.
% Typically, this method would be called to check a set of warmup points or
% points generated via `gpSampler`. Also verifies if points moved from
% warmup points.
%
% USAGE:
%
%    [errorsA, errorsLUB, stuckPoints] = verifyPoints(sampleStruct)
%
% INPUT:
%    sampleStruct:    LPProblem containing points and warmup points
%
% OUTPUTS:
%    errorsA:         Row index of the constraint in `sampleStruct` that
%                     is not consistent with the given points
%    errorsLUB:       Upper and lower bounds of the constraint + tolerance
%    stuckPoints:     Index of points which did not move.
%
% .. Authors:
%       - Ellen Tsai 2007
%       - Richard Que 12/1/09 Combined with checkWP.m

[warmupPts, points] = deal(sampleStruct.warmupPts, sampleStruct.points);
%above a check to see if warmup points moved
n=size(points,2);
len=size(n,1);
stuckPoints=zeros(0,1);

for i=1:n
    len(i,1)=norm(warmupPts(:,i)-points(:,i));
    if len(i,1)<10
        stuckPoints=[stuckPoints; i];
    end
end
minL = min(len);

%check to see if points are within solution space
[A,b,csense]=deal(sampleStruct.A,sampleStruct.b,sampleStruct.csense);
t=.01; %tolerance
npoints = size(points, 2);

LHS = A*points;
EIndex = (csense == 'E');
LIndex = (csense == 'L');
GIndex = (csense == 'G');

[errorsL(:,1), errorsL(:,2)] = find(LHS(LIndex,:) - b(LIndex)* ones(1, npoints) > t);
[errorsG(:,1), errorsG(:,2)] = find(b(GIndex)* ones(1, npoints) - LHS(GIndex,:)   > t);
[errorsE(:,1), errorsE(:,2)] = find( abs(LHS(EIndex,:) - b(EIndex)* ones(1, npoints))   > t);

errorsUB = find(points > sampleStruct.ub*ones(1,npoints) + t);
errorsLB = find(points < sampleStruct.lb*ones(1,npoints) - t);

errorsA = [errorsL; errorsG; errorsE];
errorsLUB = [errorsLB; errorsUB];

end


function [x,zo]=overlapadd(f,win,inc)
%OVERLAPADD join overlapping frames together X=(F,WIN,INC)
%
% Usage for frequency-domain processing:
%       S=...;                              % input signal
%       OV=2;                               % overlap factor of 2 (4 is also often used)
%       INC=20;                             % set frame increment in samples
%       NW=INC*OV;                          % DFT window length
%       W=sqrt(hamming(NW,'periodic'));     % omit sqrt if OV=4
%       W=W/sqrt(sum(W(1:INC:NW).^2));      % normalize window
%       F=rfft(enframe(S,W,INC),NW,2);      % do STFT: one row per time frame, +ve frequencies only
%       ... process frames ...
%       X=overlapadd(irfft(F,NW,2),W,INC);  % reconstitute the time waveform (omit "X=" to plot waveform)
%
% Inputs:  F(NR,NW) contains the frames to be added together, one
%                   frame per row.
%          WIN(NW)  contains a window function to multiply each frame.
%                   WIN may be omitted to use a default rectangular window
%                   If processing the input in chunks, WIN should be replaced by
%                   ZI on the second and subsequent calls where ZI is the saved
%                   output state from the previous call.
%          INC      gives the time increment (in samples) between
%                   succesive frames [default = NW].
%
% Outputs: X(N,1) is the output signal. The number of output samples is N=NW+(NR-1)*INC.
%          ZO     Contains the saved state to allow a long signal
%                 to be processed in chunks. In this case X will contain only N=NR*INC
%                 output samples.
%

%	   Copyright (C) Mike Brookes 2009
%      Version: $Id: overlapadd.m 2470 2012-11-02 15:27:24Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[nr,nf]=size(f);            % number of frames and frame length
if nargin<2
    win=nf;                 % default increment
end
if isstruct(win)
    w=win.w;
    if ~numel(w) && length(w)~=nf
        error('window length does not match frames size');
    end
    inc=win.inc;
    xx=win.xx;
else
    if nargin<3
        inc=nf;
    end
    if numel(win)==1 && win==fix(win) && nargin<3       % win has been omitted
        inc=win;
        w=[];
    else
        w=win(:).';
        if length(w)~=nf
            error('window length does not match frames size');
        end
        if all(w==1)
            w=[];
        end
    end
    xx=[];      % partial output from previous call is null
end
nb=ceil(nf/inc);        % number of overlap buffers
no=nf+(nr-1)*inc;       % buffer length
z=zeros(no,nb);                      % space for overlapped output speech
if numel(w)
    z(repmat(1:nf,nr,1)+repmat((0:nr-1)'*inc+rem((0:nr-1)',nb)*no,1,nf))=f.*repmat(w,nr,1);
else
    z(repmat(1:nf,nr,1)+repmat((0:nr-1)'*inc+rem((0:nr-1)',nb)*no,1,nf))=f;
end
x=sum(z,2);
if ~isempty(xx)
    x(1:length(xx))=x(1:length(xx))+xx;     % add on leftovers from previous call
end
if nargout>1            % check if we want to preserve the state
    mo=inc*nr;          % completed output samples
    if no<mo
        x(mo,1)=0;
        zo.xx=[];
    else
        zo.xx=x(mo+1:end);
        zo.w=w;
        zo.inc=inc;
        x=x(1:mo);
    end
elseif ~nargout
    if isempty(xx)
        k1=nf-inc;  % dubious samples at start
    else
        k1=0;
    end
    k2=nf-inc;      % dubious samples at end
    plot(1+(0:nr-1)*inc,x(1+(0:nr-1)*inc),'>r',nf+(0:nr-1)*inc,x(nf+(0:nr-1)*inc),'<r', ...
        1:k1+1,x(1:k1+1),':b',k1+1:no-k2,x(k1+1:end-k2),'-b',no-k2:no,x(no-k2:no),':b');
    xlabel('Sample Number');
    title(sprintf('%d frames of %d samples with %.0f%% overlap = %d samples',nr,nf,100*(1-inc/nf),no));
end


function sF = power(sF1,sF2)
%
% Syntax
%   sF = sF1.^a
%

if isnumeric(sF1)
  f = @(v) sF1 .^ eval(sF2, v);

  bw = sF1 * sF2.bandwidth;
  sF = S2FunHarmonic.quadrature(f,'bandwidth',min(bw,256));
elseif isnumeric(sF2)
  f = @(v) eval(sF1, v) .^ sF2;

  bw = sF1.bandwidth * sF2;
  sF = S2FunHarmonic.quadrature(f,'bandwidth',min(bw,256));
else
  f = @(v) eval(sF1, v) .^ eval(sF2, v);

  bw = max(sF1.bandwidth, sF2.bandwidth);
  sF = S2FunHarmonic.quadrature(f,'bandwidth',min(bw,256));
end

end


function [grad,isBdNode] = gradmatrix(edge,isBdEdge)
%% GRADMATRIX matrix for the gradient of a nodal linear element
%
% grad = gradmatrix(edge) returns the sparse matrix grad which is an NE by 3N
% matrix mapping linear nodal element (a vector with length N) to lowest
% order linear edge element (a vector with length NE). The trace is zero at
% certain boundary edges given by isBdEdge.
%
% grad = gradmatrix(edge) returns the corresponding matrix without boundary
% conditions.
%
% This function is used in mgMaxwell to construct transfer operators for
% the HX preconditioner.
%
% See also mgMaxwell, mgMaxwell1, mgMaxwell2, node2edgematrix1
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if nargin<=1, isBdEdge = []; end
NE = size(edge,1); N  = double(max(edge(:)));
i = repmat((1:NE)',2,1);
j = double(edge(:));
s = [-ones(NE,1),ones(NE,1)];
if any(isBdEdge) % no grad on boundary edges
    bdEdge = edge(isBdEdge,:);
    isBdNode = false(N,1);
    isBdNode(bdEdge(:)) = true;
    idx = ~(isBdEdge(i) | isBdNode(j));
else
    idx = 1:length(i);
end
grad = sparse(i(idx),j(idx),s(idx),NE,N);


%These tests are using the Human-GEM model.

%%%%%%%%%%%%%%%%%%%%%%%%%%
% TC0901 - Test that all reactions in the minimal model to be
%          used by the MILP can carry flux > 0.1
%%%%%%%%%%%%%%%%%%%%%%%%%%

%In the MILP, the flux of reactions with negative rxn scores cannot exceed 100
%At the same time, the flux of positive reactions need to be >= 0.1 to be able
%to turn them on. Likewise, we force the flux on essential reactions to be >= 0.1.
%Therefore, it is important that all reactions can get a flux > 0.1, while all
%reactions are limited to a flux <= 100. We can test this in a similar way to the
%haveFlux function in RAVEN, so that code is copied here and modified.

%This code is partly copied from the function haveFlux
%The prepData takes a couple of hours to generate. If you have it somewhere,
%better to load it
%cd C:\Work\MatlabCode\components\human-GEM\Human-GEMftINIT\Human-GEM %replace this with the root of the Human-GEM repo.
%ihuman = importYaml('model/Human-GEM.yml');
%save('model/Human-GEM.mat', 'ihuman');
%load('model/Human-GEM.mat')
%prepData = prepHumanModelForftINIT(ihuman, false);
prepData2 = load('model/PrepData2.mat').prepData;

upperLimit = 100;
lowerLimit = 0.1;

model = prepData2.minModel;
model = prepData.minModel;

model.lb(model.lb==-inf)=-upperLimit;
model.ub(model.ub==inf)=upperLimit;
model.lb(model.lb==-1000)=-upperLimit;
model.ub(model.ub==1000)=upperLimit;

%check
unique(model.lb) %ok, only 0 and -100
unique(model.ub) %ok, only 100

%First make a loop where we optimize for all
onFwd = false(numel(model.rxns,1));
onRev = false(numel(model.rxns,1));

iter = 1;
while true
    disp([num2str(iter) ': ' num2str(sum(onFwd))])
    model.c=ones(numel(model.c),1);
    model.c(onFwd) = 0; %don't include the ones already verified

    sol=solveLP(model);
    if isempty(sol.x)
        disp('Failed');
        break;
    end

    currRes = sol.x > lowerLimit;
    if sum(currRes & ~onFwd) == 0
        break;%we didn't turn any more rxns on
    end

    onFwd = onFwd | currRes;
    iter = iter + 1;
end

sum(~onFwd)%zero, perfect

%Now the other direction (reversible):
onRev = false(numel(model.rxns,1));

iter = 1;
while true
    disp([num2str(iter) ': ' num2str(sum(onRev))])
    model.c=-ones(numel(model.c),1);
    model.c(~model.rev) = 0;
    model.c(onRev) = 0; %don't include the ones already verified

    sol=solveLP(model);
    if isempty(sol.x)
        disp('Failed');
        break;
    end

    currRes = sol.x < -lowerLimit;
    if sum(currRes & ~onRev) == 0
        break;%we didn't turn any more rxns on
    end

    onRev = onRev | currRes;
    iter = iter + 1;
end

sum(~onRev(model.rev == 1))%

onRev = false(numel(model.rxns,1));

indLeft = find(model.rev == 1 & ~onRev);

constructEquations(model, model.rxns(indLeft))%no scaling here...

%go trough the last one by one
for i = 1:numel(indLeft)
    model.c=zeros(numel(model.c),1);
    model.c(indLeft(i)) = -1;

    sol=solveLP(model);
    if isempty(sol.x)
        disp('Failed');
        break;
    end

    currRes = sol.x(indLeft(i)) < -lowerLimit;
    disp([num2str(indLeft(i)) ': ' num2str(currRes)])
    onRev(indLeft(i)) = currRes;
    onRev = onRev | currRes;
    iter = iter + 1;
end

sum(~onRev(model.rev == 1))%0, perfect. This means that we can use the limits 0.1 and 100 in the MILP!


function t=talACPCPointsToInplane
% t=talACPCPointsToInplane
%
% Computes the coordinates of the talairach anchor points (ac,pc, sac
% etc..) in the coordinate frame of the inplanes. This is useful when
% converting all the data to analyze format for going into SPM
% Assumes that the talairach points exist in the usual anatomy directory and that mrSESSION.alignment exists.
% arw 110105 : Wrote it.
%

% Get voxel sizes to make sure that the transformation preserves volume
mrGlobals;

ipVoxSize = mrSESSION.inplanes.voxelSize;
volVoxSize = readVolAnatHeader(vANATOMYPATH);

% Transform ROI coordinates
xform = inv(mrSESSION.alignment);
talPoints = loadTalairachXform(mrSESSION.subject);
talPoints.refPoints

rp=talPoints.refPoints;

acpc=[talPoints.refPoints.acXYZ;talPoints.refPoints.pcXYZ];
disp(acpc)
acpc2=acpc(:,[2 1 3]);

[d,t]= xformROIcoords(acpc2',xform,volVoxSize,ipVoxSize)

disp('Note: Y axis is anterior->posterior, X axis is left to right on screen (right to left in brain)');
disp('AC Coords (y,x,slice) :');
disp(round(t(1:3,1)));
disp('PC Coords (y,x,slice) :');
disp(round(t(1:3,2)));

t=t(1:3,:);


plot(rho,h,'LineWidth',2,'Color','g'),grid,ylabel('Altitude: h [m]'),xlabel('Density: \rho [kg/m^3]')


function [ spiketimes_jitt ] = JitterSpiketimes(spiketimes,jitterwin,varargin)
%[ spiketimes ] = JitterSpiketimes(spiketimes,jitterwin) jitters spiketimes
%of all cells within a designated window.
%
%INPUT
%   spiketimes  {Ncells} cell array of [Nspikes] vectors of spiketimes for
%               each cell (seconds)
%   jitterwin   time window within which to jitter spike times (seconds)
%
%   (options)
%       'jittertype'    'window' or 'centered'.
%                       -'centered' jitters each spike around its time.
%                       -'window' jitters within its window (more rigorous, default)
%                           see Jonathan Platkewitz paper
%
%OUTPUT
%   spiketimes  jittered spiketimes
%
%
%DLevenstein 2016
%%
%parse args
p = inputParser;
addParameter(p,'jittertype','window')

parse(p,varargin{:})
jittertype = p.Results.jittertype;

%%
numcells = length(spiketimes);
if isa(spiketimes,'tsdArray')
    for c = 1:numcells
        spiketimestemp{c} = Range(spiketimes{c},'s');
    end
    spiketimes = spiketimestemp;
    clear spiketimestemp
end

%Do this with cellfun....
switch jittertype
    case 'centered'
        for c = 1:numcells
            spiketimes_jitt{c} = spiketimes{c}+2*jitterwin*rand(size(spiketimes{c}))-jitterwin;
        end
    case 'window'
        %Question: add global offset, so not always starting windows at same
        %points in recording?
        spiketimes_jitt = cellfun(@(X) ...
            jitterwin.*(floor(X/jitterwin)+rand(size(X))),...
            spiketimes,'UniformOutput',false);
end

%Re-sort
spiketimes_jitt = cellfun(@(X) sort(X),spiketimes_jitt,'UniformOutput',false);

end


function test_online_nmf(varargin)
% Test file for stochastic/online NMF.
%
% Created by H.Kasai on Apr. 17, 2018

    if nargin < 1
        clc;
        clear;
        close all;
        rng('default')

        m = 500;
        n = 100;
        V = rand(m,n);
        rank = 20;
        options = [];
        options.verbose = 1;
        max_epoch = 100;
        options.max_epoch = max_epoch;
        health_check_mode = false;
        calc_sol = 1;
        outlier_rho = 0.0;
        lambda = 1;
    else
        V = varargin{1};
        rank = varargin{2};
        options = varargin{3};
        max_epoch = options.max_epoch;
        calc_sol = 0;
        health_check_mode = true;
    end

    max_h_repeat    = 5;
    svrmu_inneriter = 5;

    % initialize factor matrices
    [x_init, ~] = generate_init_factors(V, rank, []);
    options.x_init = x_init;

    %% calc solution
    if calc_sol

        clear options_sol;
        options_sol.max_epoch = max_epoch*100;
        options_sol.x_init = x_init;
        options_sol.verbose = 0;
        options_sol.verbose = 1;

        if ~outlier_rho
            fprintf('Calculating f_opt by HALS ...\n');
            options_sol.alg = 'hals';
            [w_sol, infos_sol] = als_nmf(V, rank, options_sol);
        else
            fprintf('Calculating f_opt by R-NMF ...\n');
            options_sol.lambda = lambda;
            [w_sol, infos_sol] = robust_mu_nmf(V, rank, options_sol);
        end

        f_opt = infos_sol.cost(end);
        fprintf('Done.. f_opt: %.16e\n', f_opt);
    else
        f_opt = -Inf;
    end

    %% online_mu_nmf
    [w_onmf, infos_onmf] = online_mu_nmf(V, rank, options);

    %% acc_online_mu_nmf
    options.rep_mode = 'fix';
    options.w_repeat = 1;
    options.h_repeat = max_h_repeat;
    [w_onmf_acc, infos_onmf_acc] = acc_online_mu_nmf(V, rank, options);

    options.rep_mode = 'adaptive';
    options.w_repeat = 1;
    options.h_repeat = max_h_repeat;
    [w_onmf_acc, infos_onmf_acc] = acc_online_mu_nmf(V, rank, options);

    %% inf
    [w_inmf, infos_inmf] = incremental_mu_nmf(V, rank, options);

    options.online = 1; % online mode
    [w_online_inmf, infos_online_inmf] = incremental_mu_nmf(V, rank, options);

    %% asag_mu
    [w_asag_mu_nmf, infos_asag_mu_nmf] = asag_mu_nmf(V, rank, options);

    %% smu
    [w_smu_nmf, infos_smu_nmf] = smu_nmf(V, rank, options);

    options.accel = true;
    options.rep_mode = 'fix';
    options.w_repeat = 1;
    options.h_repeat = max_h_repeat;
    [w_smu_nmf_acc, infos_smu_nmf_acc] = smu_nmf(V, rank, options);
    options.accel = false;

    options.ls = false;
    options.rep_mode = 'adaptive';
    options.w_repeat = 1;
    options.h_repeat = max_h_repeat;
    [w_smu_nmf_acc, infos_smu_nmf_acc] = smu_nmf(V, rank, options);
    options.accel = 0;
    options.ls = 0;

    %% spg
    [w_spg_nmf, infos_spg_nmf] = spg_nmf(V, rank, options);

    options.accel = 1;
    options.h_repeat = max_h_repeat;
    [w_spg_nmf, infos_spg_nmf] = spg_nmf(V, rank, options);

    options.accel = 0;
    %options.h_repeat = max_h_repeat;
    options.W_sub_mode = 'Nesterov';
    [w_spg_nmf, infos_spg_nmf] = spg_nmf(V, rank, options);

    options.accel = 0;
    options.ls = 1;
    [w_spg_nmf, infos_spg_nmf] = spg_nmf(V, rank, options);

    %% svrmu
    options.x_init.R = zeros(size(V)); % enfore non-robust mode
    options.fast_calc = 0;
    options.repeat_inneriter = svrmu_inneriter;
    options.accel = 0;
    options.ls = 0;
    options.ratio = 1; % 1: original 1<: adaptive
    options.robust = false;
    options.max_epoch = floor(max_epoch / (options.repeat_inneriter + 1));
    [w_svrmu_nmf, infos_svrmu_nmf] = svrmu_nmf(V, rank, options);

    options.accel = 1;
    options.rep_mode = 'fix';
    [w_svrmu_acc_nmf, infos_svrmu_acc_nmf] = svrmu_nmf(V, rank, options);

    options.rep_mode = 'adaptive';
    options.accel = 0;
    [w_svrmu_acc_nmf, infos_svrmu_acc_nmf] = svrmu_nmf(V, rank, options);

    options.accel = 1;
    [w_svrmu_acc_nmf, infos_svrmu_acc_nmf] = svrmu_nmf(V, rank, options);

    options.fast_calc = 0;
    options.accel = 1;
    options.rep_mode = 'adaptive';
    options.ratio = 0.3; % 1: original 1<: adaptive
    options.robust = false;
    [w_svrmu_acc_adaptive_nmf, infos_svrmu_acc_adaptive_nmf] = svrmu_nmf(V, rank, options);

end


function [ vX ] = HybridOrthogonalProjectionOntoConvexSets( cProjFun, vY, numIterations, stopThr, compMethod )
% ----------------------------------------------------------------------------------------------- %
% [ vX ] = HybridOrthogonalProjectionOntoConvexSets( cProjFun, vY, numIterations, stopThr, compMethod )
%   Solves \arg \min_{x} 0.5 || x - y ||, s.t. x \in \bigcap {C}_{i} using
%   Hybrid Orthogonal method.
% Input:
%   - cProjFun      -   Array of Projection Functions.
%                       Cell array of anonymouse functions which each is a
%                       projection into a sub space.
%                       Structure: Cell Array.
%                       Type: NA.
%                       Range: NA.
%   - vY            -   Input Vector.
%                       Input vector to be projected.
%                       Structure: Vector (m x 1).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - numIterations -   Number of Iterations.
%                       Sets the number of iterations of the algorithm.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: {1, 2, ...}.
%   - stopThr       -   Stopping Threshold.
%                       Sets the threshold between consecutive iterations
%                       for stopping the algorithm.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (0, inf).
%   - compMethod    -   Hybrid Method.
%                       Sets the hybrid method to calculate.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: {1, 2}.
% Output:
%   - vX            -   Solution Vector.
%                       The solution to the optimization problem.
%                       Structure: Vector (m x 1).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  Quadratic Optimization of Fixed Points of Non Expensive Mappings in Hilbert Space (https://doi.org/10.1080/01630569808816822).
% Remarks:
%   1.  B
% TODO:
%   1.  C
% Release Notes:
%   -   1.0.000     19/03/2020  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

COMP_METHOD_A = 1;
COMP_METHOD_B = 2;

switch(compMethod)
    case(COMP_METHOD_A)
        vX = HybridOrthogonalProjectionOntoConvexSetsA(cProjFun, vY, numIterations, stopThr);
    case(COMP_METHOD_B)
        vX = HybridOrthogonalProjectionOntoConvexSetsB(cProjFun, vY, numIterations, stopThr);
end

end

function [ vX ] = HybridOrthogonalProjectionOntoConvexSetsA( cProjFun, vY, numIterations, stopThr )

numSets     = size(cProjFun, 1);
numElements = size(vY, 1);

vX = vY;
vU = vX;

kk = 0;

for ii = 1:numIterations

    vU(:) = vX;

    % The loop is as vX changes slowly or not at all with each projection.
    % Hence in order to prevent pre mature stopping one should apply all
    % projections and then generate new vX.
    for jj = 1:numSets
        kk = kk + 1;
        % See Quadratic Optimization of Fixed Points of Non Expensive
        % Mappings in Hilbert Space (Page 12, Equation 20, The note after Equation 21)
        % paramLambdaN = 1 / ((kk + 1) ^ 0.99);
        % See Quadratic Optimization of Fixed Points of Non Expensive
        % Mappings in Hilbert Space (Page 14, Equation 30)
        % Pay attention that Equation 30 allows use of 1 / n.
        paramLambdaN = 1 / (kk + 1);
        % This still require the Fixed Point of the intersection is the
        % same for any cyclic variation of the Sets (Which happens for nay
        % Non Expensive Projection).
        vX(:) = (paramLambdaN * vY) + (1 - paramLambdaN) * cProjFun{jj}(vX);
    end

    stopCond = sum(abs(vU - vX)) < stopThr;

    if(stopCond)
        break;
    end
end

end

function [ vX ] = HybridOrthogonalProjectionOntoConvexSetsB( cProjFun, vY, numIterations, stopThr )

numSets     = size(cProjFun, 1);
numElements = size(vY, 1);

vX = vY;
vU = vX;
vT = vX;

vW = rand(numSets, 1);
vW = vW / sum(vW);

for ii = 1:numIterations

    vU(:) = vX;

    % See Quadratic Optimization of Fixed Points of Non Expensive Mappings
    % in Hilbert Space (Page 18, Equation 44)
    vT(:) = 0;
    for jj = 1:numSets
        vT(:) = vT + (vW(jj) * cProjFun{jj}(vX));
    end
    paramLambdaN = 1 / (ii + 1);
    vX(:) = (paramLambdaN * vY) + (1 - paramLambdaN) * vT;

    stopCond = sum(abs(vU - vX)) < stopThr;

    if(stopCond)
        break;
    end
end

end


function [ net,res,opts ] = qrnn_ff( net,opts )
%NET_FF Summary of this function goes here
%   Detailed explanation goes here

    n_frames=opts.parameters.n_frames;
    n_hidden_nodes=opts.parameters.n_hidden_nodes;
    n_input_nodes=size(opts.input_data,1);
    batch_size=opts.parameters.batch_size;

    %classification
    if isfield(opts,'input_labels')
        res.Fit(1).class=opts.input_labels(:);%vectorize all outputs
    end

    %Process inputs, clarity is more important here
    res.Gates(1).x=reshape(opts.input_data,n_input_nodes,[]);%inputs
    res.Input(1).x=res.Gates(1).x;%inputs

    [ net{1},res.Gates,opts ] = net_ff( net{1},res.Gates,opts );
    [ net{2},res.Input,opts ] = net_ff( net{2},res.Input,opts );

    res.Gates(end).x=reshape(res.Gates(end).x,n_hidden_nodes,batch_size,n_frames);
    res.Input(end).x=reshape(res.Input(end).x,n_hidden_nodes,batch_size,n_frames);
    res.Hidden=zeros(n_hidden_nodes,batch_size,n_frames,'like',res.Input(end).x);
    %Hidden_diff is used in bp.
    res.Hidden_diff=zeros(n_hidden_nodes,batch_size,n_frames,'like',res.Input(end).x);

    res.Hidden_diff(:,:,1)=res.Input(end).x(:,:,1);
    res.Hidden(:,:,1)=res.Gates(end).x(:,:,1).*res.Hidden_diff(:,:,1);
    for f=2:n_frames
        %Update hidden nodes;
        res.Hidden_diff(:,:,f)=res.Input(end).x(:,:,f)-res.Hidden(:,:,f-1);
        res.Hidden(:,:,f)=res.Hidden(:,:,f-1) + res.Gates(end).x(:,:,f).*res.Hidden_diff(:,:,f);
    end

    res.Fit(1).x=res.Hidden;

    %Data fitting transform
    res.Fit(1).x=reshape(res.Fit(1).x,n_hidden_nodes,[]);
    [ net{3},res.Fit,opts ] = net_ff( net{3},res.Fit,opts );

    %%%summarize
    if isfield(opts,'input_labels')
        opts.err=error_multiclass(res.Fit(1).class,res.Fit)./numel(res.Fit(1).class);
    end
    opts.loss=mean(res.Fit(end).x(:));
end


% SaveCalPar   save calibration parameters in different formats
%
% [Cst,Rot] = savecalpar(P,config)
% P ... 3*CAM x 4 matrix containing result of selfcalibration, see EUCLIDIZE
% config ... configuration structure, see CONFIGDATA
%
%
% Cst ... CAMSx3   matrix containing the camera centers (in world coord.)
% Rot ... 3*CAMSx3 matrix containing camera rotation matrices

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

function [Cst,Rot] = savecalpar(P,config)

Octave = exist('OCTAVE_VERSION', 'builtin') ~= 0;

idxused = config.cal.cams2use;

CAMS = size(P,1)/3;

Cst = zeros(CAMS,3);
Pst = zeros(3*CAMS,3);
Rot = [];
for i=1:CAMS,
  % do not save P matrices in separate files
  if 1
	Pmat = P(i*3-2:i*3,:);
        if Octave
          save(sprintf(config.files.CalPmat,idxused(i)),'Pmat');
        else
          save(sprintf(config.files.CalPmat,idxused(i)),'Pmat','-ASCII');
        end
  end
  sc = norm(P(i*3,1:3));
  % first normalize the Projection matrices to get normalized pixel points
  P(i*3-2:i*3,:) = P(i*3-2:i*3,:)./sc;
  % decompose the matrix by using rq decomposition
  [K,R] = rq(P(i*3-2:i*3,1:3));
  tvec= inv(K)*P(i*3-2:i*3,4);			% translation vector
  C	  = -R'*tvec;						% camera center
  % Stephi calib params
  Pstephi		   = R'*inv(K);
  Pst(i*3-2:i*3,:) = Pstephi;
  Cst(i,:)		   = C';
  % Stephi requires to save the pars in more "wordy" form
  fid = fopen(sprintf(config.files.StCalPar,idxused(i)),'wt');
  if ~fid
	error('SaveCalPar: The camera cal file cannot be opened');
  else
	fprintf(fid,'C1 = %f \n', C(1));
	fprintf(fid,'C2 = %f \n', C(2));
	fprintf(fid,'C3 = %f \n', C(3));
	fprintf(fid,'\n');
	fprintf(fid,'P11 = %f \n', Pstephi(1,1));
	fprintf(fid,'P12 = %f \n', Pstephi(1,2));
	fprintf(fid,'P13 = %f \n', Pstephi(1,3));
	fprintf(fid,'P21 = %f \n', Pstephi(2,1));
	fprintf(fid,'P22 = %f \n', Pstephi(2,2));
	fprintf(fid,'P23 = %f \n', Pstephi(2,3));
	fprintf(fid,'P31 = %f \n', Pstephi(3,1));
	fprintf(fid,'P32 = %f \n', Pstephi(3,2));
	fprintf(fid,'P33 = %f \n', Pstephi(3,3));
	fclose(fid);
  end
  Rot	 = [Rot;R];
  % Prithwijit requires to save the pars in more "wordy" form
  if 0 % do not save in the Prithwijit format
	fid = fopen(sprintf(config.files.CalPar,idxused(i)),'wt');
	if ~fid
	  error('SaveCalPar: The camera cal file cannot be opened');
	else
	  fprintf(fid,'R11 = %f \n',R(1,1));
	  fprintf(fid,'R12 = %f \n',R(1,2));
	  fprintf(fid,'R13 = %f \n',R(1,3));
	  fprintf(fid,'R21 = %f \n',R(2,1));
	  fprintf(fid,'R22 = %f \n',R(2,2));
	  fprintf(fid,'R23 = %f \n',R(2,3));
	  fprintf(fid,'R31 = %f \n',R(3,1));
	  fprintf(fid,'R32 = %f \n',R(3,2));
	  fprintf(fid,'R33 = %f \n',R(3,3));
	  fprintf(fid,'\n');
	  fprintf(fid,'t11 = %f \n',tvec(1));
	  fprintf(fid,'t21 = %f \n',tvec(2));
	  fprintf(fid,'t31 = %f \n',tvec(3));
	  fprintf(fid,'\n');
	  fprintf(fid,'K11 = %f \n',K(1,1));
	  fprintf(fid,'K12 = %f \n',K(1,2));
	  fprintf(fid,'K13 = %f \n',K(1,3));
	  fprintf(fid,'K21 = %f \n',K(2,1));
	  fprintf(fid,'K22 = %f \n',K(2,2));
	  fprintf(fid,'K23 = %f \n',K(2,3));
	  fprintf(fid,'K31 = %f \n',K(3,1));
	  fprintf(fid,'K32 = %f \n',K(3,2));
	  fprintf(fid,'K33 = %f \n',K(3,3));
	  fprintf(fid,'\n');
	  fclose(fid);
	end
  end
end

% save Stehpi params
if Octave
  save(config.files.Pst,'Pst');
  save(config.files.Cst,'Cst');
else
  save(config.files.Pst,'Pst','-ASCII');
  save(config.files.Cst,'Cst','-ASCII');
end


function images = U_prepare_image(im,IMAGE_MEAN,cnn_name,opt)
% ------------------------------------------------------------------------
if ~exist('IMAGE_MEAN','var') || isempty(IMAGE_MEAN)
    switch cnn_name
    case {'alex','nin','gnet'} % alexnet, nin, inception
        d = load('data/ilsvrc_2012_mean.mat');
        IMAGE_MEAN = d.image_mean;
    case 'vgg16'
        IMAGE_MEAN = repmat(reshape([103.939, 116.779, 123.68],[1,1,3]),[256 256]);
    end
end
if ~exist('opt','var');opt=0;end

IMAGE_DIM = 256;
switch opt
    case -1
        % visual: denormalize
        switch cnn_name
            case 'alex';CROPPED_DIM = 227;
            case {'vgg16','nin','gnet'};CROPPED_DIM = 224;
        end
        indices = [0 IMAGE_DIM-CROPPED_DIM] + 1;
        center = floor(indices(2) / 2)+1;
        images = bsxfun(@plus,permute(im(:,:,[3 2 1],:),[2,1,3,4]) , IMAGE_MEAN(center:center+CROPPED_DIM-1,center:center+CROPPED_DIM-1,3:-1:1));
    case -2
        % visual: normalize
        switch cnn_name
            case 'alex';CROPPED_DIM = 227;
            case {'vgg16','nin','gnet'};CROPPED_DIM = 224;
        end
        indices = [0 IMAGE_DIM-CROPPED_DIM] + 1;
        center = floor(indices(2) / 2)+1;
        images = bsxfun(@minus,im(:,:,[3 2 1],:) , IMAGE_MEAN(center:center+CROPPED_DIM-1,center:center+CROPPED_DIM-1,:));
        images = permute(images,[2,1,3,4]);
    case {0,1,5,6}
        switch cnn_name
            case 'alex'
                % alexnet
                CROPPED_DIM = 227;
                % resize to fixed input size
                im = single(im);
                im = imresize(im, [IMAGE_DIM IMAGE_DIM], 'bilinear');
                % permute from RGB to BGR (IMAGE_MEAN is already BGR)
                im = im(:,:,[3 2 1]) - IMAGE_MEAN;

                % oversample (4 corners, center, and their x-axis flips)
                images = zeros(CROPPED_DIM, CROPPED_DIM, 3, 10, 'single');
                indices = [0 IMAGE_DIM-CROPPED_DIM] + 1;
                curr = 1;
                for i = indices
                    for j = indices
                        images(:, :, :, curr) = ...
                            permute(im(i:i+CROPPED_DIM-1, j:j+CROPPED_DIM-1, :), [2 1 3]);
                        images(:, :, :, curr+5) = images(end:-1:1, :, :, curr);
                        curr = curr + 1;
                    end
                end
                center = floor(indices(2) / 2)+1;
                images(:,:,:,5) = ...
                    permute(im(center:center+CROPPED_DIM-1,center:center+CROPPED_DIM-1,:), ...
                    [2 1 3]);
                images(:,:,:,10) = images(end:-1:1, :, :, curr);
            case 'vgg16'
                CROPPED_DIM = 224;

                % resize to fixed input size
                im = single(im);

                if size(im, 1) < size(im, 2)
                    im = imresize(im, [IMAGE_DIM NaN]);
                else
                    im = imresize(im, [NaN IMAGE_DIM]);
                end

                % RGB -> BGR
                im = im(:, :, [3 2 1]);

                % oversample (4 corners, center, and their x-axis flips)
                images = zeros(CROPPED_DIM, CROPPED_DIM, 3, 10, 'single');

                indices_y = [0 size(im,1)-CROPPED_DIM] + 1;
                indices_x = [0 size(im,2)-CROPPED_DIM] + 1;
                center_y = floor(indices_y(2) / 2)+1;
                center_x = floor(indices_x(2) / 2)+1;

                % hack the mean image
                IMAGE_MEAN =  repmat(reshape([103.939, 116.779, 123.68],[1,1,3]),[CROPPED_DIM CROPPED_DIM]);
                curr = 1;
                for i = indices_y
                    for j = indices_x
                        images(:, :, :, curr) = ...
                            permute(im(i:i+CROPPED_DIM-1, j:j+CROPPED_DIM-1, :)-IMAGE_MEAN, [2 1 3]);
                        images(:, :, :, curr+5) = images(end:-1:1, :, :, curr);
                        curr = curr + 1;
                    end
                end
                images(:,:,:,5) = ...
                    permute(im(center_y:center_y+CROPPED_DIM-1,center_x:center_x+CROPPED_DIM-1,:)-IMAGE_MEAN, ...
                    [2 1 3]);
                images(:,:,:,10) = images(end:-1:1, :, :, curr);
        end

        if opt==1
            images = images(:,:,:,5);
        end

end


function [pE,gE,pC,gC] = spm_ind_priors(A,B,C,Nm,Nf)
% prior moments for a neural-mass model of induced responses
% FORMAT [pE,gE,pC,gC] = spm_ind_priors(A,B,C,dipfit,Nu,Nf)
% A{2},B{m},C  - binary constraints on extrinsic connections
% Nm           - number of frequency modes used
% Nf           - number of frequency modes explained
%
% pE - prior expectation - f(x,u,P,M)
% gE - prior expectation - g(x,u,G,M)
%
% connectivity parameters
%--------------------------------------------------------------------------
%    pE.A    - trial-invariant
%    pE.B{m} - trial-dependent
%    pE.C    - stimulus-stimulus dependent
%
% stimulus and noise parameters
%--------------------------------------------------------------------------
%    pE.R    - onset and dispersion
%
% pC - prior covariances: cov(spm_vec(pE))
%__________________________________________________________________________
%
% David O, Friston KJ (2003) A neural mass model for MEG/EEG: coupling and
% neuronal dynamics. NeuroImage 20: 1743-1755
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_ind_priors.m 5900 2014-02-27 21:54:51Z karl $

% orders
%--------------------------------------------------------------------------
if nargin < 5; Nf = Nm; end
n    = size(C,1);                                 % number of sources
nu   = size(C,2);                                 % number of inputs
D    = sparse(1:Nm,1:Nm,1,Nf,Nf);

% set extrinsic connectivity - linear and nonlinear (cross-frequency)
%--------------------------------------------------------------------------
E.A  = kron(speye(Nf,Nf), -speye(n,n));
V.A  = kron(D,A{1}) + kron(D*(1 - speye(Nf,Nf))*D,A{2});

% input-dependent
%--------------------------------------------------------------------------
for i = 1:length(B)
    E.B{i} = sparse(n*Nf,n*Nf);
    V.B{i} = kron(D*ones(Nf,Nf)*D,B{i}) & V.A;
end

% exogenous inputs
%--------------------------------------------------------------------------
E.C  = kron(D*ones(Nf,1),C - C);
V.C  = kron(D*ones(Nf,1),C);

% set stimulus parameters: magnitude, onset and dispersion
%--------------------------------------------------------------------------
E.R  = kron(ones(nu,1),[0 0]);
V.R  = kron(ones(nu,1),[1/16 1/16]);

% prior moments
%--------------------------------------------------------------------------
pE   = E;
gE   = [];
pC   = diag(sparse(spm_vec(V)));
gC   = [];


% DEMO3B - Chaining files with ncdataset and finding attributes using 'value4key'

echo('on')
o2_ = 'Oxygen';
time_ = 'esecs';

%% ---- Access the OpenDAP datasets
m2006 = ncdataset('http://dods.mbari.org/cgi-bin/nph-nc/data/ssdsdata/deployments/m1/200610/m1_aanderaaoxy_20070105.nc');
m2007 = ncdataset('http://dods.mbari.org/cgi-bin/nph-nc/data/ssdsdata/deployments/m1/200711/m1_aanderaaoxy_20071106.nc');
m2008 = ncdataset('http://dods.mbari.org/cgi-bin/nph-nc/data/ssdsdata/deployments/m1/200810/m1_aanderaaoxy_20081008.nc');
t = [m2006.time(time_); m2007.time(time_); m2008.time(time_)];
o2 = [m2006.data(o2_); m2007.data(o2_); m2008.data(o2_)];

%% ---- Find the units of Oxygen and label the display
attr = m2008.attributes(o2_);
units = value4key(attr, 'units'); % Retrieve the units value
name = value4key(attr, 'long_name'); % Retrieve the long_name value

%% ---- Plot the data
figure;
plot(t, o2);...
datetick('x');...
ylabel([name ' [' units ']']);...
title({'Surface Temperature at M1 Mooring in Monterey Bay',m2006.location,m2007.location,m2008.location},'interpreter','none');...
grid;...
shg
echo('off')


%% OPTI Toolbox AMPL Demo
%
% This file loads a number of supplied examples and shows how to solve
% them using the OPTI Toolbox. You should read and complete Basic_demo.m
% BEFORE running the below examples.
%
% The underlying parser uses Netlib's AMPL Solver Library (ASL) Interface.
%
%   Copyright (C) 2011 Jonathan Currie (I2C2)

%% Loading an AMPL Problem
% OPTI Toolbox is supplied with a number of example AMPL problems ranging
% from LP to NLP including continuous and discrete problems. To load an
% AMPL model it must be in .NL format, see the supplied user's guide page
% on AMPL Interfacing in order to generate the .NL file from your model. To
% load an AMPL problem, simply use the command below. Returned will be a
% optiprob structure containing the data in the file.

prob = amplRead('diet.nl')

%% Example 1 - Solving a Loaded AMPL Problem
% Solving a loaded AMPL problem is simple, just pass it to the opti
% constructor and call solve. You will note that the command amplRead will
% automatically determine the problem type (LP, QP, NLP, etc) based on
% properties supplied by the ASL.

Opt = opti(prob)

x = solve(Opt)

%% Loading an AMPL Nonlinear Program
% The AMPL format can easily specify nonlinear problems unlike the LP or
% MPS formats.

prob = amplRead('ch3.nl')

%% Example 2 - Solving a Nonlinear Problem from AMPL
% In order to solve a nonlinear problem the model .nl file
% remains open after the amplRead function is called to allow callbacks for
% the objective, gradient, etc. Calling solve() will automatically close
% the interface for linear and quadratic problems, however you must manually
% call close for nonlinear problems. Alternatively it will be automatically
% called once Matlab is closed to clean up ASL memory.

Opt = opti(prob)

x = solve(Opt)

asl('close')

%% Loading an Integer Problem
% AMPL will reorder the variables from the original model based on internal
% rules (see the supplied pdfs). Based on this the ASL indicates which
% variables have binary or integer constraints, which are passed through
% the amplRead interface:

prob = amplRead('multmip1.nl')

%% Example 3 - Solving an AMPL Integer Problem
% As the optiprob structure can fully define an integer problem, no change
% is required to build and solve the MIP

Opt = opti(prob)

[x,fval] = solve(Opt);
fval

%% A Note On Hessians
% As of OPTI v1.58 the Hessian of an NLP will automatically be added to the
% problem structure. However note it is the Hessian of the Lagrangian, with
% the calling form:
%
%   H = hessian(x,sigma,lambda)
%
% This means the Hessian includes second derivatives of both the objective
% and the constraints. Sigma is a scalar scaling factor on the objective
% derivatives only, and lambda are the lagrange multipliers at the current
% point x (scaling the constraint derivatives).


%% Copyright (C) 2014-2016, 2022 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy is free software; you can redistribute it and/or modify
%% it under the terms of the GNU General Public License as published
%% by the Free Software Foundation; either version 3 of the License,
%% or (at your option) any later version.
%%
%% This software 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 software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym acosd (@var{x})
%% Symbolic inverse cos function with output in degrees.
%%
%% Example:
%% @example
%% @group
%% acosd (sqrt (sym (2))/2)
%%   @result{} (sym) 45
%%
%% syms x
%% y = acosd (x)
%%   @result{} y = (sym)
%%       180⋅acos(x)
%%       ───────────
%%            π
%% @end group
%% @end example
%%
%% @seealso{@@sym/cosd, @@sym/acos}
%% @end defmethod

function y = acosd(x)
  if (nargin ~= 1)
    print_usage ();
  end
  y = elementwise_op ('lambda a: deg(acos(a))', x);
end

%!error acosd (sym(1), 2)
%!assert (isequaln (acosd (sym(nan)), sym(nan)))

%!test
%! f1 = acosd (sym(1)/2);
%! f2 = acosd (1/2);
%! assert (double (f1), f2, -eps)

%!test
%! D = [1 2; 3 4]/4;
%! A = sym([1 2; 3 4])/4;
%! f1 = acosd (A);
%! f2 = acosd (D);
%! assert (double (f1), f2, -eps)


function y=absx2(x)

% y=absx2(x)
%
% Faster implementation of y=abs(x).^2.

% Copyright 2012 Evrytania LLC (http://www.evrytania.com)
%
% Written by James Peroulas <james@evrytania.com>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

y=real(x).*real(x)+imag(x).*imag(x);


function newmap = bluewhitered(m)
%BLUEWHITERED   Blue, white, and red color map.
%   BLUEWHITERED(M) returns an M-by-3 matrix containing a blue to white
%   to red colormap, with white corresponding to the CAXIS value closest
%   to zero.  This colormap is most useful for images and surface plots
%   with positive and negative values.  BLUEWHITERED, by itself, is the
%   same length as the current colormap.
%
%   Examples:
%   ------------------------------
%   figure
%   imagesc(peaks(250));
%   colormap(bluewhitered(256)), colorbar
%
%   figure
%   imagesc(peaks(250), [0 8])
%   colormap(bluewhitered), colorbar
%
%   figure
%   imagesc(peaks(250), [-6 0])
%   colormap(bluewhitered), colorbar
%
%   figure
%   surf(peaks)
%   colormap(bluewhitered)
%   axis tight
%
%   See also HSV, HOT, COOL, BONE, COPPER, PINK, FLAG,
%   COLORMAP, RGBPLOT.

if nargin < 1
   m = size(get(gcf,'colormap'),1);
end

bottom = [0 0 0.5];
botmiddle = [0 0.5 1];
middle = [1 1 1];
topmiddle = [1 0 0];
top = [0.5 0 0];

% Find middle
lims = get(gca, 'CLim');

% Find ratio of negative to positive
if (lims(1) < 0) & (lims(2) > 0)
    % It has both negative and positive
    % Find ratio of negative to positive
    ratio = abs(lims(1)) / (abs(lims(1)) + lims(2));
    neglen = round(m*ratio);
    poslen = m - neglen;

    % Just negative
    new = [bottom; botmiddle; middle];
    len = length(new);
    oldsteps = linspace(0, 1, len);
    newsteps = linspace(0, 1, neglen);
    newmap1 = zeros(neglen, 3);

    for i=1:3
        % Interpolate over RGB spaces of colormap
        newmap1(:,i) = min(max(interp1(oldsteps, new(:,i), newsteps)', 0), 1);
    end

    % Just positive
    new = [middle; topmiddle; top];
    len = length(new);
    oldsteps = linspace(0, 1, len);
    newsteps = linspace(0, 1, poslen);
    newmap = zeros(poslen, 3);

    for i=1:3
        % Interpolate over RGB spaces of colormap
        newmap(:,i) = min(max(interp1(oldsteps, new(:,i), newsteps)', 0), 1);
    end

    % And put 'em together
    newmap = [newmap1; newmap];

elseif lims(1) >= 0
    % Just positive
    new = [middle; topmiddle; top];
    len = length(new);
    oldsteps = linspace(0, 1, len);
    newsteps = linspace(0, 1, m);
    newmap = zeros(m, 3);

    for i=1:3
        % Interpolate over RGB spaces of colormap
        newmap(:,i) = min(max(interp1(oldsteps, new(:,i), newsteps)', 0), 1);
    end

else
    % Just negative
    new = [bottom; botmiddle; middle];
    len = length(new);
    oldsteps = linspace(0, 1, len);
    newsteps = linspace(0, 1, m);
    newmap = zeros(m, 3);

    for i=1:3
        % Interpolate over RGB spaces of colormap
        newmap(:,i) = min(max(interp1(oldsteps, new(:,i), newsteps)', 0), 1);
    end

end
%
% m = 64;
% new = [bottom; botmiddle; middle; topmiddle; top];
% % x = 1:m;
%
% oldsteps = linspace(0, 1, 5);
% newsteps = linspace(0, 1, m);
% newmap = zeros(m, 3);
%
% for i=1:3
%     % Interpolate over RGB spaces of colormap
%     newmap(:,i) = min(max(interp1(oldsteps, new(:,i), newsteps)', 0), 1);
% end
%
% % set(gcf, 'colormap', newmap), colorbar


function[]=makefigs_twodstats
%MAKEFIGS_TWODSTATS  Makes a sample figure for TWODSTATS.

if isempty(which('drifters.mat'))
    disp('Sorry, TWODSTATS can''t find DRIFTERS.MAT.')
    return
end

%This make take a few minutes...

load drifters,use drifters
cv=cellpair(u,v);
vindex(lat,lon,cv,1:10:length(lat),1);

tic;[mat,xmid,ymid]=twodstats(lon,lat,cellabs(cv),-180.5:180.5,-89.5:89.5);etime1=toc;

figure,jpcolor(xmid,ymid,mat)
axis([-180 180 -70 90]),latratio(30),topoplot
xlabel('Longitude'),ylabel('Latitude')
title('Mean Speed from the Global Surface Drifter Dataset')
caxis([8 60]),h=colorbar('EastOutside');colormap lansey
h.Label.String='Mean Speed (cm/s)';

%To print
if 0
    currentdir=pwd;
    cd([whichdir('jlab_license') '/figures'])
    print -dpng twodstats
    crop twodstats.png
    cd(currentdir)
end


function [Eft, Varft, lpyt, Eyt, Varyt] = gpla_loopred(gp, x, y, varargin)
%GPLA_LOOPRED  Leave-one-out predictions with Laplace approximation
%
%  Description
%    [EFT, VARFT, LPYT, EYT, VARYT] = GPLA_LOOPRED(GP, X, Y, OPTIONS)
%    takes a Gaussian process structure GP together with a matrix X
%    of training inputs and vector Y of training targets, and
%    evaluates the leave-one-out predictive distribution at inputs
%    X and returns means EFT and variances VARFT of latent
%    variables, the logarithm of the predictive densities PYT, and
%    the predictive means EYT and variances VARYT of observations
%    at input locations X.
%
%    OPTIONS is optional parameter-value pair
%      z      - optional observed quantity in triplet (x_i,y_i,z_i)
%               Some likelihoods may use this. For example, in case of
%               Poisson likelihood we have z_i=E_i, that is, expected value
%               for ith case.
%
%    Laplace leave-one-out is approximated in linear response style
%    by expressing the solutions for LOO problem in terms of
%    solution for the full problem. The computationally cheap
%    solution can be obtained by making the assumption that the
%    difference between these two solution is small such that their
%    difference may be treated as an Taylor expansion truncated at
%    first order (Winther et al 2012, in progress).
%
%  See also
%    GP_LOOPRED, GP_PRED
%
% Copyright (c) 2011-2012  Aki Vehtari, Ville Tolvanen

% 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 = 'GPLA_LOOPRED';
  ip.addRequired('gp', @(x) isstruct(x));
  ip.addRequired('x', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addRequired('y', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('z', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('method', 'cavity', @(x) ismember(x, {'lrs' 'cavity' 'inla'}))
  ip.addParamValue('fcorr', 'off', @(x) ismember(x, {'off', 'fact', 'cm2', 'on'}));
  ip.parse(gp, x, y, varargin{:});
  z=ip.Results.z;
  method = ip.Results.method;
  fcorr=ip.Results.fcorr;
  [tn,nin] = size(x);

  switch method

    case 'lrs'
      % Manfred Opper and Ole Winther (2000). Gaussian Processes for
      % Classification: Mean-Field Algorithms. In Neural
      % Computation, 12(11):2655-2684.
      %
      % Ole Winther et al (2012). Work in progress.

      % latent posterior
      [f, sigm2ii] = gpla_pred(gp, x, y, 'z', z, 'tstind', [], 'fcorr', fcorr);
      if isnan(f)
        Eft=NaN; Varft=NaN; lpyt=NaN; Eyt=NaN; Varyt=NaN;
        return
      end

      deriv = gp.lik.fh.llg(gp.lik, y, f, 'latent', z);
      La = 1./-gp.lik.fh.llg2(gp.lik, y, f, 'latent', z);
      % really large values don't contribute, but make variance
      % computation unstable. 2e15 approx 1/(2*eps)
      La = min(La,2e15);

      switch gp.type
        case 'FULL'
          % FULL GP (and compact support GP)
          K = gp_trcov(gp,x);
          Varft=1./diag(inv(K+diag(La)))-La;

        case 'FIC'
          % FIC
          % Use inverse lemma for FIC low rank covariance matrix approximation
          % Code adapated from gp_pred

          u = gp.X_u;
          m = size(u,1);
          % Turn the inducing vector on right direction
          if size(u,2) ~= size(x,2)
            u=u';
          end
          [Kv_ff, Cv_ff] = gp_trvar(gp, x);  % 1 x f  vector
          K_fu = gp_cov(gp, x, u);   % f x u
          K_uu = gp_trcov(gp, u);     % u x u, noiseles covariance K_uu
          Luu = chol(K_uu,'lower');
          B=Luu\(K_fu');
          Qv_ff=sum(B.^2)';
          % Add also La to the vector of diagonal elements
          Lav = Cv_ff-Qv_ff + La;   % 1 x f, Vector of diagonal elements

          % iLaKfu = diag(inv(Lav))*K_fu = inv(La)*K_fu
          iLaKfu = zeros(size(K_fu));  % f x u,
          n=size(x,1);
          for i=1:n
            iLaKfu(i,:) = K_fu(i,:)./Lav(i);  % f x u
          end
          A = K_uu+K_fu'*iLaKfu;
          A = (A+A')./2;
          L = iLaKfu/chol(A);

          %Varft=1./diag(inv(K+diag(La)))-La;
          Varft=1./(1./Lav - sum(L.^2,2))-La;

        case {'PIC' 'PIC_BLOCK'}
          % PIC
          % Use inverse lemma for PIC low rank covariance matrix approximation
          % Code adapated from gp_pred (here Lab is same La in gp_pred)

          u = gp.X_u;
          ind = gp.tr_index;
          if size(u,2) ~= size(x,2)
            % Turn the inducing vector on right direction
            u=u';
          end

          % Calculate some help matrices
          [Kv_ff, Cv_ff] = gp_trvar(gp, x);  % 1 x f  vector
          K_fu = gp_cov(gp, x, u);         % f x u
          K_uu = gp_trcov(gp, u);    % u x u, noiseles covariance K_uu
          Luu = chol(K_uu)';

          % Evaluate the Lambda (La) for specific model
          % Q_ff = K_fu*inv(K_uu)*K_fu'
          % Here we need only the diag(Q_ff), which is evaluated below
          B=Luu\K_fu';
          iLaKfu = zeros(size(K_fu));  % f x u
          for i=1:length(ind)
            Qbl_ff = B(:,ind{i})'*B(:,ind{i});
            [Kbl_ff, Cbl_ff] = gp_trcov(gp, x(ind{i},:));
            % Add also La to the diagonal
            Lab{i} = Cbl_ff - Qbl_ff + diag(La(ind{i}));
            iLaKfu(ind{i},:) = Lab{i}\K_fu(ind{i},:);
          end
          A = K_uu+K_fu'*iLaKfu;
          A = (A+A')./2;            % Ensure symmetry
          L = iLaKfu/chol(A);

          % From this on evaluate the prediction
          % See Snelson and Ghahramani (2007) for details
          n=size(y,1);
          iCv=zeros(n,1);
          for i=1:length(ind)
            iCv(ind{i},:) = diag(inv(Lab{i}));
          end

          %Varft=1./diag(inv(K+diag(La)))-La;
          Varft=1./(iCv - sum(L.^2,2))-La;

        case 'CS+FIC'
          % CS+FIC
          % Use inverse lemma for CS+FIC
          % Code adapated from gp_pred (Here Las is same as La in gp_pred)

          u = gp.X_u;
          if size(u,2) ~= size(x,2)
            % Turn the inducing vector on right direction
            u=u';
          end

          n = size(x,1);
          m = size(u,1);
          ncf = length(gp.cf);

          % Indexes to all non-compact support and compact support covariances.
          cf1 = [];
          cf2 = [];

          % Loop through all covariance functions
          for i1 = 1:ncf
            if ~isfield(gp.cf{i1},'cs')
              % Non-CS covariances
              cf1 = [cf1 i1];
            else
              % CS-covariances
              cf2 = [cf2 i1];
            end
          end

          % First evaluate needed covariance matrices
          % v defines that parameter is a vector
          [Kv_ff, Cv_ff] = gp_trvar(gp, x, cf1); % f x 1  vector
          K_fu = gp_cov(gp, x, u, cf1);          % f x u
          K_uu = gp_trcov(gp, u, cf1);    % u x u, noiseles covariance K_uu
          K_uu = (K_uu+K_uu')./2;         % ensure the symmetry of K_uu

          Luu  = chol(K_uu)';
          % Evaluate the Lambda (La)
          % Q_ff = K_fu*inv(K_uu)*K_fu'
          B=Luu\(K_fu');       % u x f
          Qv_ff=sum(B.^2)';
          % Add also La to the vector of diagonal elements
          Lav = Cv_ff-Qv_ff + La;   % f x 1, Vector of diagonal elements

          K_cs = gp_trcov(gp,x,cf2);
          Las = sparse(1:n,1:n,Lav,n,n) + K_cs;

          iLaKfu = Las\K_fu;
          A = K_uu+K_fu'*iLaKfu;
          A = (A+A')./2;     % Ensure symmetry
          L = iLaKfu/chol(A);

          %Varft=1./diag(inv(K+diag(La)))-La;
          Varft=1./(diag(inv(Las)) - sum(L.^2,2))-La;

        otherwise
          error('Unknown type of Gaussian process')
      end

      % check if cavity variances are negative
      ii=find(Varft<0);
      if ~isempty(ii)
        warning('gpla_loopred: some LOO latent variances are negative');
        Varft(ii) = gp.jitterSigma2;
      end
      Eft=f-Varft.*deriv;

      if nargout==3
        lpyt = gp.lik.fh.predy(gp.lik, Eft, Varft, y, z);
      elseif nargout>3
        [lpyt,Eyt,Varyt] = gp.lik.fh.predy(gp.lik, Eft, Varft, y, z);
      end

    case 'cavity'
      % using EP equations

      % latent posterior
      [f, sigm2ii] = gpla_pred(gp, x, y, 'z', z, 'tstind', [], 'fcorr', fcorr);
      % remove the jitter variance (see sigm2_t below)
      sigm2ii  = sigm2ii-gp.jitterSigma2;

      % "site parameters"
      W        = -gp.lik.fh.llg2(gp.lik, y, f, 'latent', z);
      deriv    = gp.lik.fh.llg(gp.lik, y, f, 'latent', z);
      % add the jitter variance (see sigm2ii above)
      sigm2_t  = 1./W+gp.jitterSigma2;
      mu_t     = f + sigm2_t.*deriv;

      % "cavity parameters"
      sigma2_i = 1./(1./sigm2ii-1./sigm2_t);
      myy_i    = sigma2_i.*(f./sigm2ii-mu_t./sigm2_t);
      % check if cavity varianes are negative
      ii=find(sigma2_i<0);
      if ~isempty(ii)
        warning('gpla_loopred: some cavity variances are negative');
        sigma2_i(ii) = sigm2ii(ii);
        myy_i(ii) = f(ii);
      end

      % leave-one-out predictions
      Eft=myy_i;
      Varft=sigma2_i;

      if nargout==3
        lpyt = gp.lik.fh.predy(gp.lik, Eft, Varft, y, z);
      elseif nargout>3
        [lpyt,Eyt,Varyt] = gp.lik.fh.predy(gp.lik, Eft, Varft, y, z);
      end

    case 'inla'
      % Leonhard Held and Birgit Schr�dle and H�vard Rue (2010)
      % Posterior and Cross-validatory Predictive Checks: A
      % Comparison of MCMC and INLA. In (eds) Thomas Kneib and
      % Gerhard Tutz, Statistical Modelling and Regression
      % Structures, pp. 91-110. Springer.

      % latent posterior
      [f, sigm2ii, lp] = gpla_pred(gp, x, y, 'z', z, 'tstind', [], 'fcorr', fcorr);

      Eft = zeros(tn,1);
      Varft = zeros(tn,1);
      lpyt = zeros(tn,1);
      minf = f-6.*sqrt(sigm2ii);
      maxf = f+6.*sqrt(sigm2ii);
      for i=1:tn
        if isempty(z)
          z1 = [];
        else
          z1 = z(i);
        end
        [m0, m1, m2] = quad_moments(@(x) norm_pdf(x, f(i), sqrt(sigm2ii(i)))./llvec(gp.lik,y(i),x,z1), minf(i), maxf(i));
        Eft(i) = m1;
        Varft(i) = m2-Eft(i)^2;
        lpyt(i) = -log(m0);
      end

      if nargout>3
        [tmp,Eyt,Varyt] = gp.lik.fh.predy(gp.lik, Eft, Varft, y, z);
      end
      if sum((abs(lpyt)./abs(lp) > 5) == 1) > 0.1*tn;
        warning('Very bad predictive densities, gpla_loopred might not be reliable, check results!');
      end

  end

end

function expll = llvec(gplik, y, f, z)
  for i=1:size(f,2)
    expll(i) = exp(gplik.fh.ll(gplik, y, f(i), z));
  end
end


% sgwt_check_connected : Check connectedness of graph
%
% function r=sgwt_check_connected(A)
%
% returns 1 if graph is connected, 0 otherwise
% Uses boost graph library breadth first search
%
% Inputs :
% A - adjacency matrix
%
% Outputs :
% r - result
%

% This file is part of the SGWT toolbox (Spectral Graph Wavelet Transform toolbox)
% Copyright (C) 2010, David K. Hammond.
%
% The SGWT toolbox is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% The SGWT toolbox 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 the SGWT toolbox.  If not, see <http://www.gnu.org/licenses/>.

function r=sgwt_check_connected(A)
  d=bfs(A,1);
  r=~any(d==-1);


% Johann Diep (johann.diep@esa.int) - August 2021
%
% This script builds the structure for the visual dataset.

%% Conversion Constants

G =  9.80665;       % Gravity constant, m/s^2
G2MSS = G;          % g to m/s^2
MSS2G = (1/G);      % m/s^2 to g

D2R = (pi/180);     % degrees to radians
R2D = (180/pi);     % radians to degrees

KT2MS = 0.514444;   % knot to m/s
MS2KMH = 3.6;       % m/s to km/h

%% OpenVINS PLANETARY DATASET ERROR PROFILE

load('./canada-planetary-data/data//gnss_planetary_r.mat');
load('./canada-planetary-data/data//imu_planetary.mat');

visual_data_planetary_pose = readtable('./external-data/canada/pose_data.csv');
visual_data_planetary_twist = readtable('./external-data/canada/twist_data.csv');
visual_data_planetary_posecov = readtable('./external-data/canada/posecov_data.csv');
visual_data_planetary_twistcov = readtable('./external-data/canada/twistcov_data.csv');

%   visual, visual data structure
%         t: Vx1 time vector (seconds)
%       lat: Vx1 latitude (radians).
%       lon: Vx1 longitude (radians).
%         h: Vx1 altitude (m).
%       vel: Vx3 NED velocities (m/s).
%     covvm: Vx36 velocity and position covariance matrices (m^2 and m^2/s^2).
%      covv: Vx9 velocity covariance matrices (m^2/s^2).
%      larm: 3x1 lever arm from IMU to camera (x-fwd, y-right, z-down) (m).
%      freq: 1x1 sampling frequency (Hz).
%       eps: 1x1 time interval to compare current IMU time to current GNSS time vector (s).

Cutoff = 1500;

% parsing the timestamps of the measurements
Seconds = table2array(visual_data_planetary_pose(Cutoff:end,4));
Nanoseconds = table2array(visual_data_planetary_pose(Cutoff:end,5));
Timestamps = Seconds + Nanoseconds/10^(9);
visual_planetary.t = Timestamps - Timestamps(1);

% position and velocity preprocessing
Positions = table2array(visual_data_planetary_pose(Cutoff:end,6:7))-table2array(visual_data_planetary_pose(Cutoff,6:7));
Heights = table2array(visual_data_planetary_pose(Cutoff:end,8))-table2array(visual_data_planetary_pose(Cutoff,8));
RotationAngle = deg2rad(-100);
RotationMatrix = [cos(RotationAngle),-sin(RotationAngle);sin(RotationAngle),cos(RotationAngle)];
Positions_rotated = RotationMatrix * Positions';

[visual_planetary.lat, visual_planetary.lon, visual_planetary.h] = ned2geodetic(Positions_rotated(2,:)',Positions_rotated(1,:)',-Heights, ...
    gnss_planetary_r.lat(1),gnss_planetary_r.lon(1),gnss_planetary_r.h(1),wgs84Ellipsoid,'radians');

Velocities = table2array(visual_data_planetary_twist(Cutoff:end,1:2));
Velocities_rotated = RotationMatrix * Velocities';
visual_planetary.vel(:,1:2) = Velocities_rotated';
visual_planetary.vel(:,3) = -table2array(visual_data_planetary_twist(Cutoff:end,3));

% parsing the covariances
Count = 1;
for i = Cutoff:size(table2array(visual_data_planetary_posecov),1)
        PoseCovariance = table2array(visual_data_planetary_posecov(i,:));
        TwistCovariance = table2array(visual_data_planetary_twistcov(i,:));

        PoseCovariance = reshape(PoseCovariance,[6,6])';
        TwistCovariance = reshape(TwistCovariance,[6,6])';

        PositionCovariance = PoseCovariance(1:3,1:3);
        VelocityCovariance = TwistCovariance(1:3,1:3);

        % [~,Std] = corrcov(PositionCovariance);
        % Std_x(i) = Std(1);
        % Std_y(i) = Std(2);
        % Std_z(i) = Std(3);

        IntermediateCovariance = [VelocityCovariance,zeros(3,3);zeros(3,3),PositionCovariance];

        visual_planetary.covv(Count,:) = reshape(VelocityCovariance',[1,9]);
        visual_planetary.covvm(Count,:) = reshape(IntermediateCovariance',[1,36]);
        Count = Count + 1;
end

visual_planetary.larm = [0,0,0]'; % no lever arm needed in case of OpenVINS
visual_planetary.eps = mean(diff(imu_planetary.t))*20; % rule of thumb for choosing eps

% downsample
visual_planetary.t = downsample(visual_planetary.t,33);
visual_planetary.lat = downsample(visual_planetary.lat,33);
visual_planetary.lon = downsample(visual_planetary.lon,33);
visual_planetary.h = downsample(visual_planetary.h,33);
visual_planetary.vel = downsample(visual_planetary.vel,33);
visual_planetary.covv = downsample(visual_planetary.covv,33);
visual_planetary.covvm = downsample(visual_planetary.covvm,33);

visual_planetary.freq = 1/mean(diff(visual_planetary.t)); % estimating the frequency

save('./canada-planetary-data/data/visual_planetary.mat','visual_planetary');


function y = nanstd(varargin)
%Replacement for Matlab NANSTD Standard deviation, ignoring NaNs.
%

y = sqrt(nanvar(varargin{:}));

return;


%% Global Patch Collider Demo
%
% This sample trains the forest for the Global Patch Collider and stores
% output to the file "forest.yml.gz".
%
% It then finds correspondences between two images using Global Patch Collider
% and calculates error using provided ground truth flow.
%
% It will look for the file named "forest.yml.gz" with a learned forest.
% You can obtain the "forest.yml.gz" either by manually training it or by
% downloading one of the files trained on some publicly available dataset from
% here: <https://drive.google.com/open?id=0B7Hb8cfuzrIIZDFscXVYd0NBNFU>
%
% Sources:
%
% * <https://github.com/opencv/opencv_contrib/blob/3.2.0/modules/optflow/samples/gpc_train.cpp>
% * <https://github.com/opencv/opencv_contrib/blob/3.2.0/modules/optflow/samples/gpc_evaluate.cpp>
%

%% 1) Train

%%
% input training images
imgs1 = {
    fullfile(mexopencv.root(), 'test', 'RubberWhale1.png')
};
imgs2 = {
    fullfile(mexopencv.root(), 'test', 'RubberWhale2.png')
};
groundTruths = {
    fullfile(mexopencv.root(), 'test', 'RubberWhale.flo')
};
assert(isequal(numel(imgs1), numel(imgs2), numel(groundTruths)));

if exist(groundTruths{1}, 'file') ~= 2
    % attempt to download ground thruth flow from GitHub
    disp('Downloading FLO...')
    url = 'https://cdn.rawgit.com/opencv/opencv_extra/3.2.0/testdata/cv/optflow/RubberWhale.flo';
    urlwrite(url, groundTruths{1});
end

%%
% Global Patch Collider training parameters
params = {
    'MaxTreeDepth',20, ...      % Maximum tree depth to stop partitioning
    'MinNumberOfSamples',3, ... % Minimum number of samples in the node to stop partitioning
    'DescriptorType','DCT', ... % Descriptor type. Set to DCT for quality, WHT for speed
    'PrintProgress',false       % Set to false for quiet mode, set to true to print progress
};
forestDumpPath = fullfile(tempdir(), 'forest.yml.gz');

%%
% train the forest for the Global Patch Collider and save it
if exist(forestDumpPath, 'file') ~= 2
    gpc = cv.GPCForest();
    tic
    gpc.train(imgs1, imgs2, groundTruths);
    toc
    gpc.save(forestDumpPath);
end

%% 2) Evaluate

%%
% test images
fromPath = imgs1{1};
toPath = imgs2{1};
gtPath = groundTruths{1};

from = imread(fromPath);
to = imread(toPath);
flo = cv.readOpticalFlow(gtPath);

%%
% load pretrained forest
forest = cv.GPCForest();
assert(exist(forestDumpPath,'file') == 2, 'No file with a trained model');
forest.load(forestDumpPath);

%%
% find correspondences between two the images using GPC
tic
corresp = forest.findCorrespondences(from, to, 'UseOpenCL',false);
toc
fprintf('Found %d matches\n', numel(corresp));

%%
% calculate error using provided ground truth flow
gtU = flo(:,:,1);
gtV = flo(:,:,2);
a = cat(1, corresp.first);
b = cat(1, corresp.second);
ind = sub2ind(size(gtU), a(:,2), a(:,1));
gtDisplacement = [gtU(ind) gtV(ind)];
c = a + gtDisplacement;

% check for correct flow vector
mask = all(isfinite(gtDisplacement) & (gtDisplacement < 1e9), 2);
a = a(mask,:);
b = b(mask,:);
c = c(mask,:);

err = mean(sqrt(sum((b - c).^2, 2)));
fprintf('Average endpoint error = %f px.\n', err);

%%
% display flows as color images
clr = getFlowColor(b - a);
dispOut = zeros(size(from), 'single');
dispOut(:,:,3) = 1;
dispOut = cv.circle(dispOut, a, 3, 'Colors',clr, 'Thickness','Filled');
dispOut = cv.cvtColor(dispOut, 'HSV2RGB');

clr = getFlowColor(b - c, false, 32);
dispErr = zeros(size(from), 'single');
dispErr(:,:,3) = 1;
dispErr = cv.circle(dispErr, a, 3, 'Colors',clr, 'Thickness','Filled');
dispErr = cv.cvtColor(dispErr, 'HSV2RGB');

dispGT = getFlowColor([gtU(:) gtV(:)]);
dispGT = reshape(dispGT(:,1:3), [size(gtU) 3]);
dispGT = cv.cvtColor(dispGT, 'HSV2RGB');

%%
% show results
opts = {'FontScale',0.8, 'Color','k', 'LineType','AA'};
str = 'Sparse matching: Global Patch Collider';
dispOut = cv.putText(dispOut, str, [20 40], opts{:});
str = sprintf('Average EPE: %.2f', err);
dispOut = cv.putText(dispOut, str, [20 80], opts{:});
str = sprintf('Number of matches: %d', nnz(mask));
dispOut = cv.putText(dispOut, str, [20 120], opts{:});

figure(1), imshow(dispOut), title('Correspondences')
figure(2), imshow(dispErr), title('Error')
figure(3), imshow(dispGT), title('Ground Truth')

%% Helper function

function clr = getFlowColor(UV, logScale, scaleDown)
    if nargin < 2, logScale = true; end
    if nargin < 3, scaleDown = 5; end

    angle = (atan2(-UV(:,2), -UV(:,1)) + pi) * 180 / pi;
    angle(all(UV == 0, 2)) = 0;

    radius = sqrt(sum(UV.^2, 2));
    if logScale
        radius = log(radius + 1);
    end
    radius = radius ./ scaleDown;
    radius = min(radius, 1);

    clr = [angle radius];
    clr(:,3) = 1;
    clr(:,4) = 0;
end


function ppg_filt(up)
%PPG_FILT extracts respiratory signals using various filtering techniques
% from the PPG signal as specified in PC's literature review.
%	            ppg_filt(option, up)
%
%	Inputs:
%		option          the option which has led to this function being used
%       up              universal parameters structure
%
%	Outputs:
%       ...
%

fprintf('\n--- Extracting Respiratory Signals using Filtering Techniques ');
log_int_respSig = 0;             % Has value 1 unless this is a final respiratory signal

for subj = up.paramSet.subj_list

    %% Cycle through each method
    for filt_no = 1 : length(up.al.options.ppg_filt)

        %% Skip if this processing has been done previously
        eval(['save_name = ''ppg', up.paths.filenames.filt '_' up.al.options.ppg_filt{filt_no} ''';']);
        iden_resp_sig_file_ending
        savepath = [up.paths.data_save_folder, num2str(subj), ending];
        exist_log = check_exists(savepath, save_name);
        if exist_log
            continue
        end

        %% Load relevant data
        if ~exist('data', 'var')
            load([up.paths.data_load_folder, up.paths.data_load_filename]);
        end
        % Extract PPG data
        rel_data = data(subj).ppg;
        rel_data.fs = data(subj).ppg.fs;

        %% Filter the raw signal using this method
        respWave = feval(up.al.options.ppg_filt{filt_no}, rel_data, up);

        %% Band-pass filter
        filtered_data = bpf_signal_to_remove_non_resp_freqs(respWave, respWave.fs, up);
        eval([save_name ' = filtered_data;']);

        %% Save processed data
        save_or_append_data

    end

end

end


function [x,f,funEvals] = minConF_BC(funObj,x,LB,UB,options)
% function [x,f] = minConF_BC(funObj,x,LB,UB,options)
%
% Function for using Two-Metric Projection to solve problems of the form:
%   min funObj(x)
%   s.t. LB_i <= x_i <= UB_i
%
%   @funObj(x): function to minimize (returns gradient as second argument)
%
%   options:
%       verbose: level of verbosity (0: no output, 1: final, 2: iter (default), 3:
%       debug)
%       optTol: tolerance used to check for progress (default: 1e-7)
%       maxIter: maximum number of calls to funObj (default: 250)
%       numDiff: compute derivatives numerically (0: use user-supplied
%       derivatives (default), 1: use finite differences, 2: use complex
%       differentials)
%       method: 'sd', 'lbfgs', 'newton'

nVars = length(x);

% Set Parameters
if nargin < 5
    options = [];
end
[verbose,numDiff,optTol,maxIter,suffDec,interp,method,corrections,damped] = ...
    myProcessOptions(...
    options,'verbose',3,'numDiff',0,'optTol',1e-6,'maxIter',500,'suffDec',1e-4,...
    'interp',1,'method','lbfgs','corrections',100,'damped',0);

% Output Log
if verbose >= 2
    fprintf('%10s %10s %15s %15s %15s\n','Iteration','FunEvals','Step Length','Function Val','Opt Cond');
end

% Make objective function (if using numerical derivatives)
funEvalMultiplier = 1;
if numDiff
    if numDiff == 2
        useComplex = 1;
    else
        useComplex = 0;
    end
    funObj = @(x)autoGrad(x,useComplex,funObj);
    funEvalMultiplier = nVars+1-useComplex;
end

% Evaluate Initial Point
x = projectBounds(x,LB,UB);
if strcmp(method,'newton')
    [f,g,H] = funObj(x);
    secondOrder = 1;
else
    [f,g] = funObj(x);
    secondOrder = 0;
end
funEvals = 1;
if verbose >= 1
  fprintf('Initial function value: %15.5e\n', f);
end

% Compute Working Set
working = ones(nVars,1);
working((x < LB+optTol*2) & g >= 0) = 0;
working((x > UB-optTol*2) & g <= 0) = 0;
working = find(working);

% Check Optimality
if isempty(working)
    if verbose >= 1
        fprintf('All variables are at their bound and no further progress is possible at initial point\n');
    end
    return;
elseif norm(g(working)) <= optTol
    if verbose >=1
        fprintf('All working variables satisfy optimality condition at initial point\n');
    end
    return;
end

if verbose >= 3
    switch method
        case 'sd'
            fprintf('Steepest Descent\n');
        case 'lbfgs'
            fprintf('L-BFGS\n');
        case 'bfgs'
            fprintf('BFGS\n');
        case 'newton'
            fprintf('Newton\n');
    end
end

i = 1;
while funEvals <= maxIter

    % Compute Step Direction
    d = zeros(nVars,1);
    switch(method)
        case 'sd'
            d(working) = -g(working);
        case 'lbfgs'
            if i == 1
                d(working) = -g(working);
                old_dirs = zeros(nVars,0);
                old_stps = zeros(nVars,0);
                Hdiag = 1;
            else
                if damped
                    [old_dirs,old_stps,Hdiag] = dampedUpdate(g-g_old,x-x_old,corrections,verbose==3,old_dirs,old_stps,Hdiag);
                else
                    [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,x-x_old,corrections,verbose==3,old_dirs,old_stps,Hdiag);
                end
                    curvSat = sum(old_dirs(working,:).*old_stps(working,:)) > 1e-10;
               d(working) = lbfgsC(-g(working),old_dirs(working,curvSat),old_stps(working,curvSat),Hdiag);
            end
            g_old = g;
            x_old = x;
        case 'bfgs'
            if i == 1
                d(working) = -g(working);
                B = eye(nVars);
            else
                y = g-g_old;
                s = x-x_old;

                ys = y'*s;

                if i == 2
                    if ys > 1e-10
                        B = ((y'*y)/(y'*s))*eye(nVars);
                    end
                end
                if ys > 1e-10
                    B = B + (y*y')/(y'*s) - (B*s*s'*B)/(s'*B*s);
                else
                    if verbose == 2
                        fprintf('Skipping Update\n');
                    end
                end
                d(working) = -B(working,working)\g(working);
            end
            g_old = g;
            x_old = x;

        case 'newton'
            [R,posDef] = chol(H(working,working));

            if posDef == 0
                d(working) = -R\(R'\g(working));
            else
                if verbose == 3
                    fprintf('Adjusting Hessian\n');
                end
                H(working,working) = H(working,working) + eye(length(working)) * max(0,1e-12 - min(real(eig(H(working,working)))));
                d(working) = -H(working,working)\g(working);
            end
        otherwise
            fprintf('Unrecognized Method: %s\n',method);
            break;
    end

    % Check that Progress can be made along the direction
    f_old = f;
    gtd = g'*d;
    if gtd > -optTol
        if verbose >= 2
            fprintf('Directional Derivative below optTol\n');
        end
        break;
    end

    % Select Initial Guess to step length
    if i == 1 && ~secondOrder
        t = min(1,1/sum(abs(g(working))));
    else
        t = 1;
    end

    % Evaluate the Objective and Projected Gradient at the Initial Step
    x_new = projectBounds(x+t*d,LB,UB);
    if secondOrder
        [f_new,g_new,H] = funObj(x_new);
    else
        [f_new,g_new] = funObj(x_new);
    end
    funEvals = funEvals+1;

    % Backtracking Line Search
    lineSearchIters = 1;
    while f_new > f + suffDec*g'*(x_new-x) || ~isLegal(f_new)
        temp = t;
        if interp == 0 || ~isLegal(f_new) || ~isLegal(g_new)
            if verbose == 3
                fprintf('Halving Step Size\n');
            end
            t = .5*t;
        else
            if verbose == 3
                fprintf('Cubic Backtracking\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d]);
        end

        % Adjust if change is too small
        if t < temp*1e-3
            if verbose == 3
                fprintf('Interpolated value too small, Adjusting\n');
            end
            t = temp*1e-3;
        elseif t > temp*0.6
            if verbose == 3
                fprintf('Interpolated value too large, Adjusting\n');
            end
            t = temp*0.6;
        end

        % Check whether step has become too small
        if sum(abs(t*d)) < optTol
            if verbose == 3
                fprintf('Line Search failed\n');
            end
            t = 0;
            f_new = f;
            g_new = g;
            break;
        end

        % Evaluate New Point
        x_new = projectBounds(x+t*d,LB,UB);
        [f_new,g_new] = funObj(x_new);
        funEvals = funEvals+1;
        lineSearchIters = lineSearchIters+1;

    end

    % Take Step
    x = x_new;
    f = f_new;
    g = g_new;

    % Compute Working Set
    working = ones(nVars,1);
    working((x < LB+optTol*2) & g >= 0) = 0;
    working((x > UB-optTol*2) & g <= 0) = 0;
    working = find(working);

    % Output Log
    if verbose >= 2
        fprintf('%10d %10d %15.5e %15.5e %15.5e %15.5e\n',i,funEvals*funEvalMultiplier,t,f,sum(abs(g(working))),norm(g(working)));
    end

    % Check Optimality
    if isempty(working)
        if verbose >= 1
            fprintf('All variables are at their bound and no further progress is possible\n');
        end
        break;
    elseif norm(g(working)) <= optTol
        if verbose >=1
            fprintf('All working variables satisfy optimality condition\n');
        end
        break;
    end

    % Check for lack of progress
    if sum(abs(t*d)) < optTol
        if verbose >= 1
            fprintf('Step size below optTol\n');
        end
        break;
    end

    if abs(f-f_old) < optTol
        if verbose >= 1
            fprintf('Function value changing by less than optTol\n');
        end
        break;
    end

    if funEvals*funEvalMultiplier > maxIter
        if verbose >= 1
            fprintf('Function Evaluations exceeds maxIter\n');
        end
        break;
    end

    % If necessary, compute Hessian
    if secondOrder && lineSearchIters > 1
        [f_new,g_new,H] = funObj(x);
    end

    i = i + 1;
end
end

function [x] = projectBounds(x,LB,UB)
x(x < LB) = LB(x < LB);
x(x > UB) = UB(x > UB);
end


% LORENZ system
% Evaluate ensemble of models (e.g. for varying training data length, noise
% level, etc.) in MPC

clear all, close all, clc
addpath('../utils');

WORKING = 0;
SystemModel = 'LORENZ';

%% Parameters
InputSignalType = 'sphs';
ModelName = 'NARX'; % DMDc, NARX, SINDYc
TrainAlg = 'trainbr'; %trainlm, trainbr % Choose: trainbr: eta_vec ~= 0, trainlm: eta_vec = 0
select_model = ModelName;
dep_trainlength = 1;
dep_noise = 0;

% Dependency on training length, noise-free training data
% Ntrain_vec = [5:15,20:5:95,100:100:1000];%,1500:500:3000];
% eta_vec = 0;
% Nr = 1;

% Dependency on training length, fixed noise level of data
Ntrain_vec = [5:15,20:5:95,100:100:1000,1250,1500,2000,3000];
eta_vec = 0.05;
Nr = 1;

% Some parameters
N_LENGTHS = length(Ntrain_vec);
Nmodels = N_LENGTHS;

%% Folders
figpath0 = ['../FIGURES/EX_',SystemModel,'_Dependencies/'];
datapath0 = ['../DATA/EX_',SystemModel,'_Dependencies/'];

if strcmp(ModelName,'NARX')
    figpath = [figpath0, ModelName, '/',TrainAlg,'/TL/'];
    datapath = [datapath0, ModelName, '/',TrainAlg,'/TL/'];
else
    figpath = [figpath0, ModelName, '/TL/'];
    datapath = [datapath0, ModelName, '/TL/'];
end

mkdir(figpath)
mkdir(datapath)
%% Load all models
switch ModelName
    case 'DMDc'
        Models(1:Nmodels) = struct('name',[],'sys',[],'Ndelay', [], 'Ttraining', [],'dt',[], 'Err', [], 'ErrM', []);
    case 'SINDYc'
        Models(1:Nmodels) = struct('name',[],'polyorder',[],'usesine',[],'Xi',[],'dt',[],'N',[], 'Ttraining', [], 'Err', [], 'ErrM', []);
    case 'NARX'
        Models(1:Nmodels) = struct('name',[],'net',[],'stateDelays',[],'inputDelays',[], 'hiddenSizes', [], 'dt',[], 'Ttraining', [], 'Err', [], 'ErrM', []);
end

if eta_vec == 0
    for iModel = 1:Nmodels
        iN = iModel;
        load(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_N',sprintf('%04g',Ntrain_vec(iN)), ...
            '_Eta',sprintf('%03g',100*eta_vec),'_iR1','.mat']))
        Models(iModel) = Model;
    end
elseif eta_vec~=0
    for iModel = 1:Nmodels
        iN = iModel;
        load(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_N',sprintf('%04g',Ntrain_vec(iN)), ...
            '_Eta',sprintf('%03g',100*eta_vec),'_BEST_MODEL.mat']))
        Models(iModel) = Model;
    end
end

%% Run MPC for all models
runMPC

%% Save results
save(fullfile(datapath,['EX_',SystemModel,'_MPC_',ModelName,'_',InputSignalType,'_TrainLength','.mat']),'Results')

%% Show performance
figure,hold on
for iM = 1:Nmodels
    Jend = cumsum(Results(iM).J);
    plot(Ntrain_vec(iM),Jend(end),'ok')
end
set(gca,'xscale','log','yscale','log')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%                                                                  %%%%%
%%%%    IEEE PES Power Grid Library - Optimal Power Flow - v21.07     %%%%%
%%%%          (https://github.com/power-grid-lib/pglib-opf)           %%%%%
%%%%             Benchmark Group - Small Angle Difference             %%%%%
%%%%                         29 - July - 2021                         %%%%%
%%%%                                                                  %%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function mpc = pglib_opf_case5_pjm__sad
mpc.version = '2';
mpc.baseMVA = 100.0;

%% area data
%	area	refbus
mpc.areas = [
	1	 4;
];

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
mpc.bus = [
	1	 2	 0.0	 0.0	 0.0	 0.0	 1	    1.00000	    0.00000	 230.0	 1	    1.10000	    0.90000;
	2	 1	 300.0	 98.61	 0.0	 0.0	 1	    1.00000	    0.00000	 230.0	 1	    1.10000	    0.90000;
	3	 2	 300.0	 98.61	 0.0	 0.0	 1	    1.00000	    0.00000	 230.0	 1	    1.10000	    0.90000;
	4	 3	 400.0	 131.47	 0.0	 0.0	 1	    1.00000	    0.00000	 230.0	 1	    1.10000	    0.90000;
	5	 2	 0.0	 0.0	 0.0	 0.0	 1	    1.00000	    0.00000	 230.0	 1	    1.10000	    0.90000;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin
mpc.gen = [
	1	 20.0	 0.0	 30.0	 -30.0	 1.0	 100.0	 1	 40.0	 0.0;
	1	 85.0	 0.0	 127.5	 -127.5	 1.0	 100.0	 1	 170.0	 0.0;
	3	 260.0	 0.0	 390.0	 -390.0	 1.0	 100.0	 1	 520.0	 0.0;
	4	 100.0	 0.0	 150.0	 -150.0	 1.0	 100.0	 1	 200.0	 0.0;
	5	 300.0	 0.0	 450.0	 -450.0	 1.0	 100.0	 1	 600.0	 0.0;
];

%% generator cost data
%	2	startup	shutdown	n	c(n-1)	...	c0
mpc.gencost = [
	2	 0.0	 0.0	 3	   0.000000	  14.000000	   0.000000;
	2	 0.0	 0.0	 3	   0.000000	  15.000000	   0.000000;
	2	 0.0	 0.0	 3	   0.000000	  30.000000	   0.000000;
	2	 0.0	 0.0	 3	   0.000000	  40.000000	   0.000000;
	2	 0.0	 0.0	 3	   0.000000	  10.000000	   0.000000;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status	angmin	angmax
mpc.branch = [
	1	 2	 0.00281	 0.0281	 0.00712	 400.0	 400.0	 400.0	 0.0	 0.0	 1	 -1.33164584752	 1.33164584752;
	1	 4	 0.00304	 0.0304	 0.00658	 426.0	 426.0	 426.0	 0.0	 0.0	 1	 -1.33164584752	 1.33164584752;
	1	 5	 0.00064	 0.0064	 0.03126	 426.0	 426.0	 426.0	 0.0	 0.0	 1	 -1.33164584752	 1.33164584752;
	2	 3	 0.00108	 0.0108	 0.01852	 426.0	 426.0	 426.0	 0.0	 0.0	 1	 -1.33164584752	 1.33164584752;
	3	 4	 0.00297	 0.0297	 0.00674	 426.0	 426.0	 426.0	 0.0	 0.0	 1	 -1.33164584752	 1.33164584752;
	4	 5	 0.00297	 0.0297	 0.00674	 240.0	 240.0	 240.0	 0.0	 0.0	 1	 -1.33164584752	 1.33164584752;
];

% INFO    : === Translation Options ===
% INFO    : Phase Angle Bound:           1.33164584752 (deg.)
% INFO    :
% INFO    : === Generator Bounds Update Notes ===
% INFO    :
% INFO    : === Base KV Replacement Notes ===
% INFO    :
% INFO    : === Transformer Setting Replacement Notes ===
% INFO    :
% INFO    : === Line Capacity Monotonicity Notes ===
% INFO    :
% INFO    : === Writing Matpower Case File Notes ===


function mColormap = gui_Colormap_BlueYellowRed(nSize)

% If size is not specified, set it to 256
if nargin < 1
  nSize = 256;
end

mColormap = [];

nSteps = floor(nSize * 0.25);
mColormap = [mColormap; gui_Interpolate([0.000000, 0.000000, 1.000000], [0.500000, 0.500000, 0.500000], nSteps)];

nSteps = floor(nSize * 0.25);
mColormap = [mColormap; gui_Interpolate([0.500000, 0.500000, 0.500000], [1.000000, 1.000000, 0.000000], nSteps)];

nSteps = floor(nSize * 0.25);
mColormap = [mColormap; gui_Interpolate([1.000000, 1.000000, 0.000000], [1.000000, 0.500000, 0.000000], nSteps)];

nSteps = floor(nSize * 0.25);
mColormap = [mColormap; gui_Interpolate([1.000000, 0.500000, 0.000000], [1.000000, 0.000000, 0.000000], nSteps)];


% TSERRORPLOT(Y, E)
%
% TSERRORPLOT(X, Y, E)
%
% TSERRORPLOT(X, Y, L, U)
%
% Plots D signals with errors.
%
% X : N x 1 matrix of inputs
% Y : N x D matrix of function values
% E : N x D or N x 1 or 1 x D or 1 x 1 matrix of errors
% L : N x D or N x 1 or 1 x D or 1 x 1 matrix of lower errors
% U : N x D or N x 1 or 1 x D or 1 x 1 matrix of upper errors
%
% See ERRORPLOT for details on the optional arguments.

% Copyright (c) 2010 Jaakko Luttinen

function hax = tserrorplot(Y, E, varargin)

% Check whether the input was (Y,E) or (X,Y,E) or (X,Y,L,U)
if nargin >= 3 && isnumeric(varargin{1})
  x = Y;
  Y = E;
  if nargin >= 4 && isnumeric(varargin{2})
    % (X,Y,L,U)
    L = varargin{1};
    U = varargin{2};
    params = varargin(3:end);
  else
    % (X,Y,E)
    L = varargin{1};
    U = varargin{1};
    params = varargin(2:end);
  end
else
  % (Y,E)
  x = (1:(size(Y,1)))';
  L = E;
  U = E;
  params = varargin;
end

% Parse optional arguments
options = struct();
[options, errmsg, remopts] = argparse(options, params{:});
error(errmsg);

% Scale L and U to proper sizes
L = bsxfun(@times, ones(size(Y)), L);
U = bsxfun(@times, ones(size(Y)), U);

figure

D = size(Y,2);
hax = zeros(D,1);

for d=1:D
  % Plot with errors
  hax(d) = subplot(D,1,d);
  errorplot(x(:),Y(:,d),L(:,d),U(:,d), remopts{:});
end

if nargout < 1
  clear hax;
end