clear all
close all

disp('Data from multiple subjects');

disp('Estimate mixed effects using Langevin Monte Carlo');
disp('Using LDS model with constrained connectivity');

lds.model='forward';

% Number of dynamical states e.g. brain areas
d=4;

% Observation noise
lds.sd=0.1;

% Prior over initial states
lds.R.pE=linspace(3,1.5,d)';
lds.R.pC=0.5^2*eye(d);

% Number of subjects
lds.Nsub=3;

% Number of observations per subject
lds.Nobs=5;

lds.init_par='random';
lds.flow_par='fixed';

% Generate group data
[lds.pinit,lds.pflow,lds.names,M,U,Y] = mci_lds_group_data (lds);

% Assign init/flow as random/fixed effects
assign.init_par='random';
assign.flow_par='fixed';
assign.out_par='known';

MCI.assign=assign;

MCI.fixed.pE=M{1}.pE;
MCI.fixed.pC=M{1}.pC;

% Initialisation
i0=mci_interp_init(Y,M{1});
a0=[];
if strcmp(assign.init_par,'random')
    MCI.pinit0=i0;
else
    MCI.pinit0=mean(i0,2);
end
if strcmp(assign.flow_par,'random')
    MCI.pflow0=spm_vec(M{1}.pE)*ones(1,lds.Nsub);
    if ~isempty(a0), MCI.pflow0(1:d,:)=a0; end
else
    MCI.pflow0=spm_vec(M{1}.pE);
    if ~isempty(a0), MCI.pflow0(1:d,:)=mean(a0,2); end
end
MCI.pout0=[];

MCI.M=M; MCI.U=U; MCI.Y=Y;
MCI.update_obs_noise=1;
MCI.verbose=1;
MCI.total_its=16;
MCI.rinit=0.25;

tic;
MCI = spm_mci_mfx_dynamic (MCI);
toc

rmse=mci_lds_plot_params (MCI,lds);

for n=1:lds.Nsub,
    mci_lds_plot_fit (MCI,lds,n,1);
end

disp('True dynamics:');
[f,Atrue] = mci_lds_fx (lds.pinit(:,1),U{1},lds.pflow,M{1});
disp(Atrue);

disp('Estimated dynamics:');
[f,Aest] = mci_lds_fx (MCI.pinit,U{1},MCI.pflow,M{1});
disp(Aest);

mci_plot_noiseSD (MCI.Ce,MCI.post_ind);


function r8mat_transpose_test ( )

%*****************************************************************************80
%
%% R8MAT_TRANSPOSE_TEST tests R8MAT_TRANSPOSE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2015
%
%  Author:
%
%    John Burkardt
%
  m = 5;
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8MAT_TRANSPOSE_TEST\n' );
  fprintf ( 1, '  R8MAT_TRANSPOSE transposes an R8MAT.\n'; );

  a = r8mat_indicator ( m, n );
  r8mat_print ( m, n, a, '  Matrix A:' );

  at = r8mat_transpose ( m, n, a );
  r8mat_print ( n, m, at, '  Transposed matrix At:' );

  return
end


% function [B, Bb, Bw, pc_b, sc_b, pc_w, sc_w] = mlpcr2(X,Y,varargin)
%
% Multilevel PCR v3.0 designed for prediction on datasets with multiple
% subjects and multiple observations per subject. Unlike version 2 this
% supports full mixed effects modeling. See randSlope and randInt
% arguments.
%
% Identifies within block (subject) eigenvectors with optional balancing
% across subjects. Computes loadings of full dataset on these within
% subject eigenvectors (so they can vary both within and between blocks).
% Computes a separate PCA on the residual and obtains a second set of
% orthogonal components and scores which only vary between blocks. Performs
% regression of between and within loadings (jointly) on pain outcome, and
% projects regression coefficients back to voxel space using within and
% between eigenvectors.
%
% regression is OLS (default) or moore-penrose pseudoinverse (rank
% deficient data), not some mixed effects thing. Within subject PCA is
% rate limiting step.
%
% If the defaults are used then the result is identical to PCR, only you
% get both within and between subject predictive models in addition to the
% full model. Different results may be obtained with hyperparameter
% optimization or concensus PCA enabled.
%
% Input ::
%
%   X           - n x p data matrix
%
%   Y           - n x 1 outcome vector
%
%   'subjIDs    - n x 1 vector (ideal) or cellstr (throws warning, but
%                   fine) indicating group affilitations. Subjects must be
%                   adjacent. Don't intermix blocks with one another. This
%                   function was not designed for this scenario and will
%                   break even with equivalently intermixed block labels.
%
% Optional Input ::
%
%   'numcomponents'
%               - 1 x 2 numeric vector indicating number of PCA dimensions
%                   to retain at the between and within levels
%                   (respectively). Use Inf to autoselect based on
%                   available degrees of freedom. default: [Inf, Inf]
%                 Note 1: bayesopt from the Statistics and Machine Leraning
%                   matlab toolbox provides an elegant way to optimize
%                   these variables.
%                 Note 2: More dimensions result in less biased solutions,
%                   which is a good reason for using the default [Inf,Inf].
%                   However task driven variance is going to be captured by
%                   within components, so [0, Inf] is also a sensible
%                   default if you wish to treat trait differences as
%                   noise.
%
%   'cpca'      - followed by 0/1 to indicate whether or not concensus PCA
%                   (Westerhuis, et al. 1998) should be used in place of
%                   standard pca. If concensus PCA is enabled eigenvectors
%                   will be selected such that variance is explained
%                   equally across all blocks. Otherwise eigenvectors will
%                   best represent blocks with the most observations.
%                 Note 1: for optimization of dimension hyperparameters
%                   specify a custom loss function that also balances the
%                   weight of each block. fmri_data/predict won't do this
%                   and will consequently fight against CPCA. A future
%                   update may fix this, and if so this note should be
%                   removed.
%                 Note 2: In principle concensus PCA could be implemened
%                   for traditional PCR too. It just hasn't been.
%
%   'randInt'   - Fit a random intercept model. Default=false
%
%   'randSlope' - Fit a random slope model (within effects only).
%                   Default=false
%
%   'fitlmeOpts'
%               - Cell array of options to passthrough to fitlme. Run with
%               {'CovariancePattern','isotropic'} by default.
%
% Output ::
%
%   B           - B(1) is intercept, B(2:end) are regression weights in X
%                   space.
%
%   Bb          - Bb(1) is intercept. Bb(2:end) are regression weights of
%                   between block variance components in X space.
%
%   Bw          - Bw(1) is zero. Bb(2:end) are regression weights of within
%                   block variance components in X space.
%
%   pc_b        - between eigenvectors
%
%   sc_b        - scores on between eigenvectors
%
%   pc_w        - within eigenvectors
%
%   sc_w        - scores on within eigenvectors
%
%
% Version History ::
%
%   MLPCR was originally developed using mixed effects models (version 1,
%   Petre, et al., 2019), however model fitting was prohibitively slow and
%   consequently performance characteristics could not be fully evaluated.
%   Version 2.0 substitutes (optionally weighted) OLS for much faster
%   convergence, and minimaly different weight fitting. Version 1 is much
%   more flexible than version 2, and is still available in CanlabPrivate
%   for potential future development. It has been removed from CanlabCore.
%   Version 3.0 reintroduces mixed effects modelling as a non-default
%   option, but abides by better coding practices, following in the
%   footsteps of version 2.0.
%
% References ::
%
%   Petre B, Woo W, Losin E, Eisenbarth H, Wager TD. (2019) Separate within
%       -subject and individual-difference predictions with multilevel
%       MVPA. Society for Neuroscience, San Diego, CA. (included with
%       canlabCore mlpcr library as pdf).
%
%   Westerhuis J, Kourti T, MacGregor J. (1998) Analysis of Multiblock and
%       Hierarchical PCA and PLS Methods. Journal of Chemometrics 12(5).
%
%
% Designed and writen by Bogdan, 5/4/2020
%
%
%
% ToDo:
% - Enable between or within dimension retention priority for bootstrapping
%   (allowing the user to force retention of one or the other even if
%   eigenvalue rank doesn't justify it)
% - passthrough options to higher level cv_mlpcr, cv_mlpcr_bt and
%   cv_mlpcr_wi scripts. Concensus PCA should also use concensus cv_err,
%   cv_mlpcr_wi and cv_mlpcr_bt should have within and between priority
%   (respectively) by default.

function [B, Bb, Bw, pc_b, sc_b, pc_w, sc_w, b] = mlpcr3(X,Y,varargin)
    subjIDs = [];
    wiDim = Inf;
    btDim = Inf;
    cpca = 0;
    randInt = 0;
    randSlope = 0;
    fitlmeOpts = {};
    for i = 1:length(varargin)
        if ischar(varargin{i})
            switch varargin{i}
                case 'subjIDs'
                    subjIDs = varargin{i+1}(:);
                case 'numcomponents'
                    nc = varargin{i+1};
                    btDim = nc(1);
                    wiDim = nc(2);
                case 'cpca'
                    cpca = varargin{i+1};
                case 'randInt'
                    randInt = varargin{i+1};
                case 'randSlope'
                    randSlope = varargin{i+1};
                case 'fitlmeOpts'
                    fitlmeOpts = varargin{i+1};
            end
        end
    end

    if randInt || randSlope
        if ~ismember('CovariancePattern',fitlmeOpts)
            fitlmeOpts = [fitlmeOpts, {'CovariancePattern', 'isotropic'}];
        end
    end

    % check for intercept and remove if present
    %{
    intercept = ones(1,size(X,2))';
    wh = cellfun(@(x) isequal(x,intercept),num2cell(X,1));
    if ~isempty(wh)
        warning('Your input X should not contain an intercept. Removing it. Model parameters will corespond to intercept free design')
        X(:,wh) = [];
    end
    %}

    % we need adjacent subjIDs, so let's ensure that
    [subjIDs, newOrder] = sortrows(subjIDs(:));
    [~,origOrder] = sort(newOrder);

    X = X(newOrder,:);
    Y = Y(newOrder);

    if isempty(subjIDs)
        error('Cannot perform multilevel PCR without a subjIDs identifier');
    end

    [~, grp_exemplar, subjIDs] = unique(subjIDs,'rows','stable');
    uniq_grp = unique(subjIDs);

    % get centering and expansion matrices
    n_grp = length(uniq_grp);
    cmat = cell(1,n_grp);
    emat = cell(1,n_grp);
    sf = []; % scale factor for imbalanced datasets
    for i = 1:n_grp
        this_grp = uniq_grp(i);
        this_n = sum(this_grp == subjIDs);
        cmat{i} = eye(this_n) - 1/this_n;
        emat{i} = ones(this_n,1);
        sf = [sf(:); 1/sqrt(this_n)*ones(this_n,1)];
    end
    cmat = blkdiag(cmat{:});
    emat = blkdiag(emat{:});
    if ~cpca
        sf = ones(size(sf));
    end

    % compute preliminary within fractions
    Xw = cmat*X;

    if wiDim > 0
        % determine within dimension retention
        if wiDim > length(subjIDs) - length(uniq_grp)
            if wiDim < Inf % something user supplied was too big
                warning('Max wiDim exceeds max df, reseting wiDim to %d',length(subjIDs) - length(uniq_grp));
            end

            wiDim = min(length(subjIDs) - length(uniq_grp), size(X,2));
        end

        % Get concensus PCA solution, which weighs each block equally
        % requires Matlab 2016b or later perform operation across all
        %   columns like this with sf
        [pc_w,~,~] = svd((sf.*Xw)', 'econ');
        pc_w = pc_w(:,1:wiDim);

        sc_w = X*pc_w;

        % note: sc_w*pc_w' ~= Xw, it pulls scores from the entire dataset,
        % 	Some of the between fraction may vary along the within
        % 	components, and we want to pull that into the within scores.
        % 	sc_w*pc_w' is not necessarily mean zero within subject.

        % modified Xb, invariant with subject, but not exactly the subject
        %   ean either due to the missing variance along pc_w
        Xr = X - sc_w*pc_w';
        Xb = Xr - cmat*Xr; % if we don't have full wiDim then there's residual within variance to remove
        Xb = Xb(grp_exemplar,:);
    else
        [pc_w, sc_w] = deal([]);

        Xb = X - Xw;
        Xb = Xb(grp_exemplar,:);
    end

    if btDim > 0
        % get between components from residual between fraction
        if btDim > length(uniq_grp) - 1
            if btDim < Inf % something user supplied was too big
                warning('Max btDim exceeds max df, reseting btDim to %d',length(uniq_grp) - 1);
            end

            btDim = min(length(uniq_grp) - 1, size(Xb,2));
        end

        [pc_b,~,~] = svd(scale(Xb,1)','econ');
        pc_b = pc_b(:,1:btDim);
        sc_b = Xb*pc_b;
    else
        [pc_b, sc_b] = deal([]);
    end

    if ~isempty(sc_b)
        sc_b = emat*sc_b;
        bDim = 1:size(sc_b,2);
        wDim = (1:size(sc_w,2)) + bDim(end);
    else
        bDim = [];
        wDim = 1:size(sc_w,2);
    end
    sc = [sc_b, sc_w];
    pc = [pc_b, pc_w];

    % 3/8/13 TW solution from cv_pcr code to use numcomps, because sc is
    %   not always full rank during bootstrapping.
    % Modified by Bogdan for use in mlpcr to keep componens with the
    %   highest ranking eigenvectors (not necessarily sequential in mlpcr
    %   because of within/between stratification)
    % ToDo: create a bootstrap priority option to allow the user to specify
    %   whether they want to prefer between or within component retention
    %   regardless of eigenvalue size.
    if rank(sc) == size(sc,2)
        numcomps = rank(sc);
        retainComps = 1:numcomps;

        bDim(~ismember(bDim,retainComps)) = [];
        wDim(~ismember(wDim,retainComps)) = [];
    elseif rank(sc) < size(sc,2)
        error('You will run into problems when reconstructing Bw at the end. Fix the wDim and bDim indexing');
        numcomps = rank(sc)-1;
        [~,compRank] = sort(var(sc),'descend');
        retainComps = sort(compRank(1:numcomps));

        [bDimnew, wDimnew] = deal(zeros(length(compRank),1));

        bDim(~ismember(bDim,retainComps)) = [];
        wDim(~ismember(wDim,retainComps)) = [];

        if ~any(ismember(bDim,retainComps)) && btDim > 0
            warning('All between dimensions dropped due to rank deficiency');
        end
        if ~any(ismember(wDim,retainComps)) && wiDim > 0
            warning('All within dimensions dropped due to rank deficiency');
        end

        sc_w = sc(:,wDim);
        sc_b = sc(:,bDim);
        pc_w = pc(:,wDim);
        pc_b = pc(:,bDim);

        bDimnew(bDim) = 1;
        bDimnew = bDimnew(retainComps);
        bDim = find(bDimnew);

        wDimnew(wDim) = 1;
        wDimnew = wDimnew(retainComps);
        wDim = find(wDimnew);
    end

    xx = [ones(size(Y, 1), 1) sc(:, retainComps)];

    if ~randInt && ~randSlope % fit a LS or weighted LS model
        if rank(xx) <= size(sc, 2)
            % compute (optional: weighted) pseudoinverse if not full rank
            tol = max(size(xx))*eps(norm(xx));
            [u,s,v] = svd(sf.*xx,'econ');
            s = diag(s);
            s(s>tol) = 1./s(s>tol);
            s = diag(s);
            pinv_xx = v*s*u';

            b = pinv_xx * (sf.*Y);
        else
            b = inv(xx'*diag(sf.^2)*xx)*xx'*diag(sf.^2)*Y;
        end
    else % fit random effects model
        if rank(xx) <= size(sc,2)
            warning(['Rank deficient data found. This is not supported with mixed effects models.',...
                'If you''re bootstrapping consider using the fixed effects approach instead']);
        end
        xxRE = [];
        if randInt
            xxRE = [xxRE, ones(size(Y,1),1)];
        end
        if randSlope
            xxRE = [xxRE, sc_w];
        end

        %m = fitlmematrix(double(sf.*xx), sf.*Y, double(sf.*xxRE), categorical(subjIDs), fitlmeOpts{:});
        %try
            m = fitlmematrix(double(xx), Y, double(xxRE), categorical(subjIDs), fitlmeOpts{:});
        %catch
        %    keyboard
        %end
        b = m.fixedEffects;
    end

    if ~isempty(retainComps)
        B = [b(1); pc(:,retainComps)*b(2:end)];
    else
        B = [b;  zeros(size(X,2),1)];
    end

    if isempty(bDim)
        Bb = [b(1); zeros(size(X,2),1)];
    else
        %Bb = [b(1); pc_b(:,bDim)*b(bDim + 1)];
        Bb = [b(1); pc(:,bDim)*b(bDim + 1)];
    end

    if isempty(wDim)
        Bw = [0; zeros(size(X,2),1)];
    else
        %Bw = [0; pc_w(:,wDim)*b(wDim + 1)];
        Bw = [0; pc(:,wDim)*b(wDim+1)];
    end

    if ~isempty(sc_b), sc_b = sc_b(origOrder,:); end
    if ~isempty(sc_w), sc_w = sc_w(origOrder,:); end
end


% [threshx,cost,classorder] = onedim2kclass(x,idx,...)
%
% finds the k-1 thresholds that best separate the input one-dimensional
% data vector x into k classes
%
% input:
%
% x: n x 1 is the n one-dimensional data points
% idx: n x 1 is the integer labels for each data point
%
% optional:
%
% 'issorted': true/false, whether the list is sorted already
% 'classorder': 1 x k vector where classorder(1) comes first and
% classorder(k) comes last. otherwise, classorder is set based on means of each
% class
%
% output:
% threshx: k+1x1, class classorder(i) is >= threshx(i) and < thresh(i+1)
% cost: number of misclassified samples
% classorder: 1 x k
%
function [threshx,cost,classorder] = onedim2kclass(x,idx,varargin)

x = x(:);
idx = idx(:);
n = length(x);

[issorted,classorder] = myparse(varargin,'issorted',false,'classorder',nan);

%k = max(idx); % right now, assuming labels are 1,...,k

% make labels 1,...,k
oldidx = idx;
[newidx2oldidx0,tmp,idx] = unique(idx);
%[tmp,oldidx2newidx0] = sort(newidx2oldidx0);
k = length(newidx2oldidx0);
if ~any(isnan(classorder)),
  classorder0 = classorder;
  for i = 1:k,
    classorder(classorder0==newidx2oldidx0(i)) = i;
  end
end

if k == 1,
  threshx = [min(x),max(x)+eps];
  cost = 0;
  if isnan(classorder),
    classorder = newidx2oldidx0;
  end
  return;
end

%if k == 2,
%  % use onedim2class
%  % idx should be -1,1
%  idx = 2*idx-3;
%  [thresh,lower,cost] = onedim2class(x,idx);
%  if lower == -1,
%    order = [1,2];
%  else
%    order = [2,1];
%  end
%  classorder = order;
%  %classorder = newidx2oldidx(order);
%  return;
%end

% choose order for classes based on order of means
if any(isnan(classorder)),

  mu = zeros(1,k);
  for k1 = 1:k,
    mu(k1) = mean(x(idx==k1));
  end
  [sortedmu,order] = sort(mu);
  [tmp,oldidx2newidx] = sort(order);
  classorder = newidx2oldidx0(order);
  % relabel
  idx0 = idx;
  for k1 = 1:k,
    idx(idx0==k1) = oldidx2newidx(k1);
  end

else
  [tmp,oldidx2newidx] = sort(classorder);
end

if ~issorted,
  [x,xorder] = sort(x);
  idx = idx(xorder);
  [tmp,xreorder] = sort(xorder);
end
% deal with duplicates: cannot have a class start at i if x(i) == x(i-1)
% isdup(i) = x(i) == x(i-1)
isdup = [x(1:end-1) == x(2:end);false];
dupidx = find(isdup);

% costs(n1+1,k1) is the min cost of classifying samples 1:n1 as classes 1:k1
costs = inf(n+1,k-1);
% prev(n1+1,k1)+1 is the start of class k1 when classifying samples 1:n1
prev = zeros(n+1,k-1);

% base cases

% for i = 1,...,n
% cost(i+1,1) is cost of classifying 1:i as 1 is
% sum(idx(1:i)~=k) = c(i+1)
% for i = 0, cost(i+1,1) = 0
costs(2:n+1,1) = cumsum(double(idx~=1));
% cannot end at dupidx - 1
costs(dupidx+1) = inf;

% for k1 = 1,...,k cost of classifying 1:0 as 1:k1 is 0
costs(1,:) = 0;

% compute for increasing k, up to k-1
for k1 = 2:k-1,

  c = [0;cumsum(double(idx~=k1))];

  for n1 = 1:n,
    if isdup(n1), continue; end
    % compute one-class cost for i = 1:n1+1, end = n1
    oneclasscost = c(n1+1)-c(1:n1+1);
    % prev(n1+1,k1) is the start of the last class
    [costs(n1+1,k1),prev(n1+1,k1)] = min( costs(1:n1+1,k1-1) + oneclasscost );
  end

end

% compute for n,k
c = [0;cumsum(double(idx~=k))];
oneclasscost = c(n+1)-c(1:n+1);
[cost,prevlast] = min( costs(1:n+1,k-1) + oneclasscost );

% get thresholds
% class k1 goes from thresh(k1) to thresh(k1+1)-1
thresh = ones(k+1,1);
thresh(end) = n+1;
for k1 = k:-1:1,
  if prevlast < 1,
    thresh(1:k1) = 1;
    break;
  end
  thresh(k1) = prevlast;
  if k1 > 1,
    prevlast = prev(prevlast,k1-1);
  end
end

% convert from indices to data points
threshx = zeros(k+1,1);
threshx(thresh<=1) = x(1);
threshx(thresh==n+1) = x(n)+1;
other = thresh > 1 & thresh <= n;
threshx(other) = (x(thresh(other)-1)+x(thresh(other)))/2;
%threshx(other) = x(thresh(other));

if exist('classorder0','var')
  classorder = classorder0;
end

% here is some code for testing
if 0,
n = 100;
k = 3;
ntests = 100;
sig = .1;
for test = 1:ntests,
  %x = sort(rand(n,1));
  idxtrue = randsample(k,n,true);
  mutrue = sort(rand(k,1));
  x = randn(n,1)*sig + mutrue(idxtrue);
  [x,order] = sort(x);
  idxtrue = idxtrue(order);
  tic;
  [mu,cost,idxdp,threshx] = onedimkmeans(x,k);
  t = toc;
  fprintf('DP score = %f, diff = %e, time = %f, centers = ',cost,cost-sum(sumD),t);
  fprintf('%f, ',mu);
  fprintf('\n');
  if cost > sum(sumD),
    if all(idxkm==idxdp),
      fprintf('Bigger cost, diff = %e, but all idx are the same\n',cost-sum(sumD));
      pause(1);
    else
      fprintf('*****Bigger cost, diff = %e, differing indices\n',cost-sum(sumD));
      fprintf('Differing indices: ');
      fprintf('%d, ',find(idxkm~=idxdp));
      fprintf('\n');
      break;
    end
  end
  diffcost(test) = cost-sum(sumD);
  fprintf('\n');
end
end


function jac = p00_jac ( test, neqn, t, y )

%*****************************************************************************80
%
%% P00_JAC evaluates the jacobian for any problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer TEST, the problem number.
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the jacobian.
%
%    Output, real JAC(NEQN,NEQN), the jacobian matrix.
%
  if ( test == 1 )
    jac = p01_jac ( neqn, t, y );
  elseif ( test == 2 )
    jac = p02_jac ( neqn, t, y );
  elseif ( test == 3 )
    jac = p03_jac ( neqn, t, y );
  elseif ( test == 4 )
    jac = p04_jac ( neqn, t, y );
  elseif ( test == 5 )
    jac = p05_jac ( neqn, t, y );
  elseif ( test == 6 )
    jac = p06_jac ( neqn, t, y );
  elseif ( test == 7 )
    jac = p07_jac ( neqn, t, y );
  elseif ( test == 8 )
    jac = p08_jac ( neqn, t, y );
  elseif ( test == 9 )
    jac = p09_jac ( neqn, t, y );
  elseif ( test == 10 )
    jac = p10_jac ( neqn, t, y );
  elseif ( test == 11 )
    jac = p11_jac ( neqn, t, y );
  elseif ( test == 12 )
    jac = p12_jac ( neqn, t, y );
  elseif ( test == 13 )
    jac = p13_jac ( neqn, t, y );
  elseif ( test == 14 )
    jac = p14_jac ( neqn, t, y );
  elseif ( test == 15 )
    jac = p15_jac ( neqn, t, y );
  elseif ( test == 16 )
    jac = p16_jac ( neqn, t, y );
  elseif ( test == 17 )
    jac = p17_jac ( neqn, t, y );
  elseif ( test == 18 )
    jac = p18_jac ( neqn, t, y );
  elseif ( test == 19 )
    jac = p19_jac ( neqn, t, y );
  elseif ( test == 20 )
    jac = p20_jac ( neqn, t, y );
  elseif ( test == 21 )
    jac = p21_jac ( neqn, t, y );
  elseif ( test == 22 )
    jac = p22_jac ( neqn, t, y );
  elseif ( test == 23 )
    jac = p23_jac ( neqn, t, y );
  elseif ( test == 24 )
    jac = p24_jac ( neqn, t, y );
  elseif ( test == 25 )
    jac = p25_jac ( neqn, t, y );
  elseif ( test == 26 )
    jac = p26_jac ( neqn, t, y );
  elseif ( test == 27 )
    jac = p27_jac ( neqn, t, y );
  elseif ( test == 28 )
    jac = p28_jac ( neqn, t, y );
  elseif ( test == 29 )
    jac = p29_jac ( neqn, t, y );
  elseif ( test == 30 )
    jac = p30_jac ( neqn, t, y );
  elseif ( test == 31 )
    jac = p31_jac ( neqn, t, y );
  elseif ( test == 32 )
    jac = p32_jac ( neqn, t, y );
  elseif ( test == 33 )
    jac = p33_jac ( neqn, t, y );
  elseif ( test == 34 )
    jac = p34_jac ( neqn, t, y );
  elseif ( test == 35 )
    jac = p35_jac ( neqn, t, y );
  elseif ( test == 36 )
    jac = p36_jac ( neqn, t, y );
  elseif ( test == 37 )
    jac = p37_jac ( neqn, t, y );
  elseif ( test == 38 )
    jac = p38_jac ( neqn, t, y );
  elseif ( test == 39 )
    jac = p39_jac ( neqn, t, y );
  elseif ( test == 40 )
    jac = p40_jac ( neqn, t, y );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P00_JAC - Fatal error!\n' );
    fprintf ( 1, '  Unrecognized problem number = %d\n', test );
    error ( 'P00_JAC - Fatal error!' );
  end

  return
end


function power_method_test ( )

%*****************************************************************************80
%
%% POWER_METHOD_TEST tests the POWER_METHOD library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 May 2008
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'POWER_METHOD_TEST\n' );
  fprintf ( 1, '  MATLAB version:\n' );
  fprintf ( 1, '  Test the POWER_METHOD library.\n' );

  power_method_test01 ( );
  power_method_test02 ( );
  power_method_test03 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'POWER_METHOD_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function power_method_test01 ( )

%*****************************************************************************80
%
%% POWER_METHOD_TEST01 uses POWER_METHOD on the Fibonacci2 matrix.
%
%  Discussion:
%
%    This matrix, despite having a single dominant eigenvalue, will generally
%    converge only very slowly under the power method.  This has to do with
%    the fact that the matrix has only 3 eigenvectors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 July 2008
%
%  Author:
%
%    John Burkardt
%
  n = 50;

  a = fibonacci2 ( n );

  seed = 123456789;

  [ x, seed ] = r8vec_uniform_01 ( n, seed );

  it_max = 300;
  tol = 0.000001;

  phi = ( 1.0 + sqrt ( 5.0 ) ) / 2.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POWER_METHOD_TEST01\n' );
  fprintf ( 1, '  Use the power method on the Fibonacci2 matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N       = %d\n', n );
  fprintf ( 1, '  Maximum iterations   = %d\n', it_max );
  fprintf ( 1, '  Error tolerance      = %g\n', tol );

  ctime1 = cputime;

  [ x, lambda, it_num ] = power_method ( n, a, x, it_max, tol );

  ctime2 = cputime;
  ctime = ctime2 - ctime1;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of iterations = %d\n', it_num );
  fprintf ( 1, '  CPU time             = %f\n', ctime );
  fprintf ( 1, '  Estimated eigenvalue = %f\n', lambda );
  fprintf ( 1, '  Correct value        = %f\n', phi );
  fprintf ( 1, '  Error                = %e\n', abs ( lambda - phi ) );
%
%  X2 is the exact eigenvector.
%
  x2(1:n,1) = phi.^(0:n-1);
  x2 = x2 / norm ( x2 );
%
%  The sine of the angle between X and X2 is a measure of error.
%
  cos_x1x2 = x' * x2;
  sin_x1x2 = sqrt ( ( 1.0 - cos_x1x2 ) * ( 1.0 + cos_x1x2 ) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sine of angle between true and estimated vectors = %e\n', ...
    sin_x1x2 );

  return
end
function power_method_test02 ( )

%*****************************************************************************80
%
%% POWER_METHOD_TEST02 uses POWER_METHOD2 on the Fibonacci2 matrix.
%
%  Discussion:
%
%    This matrix, despite having a single dominant eigenvalue, will generally
%    converge only very slowly under the power method.  This has to do with
%    the fact that the matrix has only 3 eigenvectors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 July 2008
%
%  Author:
%
%    John Burkardt
%
  n = 50;

  a = fibonacci2 ( n );

  seed = 123456789;

  [ x, seed ] = r8vec_uniform_01 ( n, seed );

  it_max = 300;
  tol = 0.000001;

  phi = ( 1.0 + sqrt ( 5.0 ) ) / 2.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POWER_METHOD_TEST02\n' );
  fprintf ( 1, '  Use the power method2 on the Fibonacci2 matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N       = %d\n', n );
  fprintf ( 1, '  Maximum iterations   = %d\n', it_max );
  fprintf ( 1, '  Error tolerance      = %g\n', tol );

  ctime1 = cputime;

  [ lambda, v, it_num ] = power_method2 ( n, a, x, it_max, tol );

  ctime2 = cputime;
  ctime = ctime2 - ctime1;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of iterations = %d\n', it_num );
  fprintf ( 1, '  CPU time             = %f\n', ctime );
  fprintf ( 1, '  Estimated eigenvalue = %f  %f\n', real ( lambda ), imag ( lambda ) );
  fprintf ( 1, '  Correct value        = %f\n', phi );
  fprintf ( 1, '  Error                = %e\n', abs ( lambda - phi ) );

  return
end
function power_method_test03 ( )

%*****************************************************************************80
%
%% POWER_METHOD_TEST03 uses POWER_METHOD2 on the TRIS matrix.
%
%  Discussion:
%
%    This matrix, despite having a single dominant eigenvalue, will generally
%    converge only very slowly under the power method.  This has to do with
%    the fact that the matrix has only 3 eigenvectors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 July 2008
%
%  Author:
%
%    John Burkardt
%
  n = 50;

  alpha = -1.0;
  beta = 10.0;
  gamma = 8.0;

  a = tris ( n, n, alpha, beta, gamma );

  seed = 123456789;

  [ x, seed ] = r8vec_uniform_01 ( n, seed );

  it_max = 4000;
  tol = 0.000001;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POWER_METHOD_TEST03\n' );
  fprintf ( 1, '  Use the power method2 on the TRIS matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N       = %d\n', n );
  fprintf ( 1, '  Maximum iterations   = %d\n', it_max );
  fprintf ( 1, '  Error tolerance      = %g\n', tol );

  ctime1 = cputime;

  [ lambda, v, it_num ] = power_method2 ( n, a, x, it_max, tol );

  ctime2 = cputime;
  ctime = ctime2 - ctime1;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of iterations = %d\n', it_num );
  fprintf ( 1, '  CPU time             = %f\n', ctime );
  fprintf ( 1, '  Estimated eigenvalue = %f  %f\n', ...
    real ( lambda ), imag ( lambda ) );

  lambda_vec = tris_eigenvalues ( n, alpha, beta, gamma );

  lambda_max = lambda_vec(1);

  for i = 2 : n
    if ( abs ( lambda_max ) < abs ( lambda_vec(i) ) )
      lambda_max = lambda_vec(i);
    end
  end

  fprintf ( 1, '  Correct value        = %f  %f\n', ...
    real ( lambda_max ), imag ( lambda_max ) );
  fprintf ( 1, '  Error                = %e\n', abs ( lambda - lambda_max ) );

  return
end
function a = fibonacci2 ( n )

%*****************************************************************************80
%
%% FIBONACCI2 returns the Fibonacci2 matrix.
%
%  Example:
%
%    N = 5
%
%    0 1 0 0 0
%    1 1 0 0 0
%    0 1 1 0 0
%    0 0 1 1 0
%    0 0 0 1 1
%
%  Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    A is tridiagonal.
%
%    Because A is tridiagonal, it has property A (bipartite).
%
%    A is banded, with bandwidth 3.
%
%    A is integral: int ( A ) = A.
%
%    A is a zero/one matrix.
%
%    If N = 1 then
%      det ( A ) = 0
%    else
%      det ( A ) = (-1)**(N-1)
%
%    If 1 < N, then A is unimodular.
%
%    For 2 <= N, A has the eigenvalues:
%
%      PHI   (once),
%      1     (N-2) times,
%      1-PHI (once).
%
%    When applied to a Fibonacci1 matrix B, the Fibonacci2 matrix
%    A produces the "next" Fibonacci1 matrix C = A*B.
%
%    Let PHI be the golden ratio (1+sqrt(5))/2.
%
%    For 2 <= N, the eigenvalues and eigenvectors are:
%
%    LAMBDA(1)     = PHI,     vector = (1,PHI,PHI^2,...PHI^(N-1));
%    LAMBDA(2:N-1) = 1        vector = (0,0,0,...,0,1);
%    LAMBDA(N)     = 1 - PHI. vector = ((-PHI)^(N-1),(-PHI)^(N-2),...,1)
%
%    Note that there is only one eigenvector corresponding to 1.
%    Hence, for 3 < N, the matrix is defective.  This fact means,
%    for instance, that the convergence of the eigenvector in the power
%    method will be very slow.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  for i = 1 : n
    for j = 1 : n

      if ( i == 1 )

        if ( j == 2 )
          a(i,j) = 1.0;
        else
          a(i,j) = 0.0;
        end

      else

        if ( j == i-1 | j == i )
          a(i,j) = 1.0;
        else
          a(i,j) = 0.0;
        end

      end

    end
  end

  return
end
function [ r, seed ] = r8vec_uniform_01 ( n, seed )

%*****************************************************************************80
%
%% R8VEC_UNIFORM_01 returns a unit pseudorandom R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Second Edition,
%    Springer, 1987,
%    ISBN: 0387964673,
%    LC: QA76.9.C65.B73.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, December 1986, pages 362-376.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley, 1998,
%    ISBN: 0471134031,
%    LC: T57.62.H37.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, Number 2, 1969, pages 136-143.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real R(N), the vector of pseudorandom values.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  i4_huge = 2147483647;

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8VEC_UNIFORM_01 - Fatal error!' );
  end

  for i = 1 : n

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + i4_huge;
    end

    r(i) = seed * 4.656612875E-10;

  end

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end
function a = tris ( m, n, x, y, z )

%*****************************************************************************80
%
%% TRIS returns the tridiagonal scalar matrix.
%
%  Formula:
%
%    if ( J = I-1 )
%      A(I,J) = X
%    elseif ( J = I )
%      A(I,J) = Y
%    elseif ( J = I + 1 )
%      A(I,J) = Z
%    else
%      A(I,J) = 0
%
%  Example:
%
%    M = 5, N = 5, X = 1, Y = 2, Z = 3
%
%    2 3 0 0 0
%    1 2 3 0 0
%    0 1 2 3 0
%    0 0 1 2 3
%    0 0 0 1 2
%
%  Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    A is tridiagonal.
%
%    Because A is tridiagonal, it has property A (bipartite).
%
%    A is banded, with bandwidth 3.
%
%    A is Toeplitz: constant along diagonals.
%
%    If Y is not zero, then for A to be singular, it must be the case that
%
%      0.5 * Y / sqrt ( X * Z ) < 1
%
%    and
%
%      cos (K*PI/(N+1)) = - 0.5 * Y / sqrt ( X * Z ) for some 1 <= K <= N.
%
%    If Y is zero, then A is singular when N is odd, or if X or Z is zero.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    A has eigenvalues
%
%      LAMBDA(I) = Y + 2 * sqrt(X*Z) * COS(I*PI/(N+1))
%
%    The eigenvalues will be complex if X * Z < 0.
%
%    If X = Z, the matrix is symmetric.
%
%    As long as X and Z are nonzero, the matrix is irreducible.
%
%    If X = Z = -1, and Y = 2, the matrix is a symmetric, positive
%    definite M matrix, the negative of the second difference matrix.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    John Todd,
%    Basic Numerical Mathematics,
%    Volume 2: Numerical Algebra,
%    Academic Press, 1978, page 155.
%
%  Parameters:
%
%    Input, integer M, N, the order of A.
%
%    Input, real X, Y, Z, the scalars that define A.
%
%    Output, real A(M,N), the matrix.
%
  for i = 1 : m
    for j = 1 : n

      if ( j == i - 1 )
        a(i,j) = x;
      elseif ( j == i )
        a(i,j) = y;
      elseif ( j == i + 1 )
        a(i,j) = z;
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end
function lambda = tris_eigenvalues ( n, x, y, z )

%*****************************************************************************80
%
%% TRIS_EIGENVALUES returns the eigenvalues of the tridiagonal scalar matrix.
%
%  Discussion:
%
%    The eigenvalues will be complex if X * Z < 0.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real X, Y, Z, the scalars that define A.
%
%    Output, complex LAMBDA(N), the eigenvalues.
%
  for i = 1 : n
    angle = i * pi / ( n + 1 );
    lambda(i) = y + 2.0 * sqrt ( x * z ) * cos ( angle );
  end

  return
end


function [x,y,z] = doseCOM(doseStruct)
%"doseCOM"
%   Find [x,y,z] center of mass of dose contained in doseStruct.  Non
%   uniform slice width is taken into consideration.
%
%   doseStruct is planC{indexS.dose}(doseNum) where doseNum is the number
%   of the desired dose center of mass.
%
%JRA 3/12/04
%
%Usage:
%   function [x,y,z] = doseCOM(doseStruct)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

[xVals, yVals, zVals] = getDoseXYZVals(doseStruct);

dA = getDoseArray(doseStruct);

[xMesh, yMesh] = meshgrid(xVals, yVals);

for i=1:size(dA, 3);
    slice = dA(:,:,i);
    %Weighted mesh.
    wX = xMesh .* slice;
    wY = yMesh .* slice;
    sliceDose(i) = sum(slice(:));
    if sliceDose(i) == 0
        x(i) = 0;
        y(i) = 0;
    else
        x(i) = sum(wX(:)) / sliceDose(i);
        y(i) = sum(wY(:)) / sliceDose(i);
    end
end

%Get voxel thickness at each slice.
zDiff = diff(zVals);
dividers = [zVals(1)-zDiff(1)/2 zDiff/2 + zVals(1:end-1) zVals(end)+zDiff(end)/2];
voxThickness = diff(dividers);
totalHeight = sum(voxThickness);

totalDose = sum(sliceDose);
%Weight x,y,z values by dose on that slice/voxelThickness.
x = sum(x     .* sliceDose / voxThickness) / totalDose * totalHeight;
y = sum(y     .* sliceDose / voxThickness) / totalDose * totalHeight;
z = sum(zVals .* sliceDose / voxThickness) / totalDose * totalHeight;


function peakIdxV = findFirstPeak(X)
% AI  10/17/16
% ========================================================================
% X  : Input data matrix (nVox x nTimePts)
% ========================================================================

nVox = size(X,1);

% Find pts of local maxima 't' where x(t)>x(t-1) & x(t)>x(t+1)
test1_M = [zeros(nVox,1) diff(X,1,2)];
test2_M = [-diff(X,1,2) zeros(nVox,1)];
test12_M = test1_M>=0 & test2_M>=0;
% Retain local maxima that are at least 80% of the max. signal intensity
test3_M = bsxfun(@gt,X,.8*max(X,[],2));
allPeaksM = test12_M & test3_M;
% First peak
[~,peakIdxV] = max(allPeaksM,[],2);   %Max returns the index corresponding to the
                                      %first occurrence of maximum (here 1)
                                      %If no peaks are found, returns first point)

end

github-repos/MATLAB/cerr-CERR/CERR-d320754abad9dcb78508ab69f33ae9f644202114/CERR_core/PlanAnalysis/DCE-MR analysis/TTHP analysis for DCE-MR/findFirstPeak.m


function y = limiter(a,b,limtype)
% Limiter function as defined in Sect. 4.8 and as used in Sect. 10.8
if  b==0,  y=0; else
  if limtype == 2          	% van Albada
    y = (a^2 + a*b)/(a^2 + b^2);
  elseif limtype == 1		% Minmod
    y = max(0, min(a/b,1));
  else				% No limiting, no MUSCL
    y = 0;
  end
end


function r=getRange(xCart,useHalfRange,zTx,zRx)
%%GETRANGE Obtain the bistatic (or monostatic) range measurements of
%          targets in the absence of refraction under non-relativistic
%          mechanics, ignoring atmospheric effects. The transmitter and the
%          target can be collocated.
%
%INPUTS: xCart The numDimXN set of N target positions. numDim is typically
%              2 or 3.
% useHalfRange A boolean value specifying whether the bistatic range value
%             should be divided by two. This normally comes up when
%             operating in monostatic mode, so that the range reported is
%             a one-way range. The default if this parameter is not
%             provided is false.
%         zTx A numDimXN matrix of the positions of the transmitters . If
%             this parameter is omitted or an empty matrix is passed, the
%             transmitters are assumed to be at the origin. If only a
%             single vector is passed, then the transmitter position is
%             assumed the same for all of the target states being converted.
%         zRx A numDimXN matrix of the positions of the receivers. If this
%             parameter is omitted or an empty matrix is passed, the
%             receivers are assumed to be at the origin. If only a single
%             vector is passed, then the receiver position is assumed the
%             same for all of the target states being converted.
%
%OUTPUTS: r The 1XN bistatic ranges of the targets.
%
%Bistatic range is discussed in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%February 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

N=size(xCart,2);
numDim=size(xCart,1);

if(nargin<4||isempty(zRx))
    zRx=zeros(numDim,N);
elseif(size(zRx,2)==1)
    zRx=repmat(zRx,[1,N]);
end

if(nargin<3||isempty(zTx))
    zTx=zeros(numDim,N);
elseif(size(zTx,2)==1)
    zTx=repmat(zTx,[1,N]);
end

if(nargin<2||isempty(useHalfRange))
    useHalfRange=false;
end

r=sqrt(sum((xCart-zTx(1:numDim,:)).^2,1))+sqrt(sum((xCart-zRx(1:numDim,:)).^2,1));

if(useHalfRange)
   r=r/2;
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% thermalLB.m: Rayleigh Benard Convection, using a LB method,
%   based on [Z.Guo, e.a., http://dx.doi.org/10.1002/fld.337].
%   Boussinesq approximation is used for the buoyancy term:
%     - Fluid is approximated with incompressible Navier-Stokes
%       equations including a body force term, and simulated
%       with a BGK model
%     - Temperature is approximated with advection-diffusion
%       equation and simulated with a BGK model
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Lattice Boltzmann sample, written in matlab
% Copyright (C) 2008 Andrea Parmigiani, Orestis Malaspinas, Jonas Latt
% Address: Rue General Dufour 24,  1211 Geneva 4, Switzerland
% E-mail: andrea.parmigiani@terre.unige.ch
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This program is free software; you can redistribute it and/or
% modify it under the terms of the GNU General Public License
% as published by the Free Software Foundation; either version 2
% of the License, or (at your option) any later version.
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
% You should have received a copy of the GNU General Public
% License along with this program; if not, write to the Free
% Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
% Boston, MA  02110-1301, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear

% GENERAL FLOW CONSTANTS

ly           = 51;
aspect_ratio = 2;
lx           = aspect_ratio*ly;
delta_x      = 1./(ly-2);
Pr           = 1.;
Ra           = 20000.; % Rayleigh number
gr           = 0.001;  % Gravity
buoyancy     = [0,gr];

Thot  = 1; % Heating on bottom wall
Tcold = 0; % Cooling on top wall
T0 = (Thot+Tcold)/2;

delta_t = sqrt(gr*delta_x);
% nu: kinematic viscosity in lattice units
nu      = sqrt(Pr/Ra)*delta_t/(delta_x*delta_x);
% k: thermal diffusivity
k       = sqrt(1./(Pr*Ra))*delta_t/(delta_x*delta_x);
omegaNS = 1./(3*nu+0.5); % Relaxation parameter for fluid
omegaT  = 1./(3.*k+0.5); % Relaxation parameter for temperature

maxT   = 80000;    % total number of iterations
tPlot  = 100;      % iterations between successive graphical outputs
tStatistics = 10;  % iterations between successive file accesses

% D2Q9 LATTICE CONSTANTS
tNS   = [4/9, 1/9,1/9,1/9,1/9, 1/36,1/36,1/36,1/36];
cxNS  = [  0,   1,  0, -1,  0,    1,  -1,  -1,   1];
cyNS  = [  0,   0,  1,  0, -1,    1,   1,  -1,  -1];
oppNS = [  1,   4,  5,  2,  3,    8,   9,   6,   7];

% D2Q5 LATTICE CONSTANTS
tT   = [1/3, 1/6, 1/6, 1/6, 1/6];
cxT  = [  0,   1,   0,  -1,   0];
cyT  = [  0,   0,   1,   0,  -1];
oppT = [  1,   4,   5,   2,   3];

[y,x] = meshgrid(1:ly,1:lx);

% INITIAL CONDITION FOR FLUID: (rho=1, u=0) ==> fIn(i) = t(i)
fIn = reshape( tNS' * ones(1,lx*ly), 9, lx, ly);

% INITIAL CONDITION FOR TEMPERATURE: (T=0) ==> TIn(i) = t(i)
tIn = reshape( tT' *Tcold *ones(1,lx*ly), 5, lx, ly);
% Except for bottom wall, where T=1
tIn(:,:,ly)=Thot*tT'*ones(1,lx);
% We need a small trigger, to break symmetry
tIn(:,lx/2,ly-1)= tT*(Thot + (Thot/10.));

% Open file for statistics
fid = fopen('thermal_statistics.dat','w');
fprintf(fid,'Thermal Statistics: time-step --- uy[nx/2,ny/2] --- Nu\n\n\n');

% MAIN LOOP (TIME CYCLES)
for cycle = 1:maxT
  % MACROSCOPIC VARIABLES
   rho = sum(fIn);
   T = sum(tIn); %temperature
   ux  = reshape ( (cxNS * reshape(fIn,9,lx*ly)), 1,lx,ly) ./rho;
   uy  = reshape ( (cyNS * reshape(fIn,9,lx*ly)), 1,lx,ly) ./rho;

   % MACROSCOPIC BOUNDARY CONDITIONS
   % NO-SLIP for fluid and CONSTANT at lower and upper
   % boundary...  periodicity wrt. left-right
   % COLLISION STEP FLUID
   for i=1:9
      cuNS         = 3*(cxNS(i)*ux+cyNS(i)*uy);
      fEq(i,:,:)   = rho .* tNS(i) .* ...
                       ( 1 + cuNS + 1/2*(cuNS.*cuNS) - 3/2*(ux.^2+uy.^2) );
      force(i,:,:) = 3.*tNS(i) .*rho .* (T-T0) .* ...
                       (cxNS(i)*buoyancy(1)+cyNS(i)*buoyancy(2))/(Thot-Tcold);
      fOut(i,:,:)  = fIn(i,:,:) - omegaNS .* (fIn(i,:,:)-fEq(i,:,:)) + force(i,:,:);
   end

    % COLLISION STEP TEMPERATURE
   for i=1:5
      cu          = 3*(cxT(i)*ux+cyT(i)*uy);
      tEq(i,:,:)  = T .* tT(i) .* ( 1 + cu );
      tOut(i,:,:) = tIn(i,:,:) - omegaT .* (tIn(i,:,:)-tEq(i,:,:));
   end

   % MICROSCOPIC BOUNDARY CONDITIONS FOR FLUID
   for i=1:9
        fOut(i,:,1)  = fIn(oppNS(i),:,1);
        fOut(i,:,ly) = fIn(oppNS(i),:,ly);
   end

   % STREAMING STEP FLUID
   for i=1:9
      fIn(i,:,:) = circshift(fOut(i,:,:), [0,cxNS(i),cyNS(i)]);
   end

   % STREAMING STEP FLUID
   for i=1:5
      tIn(i,:,:) = circshift(tOut(i,:,:), [0,cxT(i),cyT(i)]);
   end

   % MICROSCOPIC BOUNDARY CONDITIONS FOR TEMEPERATURE
   %
   tIn(5,:,ly) = Tcold-tIn(1,:,ly)-tIn(2,:,ly)-tIn(3,:,ly)-tIn(4,:,ly);
   tIn(3,:,1)  = Thot-tIn(1,:,1)  -tIn(2,:,1) -tIn(4,:,1) -tIn(5,:,1);

   % VISUALIZATION
   if (mod(cycle,tStatistics)==0)
       u     = reshape(sqrt(ux.^2+uy.^2),lx,ly);
       uy_Nu = reshape(uy,lx,ly); % vertical velocity
       T     = reshape(T,lx,ly);
       Nu    = 1. + sum(sum(uy_Nu.*T))/(lx*k*(Thot-Tcold));
       fprintf(fid,'%8.0f  %12.8f  %12.8f\n',cycle,u(int8(lx/2),int8(ly/2))^2, Nu);
       if(mod(cycle,tPlot)==0)
           subplot(2,1,1);
           imagesc(u(:,ly:-1:1)');
           title('Fluid velocity');
           axis off; drawnow
           subplot(2,1,2);
           imagesc(T(:,ly:-1:1)')
           title(['Temperature (Nusselt number is ' num2str(Nu) ')']);
           axis off; drawnow
       end
   end
end

fclose(fid);


function [Pfa_asv, Pmiss_asv, Pmiss_spoof_asv] = obtain_asv_error_rates(tar_asv, non_asv, spoof_asv, thresh_asv)

Ntar_asv    = length(tar_asv);
Nnon_asv    = length(non_asv);
Nspoof_asv  = length(spoof_asv);

% Obtain ASV false alarm and miss rates
Pfa_asv     = sum(non_asv >= thresh_asv)./Nnon_asv;
Pmiss_asv   = sum(tar_asv <  thresh_asv)./Ntar_asv;
if isempty(spoof_asv)
    Pmiss_spoof_asv = [];
else
    Pmiss_spoof_asv = sum(spoof_asv <  thresh_asv)./Nspoof_asv;
end


% Figures 6.8-6.10: Quadratic smoothing
% Section 6.3.3
% Boyd & Vandenberghe "Convex Optimization"
% Original by Lieven Vandenberghe
% Adapted for CVX Argyris Zymnis - 10/2005
%
% Suppose we have a signal x, which does not vary too rapidly
% and that x is corrupted by some small, rapidly varying noise v,
% ie. x_cor = x + v. Then if we want to reconstruct x from x_cor
% we should solve (with x_hat as the parameter)
%        minimize ||x_hat - x_cor||_2 + lambda*phi_quad(x_hat)
%
% where phi_quad(x) = sum(x_(i+1)-x_i)^2 , for i = 1 to n-1.
% The parameter lambda controls the ''smoothness'' of x_hat.
%
% The first figure which is generated shows the original and
% the corrupted signals. The second figure shows the tradeoff curve
% obtained when varying lambda and the third figure shows three
% reconstructed signals.
%
% NOTE: This is not a good problem to use CVX on. By exploiting
% the sparsity in this case, we can solve this problem much more
% efficiently using least squares.

randn('state',0);

n = 4000;  t = (0:n-1)';
exact = 0.5*sin((2*pi/n)*t).*sin(0.01*t);
corrupt = exact + 0.05*randn(size(exact));

figure(1)
subplot(211)
plot(t,exact,'-');
axis([0 n -0.6 0.6])
title('original signal');
ylabel('ya');

subplot(212)
plot(t,corrupt,'-');
axis([0 n -0.6 0.6])
xlabel('x');
ylabel('yb');
title('corrupted signal');
%print -deps smoothrec_signals.eps % figure 6.8, page 313

A = sparse(n-1,n);
A(:,1:n-1) = -speye(n-1,n-1);  A(:,2:n) = A(:,2:n)+speye(n-1,n-1);

% tradeoff curve, figure 6.9, page 313
nopts = 100;
lambdas = logspace(-10,10,nopts);

obj1 = [];  obj2 = [];

fprintf('computing 100 points on tradeoff curve ... \n');

for i=1:nopts

  lambda = lambdas(i);
  cvx_begin quiet
    variable x(n)
    minimize(norm(x-corrupt)+lambda*norm(x(2:n)-x(1:n-1)))
  cvx_end
  obj1 = [obj1, norm(full(A*x))];
  obj2 = [obj2, norm(full(x-corrupt))];

  fprintf('tradeoff point %d\n',i);
end;

figure(2)
plot(obj2,obj1,'-');  hold on;
plot(0,norm(A*corrupt),'o');
plot(norm(corrupt),0,'o');  hold off;
xlabel('x');
ylabel('y');
title('||xhat-xcorr||_2 vs. ||D xhat||_2');
%print -deps smoothrec_tradeoff.eps % figure 6.9, page 313

%three smooth signals, figure 6.10, page 314
nopts = 3;
alphas = [8 3 1];
xrecon = [];

for i=1:3
   fprintf(1,'Reconstructed Signals: %d of 3 \n',i)
   alpha = alphas(i);
   cvx_begin quiet
    variable x(n)
    minimize(norm(x(2:n)-x(1:n-1)))
    subject to
        norm(x-corrupt) <= alpha;
   cvx_end
   xrecon = [xrecon, x];

end

figure(3)
subplot(311), plot(xrecon(:,1));
axis([0 n -0.6 0.6])
ylabel('ya');
title('||xhat-xcorr||_2=8');
subplot(312), plot(xrecon(:,2));
axis([0 n -0.6 0.6])
ylabel('yb');
title('||xhat-xcorr||_2=3');
subplot(313), plot(xrecon(:,3));
axis([0 n -0.6 0.6])
xlabel('x');
ylabel('yc');
title('||xhat-xcorr||_2=1');
%print -deps smoothrec_results.eps % figure 6.10, page 314


function [e E] = xyzankurError(X1,X2,type)

% XYZANKURERROR Computes the error between two poses in xyz format
% FORMAT
% DESC Computes the error between two poses in xyz format
% data.
% ARG X1 : first set of poses
% ARG X2 : second set of poses
% RETURN E : mean error for the sequence
%
% SEEALSO : xyzankurDraw, xyzankurModify
%
% COPYRIGHT : Carl Henrik Ek, Andreas Damianou  and Neil Lawrence, 2011
%

% MOCAP

if(nargin<2)
    error('Too few arguments');
end
if(size(X1)~=size(X2))
    error('Dimensions mismatch');
end

E = zeros(size(X1,1),1);
for(i = 1:1:size(X1,1))
    E(i) = mean(sqrt(sum((xyzankur2joint(X1(i,:)) - xyzankur2joint(X2(i,:))).^2,2)));
end
e = mean(E);

return


% Fig. 9.8  Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
 n=[1 1];
 d = [1 0 0];
 rlocus(n,d)
 axis([-6 2 -3 3])
 hold on
 r=roots([1 1 1]);
 plot(r,'*')
  z=0:.1:.9;
 wn= 1:6;
 sgrid(z, wn)
 hold off


function x = line_adj_null_left ( m, n )

%*****************************************************************************80
%
%% LINE_ADJ_NULL_LEFT returns a left null vector of the LINE_ADJ matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the order of the matrix.
%
%    Output, real X(M), a null vector
%
  if ( mod ( m, 2 ) == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LINE_ADJ_NULL_LEFT - Fatal error!\n' );
    fprintf ( 1, '  For M even, there is no null vector.\n' );
    error ( 'LINE_ADJ_NULL_LEFT - Fatal error!' );
  end

  x = zeros ( m, 1 );

  x(1:4:m,1) =  1.0;
  x(3:4:m,1) = -1.0;

  return
end


function b = r8gd_to_r8ge ( n, ndiag, offset, a )

%*****************************************************************************80
%
%% R8GD_TO_R8GE copies a R8GD matrix to a R8GE matrix.
%
%  Discussion:
%
%    The R8GD storage format is suitable for matrices whose only nonzero entries
%    occur along a few diagonals, but for which these diagonals are not all
%    close enough to the main diagonal for band storage to be efficient.
%
%    In that case, we assign the main diagonal the offset value 0.
%    Each successive superdiagonal gets an offset value 1 higher, until
%    the highest superdiagonal (the A(1,N) entry) is assigned the offset N-1.
%    Similarly, the subdiagonals are assigned offsets of -1 through -(N-1).
%
%    Now, assuming that only a few of these diagonals contain nonzeros,
%    then for the I-th diagonal to be saved, we stored its offset in
%    OFFSET(I), and its entries in column I of the matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer NDIAG, the number of diagonals of the matrix
%    that are stored in the array.
%    NDIAG must be at least 1, and no more than 2 * N - 1.
%
%    Input, integer OFFSET(NDIAG), the offsets for the diagonal storage.
%
%    Input, real A(N,NDIAG), the R8GD matrix.
%
%    Output, real B(N,N), the R8GE matrix.
%
  b(1:n,1:n) = 0.0E+00;

  for i = 1 : n
    for diag = 1 : ndiag
      j = i + offset(diag);
      if ( 1 <= j & j <= n )
        b(i,j) = a(i,diag);
      end
    end
  end

  return
end


function triangulation_test23 ( )

%*****************************************************************************80
%
%% TEST23 tests TRIANGULATION_ORDER6_BOUNDARY_EDGE_COUNT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 August 2006
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST23\n' );
  fprintf ( 1, '  TRIANGULATION_ORDER6_BOUNDARY_EDGE_COUNT counts the\n' );
  fprintf ( 1, '    boundary edges in an order 6 triangulation.\n' );

  [ node_num, triangle_num, hole_num ] = ...
    triangulation_order6_example1_size ( );

  [ node_xy, triangle_node, triangle_neighbor ] = ...
    triangulation_order6_example1 ( node_num, triangle_num );

  boundary_edge_num = triangulation_order6_boundary_edge_count ( ...
    triangle_num, triangle_node );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of boundary edges = %d\n', boundary_edge_num );
  fprintf ( 1, '  Correct number =           %d\n', 16 );

  return
end


function [h] = fscatter3(X,Y,Z,C,cmap);
% [h] = fscatter3(X,Y,Z,C,cmap);
% Plots point cloud data in cmap color classes and 3 Dimensions,
% much faster and very little memory usage compared to scatter3 !
% X,Y,Z,C are vectors of the same length
% X,Y,Z,C might be put in as structure points.x,points.y,points.z,points.int
% with C being used as index into colormap (can be any values though)
% cmap is optional colourmap to be used
% h are handles to the line objects

% Felix Morsdorf, Jan 2003 (last update Oct. 2010), Remote Sensing Laboratory Zuerich

  if nargin == 1
    if isfield(X,'int')
      C = X.int;
      Z = X.z;
    else
      C = X.z;
      Z = C;
    end
    Y = X.y;
    NX = X.x;
    clear X;
    X = NX;clear NX;
    numclass = 256; % Number of color classes
    cmap = myspecmap(256);
    siz = 5;
  elseif nargin == 4
    numclass = 256; % Number of color classes
    cmap = hsv(256);
    siz = 5;
  elseif nargin == 5
    numclass = max(size(cmap));
    siz = 5;
    if numclass == 1
      siz = cmap;
      cmap = hsv(256);
      numclass = 256;
    end
  elseif nargin == 6
    numclass = max(size(cmap));
    if numclass == 1
      siz = cmap;
      cmap = hsv(256);
      numclass = 256;
    end
  end

% avoid too many calculations

mins = min(C);
maxs = max(C);
minz = min(Z);
maxz = max(Z);
minx = min(X);
maxx = max(X);
miny = min(Y);
maxy = max(Y);

% construct colormap :

col = cmap;

% determine index into colormap

ii = floor( (C - mins ) * (numclass-1) / (maxs - mins) );
ii = ii + 1;

colormap(cmap);

hold on
k = 0;o = k;
for j = 1:numclass
  jj = (ii(:)== j);
  if ~isempty(jj)
    k = k + 1;
    h = plot3(X(jj),Y(jj),Z(jj),'.','color',col(j,:),'markersize',siz);
    if ~isempty(h)
      o = o+1;
        hp(o) = h;
    end
  end
end
caxis([min(C) max(C)])
axis equal;rotate3d on;view(3);
box on
hcb = colorbar('location','east');


function x = gompertz_cdf_inv ( cdf, a, b )

%*****************************************************************************80
%
%% GOMPERTZ_CDF_INV inverts the Gompertz CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Johnson, Kotz, and Balakrishnan,
%    Continuous Univariate Distributions, Volume 2, second edition,
%    Wiley, 1994, pages 25-26.
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%
%    Input, real A, B, the parameters of the PDF.
%    1 < A, 0 < B.
%
%    Output, real X, the corresponding argument.
%
  if ( cdf < 0.0 )
    x = 0.0;
  elseif ( cdf < 1.0 )
    x = log ( 1.0 - log ( 1.0 - cdf ) * log ( a ) / b  ) / log ( a );
  else
    x = r8_huge ( );
  end

  return
end


function lambda = invol_eigenvalues ( n )

%*****************************************************************************80
%
%% INVOL_LAMBDA returns the eigenvalues of the INVOL matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  k = floor ( n / 2 );

  lambda(1:k,1) =   +1.0;
  lambda(k+1:n,1) = -1.0;

  return
end


function [X, y, XTest, yTest] = mappingLoadData(dataset, seedVal)

% MAPPINGLOADDATA Load a regression or classification dataset.

if nargin < 2
  seedVal = 1e5;
end
randn('seed', seedVal)
rand('seed', seedVal)
XTest = [];
yTest = [];
switch dataset
 %/~
 case 'pumadynSeeger'
  data = load('Dataset.data');
  ind = randperm(size(data, 1));
  indTr = ind(1:7168);
  indTe = ind(7169:end);
  X = data(indTr, 1:end-1);
  y = data(indTr, end);
  XTest = data(indTe, 1:end-1);
  yTest = data(indTe, end);
  Xscale = sqrt(var(X));
  for j = 1:length(Xscale);
    X(:, j) = X(:, j)/Xscale(j);
    XTest(:, j) = XTest(:, j)/Xscale(j);
  end
  augX = [X ones(size(X, 1), 1)];
  w = inv(augX'*augX)*augX'*y;
  y = y -augX*w;
  augTestX = [XTest ones(size(XTest, 1), 1)];
  yTest = yTest - augTestX*w;
  yscale = sqrt(var(y));
  y = y/yscale;
  yTest = yTest/yscale;
 case 'pumadyn'

  % Data is variance 1, no need to normalise.
  data = load('Dataset.data');
  ind = randperm(size(data, 1));
  indTr = ind(1:7168);
  indTe = ind(7169:end);
  X = data(indTr, 1:end-1);
  y = data(indTr, end);
  XTest = data(indTe, 1:end-1);
  yTest = data(indTe, end);
  %~/
 case 'usps'
  load usps_train
  X = ALL_DATA;
  range =  min(ALL_T):max(ALL_T);
  for i = 1:length(range)
    y(:, i) = (ALL_T == range(i))*2 - 1;
  end
  if nargout > 2
    load usps_test
    XTest = ALL_DATA;
    range =  min(ALL_T):max(ALL_T);
    for i = 1:length(range)
      yTest(:, i) = (ALL_T == range(i))*2 - 1;
    end
  end

 case {'usps0', 'usps1', 'usps2', 'usps3', 'usps4', 'usps5', 'usps6', 'usps7', 'usps8', 'usps9'}
  digitNo = str2num(dataset(end));
  load usps_train
  X = ALL_DATA;
  range =  min(ALL_T):max(ALL_T);
  for i = 1:length(range)
    y(:, i) = (ALL_T == range(i))*2 - 1;
  end
  if nargout > 2
    load usps_test
    XTest = ALL_DATA;
    range =  min(ALL_T):max(ALL_T);
    for i = 1:length(range)
      yTest(:, i) = (ALL_T == range(i))*2 - 1;
    end
  end
  y = y(:, digitNo+1);
  yTest = yTest(:, digitNo+1);

 case 'regressionOne'
  try
    load regressionOneData.mat
  catch

    [void, errid] = lasterr;
    if strcmp(errid, 'MATLAB:load:couldNotReadFile')
      numIn = 2;
      N = 500;
      X = zeros(N, numIn);
      X(1:floor(N/2), :) = ...
          randn(floor(N/2), numIn)*.5+1;
      X(floor(N/2)+1:end, :) = ...
          randn(ceil(N/2), numIn)*.5-1;
      kern = kernCreate(X, 'rbfard');
      kern.variance = 1;
      kern.inverseWidth = 20;
      kern.inputScales = [0 0.999];

      K = kernCompute(kern, X);
      y = real(gaussSamp(K, 1)') + randn(N, 1)*0.01;

      save('regressionOneData.mat', 'numIn', 'N', 'X', 'y')
    else
      error(lasterr);
    end

  end

 case 'regressionTwo'
  try
    load regressionTwoData.mat
  catch

    [void, errid] = lasterr;
    if strcmp(errid, 'MATLAB:load:couldNotReadFile')
      numIn = 2;
      N = 500;
      X = zeros(N, numIn);
      X(1:floor(N/2), :) = ...
          randn(floor(N/2), numIn)*.5+1;
      X(floor(N/2)+1:end, :) = ...
          randn(ceil(N/2), numIn)*.5-1;
      kern = kernCreate(X, 'rbfard');
      kern.variance = 1;
      kern.inverseWidth = 20;
      kern.inputScales = [0.999 .2];

      K = kernCompute(kern, X);
      y = real(gaussSamp(K, 1)') + randn(N, 1)*0.01;

      save('regressionTwoData.mat', 'numIn', 'N', 'X', 'y')
    else
      error(lasterr)
    end

  end

 case 'regressionThree'
  try
    load regressionThreeData.mat
  catch

    [void, errid] = lasterr;
    if strcmp(errid, 'MATLAB:load:couldNotReadFile')
      numIn = 2;
      N = 500;
      X = zeros(N, numIn);
      X(1:floor(N/2), :) = ...
          randn(floor(N/2), numIn)*.5+1;
      X(floor(N/2)+1:end, :) = ...
          randn(ceil(N/2), numIn)*.5-1;
      kern = kernCreate(X, 'lin');
      kern.variance = 1;

      K = kernCompute(kern, X);
      y = real(gaussSamp(K, 1)') + randn(N, 1)*0.01;
      save('regressionThreeData.mat', 'numIn', 'N', 'X', 'y')
    else
      error(lasterr);
    end
  end

 case 'regressionFour'
  try
    load regressionFourData.mat
  catch

    [void, errid] = lasterr;
    if strcmp(errid, 'MATLAB:load:couldNotReadFile')
      numIn = 2;
      N = 500;
      X = zeros(N, numIn);
      X(1:floor(N/2), :) = ...
          randn(floor(N/2), numIn)*.5+1;
      X(floor(N/2)+1:end, :) = ...
          randn(ceil(N/2), numIn)*.5-1;
      kern = kernCreate(X, 'mlp');
      kern.variance = 1;
      kern.weightVariance = 1;
      kern.biasVariance = 1;
      K = kernCompute(kern, X);
      y = real(gaussSamp(K, 1)') + randn(N, 1)*0.01;
      save('regressionFourData.mat', 'numIn', 'N', 'X', 'y')
    else
      error(lasterr);
    end
  end
 case 'classificationOne'
  try
    load classificationOneData.mat
  catch
    [void, errid] = lasterr;
    if strcmp(errid, 'MATLAB:load:couldNotReadFile')
      X = [randn(100,2)-[zeros(100, 1) 6*ones(100, 1)]; randn(100,2)+[zeros(100, 1) 6*ones(100, 1)]; randn(100, 2)];
      y = [ones(200, 1); -ones(100, 1)];
      save('classificationOneData.mat', 'X', 'y')
    else
      error(lasterr);
    end
  end
 case 'classificationTwo'

  try
    load classificationTwoData.mat
  catch
    [void, errid] = lasterr;
    if strcmp(errid, 'MATLAB:load:couldNotReadFile')
      numIn = 2;
      N = 500;

      X = zeros(N, numIn);
      X = rand(N, numIn);

      kern = kernCreate(X, 'rbf');
      kern.variance = 10;
      kern.inverseWidth = 10;

      K = kernCompute(kern, X);
      u = real(gaussSamp(K, 1)');

      p = cumGaussian(u);
      y = 2*(rand(size(u))>p)-1;
      save('classificationTwoData.mat', 'numIn', 'N', 'X', 'u', 'y')
    else
      error(lasterr);
    end

  end

 case 'classificationThree'

  try
    load classificationThreeData.mat
  catch
    [void, errid] = lasterr;
    if strcmp(errid, 'MATLAB:load:couldNotReadFile')
      numIn = 2;
      N = 500;

      X = zeros(N, numIn);
      X = rand(N, numIn);

      kern = kernCreate(X, 'rbf');
      kern.variance = 10;
      kern.inverseWidth = 10;

      K = kernCompute(kern, X);
      u = real(gaussSamp(K, 1)');
      a = 3;
      pMinus = cumGaussian(u-a/2);
      pPlus = cumGaussian(-u-a/2);
      p =rand(size(u));
      indMinus = find(p<pMinus);
      indPlus = find(p>pMinus & p<pMinus+pPlus);
      indNone = find(p>pMinus+pPlus);
      y = zeros(N, 1);
      y(indPlus) = 1;
      y(indMinus) = -1;
      y(indNone, :) = [];
      X(indNone, :) = [];
      save('classificationThreeData.mat', 'numIn', 'N', 'X', 'u', 'y')
    else
      error(lasterr);
    end

  end

 case 'orderedOne'
  dataPerCat = 30;
  spacing = 3;

  % Generate a toy data-set of (linear) ordered categories.
  X = [randn(dataPerCat,2)-[zeros(dataPerCat, 1) repmat(3*spacing, dataPerCat, 1)]; ...
       randn(dataPerCat,2)-[zeros(dataPerCat, 1) repmat(2*spacing, dataPerCat, 1)]; ...
       randn(dataPerCat,2)-[zeros(dataPerCat, 1) repmat(spacing, dataPerCat, 1)]; ...
       randn(dataPerCat, 2); ...
       randn(dataPerCat,2)+[zeros(dataPerCat, 1) repmat(spacing, dataPerCat, 1)]; ...
       randn(dataPerCat,2)+[zeros(dataPerCat, 1) repmat(2*spacing, dataPerCat, 1)]; ...
       randn(dataPerCat,2)+[zeros(dataPerCat, 1) repmat(3*spacing, dataPerCat, 1)]];
  y = [zeros(dataPerCat, 1); ...
       repmat(1, dataPerCat, 1); repmat(2, dataPerCat, 1); ...
       repmat(3, dataPerCat, 1); repmat(4, dataPerCat, 1); ...
       repmat(5, dataPerCat, 1); repmat(6, dataPerCat, 1)];

 case 'orderedTwo'
  dataPerCat = 30;
  spacing = 3;

  % Generate a toy data-set of (linear) ordered categories.
  thetaR = [randn(dataPerCat,2)-[zeros(dataPerCat, 1) repmat(3*spacing, dataPerCat, 1)]; ...
            randn(dataPerCat,2)-[zeros(dataPerCat, 1) repmat(2*spacing, dataPerCat, 1)]; ...
            randn(dataPerCat,2)-[zeros(dataPerCat, 1) repmat(spacing, dataPerCat, 1)]; ...
            randn(dataPerCat, 2); ...
            randn(dataPerCat,2)+[zeros(dataPerCat, 1) repmat(spacing, dataPerCat, 1)]; ...
            randn(dataPerCat,2)+[zeros(dataPerCat, 1) repmat(2*spacing, dataPerCat, 1)]; ...
            randn(dataPerCat,2)+[zeros(dataPerCat, 1) repmat(3*spacing, dataPerCat, 1)]];
  thetaR(:, 1) = thetaR(:, 1);
  thetaR(:, 2) = thetaR(:, 2) + 15;
  X = [sin(thetaR(:, 1)).*thetaR(:, 2) cos(thetaR(:, 1)).*thetaR(:, 2)];
  y = [zeros(dataPerCat, 1); ...
       repmat(1, dataPerCat, 1); repmat(2, dataPerCat, 1); ...
       repmat(3, dataPerCat, 1); repmat(4, dataPerCat, 1); ...
       repmat(5, dataPerCat, 1); repmat(6, dataPerCat, 1)];

end


%% DEMO 17: Detector Rotation
%
%
%
%  Some systems have a slight rotation of the detector due to mechanicall
%  inacuracies.
%
%  According to the article "A geometric calibration method for cone beam
%  CT systems" (DOI: 10.1118/1.2198187), only Roll needs to be corrected
%  for in the algorithmic part, as the other 2 can be easily ignored if
%  "sufficiently small". In TIGRE we decided to leave that to the users
%  discretion and implemented the 3 possible detector rotation, per angle
%  if needed.
%
%  This demo shows how to use it.
%
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------
%% Initialize

clear;
close all;
%% Define Geometry
%
% VARIABLE                                   DESCRIPTION                    UNITS
%-------------------------------------------------------------------------------------
geo.DSD = 1536;                             % Distance Source Detector      (mm)
geo.DSO = 1000;                             % Distance Source Origin        (mm)
% Detector parameters
geo.nDetector=[512; 512];					% number of pixels              (px)
geo.dDetector=[0.8; 0.8]; 					% size of each pixel            (mm)
geo.sDetector=geo.nDetector.*geo.dDetector; % total size of the detector    (mm)
% Image parameters
geo.nVoxel=[128;128;128];                   % number of voxels              (vx)

% a bit smaller than usual because the demo includes a very big detector
% angle for showcase
geo.sVoxel=[256;256;256]/2;                 % total size of the image       (mm)

geo.dVoxel=geo.sVoxel./geo.nVoxel;          % size of each voxel            (mm)
% Offsets
geo.offOrigin =[0;0;0];                     % Offset of image from origin   (mm)
geo.offDetector=[0; 0];                     % Offset of Detector            (mm)

% Auxiliary
geo.accuracy=0.5;                           % Accuracy of FWD proj          (vx/sample)

%% Geometry has an additional field for the rotation.
%
%
angles=linspace(0,2*pi,100);

% lets define 3 angles, with big variability in each direction

roll=angles;
pitch=0.7*linspace(0,1,size(angles,2));
yaw=0.7*linspace(0,1,size(angles,2));

% Fill rotDetector. It can also be a 3x1, if its not angle dependent. e.g.
% geo.rotDetector=[pi/5; 0; pi/10];

geo.rotDetector=[roll;pitch;yaw];

%% Load data and generate projections
% define angles
% Load thorax phatom data
head=headPhantom(geo.nVoxel);
% generate projections
projections=Ax(head,geo,angles,'interpolated');

%% Lets plot the projections with a rotated detector
plotProj(projections,angles);

%% lets reconstruct with and without detector rotation
%
imgRotDet=OS_SART(projections,geo,angles,50);

% No rotation
geo.rotDetector=[0;0;0];
projections2=Ax(head,geo,angles,'interpolated');

imgnoRot=OS_SART(projections2,geo,angles,50);

% %% Plot to show that indeed the reconstruction is right
%
%
%
plotImg([imgRotDet imgnoRot] ,'dim',3)


function estimation_results = se3EKF_SLAM(data, NumberOfSteps)
% R-EKF SLAM

% load pre-given data: odometry and observations
if nargin < 1
    load('./data.mat');
end

data_matrix = data.state;

odom_sigma = data.odom_sigma;
obsv_sigma = data.obsv_sigma;

% odoCov = data.odom_cov;   % constant variable
% obsCov = data.obse_cov;   % constant variable

%%%%%%%%%%%%%%%%%%%% Estimation_X is used to save the state in each step %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%% In every step, all elements of Estimation_X will be changed %%%%%%%%%%%%
Estimation_X.orientation = data.poses.orientation(1:3,1:3);
Estimation_X.position    = data.poses.position(:,1);
Estimation_X.cov         = sparse(6,6);
Estimation_X.landmarks   = [];       % the landmarks observed until this step (included), 4*N format, the 4-th row is the index
Estimation_X0.IndexObservedNew=[];
Estimation_X0.IndexObservedAlreadyThis=[];
%%%%%%%%%%%%%%%%%%%% Estimation_X is used to save the state in each step %%%%%%%%%%%%%%%%%%%%

% Initialize
if nargin < 2
    NumberOfSteps = max(data_matrix(:,4));  % step instead of pose,  hence, it does not include pose 0
elseif NumberOfSteps > max(data_matrix(:,4))
    NumberOfSteps = max(data_matrix(:,4));
end
estimation_results = cell(1, NumberOfSteps+1);
estimation_results{1} = Estimation_X;
row_idx = (data_matrix(:, end) <= NumberOfSteps+1);
data_matrix = data_matrix(row_idx, :);

for i = 0:NumberOfSteps
    IndexOfCurrentStepInDataMatrix = find(data_matrix(:,4) == i);
    m = size(IndexOfCurrentStepInDataMatrix, 1);
    if ( mod(i, 50) == 0 )
        disp(['Processing pose ', int2str(i)]);
    end
    % det(Estimation_X.cov)
    if i==NumberOfSteps-1
    a=1;
    end

    if i ~= NumberOfSteps
        OdometryFromThis2Next = data_matrix(IndexOfCurrentStepInDataMatrix(m-5):IndexOfCurrentStepInDataMatrix(m),1);
        if m > 6
            CameraMeasurementThis = [ data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(m-6) , 1 ),...
                                      data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(m-6) , 3 )];
           [Estimation_X] = se3EKF_update(Estimation_X, CameraMeasurementThis, obsv_sigma );
        end

        estimation_results{i+1} = Estimation_X;

%        propagation using odometry info
        [Estimation_X] = se3EKF_propagate(Estimation_X, OdometryFromThis2Next, odom_sigma );

    else
        a=2;
        if m > 6
            CameraMeasurementThis = [ data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(end) , 1 ) , ...
                                      data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(end) , 3 )];
            [Estimation_X] = se3EKF_update(Estimation_X, CameraMeasurementThis, obsv_sigma );
        end
        estimation_results{i+1} = Estimation_X;
    end
end
clearvars -except estimation_results


function [ point_coord, edge_point, face_order, face_point ] = ...
  icos_shape ( point_num, edge_num, face_num, face_order_max )

%*****************************************************************************80
%
%% ICOS_SHAPE describes an icosahedron.
%
%  Discussion:
%
%    The input data required for this routine can be retrieved from
%    ICOS_SIZE.
%
%    The vertices lie on the unit sphere.
%
%    The dual of an icosahedron is a dodecahedron.
%
%    The data has been rearranged from a previous assignment.
%    The STRIPACK program refuses to triangulate data if the first
%    three nodes are "collinear" on the sphere.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer POINT_NUM, the number of points (12).
%
%    Input, integer EDGE_NUM, the number of edges (30).
%
%    Input, integer FACE_NUM, the number of faces (20).
%
%    Input, integer FACE_ORDER_MAX, the maximum number of vertices per face (3).
%
%    Output, real POINT_COORD(3,POINT_NUM); the points.
%
%    Output, integer EDGE_POINT(2,EDGE_NUM), the points that make up each
%    edge, listed in ascending order of their indexes.
%
%    Output, integer FACE_ORDER(FACE_NUM), the number of vertices per face.
%
%    Output, integer FACE_POINT(FACE_ORDER_MAX,FACE_NUM); FACE_POINT(I,J)
%    is the index of the I-th point in the J-th face.  The points are listed
%    in the counter-clockwise direction defined by the outward normal at the
%    face.  The nodes of each face are ordered so that the lowest index
%    occurs first.  The faces are then sorted by nodes.
%
  dim_num = 3;
%
%  Set point coordinates.
%
  phi = 0.5 * ( sqrt ( 5.0 ) + 1.0 );
  b = 1.0 / sqrt ( 1.0 + phi * phi );
  a = phi * b;
  z = 0.0;
%
%  Set the points.
%
  point_coord(1:dim_num,1:point_num) = [ ...
      a,  b,  z; ...
      a, -b,  z; ...
      b,  z,  a; ...
      b,  z, -a; ...
      z,  a,  b; ...
      z,  a, -b; ...
      z, -a,  b; ...
      z, -a, -b; ...
     -b,  z,  a; ...
     -b,  z, -a; ...
     -a,  b,  z; ...
     -a, -b,  z ]';
%
%  Set the edges.
%
  edge_point(1:2,1:edge_num) = [ ...
     1,  2; ...
     1,  3; ...
     1,  4; ...
     1,  5; ...
     1,  6; ...
     2,  3; ...
     2,  4; ...
     2,  7; ...
     2,  8; ...
     3,  5; ...
     3,  7; ...
     3,  9; ...
     4,  6; ...
     4,  8; ...
     4, 10; ...
     5,  6; ...
     5,  9; ...
     5, 11; ...
     6, 10; ...
     6, 11; ...
     7,  8; ...
     7,  9; ...
     7, 12; ...
     8, 10; ...
     8, 12; ...
     9, 11; ...
     9, 12; ...
    10, 11; ...
    10, 12; ...
    11, 12 ]';
%
%  Set the face orders.
%
  face_order(1:face_num) = [ ...
    3, 3, 3, 3, 3, 3, 3, 3, 3, 3, ...
    3, 3, 3, 3, 3, 3, 3, 3, 3, 3 ]';
%
%  Set the faces.
%
  face_point(1:face_order_max,1:face_num) = [ ...
     1,  2,  4; ...
     1,  3,  2; ...
     1,  4,  6; ...
     1,  5,  3; ...
     1,  6,  5; ...
     2,  3,  7; ...
     2,  7,  8; ...
     2,  8,  4; ...
     3,  5,  9; ...
     3,  9,  7; ...
     4,  8, 10; ...
     4, 10,  6; ...
     5,  6, 11; ...
     5, 11,  9; ...
     6, 10, 11; ...
     7,  9, 12; ...
     7, 12,  8; ...
     8, 12, 10; ...
     9, 11, 12; ...
    10, 12, 11 ]';

  return
end


function [y,n] = audspacebw(fmin,fmax,varargin)
%AUDSPACEBW  Auditory scale points specified by bandwidth
%   Usage: y=audspacebw(fmin,fmax,bw,hitme);
%          y=audspacebw(fmin,fmax,bw);
%          y=audspacebw(fmin,fmax);
%          [y,n]=audspacebw(...);
%
%   `audspacebw(fmin,fmax,bw,scale)` computes a vector containing values
%   equistantly scaled between frequencies *fmin* and *fmax* on the
%   selected auditory scale.  All frequencies are specified in Hz.The
%   distance between two consecutive values is *bw* on the selected scale,
%   and the points will be centered on the scale between *fmin* and *fmax*.
%
%   See the help on |freqtoaud| to get a list of the supported values of the
%   *scale* parameter.
%
%   `audspacebw(fmin,fmax,bw,hitme,scale)` will do as above, but one of
%   the points is quaranteed to be the frequency *hitme*.
%
%   `[y,n]=audspacebw(...)` additionally returns the number of points *n* in
%   the output vector *y*.
%
%   See also: freqtoaud, audspace, audfiltbw

%   AUTHOR : Peter L. Søndergaard

% ------ Checking of input parameters ---------

if nargin<2
  error('%s: Too few input parameters.',upper(mfilename));
end;

if ~isnumeric(fmin) || ~isscalar(fmin) || fmin<0
  error('%s: fmin must be a non-negative scalar.',upper(mfilename));
end;

if ~isnumeric(fmax) || ~isscalar(fmax) || fmax<0
  error('%s: fmax must be a non-negative scalar.',upper(mfilename));
end;

if fmin>fmax
  error('%s: fmin must be less than or equal to fmax.',upper(mfilename));
end;

definput.import={'freqtoaud'};
definput.keyvals.hitme=[];
definput.keyvals.bw=1;

[flags,kv,bw]=ltfatarghelper({'bw','hitme'},definput,varargin);

if ~isnumeric(bw) || ~isscalar(bw) || bw<=0
  error('%s: bw must be a positive scalar.',upper(mfilename));
end;

%% ------ Computation --------------------------

if isempty(kv.hitme)
  % Convert the frequency limits to auds.
  audlimits = freqtoaud([fmin,fmax],flags.audscale);
  audrange  = audlimits(2)-audlimits(1);

  % Calculate number of points, excluding final point
  n         = floor(audrange/bw);

  % The remainder is calculated in order to center the points
  % correctly between fmin and fmax.
  remainder = audrange-n*bw;

  audpoints = audlimits(1)+(0:n)*bw+remainder/2;

  % Add the final point
  n=n+1;

else

  % Convert the frequency limits to auds.
  audlimits    = freqtoaud([fmin,fmax,kv.hitme],flags.audscale);
  audrangelow  = audlimits(3)-audlimits(1);
  audrangehigh = audlimits(2)-audlimits(3);

  % Calculate number of points, exluding final point.
  nlow = floor(audrangelow/bw);
  nhigh = floor(audrangehigh/bw);

  audpoints=(-nlow:nhigh)*bw+audlimits(3);
  n=nlow+nhigh+1;
end;

y = audtofreq(audpoints,flags.audscale);


function [ r, seed ] = r8vec_uniform_01 ( n, seed )

%*****************************************************************************80
%
%% R8VEC_UNIFORM_01 returns a unit pseudorandom R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real R(N,1), the vector of pseudorandom values.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  i4_huge = 2147483647;

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8VEC_UNIFORM_01 - Fatal error!' );
  end

  r = zeros ( n, 1 );

  for i = 1 : n

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + i4_huge;
    end

    r(i,1) = seed * 4.656612875E-10;

  end

  return
end


function B = cleanpoly(A,tol,deg)
% function B = cleanpoly(A,tol,deg)
%
% DESCRIPTION
%   Cleans up the input polynomial.  The output polynomial includes only
%   terms whose coefficients have magnitude greater than or equal to TOL
%   and whose monomial degree is specified by DEG.
%
% INPUTS
%   A: polynomial
%   tol: scalar double specifying the coefficient tolerance
%   deg: vector of non-negative integers specifying the degrees of
%        mononmials to retain. Alternatively deg can be an N-by-2
%        cell array with deg{i,1} specifying a variable and
%        deg{i,2} specifying a vector of non-negative integers.
%        This will retain only monomials whose degree in variable
%        deg{i,1} is specified in deg{i,2}.
%
% OUTPUTS
%   B: polynomial which only contains the terms of A whose coefficients
%      have magnitude greater than or equal to tol and whose monomial
%      degree is listed in deg.
%
% SYNTAX
%   B=cleanpoly(A,tol);
%   B=cleanpoly(A,[],deg);
%   B=cleanpoly(A,tol,deg);
%
% EXAMPLE
%   pvar x1 x2 u;
%   p = 9*u^3 + u*x1^2 + 1e-6*u^2*x1*x2 + 1e-5*u*x2^2 + 2*x1^3 ...
%        - x1*x2 + 3*u + x1 + 2*x2;
%
%   % Remove terms whose coefficients has magnitude < tol
%   tol = 1e-4;
%   p1 = cleanpoly(p,tol)
%
%   % Retain linear and quadratic terms
%   p2 = cleanpoly(p,[],1:2)
%
%   % Retain terms linear in u but of degree 0,1,2,3 in x1 and x2
%   p3 = cleanpoly(p,[],{x1, 0:3; x2, 0:3; u 1})

% 1/28/2008: PJS  Added option to grab terms of a specified degree
% 4/22/2009: PJS  Added functionality to specify deg as a cell array

if nargin<3
    deg = [];
end

if ~isempty(tol)
    if isa(tol,'double') && isscalar(tol) && tol>=0
        Acoef = get(A,'Coefficient');
        idx = find(abs(Acoef) < tol);
        if ~isempty(idx)
            Acoef(idx) = 0;
            chkval = 0; % skip validity check
            A=polynomial(Acoef,A.degmat,A.varname,size(A),chkval);
        end
    else
        error('tol must be a non-negative double');
    end
end

if ~isempty(deg)

    if isa(deg,'double') && ndims(deg)==2 && ...
            all(floor(deg)==ceil(deg)) && all(deg>=0)

        % deg is a vector of non-negative integers

        Adeg = sum(A.degmat,2);
        idx = [];
        for i1=1:length(deg)
            %idx=find(Adeg~=deg(i1));
            idx=[idx; find(Adeg==deg(i1))];
        end
        idx = setdiff(1:length(Adeg),idx);

        Acoef = A.coefficient;
        Acoef(idx,:) = 0;
        chkval = 0; % skip validity check
        A=polynomial(Acoef,A.degmat,A.varname,size(A),chkval);

    elseif iscell(deg) && ndims(deg)==2 && size(deg,2)==2
        % deg is a cell array of pvars and vectors of non-neg integers

        Acoef = A.coefficient;
        for i1=1:size(deg,1)
            var = deg{i1,1};
            if ispvar(var) && all(size(var)==[1 1])
                var = char(var);
            elseif ~ischar(var)
                error('First column of cell array deg must contain strings or pvars');
            end
            vidx = find(strcmp(A.varname,var));

            if ~isempty(vidx)
                degi = deg{i1,2};
                if ~( isa(degi,'double') && ndims(degi)==2 && ...
                        all(floor(degi)==ceil(degi)) && all(degi>=0) )
                    error(['Second column of cell array deg must '...
                        'contain a vector of non-negative integers.']);
                end

                Adeg = A.degmat(:,vidx);
                cidx = [];
                for i2=1:length(degi)
                    cidx=[cidx; find(Adeg==degi(i2))];
                end
                cidx = setdiff(1:length(Adeg),cidx);
                Acoef(cidx,:) = 0;
            end
        end
        A=polynomial(Acoef,A.degmat,A.varname,size(A));
    else
        error('deg must be a vector of non-negative integers or an Nx2 cell array');
    end

end

% Combine to remove terms
B = combine(A);


%% Plotting Individual Orientations
% Basics of the plot types for individual orientations data
%
%%
% This section gives an overview over the possibilities that MTEX offers to
% visualize orientation data. Let us first load a sample EBSD data set

mtexdata forsterite

%%
% and select all individual orientations of the Iron phase

ebsd('Fo').orientations

%% Scatter Pole Figure Plot
% A pole figure showing scattered points of these data figure can be
% produced by the command <orientation.plotPDF.html plotPDF>.

plotPDF(ebsd('Fo').orientations,Miller(1,0,0,ebsd('Fo').CS))

%% Scatter (Inverse) Pole Figure Plot
% Accordingly, scatter points in inverse pole figures are produced by the
% command  <orientation.plotIPDF.html plotIPDF>.

plotIPDF(ebsd('Fo').orientations,xvector)

%% Scatter Plot in ODF Sections
% The plotting of scatter points in sections of the orientation space is carried out by the
% command <orientation.plotSection.html plotSection>. In the above examples, the number
% of plotted orientations was chosen automatically such that the
% plots not to become too crowded with points. The number of randomly chosen orientations
% can be specified by the option *points*.

plotSection(ebsd('Fo').orientations,'points',1000,'sigma','sections',9)

%% Scatter Plot in Axis Angle or Rodrigues Space
% Another possibility is to plot the single orientations directly into the
% orientation space, i.e., either in axis/angle parameterization or in Rodrigues
% parameterization.

scatter(ebsd('Fo').orientations)

%%
% Here, the optional option 'center' specifies the center of the unique
% region in the orientation space.

%% Orientation plots for EBSD and grains
% Since EBSD and grain data involves single orientations, the above plotting
% commands are also applicable for those objects.

%%
% Let us consider some grains <EBSD.calcGrains.html reconstructed> from the
% EBSD data

grains = calcGrains(ebsd);

%%
% Then the scatter plot of the individual orientations of the Iron phase in
% the inverse pole figure is achieved by

plotIPDF(ebsd('Fo').orientations,xvector,'points',1000, 'MarkerSize',3);

%%
% In the same way, the mean orientations of grains can be visualized

hold all
plotIPDF(grains('Fo').meanOrientation,xvector,'points',500, 'MarkerSize',3);
hold off

%%
% One can also use different colors on the scatter points

h = [Miller(1,0,0,ebsd('Fo').CS),Miller(1,1,0,ebsd('Fo').CS)];
plotPDF(ebsd('Fo').orientations,ebsd('Fo').mad,h,'antipodal','MarkerSize',4)

%%
% or some arbitrary data vector

plotSection(grains('Fo').meanOrientation,log(grains('Fo').area),...
  'sigma','sections',9,'MarkerSize',10);

%%
% See also <PlotTypes_demo.html#5, Scatter plots> for more information
% about scatter plot and <SphericalProjection_demo.html,spherical
% projections>  for more information on spherical projections.


%% Gabor Filter demo
%
% A GUI to interact with the 5 different Gabor filter parameters, while
% visualizing the resulting filter.
%

function varargout = gabor_filter_gui(ksize)
    % create the UI
    if nargin < 1, ksize = [121 121]; end
    h = buildGUI(ksize);
    if nargout > 0, varargout{1} = h; end
end

function onChange(~,~,h)
    %ONCHANGE  Event handler for UI controls

    % retrieve current values from UI controls
    sigma  = get(h.slid(5), 'Value') / 10;
    theta  = get(h.slid(4), 'Value') * pi/180;
    lambda = get(h.slid(3), 'Value');
    gamma  = get(h.slid(2), 'Value') / 100;
    psi    = get(h.slid(1), 'Value') * pi/180;

    % create Gabor filter
    kernel = cv.getGaborKernel('KSize',[h.ksize(2) h.ksize(1)], ...
        'Sigma',sigma, 'Theta',theta, 'Lambda',lambda, ...
        'Gamma',gamma, 'Psi',psi);

    % normalize filter to [0,1] range and resize it
    kernel = cv.normalize(kernel, 'NormType','MinMax');
    kernel = cv.resize(kernel, [h.sz(2) h.sz(1)]);

    % show result
    set(h.img, 'CData',kernel)
    set(h.txt(5), 'String',sprintf('Sigma  = %.2f',sigma))
    set(h.txt(4), 'String',sprintf('Theta  = %.2f',theta))
    set(h.txt(3), 'String',sprintf('Lambda = %.2f',lambda))
    set(h.txt(2), 'String',sprintf('Gamma  = %.2f',gamma))
    set(h.txt(1), 'String',sprintf('Psi    = %.2f',psi))
    drawnow
end

function onType(~,e,h)
    %ONTYPE  Event handler for key press on figure

    % handle keys
    switch e.Key
        case {'q', 'escape'}
            close(h.fig)
            return
        case 'h'
            onHelp([],[]);
        case 'r'
            onReset([],[],h);
    end
end

function onReset(~,~,h)
    set(h.slid(5), 'Value',400);    % sigma
    set(h.slid(4), 'Value',0);      % theta
    set(h.slid(3), 'Value',11);     % lambda
    set(h.slid(2), 'Value',100);    % gamma
    set(h.slid(1), 'Value',90);     % psi
    onChange([],[],h);
end

function onHelp(~,~)
    %ONHELP  Display usage help dialog

    helpdlg({
        'This GUI allows to interact with the 5 different Gabor filter'
        'parameters, while visualizing the resulting filter.'
        ''
        'Hot keys:'
        'ESC, q - quit the program'
        'r - reset parameters to original values'
        'h - this help dialog'
    });
end

function h = buildGUI(ksize)
    %BUILDGUI  Creates the UI

    % parameters
    sigma = 400; sigma_max = 1000;
    theta = 0;   theta_max = 180;
    lambda = 11; lambda_max = 100;
    gamma = 100; gamma_max = 200;
    psi = 90;    psi_max = 180;
    sz = [512 512];

    % build the user interface (no resizing to keep it simple)
    h = struct();
    h.ksize = ksize;  % size of the filter
    h.sz = sz;        % size of the image to show
    h.fig = figure('Name','Gabor Filter Demo', ...
        'NumberTitle','off', 'Menubar','none', 'Resize','off', ...
        'Position',[200 200 sz(2) sz(1)+129]);
    if ~mexopencv.isOctave()
        %HACK: not implemented in Octave
        movegui(h.fig, 'center');
    end
    h.ax = axes('Parent',h.fig, 'Units','pixels', 'Position',[1 130 sz(2) sz(1)]);
    if ~mexopencv.isOctave()
        h.img = imshow(zeros(sz), 'Parent',h.ax);
    else
        %HACK: https://savannah.gnu.org/bugs/index.php?45473
        axes(h.ax);
        h.img = imshow(zeros(sz));
    end
    text(5, 5, sprintf('KSize = %dx%d', ksize(2), ksize(1)), ...
        'Color','y', 'VerticalAlignment','top');

    props = {'Parent',h.fig, 'Style','text', 'String','', ...
        'FontSize',11, 'HorizontalAlignment','left'};
    h.txt(5) = uicontrol(props{:}, 'Position',[5   5 120 20]);
    h.txt(4) = uicontrol(props{:}, 'Position',[5  30 120 20]);
    h.txt(3) = uicontrol(props{:}, 'Position',[5  55 120 20]);
    h.txt(2) = uicontrol(props{:}, 'Position',[5  80 120 20]);
    h.txt(1) = uicontrol(props{:}, 'Position',[5 105 120 20]);

    props = {'Parent',h.fig, 'Style','slider', 'Min',0};
    h.slid(5) = uicontrol(props{:}, 'Position',[125   5 sz(2)-125-5 20], ...
        'Value',sigma, 'Max',sigma_max, 'SliderStep',[10 100]./(sigma_max-0));
    h.slid(4) = uicontrol(props{:}, 'Position',[125  30 sz(2)-125-5 20], ...
        'Value',theta, 'Max',theta_max, 'SliderStep',[2 20]./(theta_max-0));
    h.slid(3) = uicontrol(props{:}, 'Position',[125  55 sz(2)-125-5 20], ...
        'Value',lambda, 'Max',lambda_max, 'SliderStep',[1 10]./(lambda_max-0));
    h.slid(2) = uicontrol(props{:}, 'Position',[125  80 sz(2)-125-5 20], ...
        'Value',gamma, 'Max',gamma_max, 'SliderStep',[2 20]./(gamma_max-0));
    h.slid(1) = uicontrol(props{:}, 'Position',[125 105 sz(2)-125-5 20], ...
        'Value',psi, 'Max',psi_max, 'SliderStep',[2 20]./(psi_max-0));

    % hook event handlers, and trigger default start
    opts = {'Interruptible','off', 'BusyAction','cancel'};
    set(h.slid, 'Callback',{@onChange,h}, opts{:});
    set(h.fig, 'WindowKeyPressFcn',{@onType,h}, opts{:});
    onChange([],[],h);
end


function box = intersectBoxes3d(box1, box2)
%INTERSECTBOXES3D Intersection of two 3D bounding boxes
%
%   RES = intersectBoxes3d(BOX1, BOX2)
%
%   Example
%   box1 = [5 20 5 30 10 50];
%   box2 = [0 15 0 15 0 20];
%   intersectBoxes3d(box1, box2)
%   ans =
%       5 15 5 15 10 20
%
%   See also
%   boxes3d, drawBox3d, mergeBoxes3d
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-07-26,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% unify sizes of data
if size(box1,1) == 1
    box1 = repmat(box1, size(box2,1), 1);
elseif size(box2, 1) == 1
    box2 = repmat(box2, size(box1,1), 1);
elseif size(box1,1) ~= size(box2,1)
    error('Bad size for inputs');
end

% compute extreme coords
mini = min(box1(:,2:2:end), box2(:,2:2:end));
maxi = max(box1(:,1:2:end), box2(:,1:2:end));

% concatenate result into a new box structure
box = [maxi(:,1) mini(:,1) maxi(:,2) mini(:,2) maxi(:,3) mini(:,3)];


function n = normalDirection(L)
% normal direction

[~,~,n] = svd(matrix(L));

n = vector3d(n(:,1));


%
% LibQPEP: A Library for Globally Optimal Solving Quadratic Pose Estimation Problems (QPEPs),
%          It also gives highly accurate uncertainty description of the solutions.
%
%
% Article:
%      Wu, J., Zheng, Y., Gao, Z., Jiang, Y., Hu, X., Zhu, Y., Jiao, J., Liu, M. (2020)
%           Quadratic Pose Estimation Problems: Unified Solutions,
%           Solvability/Observability Analysis and Uncertainty Description
%           in A Globally Optimal Framework.
%
%
% Authors:      Jin Wu and Ming Liu
% Affiliation:  Hong Kong University of Science and Technology (HKUST)
% Emails:       jin_wu_uestc@hotmail.com; eelium@ust.hk
% Websites:     https://zarathustr.github.io
%               https://ram-lab.com

function [XX, ff] = optimize_quat_cov2(q, F, cov_left, solver, verbose)
if(~strcmp(solver, 'scs'))
    X = sdpvar(4, 4);
    f = q.' * X * q;
    cons = [
        X >= 0, ...
        sym2vec3(F * X * F.') == sym2vec3(cov_left)
        ];
    options = sdpsettings('solver', solver, 'verbose', verbose);
    if(strcmp(solver, 'sdpa_gmp'))
        options.sdpa_gmp.epsilonDash = 1.0e-35;
        options.sdpa_gmp.precision = 250;
    end
    optimize(cons, f, options);
    XX = value(X);
    ff = value(f);
else
    cvx_solver scs
    cvx_precision high

    cvx_begin sdp
         variable X(4,4) symmetric semidefinite
         dual variable Q
         minimize(q.' * X * q)
         X >= 0 * eye(4) : Q
         subject to
             sym2vec3(F * X * F.') == sym2vec3(cov_left)
    cvx_end
    XX = real(X);
    ff = 0;
end
end