%% OPTI Toolbox Basic Functionality Demo
%
% This file illustrates the basic functionality of the OPTI Toolbox and
% provides a collection of simple examples to get new users familiar with
% the OPTI methods.
%
% Topics covered include:
%
%   - Checking available solvers
%   - Finding a solver for a particular problem type
%   - Building an optiprob problem structure
%   - Building an optiset options structure
%   - Using the opti constructor
%   - Solving an opti object
%   - Validating the solution
%   - Plotting the solution
%
%   Copyright (C) 2011 Jonathan Currie (I2C2)

%% Checking solvers available with your OPTI distribution
% Not all solvers are supplied with the OPTI Toolbox, so to check which are
% available you can use the following function:

checkSolver

%% Matching solvers with a problem type
% Use the 'matrix' argument to print a grid of solver vs problem solved:

checkSolver('matrix')

%% Finding all solvers for a given problem type
% Specify the problem type in order to print a list of all solvers setup to
% solve that problem type. The solver at the top of the list is the default
% solver for that type. The following line prints all solvers which are
% setup to solve a Linear Program (LP) together with constraint and
% sparsity settings (see help checkSolver):

checkSolver('LP')

%% Creating an Optimization Problem
% To create an optimization problem using OPTI there are two methods
% available:
%
%   1) Pass the problem parameters directly to the opti constructor.
%
%   2) Pass the problem parameters to optiprob to generate a problem
%   structure, then pass the structure to the opti constructor.
%
%   Typically we will use (1), however (2) is provided for backwards
%   compatibility, as well as to break up larger problem descriptions.

%% Problem Fields
% Type opti or optiprob with no input arguments to view all available fields
% and groupings. Check the help documentation to see alternative naming and
% grouping strategies.

opti;

%% Options Fields
% Type optiset with no input arguments to view all available fields. Check
% the help documentation for more information.

optiset

%% Problem 1
% This is a simple two decision variable Linear Program (LP) which we will
% use for the next couple of examples.

%Problem
f = -[6 5]';                %Objective Function (min f'x)
A = [1,4; 6,4; 2, -5];      %Linear Inequality Constraints (Ax <= b)
b = [16;28;6];
lb = [0;0];                 %Bounds on x (lb <= x <= ub)
ub = [10;10];

%% Example 1 - Basic Setup
% Problems can be built using the opti constructor. Options from optiset are
% always optional. If the user does not provide any options, OPTI will
% choose default ones for the problem being solved.

Opt = opti('f',f,'ineq',A,b,'bounds',lb,ub)

%% Example 1 - Solving the Problem
% Call solve to solve the problem

[x,fval,exitflag,info] = solve(Opt)

%% Example 2 - Alternative Setup Strategies
% Naming of arguments, as well as pairing is flexible when using optiprob

Opt = opti('c',f,'ineq',A,b,'bounds',lb,ub);    %c = f

% OR
Opt = opti('grad',f,'ineq',A,b,'bounds',lb,ub); %grad = f

% OR
Opt = opti('f',f,'A',A,'b',b,'bounds',lb,ub);   %individual A,b

% OR
Opt = opti('f',f,'ineq',A,b,'lb',lb,'ub',ub);   %individual lb,ub

%% Example 3 - Choosing the Solver
% You can use optiset to select a solver for your problem. This is supplied
% to the opti constructor using optiset, together with the existing
% problem description.

opts = optiset('solver','clp');
Opt = opti(Opt,opts)
x = solve(Opt)

%% Example 3b - Choosing the Solver via 'options'
% You can also specify options when creating the initial OPTI object:

opts = optiset('solver','ooqp');
Opt = opti('f',f,'ineq',A,b,'bounds',lb,ub,'options',opts)
x = solve(Opt)

%% Example 4 - Validating the Solution
% The solution is stored inside the OPTI object, so you can validate it
% against the constraints.

[ok,msg] = checkSol(Opt)

%% Example 5 - Plotting the Solution
% Several problem types have a default plot command available IF the
% problem contains two variables.

plot(Opt)

%% Example 6 - Calling a Solver Directly
% Most OPTI solvers use standard MATLAB function prototypes (such as
% linprog or quadprog), so you can skip using the OPTI class all together:

rl = -Inf(size(b)); %note row format for solver CLP
ru = b;

[x,fval,exitflag,info] = opti_clp([],f,A,rl,ru,lb,ub)

%% Example 7 - MATLAB Optimization Toolbox Overloads
% As described in Overload_demo.m, most Optimization Toolbox functions are
% overloaded ('opti_' is placed in front) for new users.

[x,fval,exitflag,info] = opti_linprog(f,A,b,[],[],lb,ub)


function [y, yvar, gene, times, scale, rawExp, rawVar] = gpsimLoadBarencoTestData

% GPSIMLOADBARENCOTESTDATA Load in Martino Barenco's test data as processed by mmgMOS.
% FORMAT
% DESC loads in from the two Excel spread sheets
% (resultsMartino_exprs.xls and resultsMartino_se.xls) the data
% from the Barenco et al paper as processed by mmgMOS.
% RETURN y : the normalised expression levels.
% RETURN yvar : the variance of the normalised expression levels.
% RETURN gene : the gene names and Affymetrix array tags.
% RETURN times : the times of the expression measurements.
% RETURN scale : the scaling factor applied to normalise.
% RETURN rawExp : the raw gene expresion level.
% RETURN rawVar : the raw variance of the gene expression.
%
% SEEALSO : demBarencoRank
%
% COPYRIGHT : Neil D. Lawrence, 2007

% SHEFFIELDML

if exist('./data/barencoDataTest.mat') == 2
  load('./data/barencoDataTest.mat');
else

  % These excel files include results processed directly from the
  % cel files using the mmgMOS algorithm (Xuejun's code).

  % These are the expression levels.
  [numeric1, txt1] = xlsread('./data/resultsMartino_exprs.xls');
  headTxt1 = txt1(1, 2:end);
  tagTxt1 = txt1(2:end, 1);

  % These are the standard deviations.
  [numeric2, txt2] = xlsread('./data/resultsMartino_se.xls');
  headTxt2 = txt2(1, 2:end);
  tagTxt2 = txt2(2:end, 1);

  if(any(~strcmp(tagTxt2(:), tagTxt1(:))))
    error('Two files are not in same order');
  end
  if(any(~strcmp(headTxt2(:), headTxt1(:))))
    error('Two files are not in same order');
  end

  clear gene, clear ind
  % Gene IDs
  % DDB2
  gene{1, 1} = '203409_at';
  gene{1, 2} = 'DDB2';
  % BIK
  gene{2, 1} = '205780_at';
  gene{2, 2} = 'BIK';
  % TNFRSF10b (other tags include 209294_x_at and 210405_x_at)
  gene{3, 1} = '209295_at';
  gene{3, 2} = 'TNFRSF10b';
  % p21 --- we think this is CIp1/p21
  gene{4, 1} = '202284_s_at';
  gene{4, 2} = 'CIp1/p21';
  % p26 --- named as sesn1 in the platform.
  gene{5, 1} = '218346_s_at';
  gene{5, 2} = 'p26 sesn1';

  for i = 1:length(gene)
    match = find([strcmp(gene{i, 1}, tagTxt1(:))]);
    if length(match)~=1
      error('Too many or too few matches.');
    else
      ind(i) = match(1);
    end
  end
  order = [1 4 5 6 7 2 3 8 11 12 13 14 9 10 15 18 19 20 21 16 17];

  % Perform some normalisation.
  % Make sure that the average for each slide in log space is the
  % same.
  mVal = zeros(size(mean(numeric1)));
  mVal = mVal - mean(mVal);

  rawExp = numeric1(ind, order)';

  for i = 1:size(rawExp, 2)
    rawExp(:, i) = rawExp(:, i) - mVal';
  end

  rawVar = numeric2(ind, order)';
  rawVar = rawVar.*rawVar; % convert standard deviations to variances.

  yFull = exp(rawExp + rawVar/2);  % Logs are normally distributed
                                   % ... recover mean in exp space.
  yFullVar = (exp(rawVar)-1).*exp(2*rawExp + rawVar); % Logs are
                                                      % normally
                                                      % distributed
                                                      % ... recover
                                                      % variance in exp
                                                      % space.

  % Rescale so that average standard deviation of curves is 1.
  scale = mean(sqrt(var(yFull)));
  yFull = yFull/scale;
  yFullVar = yFullVar/(scale*scale);
  y{1} = yFull(1:7, :);
  y{2} = yFull(8:14, :);
  y{3} = yFull(15:21, :);
  yvar{1} = yFullVar(1:7, :);
  yvar{2} = yFullVar(8:14, :);
  yvar{3} = yFullVar(15:21, :);
  times = [0 2 4 6 8 10 12]';
  save('./data/barencoDataTest.mat', 'y', 'yvar', 'gene', 'times', 'scale', 'rawVar', 'rawExp');
end


% dc_remove remove the DC offset of the time domain signal

function [z] = dc_remove(x,a);

Num = [a -a];
Dem = [1 -a];
z = filter(Num,Dem,x);


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_funcImPlay(Temp,Event,handles)
handles = guidata(handles.MU_matrix_display);

try
    TMatrix=handles.TMatrix;
    TMatrix=TMatrix-min(TMatrix(:));
    TMatrix=uint8((double(TMatrix)/double(max(TMatrix(:))))*255);
    implay(TMatrix);
    colormap(handles.V.Color_map);
catch me
    error_msg{1,1}='ERROR!!! implay is not working in this Matlab version.';
    error_msg{2,1}=me.message;
    errordlg(error_msg);
end

end


function C = xor(A,B)
%XOR Logical XOR for sptensors.
%
%   See also SPTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Observations for sparse matrix case.
% The result of xor(a,5) is dense!
% The result of xor(a,0) is dense!
% The result of xor(a,full(a)) is dense!
% The result of xor(a,b) is sparse.

%% Case 1: One argument is a scalar
if isscalar(B) || isa(B,'tensor')
    C = xor(full(A),B);
    return;
end
if isscalar(A)
    C = xor(A,full(B));
    return;
end

%% Case 2: Both x and y are tensors of some sort
if ~isequal(size(A),size(B))
    error('Must be tensors of the same size');
end

if isa(A,'sptensor') && isa(B,'sptensor')
    C = sptensor([A.subs; B.subs], 1, size(A), @(x) length(x) == 1);
    return;
end

if isa(B,'tensor')
    Bsubs = find(B ~= 0);
    C = sptensor([A.subs; Bsubs], 1, size(A), @(x) length(x) == 1);
    return;
end

%% Otherwise
error('The arguments must be two sptensors or an sptensor and a scalar.');


classdef SegmentationLossLogistic < dagnn.Loss
    methods
        function outputs = forward(obj, inputs, params)
            sz = size(inputs{2});
            mass = sz(1) * sz(2) + 1;

            outputs{1} = vl_nnloss(inputs{1}, inputs{2}, [], ...
                'loss', obj.loss, ...
                'instanceWeights', 1./mass) ;
            n = obj.numAveraged ;
            m = n + size(inputs{1},4) ;
            obj.average = (n * obj.average + double(gather(outputs{1}))) / m ;
            obj.numAveraged = m ;
        end

        function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
            sz = size(inputs{2});
            mass = sz(1) * sz(2) + 1;
            derInputs{1} = vl_nnloss(inputs{1}, inputs{2}, derOutputs{1}, ...
                'loss', obj.loss, ...
                'instanceWeights', 1./mass) ;
            derInputs{2} = [] ;
            derParams = {} ;
        end

        function obj = SegmentationLossLogistic(varargin)
            obj.load(varargin) ;
        end
    end
end


% Profiler extension for Kernel Normalized Least-Mean-Square algorithm with
% Coherence Criterion
%
% This file is part of the Kernel Adaptive Filtering Toolbox for Matlab.
% https://github.com/steven2358/kafbox/

classdef knlms_profiler < knlms

    properties (GetAccess = 'public', SetAccess = 'private')
        elapsed = 0; % elapsed time
        prev_dict_size = 0; % previous dictionary size for growth check
    end

    methods

        function kaf = knlms_profiler(parameters) % constructor
            if nargin<1, parameters = struct(); end
            kaf = kaf@knlms(parameters);
        end

        function flops = lastflops(kaf) % flops for last iteration
            m = size(kaf.dict,1);
            if kaf.prev_dict_size < m % growing
                m1 = m - 1;
                n2 = 1;
            else
                m1 = m;
                n2 = 0;
            end
            m3 = m;
            floptions = struct(...
                'sum', 3*m3, ...
                'mult', 3*m3 + 1, ...
                sprintf('%s_kernel',kaf.kerneltype), [m1+n2, 1, size(kaf.dict,2)]);

            flops = kflops(floptions);
        end

        %% flops breakdown

        % k = kernel(x,kaf.dict,kaf.kerneltype,kaf.kernelpar);
        % kernel: m1

        % h = kernel(x,kaf.dict,kaf.kerneltype,kaf.kernelpar);
        % kernel: n2 % 1 element when growing, 0 when not

        % kaf.alpha = kaf.alpha + kaf.eta / (kaf.eps + h*h') * (y - h*kaf.alpha) * h';
        % sum: 3*m3
        % mult: 3*m3 + 1
        % div: 1

        %%

        function train_profiled(kaf,x,y)
            kaf.prev_dict_size = size(kaf.dict,1);
            t1 = tic;
            kaf.train(x,y);
            t2 = toc(t1);
            kaf.elapsed = kaf.elapsed + t2;
        end

        function bytes = lastbytes(kaf) % bytes used in last iteration
            m = size(kaf.dict,1);
            bytes = 8*(m + m*size(kaf.dict,2)); % 8 bytes for double precision
            % alpha, dict
        end

    end
end


%DEMO_HIERPRIOR  Demonstration of hierarchical prior structures for
%                  Gaussian processes
%
%  Description
%    This demonstration shows how to set hierarchical prior structures for
%    parameters of covariance function and/or likelihood function. This
%    demo is intended for showing how to set hierarchical prior structures
%    and for other testing purposes, not as a demonstration of model
%    selection procedures or comparisons. We test the hierarchical priors
%    for both regression and classification with Laplace and EP.
%
%
%  See also DEMO_REGRESSION_ROBUST, DEMO_SPATIAL1, DEMO_*
%
% Copyright (c) 2014 Ville Tolvanen

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

%% Regression
% load the data. First 100 variables are for training
% and last 100 for test

S = which('demo_hierprior');
L = strrep(S,'demo_hierprior.m','demodata/odata.txt');
x = load(L);
y = x(1:100,2);
x = x(1:100,1);
[n, nin] = size(x);

% Test data
xt = [-2.7:0.01:2.7]';
yt = 0.3+0.4*xt+0.5*sin(2.7*xt)+1.1./(1+xt.^2);

% Create hierarchial prior
pl=prior_t('mu_prior',prior_t);

% Create the model
gpcf = gpcf_sexp('magnSigma2_prior', pl, 'lengthScale_prior', pl);
gpcf2 = gpcf_linear('coeffSigma2_prior', pl);
% gpcf2 = gpcf_neuralnetwork('weightSigma2_prior', pl, 'biasSigma2_prior', pl);
lik = lik_gaussian('sigma2_prior', pl);
gp = gp_set('lik', lik, 'cf', {gpcf gpcf2}, 'jitterSigma2',1e-9);

% Parameters
[w,s]=gp_pak(gp)

% Optimize the parameters
opt=optimset('TolX',1e-5,'TolFun',1e-5,'Display','iter');
gp = gp_optim(gp,x,y,'opt',opt);

derivs={};
derivs{1}=derivativecheck(gp_pak(gp), @(ww) gp_eg(ww, gp, x, y));

[Ef,Varf,lpyt,Ey,Vary]=gp_pred(gp,x,y,xt,'yt',yt);

%% Regression with sparse approximations
% Here we optimize all the model parameters, including the inducing inputs

X_u=linspace(-2,3, 10)';
gp = gp_set('lik', lik, 'cf', {gpcf gpcf2}, 'jitterSigma2',1e-6, 'type', 'FIC', ...
  'X_u', X_u, 'infer_params', 'covariance+likelihood+inducing', ...
  'Xu_prior', prior_gaussian('mu_prior', prior_t()));
gp = gp_optim(gp,x,y,'opt',opt);
derivs{2}(:,1)=derivativecheck(gp_pak(gp), @(ww) gp_eg(ww, gp, x, y));
Ef2=gp_pred(gp,x,y,xt,'yt',yt);

gp = gp_set('lik', lik, 'cf', {gpcf gpcf2}, 'jitterSigma2',1e-6, 'type', 'VAR', ...
  'X_u', X_u, 'infer_params', 'covariance+likelihood+inducing', ...
  'Xu_prior', prior_gaussian('mu_prior', prior_t()));
gp = gp_optim(gp,x,y,'opt',opt);
derivs{2}(:,2)=derivativecheck(gp_pak(gp), @(ww) gp_eg(ww, gp, x, y));
Ef3=gp_pred(gp,x,y,xt,'yt',yt);

gp = gp_set('lik', lik, 'cf', {gpcf gpcf2}, 'jitterSigma2',1e-6, 'type', 'SOR', ...
  'X_u', X_u, 'infer_params', 'covariance+likelihood+inducing', ...
  'Xu_prior', prior_gaussian('mu_prior', prior_t()));
gp = gp_optim(gp,x,y,'opt',opt);
derivs{2}(:,3)=derivativecheck(gp_pak(gp), @(ww) gp_eg(ww, gp, x, y));
Ef4=gp_pred(gp,x,y,xt,'yt',yt);

%% Plot

plot(x,y,'.', xt, Ey, xt, Ef2, xt, Ef3, xt, Ef4);
legend('Data', 'Full-GP', 'FIC', 'VAR', 'SOR');

%% Classification with Laplace & EP (full and sparse approximation)
S = which('demo_spatial1');
data = load(strrep(S,'demo_spatial1.m','demodata/spatial1.txt'));

x = data(:,1:2);
ye = data(:,3);
y = data(:,4);
% Remove some of the data
inds=find(x(:,2)>30);
x=x(inds,:);
y=y(inds,:);
ye=ye(inds,:);

dims = [30    60     1    35];
[trindex, Xu] = set_PIC(x, dims, 7, 'corners', 0);
[n,nin] = size(x);

% Create the covariance functions
pl = prior_t('s2',10, 's2_prior', prior_t());
pm = prior_sqrtunif();
pxu={};
for i1=1:size(Xu,1)
  pxu={pxu{:} pl};
end
gpcf1 = gpcf_matern32('lengthScale', 1, 'magnSigma2', 0.03, ...
  'lengthScale_prior', pl, 'magnSigma2_prior', prior_fixed());
gpcf2 = gpcf_ppcs3('nin',nin,'lengthScale', 5, 'magnSigma2', 0.05,...
  'lengthScale_prior', pl, 'magnSigma2_prior', prior_fixed());

lik = lik_negbin();

% Create the GP structures (Laplace)
gp = gp_set('lik', lik, 'cf', gpcf1,'jitterSigma2', 1e-4, ...
            'infer_params', 'covariance+likelihood');
gp2 = gp_set('type', 'FIC', 'lik', lik, 'cf', gpcf1, 'X_u', Xu, 'Xu_prior', pxu, ...
            'jitterSigma2', 1e-4, 'infer_params', 'covariance+likelihood+inducing');
gp3 = gp_set('type', 'PIC', 'lik', lik, 'cf', gpcf1, 'X_u', Xu, 'Xu_prior', pxu,  ...
           'jitterSigma2', 1e-4, 'infer_params', 'covariance+likelihood+inducing',...
           'tr_index', trindex);
gp4 = gp_set('type', 'CS+FIC', 'lik', lik, 'cf', {gpcf1 gpcf2}, 'X_u', Xu, 'Xu_prior', pxu,  ...
           'jitterSigma2', 1e-4, 'infer_params', 'covariance+likelihood+inducing');
gp4.cf{1}.p.lengthScale=prior_fixed();
gp4.cf{1}.p.magnSigma2=prior_fixed();

opt=optimset('TolX',1e-3,'TolFun',1e-3,'Display','iter');
fprintf('Laplace \n \n');
fprintf('Full GP:\n \n')
derivs{3}(:,1)=derivativecheck(gp_pak(gp), @(ww) gp_eg(ww, gp, x, y, 'z', ye));
fprintf('FIC:\n \n')
derivs{4}(:,1)=derivativecheck(gp_pak(gp2), @(ww) gp_eg(ww, gp2, x, y, 'z', ye));
fprintf('PIC:\n \n')
derivs{4}(:,2)=derivativecheck(gp_pak(gp3), @(ww) gp_eg(ww, gp3, x, y, 'z', ye));
fprintf('CS+FIC:\n \n')
derivs{4}(:,3)=derivativecheck(gp_pak(gp4), @(ww) gp_eg(ww, gp4, x, y, 'z', ye));

% Create the GP structures (EP)
gp = gp_set('lik', lik, 'cf', gpcf1,'jitterSigma2', 1e-4, ...
            'infer_params', 'covariance+likelihood', ...
            'latent_method', 'EP');
gp2 = gp_set('type', 'FIC', 'lik', lik, 'cf', gpcf1, 'X_u', Xu,  'Xu_prior', pxu, ...
            'jitterSigma2', 1e-4, 'infer_params', 'covariance+likelihood+inducing', ...
            'latent_method', 'EP');
gp3 = gp_set('type', 'PIC', 'lik', lik, 'cf', gpcf1, 'X_u', Xu, 'Xu_prior', pxu,  ...
           'jitterSigma2', 1e-4, 'infer_params', 'covariance+likelihood+inducing',...
           'tr_index', trindex, 'latent_method', 'EP');
gp4 = gp_set('type', 'CS+FIC', 'lik', lik, 'cf', {gpcf1 gpcf2}, 'X_u', Xu,  'Xu_prior', pxu, ...
           'jitterSigma2', 1e-4, 'infer_params', 'covariance+likelihood+inducing', ...
            'latent_method', 'EP');
gp4.cf{1}.p.lengthScale=prior_fixed();
gp4.cf{1}.p.magnSigma2=prior_fixed();

fprintf('EP \n \n');
fprintf('Full GP:\n \n')
derivs{5}(:,1)=derivativecheck(gp_pak(gp), @(ww) gp_eg(ww, gp, x, y, 'z', ye));
fprintf('FIC:\n \n')
derivs{6}(:,1)=derivativecheck(gp_pak(gp2), @(ww) gp_eg(ww, gp2, x, y, 'z', ye));
fprintf('PIC:\n \n')
derivs{6}(:,2)=derivativecheck(gp_pak(gp3), @(ww) gp_eg(ww, gp3, x, y, 'z', ye));
fprintf('CS+FIC:\n \n')
derivs{6}(:,3)=derivativecheck(gp_pak(gp4), @(ww) gp_eg(ww, gp4, x, y, 'z', ye));


function [vw, figHandle] = plotEccVsPhase(vw, newfig, colored, drawROI, varargin)
%
% [vw figHandle] = plotEccVsPhase(vw, newfig, co)
%
% Plot of eccentricity versus phase for the current scan, for all
% pixels (in all slices) in the current ROI. You can use this function
% to plot visual field coverage.
%
% This function is kind of a hack, in that it expects that the pRF data are
% loaded as per rmLoadDefault: variance explained in the 'co' slot, polar
% angle in the 'ph' slot, and eccentricity (or log10(eccentricity)) in the
% 'map' slot. However, you can overload this by specifying alternate sets
% of co, eccentricity, or phase data by passing them in as parameter/value
% pairs (see OPTIONS below).
%
% the line ROI should be transformed from the cortical surface mesh
% _without_ fill layers.
%
% INPUTS
%  vw: mrVista view struct
%  newfig: whether you open a new figure or not
%  colored: whether you change the color of dots depending on the coherence
%  data.
%
% OPTIONS:
%	'cothresh', [cothresh]: specify the minimum coherence value to include
%	in the plot.
%
%	'eccthresh', [eccthresh]: specify the maximum eccentricity to include
%	in the plot.
%
%	'co', [co data]: overload the view's coherence data with your own set of
%	coherence values. Should match the size of the data in view.co.
%
%	'ecc', [ecc data]: overload the view's eccentricity data with your own set of
%	eccentricity values. Should match the size of the data in view.map.
%
%	'ph', [ph data]: overload the view's phase data with your own set of
%	polar angle values. Should match the size of the data in view.ph.
%
%  SEE ALSO: retinoPlot, rmPlotCoverage.
%
%  08/06 KA made several changes
%  aug 2008: JW added ROI title to the plot
%  sep 2008: JW added option to draw visually-referred polygon ROI
%  jan 2009: JW: removed subroutine 'ROIcaptureSelectedPoints' and put it
%                in an independent function, so that it can be called from
%                other plotting functions
%  apr 2009: RAS: clarified what needs to be set up; allows you to overload
%  new ph, ecc, and co values.

if notDefined('vw'), error('View must be defined.'); end
if notDefined('newfig'), newfig = 1; end
if notDefined('colored'), colored = 0; end

if notDefined('drawROI'), drawROI = 0; end

if isempty(viewGet(vw, 'ROIs')) || viewGet(vw, 'selected ROI') < 1
	error('No Selected ROI!');
end

% Get selpts from current ROI
ROIcoords = viewGet(vw, 'ROI coords');

% Get co and ph (vectors) for the current scan, within the
% current ROI.
I = viewGet(vw, 'ROI indices');
model = viewGet(vw, 'rmmodel');
model = model{1};
co = rmGet(model, 'varexp');
co = co(I);
ph = rmGet(model, 'pol');
ph = ph(I);
ecc = rmGet(model, 'ecc');
ecc = ecc(I);

% Remove NaNs from subCo and subAmp that may be there if ROI
% includes volume voxels where there is no data.
NaNs = find(isnan(co), 1);
if ~isempty(NaNs)
%   myWarnDlg('ROI includes voxels that have no data.  These voxels are being ignored.');
  notNaNs = find(~isnan(co));
  co = co(notNaNs);
  ph = ph(notNaNs);
  ecc = ecc(notNaNs);
end

cothresh = viewGet(vw, 'co thresh');
eccthresh = max(viewGet(vw, 'mapclipmode'));

%% parse options
for ii = 1:2:length(varargin)
	switch lower(varargin{ii})
		case 'cothresh', cothresh = varargin{ii+1};
		case 'eccthresh', eccthresh = varargin{ii+1};
		case 'ecc', ecc = varargin{ii+1};
		case 'ph',  ph = varargin{ii+1};
		case 'co', co = varargin{ii+1};
	end
end

% Find voxels which satisfy cothresh and eccthresh.
coIndices = find(co>cothresh & ecc<=eccthresh);

% Pull out co and ph for desired pixels
subCo =   co(coIndices);
subPh =   ph(coIndices);
subEcc = ecc(coIndices);
if newfig
    figHandle = figure;
else
	%figHandle = selectGraphWin;
end

% selectGraphWin;
% Window header
% headerStr = ['Eccentricity vs. phase, ROI ',ROIname,', scan ',num2str(curScan)];
% set(gcf,'Name',headerStr);
% Plot it
fontSize = 14;
symbolSize = 4;

% polar plot
subX = subEcc.*cos(subPh);
subY = subEcc.*sin(subPh);

% polar plot params
params.grid = 'on';
params.line = 'off';
params.gridColor = [0.6,0.6,0.6];
params.fontSize = fontSize;
params.symbol = 'o';
params.size = symbolSize;
params.color = 'w';
params.fillColor = 'w';
params.maxAmp = eccthresh;
% if eccthresh > 10, params.ringTicks = [0:5:eccthresh];
% else
	params.ringTicks = round( linspace(0, eccthresh, 4) );
% end

% Use 'polarPlot' to set up grid'
% clf
polarPlot(0,params);

% finish plotting it
for i=1:size(subX,2)
    if colored
        h=plot(subX(i),subY(i),'o','MarkerSize',symbolSize,'Color',[1-subCo(i) 1-subCo(i) 1-subCo(i)]);
        set(h,'MarkerFaceColor',[1-subCo(i) 1-subCo(i) 1-subCo(i)])
    else
        h=plot(subX(i),subY(i),'o','MarkerSize',symbolSize,'Color',[0 0 0]);
        set(h,'MarkerFaceColor',[0.5 0.5 0.5])
    end
end

title(vw.ROIs(vw.selectedROI).name, 'FontSize', 24, 'Interpreter', 'none');
hold off

if drawROI,
    vw = roiCapturePointsFromPlot(vw, subX, subY, coIndices, ROIcoords);
end

% Save the data in gca('UserData')
data.co = co;
data.ph = ph;
data.subCo = subCo;
data.subPh = subPh;
set(gca,'UserData',data);

return;

end


function n = ndims(t)
%NDIMS Return the number of dimensions of a tensor.
%
%   NDIMS(X) returns the number of dimensions of tensor X.
%
%   Examples
%   A = rand(4,3,1); ndims(A) %<-- Returns 2
%   X = tensor(A); ndims(X) %<-- Returns 2
%   X = tensor(A,[4 3 1]); ndims(X) %<-- Returns 3
%
%   See also TENSOR
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

n = numel(t.size);


function [llratios, models] = gpTwoSample(t, y)

% GPTWOSAMPLE Do Oliver Stegles simple two sample test on a data set.
% FORMAT
% DESC computes a two sample test using independent or combined GPs to
% determine whether two gene expression profiles were more likely to be
% independently generated or generated together.
% ARG t : cell array containing times of the two gene expression
% profiles.
% ARG y : cell array containing log expression levels of the gene
% expression. In each component of the cell array different genes are in
% different columns and different time points in different rows.
% RETURN llratios : the log likelihood ratio between the combined and
% independent models.
%
% COPYRIGHT : Neil D. Lawrence, 2010
%
% SEEALSO : multimodelCreate, gpCreate

% GP
maxIters = 500;
display = 0;
tTemp = sort(t{1});
diffs = tTemp(2:end) - tTemp(1:end-1);
diffs(~diffs) = [];
typDiff = min(diffs);
invWidth = 1/(typDiff*typDiff);
%disp(1/invWidth)
options = gpOptions('ftc');
options.scale2var1 = true;
tfull = [t{1}; t{2}];
options.kern = kernCreate(tfull, options.kern);
options.kern.comp{1}.inverseWidth = invWidth;

optionsMulti = multimodelOptions('gp', 2, 'ftc');
optionsMulti.separate = [];
llComb = zeros(size(y{1}, 2), 1);
llInd = zeros(size(y{1}, 2), 1);
models = cell(size(y{1},2), 2);

for i = 1:size(y{1}, 2)
    fprintf('Start gene %d\n', i)

    % Check for missing data.
    if any(isnan(y{1}(:, i))) || any(isnan(y{2}(:, i)))
        options.isMissingData = true;
        options.isSpherical = false;
    else
        options.isMissingData = false;
        options.isSpherical = true;
    end
    % Fit a combined Gaussian process to the data.
    model = gpCreate(1, 1, tfull, [y{1}(:, i); y{2}(:, i)], options);
    model = gpOptimise(model, display, maxIters, 0);
    llComb(i) = gpLogLikelihood(model);
    models{i, 1} = model;
    % Plot regression.
    %{
    xstar = linspace(t{1}(1), t{1}(end),100)';
    [mu, S] = gpPosteriorMeanVar(models{i,1}, xstar);
    % %       S = S - exp(2*loghypers(3,h)); % subtract noise variance
    f = [mu+2*sqrt(S); flipdim(mu-2*sqrt(S),1)];
    clf, hFill = fill([xstar; flipdim(xstar,1)], f, [0 0 1], 'FaceAlpha', .1, 'EdgeColor', [0 0 1], 'EdgeAlpha', 0.1);
    datamax = max(max(y{1,2})); datamin = min(min(y{1,2})); % Data min/max for plot limits.
    ylim([datamin datamax]), xlim([min(xstar) max(xstar)]),
    hold on, plot(xstar, mu,'b-','LineWidth',2)
    plot(t{1}, y{1}(:, i), '+b', t{2}, y{2}(:, i), 'or', 'MarkerSize', 8);
    set(get(get(hFill,'Annotation'),'LegendInformation'), 'IconDisplayStyle','off');
    legend('GP','oxidised','non-oxidised')
    xlabel('time(months)','fontsize',12,'fontweight','bold'), ylabel('gene expression','fontsize',12,'fontweight','bold')
    %}

    % Fit independent Gaussian processes to the data.
    ymulti = cell(2, 1);
    ymulti{1} = y{1}(:, i);
    ymulti{2} = y{2}(:, i);
    optionsMulti.compOptions = options;
    multiModel = multimodelCreate(1, 1, t, ymulti, optionsMulti);
    % Make sure we scale using variance of combined data
    multiModel.comp{1}.scale = model.scale;
    multiModel.comp{1}.m = gpComputeM(multiModel.comp{1});
    multiModel.comp{2}.scale = model.scale;
    multiModel.comp{2}.m = gpComputeM(multiModel.comp{2});
    % Optimise the model
    multiModel.optimiser = 'scg';
    multiModel = modelOptimise(multiModel, [], [], display, maxIters, 0);
    llInd(i) = multimodelLogLikelihood(multiModel);
    models{i,2} = multiModel;
end
llratios = llInd - llComb;
end


function [ y, m ] = month_carry_bahai ( y, m )

%*****************************************************************************80
%
%% MONTH_CARRY_BAHAI carries a year of months on the Bahai calendar.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, M, the YM date.
%
  months = year_length_months_bahai ( y );

  while ( 1 )

    if ( m <= months )
      break;
    end if

    m = m - months;
    y = y + 1;

  end

  return
end


function UpdatedDA=UpdateDA(CA,DA,Q,W)
% Update DA
% CA is the Archive that has  been updated.
% DA is the Archive that has not been updated
% Q  is the set of offspring
% W  is the set of weight vectors

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    S=[];                                                       % S is the set used for output
    Hd=[DA,Q];
    N=size(W,1);
    [~,Region_Hd] = max(1-pdist2(real(Hd.objs),W,'cosine'),[],2);
    [~,Region_CA] = max(1-pdist2(CA.objs,W,'cosine'),[],2);     % associat the individuals in Hd and CA with their corresponding subregions
    itr=1;
    while length(S)<N
        for i=1:N                                               % here i denotes the order of the subregion
            current_c=find(Region_CA==i);                       % current_c denotes that the current_c_th individual is/are already in the ith region
            if length(current_c)<itr
                for j=1:itr-length(current_c)                   % j denotes the number of solutions from Hd that need to join into the region(i)
                    current_d=find(Region_Hd==i);
                    if isempty(current_d)~=1
                        [FrontNO,~]=NDSort(Hd(current_d).objs,inf);
                        O=Hd(current_d(FrontNO==1));            % O is the set of nondominated solutions from region(i) in Hd
                        [~,Region_O] = max(1-pdist2(real(O.objs),W,'cosine'),[],2);
                        Z = min(O.objs,[],1);
                        g_tch=max(abs(O.objs-repmat(Z,length(O),1))./W(Region_O,:),[],2);
                        [~,order]=min(g_tch);
                        x_best=O(order);

                        Hd(current_d(Hd(current_d)==x_best))=[];% update Region_Hd
                        if isempty(Hd)==1
                            Region_Hd=[];
                        else
                            [~,Region_Hd] = max(1-pdist2(real(Hd.objs),W,'cosine'),[],2);
                        end
                        if length(S)<N                          % add the best individual into S
                            S=cat(2,S,x_best);
                        end
                    else
                        break;
                    end
               end
            end
            if length(S)==N
                break;
            end
        end
        itr=itr+1;
    end
    UpdatedDA=S;
end


function [no,el,regions,holes]=vol2surf(img,ix,iy,iz,opt,dofix,method,isovalues)
%
% [no,el,regions,holes]=vol2surf(img,ix,iy,iz,opt,dofix,method,isovalues)
%
% converting a 3D volumetric image to surfaces
%
% author: Qianqian Fang (fangq <at> nmr.mgh.harvard.edu)
%
% input:
%	 img: a volumetric binary image; if img is empty, vol2surf will
%	      return user defined surfaces via opt.surf if it exists
%	 ix,iy,iz: subvolume selection indices in x,y,z directions
%	 opt: function parameters
%	   if method is 'cgalsurf' or 'cgalpoly':
%	     opt=a float number>1: max radius of the Delaunay sphere(element size)
%	     opt.radbound: same as above, max radius of the Delaunay sphere
%	     opt.distbound: maximum deviation from the specified isosurfaces
%	     opt(1,2,...).radbound: same as above, for each levelset
%	   if method is 'simplify':
%	     opt=a float number<1: compression rate for surf. simplification
%	     opt.keepratio=a float less than 1: same as above, same for all surf.
%	     opt(1,2,..).keepratio: setting compression rate for each levelset
%	   opt(1,2,..).maxsurf: 1 - only use the largest disjointed surface
%				0 - use all surfaces for that levelset
%          opt(1,2,..).side: - 'upper': threshold at upper interface
%                              'lower': threshold at lower interface
%	   opt(1,2,..).maxnode: - the maximum number of surface node per levelset
%	   opt(1,2,..).holes: user specified holes interior pt list
%	   opt(1,2,..).regions: user specified regions interior pt list
%	   opt(1,2,..).surf.{node,elem}: add additional surfaces
%	   opt(1,2,..).{A,B}: linear transformation for each surface
%	   opt.autoregion: if set to 1, vol2surf will try to determine
%              the interior points for each closed surface automatically
%	 dofix: 1: perform mesh validation&repair, 0: skip repairing
%	 method: - if method is 'simplify', iso2mesh will first call
%		   binsurface to generate a voxel-based surface mesh and then
%		   use meshresample/meshcheckrepair to create a coarser mesh;
%		 - if method is 'cgalsurf', iso2mesh will call the surface
%		   extraction program from CGAL to make surface mesh
%		 - if method is not specified, 'cgalsurf' is assumed by default
%	 isovalues: a list of isovalues where the levelset is defined
%
% output:
%	 no:  list of nodes on the resulting suface mesh, 3 columns for x,y,z
%	 el:  list of trianglular elements on the surface, [n1,n2,n3,region_id]
%	 regions: list of interior points for all sub-region, [x,y,z]
%	 holes:   list of interior points for all holes, [x,y,z]
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

fprintf(1,'extracting surfaces from a volume ...\n');

el=[];
no=[];

if(isstruct(opt) & isfield(opt,'holes'))
    holes=opt.holes;
else
    holes=[];
end
if(isstruct(opt) & isfield(opt,'regions'))
    regions=opt.regions;
else
    regions=[];
end
maxlevel=0;

if(~isempty(img))

    img=img(ix,iy,iz);
    dim=size(img);
    newdim=dim+[2 2 2];
    newimg=zeros(newdim);
    newimg(2:end-1,2:end-1,2:end-1)=img;

    if(nargin<8)
        maxlevel=max(newimg(:));
        isovalues=1:maxlevel;
    else
        isovalues=unique(sort(isovalues));
        maxlevel=length(isovalues);
    end

    for i=1:maxlevel
      if(i<maxlevel)
          levelmask=int8(newimg>=isovalues(i) & newimg<isovalues(i+1));
      else
          levelmask=int8(newimg>=isovalues(i));
      end
      [levelno,levelel]=binsurface(levelmask);
      if(~isempty(levelel))
          if(isstruct(opt) & isfield(opt,'autoregion'))
              if(opt.autoregion)
                  seeds=surfseeds(levelno,levelel);
              else
                  seeds=surfinterior(levelno,levelel);
              end
          else
              seeds=surfinterior(levelno,levelel);
          end
          if(~isempty(seeds))
              disp([sprintf('region %d centroid :',i) sprintf('\t%f %f %f\n', seeds')]);
              if(~isempty(regions))
                  regions(end+1:end+size(seeds,1),:)=seeds;
              else
                  regions=seeds;
              end
          end
      end
    end

    for i=1:maxlevel
        fprintf(1,'processing threshold level %d...\n',i);

        if(nargin>=7 & strcmp(method,'simplify'))

          [v0,f0]=binsurface(newimg>=isovalues(i)); % not sure if binsurface works for multi-value arrays
          % with binsurface, I think the following line is not needed anymore
          %  v0(:,[1 2])=v0(:,[2 1]); % isosurface(V,th) assumes x/y transposed
          if(dofix)  [v0,f0]=meshcheckrepair(v0,f0);  end

          if(isstruct(opt) & length(opt)==maxlevel) keepratio=opt(i).keepratio;
          elseif (isstruct(opt) & length(opt)==1) keepratio=opt.keepratio;
          else keepratio=opt;  end;

          % first, resample the surface mesh with cgal
          fprintf(1,'resampling surface mesh for level %d...\n',i);
          [v0,f0]=meshresample(v0,f0,keepratio);

          % iso2mesh is not stable for meshing small islands,remove them (max 3x3x3 voxels)
          f0=removeisolatedsurf(v0,f0,3);

          if(dofix) [v0,f0]=meshcheckrepair(v0,f0); end

        elseif(nargin<7 | strcmp(method,'cgalsurf') | strcmp(method,'cgalpoly'))
          if(isstruct(opt) & length(opt)==maxlevel) radbound=opt(i).radbound;
          elseif (isstruct(opt) & length(opt)==1) radbound=opt.radbound;
          else radbound=opt;  end;

          maxsurfnode=40000;  % maximum node numbers for each level
          if(isstruct(opt) & length(opt)==maxlevel)
              if(isfield(opt(i),'maxnode')) maxsurfnode=opt(i).maxnode; end
          elseif (isstruct(opt) & length(opt)==1 )
              if(isfield(opt(1),'maxnode'))
                 maxsurfnode=opt.maxnode;
              end
          end

	  distbound=radbound;
          if(isstruct(opt) & length(opt)==maxlevel)
              if(isfield(opt(i),'distbound')) distbound=opt(i).distbound; end
          elseif (isstruct(opt) & length(opt)==1 )
              if(isfield(opt(1),'distbound')) distbound=opt.distbound; end
          end
	  surfside='';
          if(isstruct(opt) & length(opt)==maxlevel)
              if(isfield(opt(i),'side')) surfside=opt(i).side; end
          elseif (isstruct(opt) & length(opt)==1)
              if(isfield(opt(1),'side')) surfside=opt(1).side; end
          end
	  if(~isempty(surfside))
	     newimg0=newimg;
	  end
          if(strcmp(surfside,'upper'))
	      newimg(find(newimg<=isovalues(i)-1e-9))=isovalues(i)-1e-9;
	  elseif(strcmp(surfside,'lower'))
	      newimg(find(newimg>=isovalues(i)+1e-9))=isovalues(i)+1e-9;
	  end
          perturb=1e-4*abs(max(isovalues));
          if(all(newimg>isovalues(i)-perturb)) perturb=-perturb;  end
          [v0,f0]=vol2restrictedtri(newimg,isovalues(i)-perturb,regions(i,:),...
                     sum(newdim.*newdim)*2,30,radbound,distbound,maxsurfnode);

	  if(~isempty(surfside))
            newimg=newimg0;
	    clear newimg0;
	  end
        else
            error('method can only be one of "cgalsurf", "cgalpoly" or "simplify".');
        end

        % if use defines maxsurf=1, take only the largest closed surface
        if(isstruct(opt))
            if( (isfield(opt,'maxsurf') && length(opt)==1 && opt.maxsurf==1) | ...
                    (length(opt)==maxlevel && isfield(opt(i),'maxsurf') && opt(i).maxsurf==1))
                    f0=maxsurf(finddisconnsurf(f0));
            end
        end

        % if a transformation matrix/offset vector supplied, apply them

        if(isstruct(opt) & length(opt)==maxlevel)
          if(isfield(opt(i),'A') & isfield(opt(i),'B'))
            v0=(opt(i).A*v0'+repmat(opt(i).B(:),1,size(v0,1)))';
          end
        elseif (isstruct(opt) & length(opt)==1)
          if(isfield(opt,'A') & isfield(opt,'B'))
            v0=(opt.A*v0'+repmat(opt.B(:),1,size(v0,1)))';
          end
        end

        % if user specified holelist and regionlist, append them
        if(isstruct(opt)  & length(opt)==maxlevel)
        if(isfield(opt(i),'hole'))
                holes=[holes;opt(i).hole]
        end
        if(isfield(opt(i),'region'))
                regions=[regions;opt(i).region]
        end
        end

        if(i==0)
        el=[f0 (i+1)*ones(size(f0,1),1)];
        no=v0;
        else
        el=[el;f0+length(no) (i+1)*ones(size(f0,1),1)];
        no=[no;v0];
        end
    end

    %some final fix and scaling
    no(:,1:3)=no(:,1:3)-1; % because we padded the image with a 1 voxel thick null layer in newimg

    no(:,1)=no(:,1)*(max(ix)-min(ix)+1)/dim(1)+(min(ix)-1);
    no(:,2)=no(:,2)*(max(iy)-min(iy)+1)/dim(2)+(min(iy)-1);
    no(:,3)=no(:,3)*(max(iz)-min(iz)+1)/dim(3)+(min(iz)-1);

end  % if not isempty(img)

if(isstruct(opt) & isfield(opt,'surf'))
   for i=1:length(opt.surf)
	opt.surf(i).elem(:,4)=maxlevel+i;
        el=[el;opt.surf(i).elem+length(no)];
        no=[no;opt.surf(i).node];
   end
end

fprintf(1,'surface mesh generation is complete\n');


function best_acc = D2L2R2_top(dataset, N_train, k, lambda1, lambda2, alpha)
% function D2L2R2_top(dataset, N_train, k, lambda1, lambda2, alpha)
% * The top function of D2L2R2
% * INPUT:
%   + `dataset`: name of the dataset stored in `.mat` file in `data` folder.
%     Note that `dataset` is the file name of the `.mat`, excluding `.mat`.
%   + `N_train`: number of training samples in each class
%   + `k`: number of atoms in EACH dictionary
%   + `lambda1, lambda2, alpha`: regularization parameters.
% * To run an small example, type `D2L2r2-top` without input in MATLAB
%           command window.
% -----------------------------------------------
% Author: Tiep Vu, thv102@psu.edu, 5/13/2016 10:29:48 PM
%         (http://www.personal.psu.edu/thv102/)
% -----------------------------------------------
    addpath(genpath('utils'));
    addpath('ODL');
    addpath('LRSDL_FDDL');
    addpath('D2L2R2');
    %% test mode
    if nargin == 0
        dataset = 'myYaleB';
        N_train = 10;
        k = 8;
        lambda1 = 0.001;
        lambda2 = 0.01;
        alpha = 0.01;
    end
    %% Data preparation
    t = getTimeStr();
    [dataset, Y_train, Y_test, label_train, label_test] = train_test_split(...
        dataset, N_train);
    %% main
    [acc, rt] = D2L2R2_wrapper(Y_train, label_train, Y_test, label_test,...
                        k, lambda1, lambda2, alpha);
    %% save results
    if ~exist('results', 'dir')
        mkdir('results');
    end
    if ~exist(fullfile('results', 'D2L2R2'), 'dir')
        mkdir('results', 'D2L2R2');
    end
    fn = fullfile('results', 'D2L2R2', strcat(dataset, '_N_', ...
        num2str(N_train), '_k_', num2str(k), '_l1_', num2str(lambda1),...
        '_l2_', num2str(lambda2), '_a_', num2str(alpha), '_', t, '.mat'));
    disp(fn);

    save(fn, 'acc', 'rt');
    best_acc = max(acc);
end


function file_name = file_name_inc ( file_name )

%*****************************************************************************80
%
% FILE_NAME_INC generates the next filename in a series.
%
%  Discussion:
%
%    It is assumed that the digits in the name, whether scattered or
%    connected, represent a number that is to be increased by 1 on
%    each call.  If this number is all 9's on input, the output number
%    is all 0's.  Non-numeric letters of the name are unaffected..
%
%    If the name is empty, then the routine stops.
%
%    If the name contains no digits, the empty string is returned.
%
%  Example:
%
%      Input            Output
%      -----            ------
%      'a7to11.txt'     'a7to12.txt'  (typical case.  Last digit incremented)
%      'a7to99.txt'     'a8to00.txt'  (last digit incremented, with carry.)
%      'a9to99.txt'     'a0to00.txt'  (wrap around)
%      'cat.txt'        ' '           (no digits in input name.)
%      ' '              STOP!         (error.)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 September 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string FILE_NAME, the string to be incremented.
%
%    Output, string FILE_NAME, the incremented string.
%
  lens = length ( file_name );

  if ( lens <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_NAME_INC - Fatal error!\n' );
    fprintf ( 1, '  The input filename is empty.\n' );
    error ( 'FILE_NAME_INC - Fatal error!' );
  end

  change = 0;

  for i = lens : -1 : 1

    c = file_name(i);

    if ( '0' <= c & c <= '8' )

      change = change + 1;

      c = c + 1;

      file_name(i) = c;

      return

    elseif ( c == '9' )

      change = change + 1;

      c = '0';

      file_name(i) = c;

    end

  end

  if ( change == 0 )
    file_name = ' ';
  end

  return
end


function [surfVel] = getSurfaceVelocity(bodyInfo, ut, rVectECI, vVectECI)
%getSurfaceVelocity Summary of this function goes here
%   Detailed explanation goes here
    [~, ~, ~, ~, horzVel, ~] = getLatLongAltFromInertialVect(ut, rVectECI, bodyInfo, vVectECI);
%     bodyCircum = 2*pi*bodyInfo.radius;
%     equatSpeed = bodyCircum / bodyInfo.rotperiod; %km/s
%
%     speedAtLat = equatSpeed * cos(lat);
%     bodySurfVelECI = -normVector(cross(rVectECI, [0;0;1])) * speedAtLat;
    surfVel = horzVel;
end


% UFgetPetscMat.m
% modified from UFget_example.m
%   This script
%     (1) gets the selected index file of the UF sparse matrix collection,
%     (2) loads in matrices in matlab format in increasing order of
%         number of rows in the selected matrices,
%     (3) writes into PETSc binary format in the given directory with
%         each matrix named as A_{id}
%
%   See also UFget_example.m
%   Copyright 2009, PETSc Team.

index = UFget;

% sets selection here
f = find (index.nrows == index.ncols & index.nrows > 940000 & index.isReal) ;
[y, j] = sort (index.nrows (f)) ;
f = f (j) ;

for i = f
    %loads in matrix in matlab format
    %---------------------------------
    fprintf ('Loading %s%s%s, please wait ...\n', ...
        index.Group {i}, filesep, index.Name {i}) ;
    Problem = UFget (i,index) ;
    disp (Problem) ;
    title (sprintf ('%s:%s', Problem.name, Problem.title')) ;

    % convets to PETSc binary format and writes into ~mat/A_{id}
    %-----------------------------------------------------------
    fname = ['mat/A',num2str(i)];
    fprintf ('write matrix into petsc binary file %s ...\n',fname);
    PetscBinaryWrite(fname,Problem.A);
    %input ('hit enter to continue:') ;
end


function tbxStruct=demos
% DEMOS Demo list for The N-way Toolbox for MATLAB

% Copyright (C) 1995-2006  Rasmus Bro & Claus Andersson
% Copenhagen University, DK-1958 Frederiksberg, Denmark, rb@life.ku.dk
%
% 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.

if nargout==0,
    demo toolbox 'The N-way Toolbox for MATLAB';
    return;
end

tbxStruct.Name='The N-way Toolbox for MATLAB';
tbxStruct.Type='Toolbox';
tbxStruct.Help= {
' The N-way Toolbox for MATLAB contains state-of-the-art'
' tools for doing multi-way analysis in MATLAB.'};
tbxStruct.DemoList={
'PARAFAC Demo', 'parademo', '',
'Tucker Demo','tuckdemo', '',};


function nfgCheckBMVolume(phantomDir)
%Verify that the BlueMatter volume calculation is correct
%
%   nfgCheckBMVolume(phantomDir)
%
%
% AUTHORS:
%   2009.08.05 : AJS wrote it
%
% NOTES:

% Directories
volcheckDir = nfgGetName('volcheckDir',phantomDir);
% Input Files
goldPDBFile = nfgGetName('goldPDBFile',phantomDir);
goldInfoFile = nfgGetName('goldInfoFile',phantomDir);
b0File = nfgGetName('b0File',phantomDir);
ctrparamsFile = nfgGetName('ctrparamsFile',phantomDir);
noisyImg = nfgGetName('noisyImg',phantomDir);
bvalsFile = nfgGetName('bvalsFile',phantomDir);
bvecsFile = nfgGetName('bvecsFile',phantomDir);
wmROIFile = nfgGetName('wmROIFile',phantomDir);

mkdir(volcheckDir);

% Get bundle ID and radius of each gold pathway from info file
gi = load(goldInfoFile);
g_bundleID = zeros(1,length(gi.strand_info));
g_radius = zeros(1,length(gi.strand_info));
for ii=1:length(gi.strand_info)
    g_bundleID(ii) = gi.strand_info(ii).bundleID+1;
    g_radius(ii) = gi.strand_info(ii).radius;
end
% Get gold fibers
fgG = mtrImportFibers(goldPDBFile);

% Radii to search through
vR = 0.01:0.01:0.04;
volG = zeros(1,length(vR));
volBM = zeros(1,length(vR));
% Save a .pdb file for each bundle
for bb=1:20
    disp(['Examining bundle ' num2str(bb) ' ...']);
    vcPDB = fullfile(volcheckDir,['b' num2str(bb) '.pdb']);
    vcSBfloat = fullfile(volcheckDir,['b' num2str(bb) '_0.SBfloat']);
    fFile = fullfile(volcheckDir,['f_b' num2str(bb) '.nii.gz']);
    eFile = fullfile(volcheckDir,['e_b' num2str(bb) '.nii.gz']);
    % Write the pdb
    fg = dtiNewFiberGroup();
    fg.fibers = fgG.fibers(g_bundleID==bb);
    if ~isempty(fg.fibers)
        mtrExportFibers(fg,vcPDB);
        % Convert it to the SBfloat format
        pParamFile = [' -i ' ctrparamsFile];
        pOutFile = [' -p ' vcSBfloat];
        pInFile = [' ' vcPDB];
        pThresh = [' --thresh ' num2str(length(fg.fibers))];
        cmd = ['contrack_score' pParamFile pOutFile pThresh ' --seq' pInFile];
        disp(cmd);
        [s,r] = system(cmd);
        for rr=1:length(vR)
            % Run error calculation for each radii
            disp(' '); disp(['Calculating for raidus ' num2str(vR(rr)) ' ...']);
            b0 = niftiRead(b0File);
            fLambda = 0;
            fDiameter = vR(rr)*2;
            argSubSize = [' -s 0,' num2str(b0.dim(1)-1) ',0,' num2str(b0.dim(2)-1) ',0,' num2str(b0.dim(3)-1)];
            argDatabase = [' -d ' vcSBfloat];
            argRaw = [' -r ' noisyImg];
            argBvals = [' --val ' bvalsFile];
            argBvecs = [' --vec ' bvecsFile];
            argB0 = [' -0 ' b0File];
            argMatter = [' -m ' wmROIFile];
            argLambda = [' -w ' num2str(fLambda)];
            argDiameter = [' --diameter ' num2str(fDiameter)];
            argEFile = [' -e ' eFile];
            argFFile = [' --fraction ' fFile];
            argError = [argDatabase argRaw argBvals argBvecs argB0 argMatter argSubSize argLambda argDiameter argEFile argFFile];
            cmdError = ['trueError ' argError];
            disp(cmdError);
            [s,r] = system(cmdError);
            % Get BlueMatter volume calculation
            f = niftiRead(fFile);
            volBM(rr) = volBM(rr)+sum(f.data(:));
            % Get gold volume calculation
            volG(rr) = volG(rr)+volBundle(fg.fibers,vR(rr));
        end
    end
end

% Plot the volume functions
figure; plot(vR,volBM,'b'); hold on; plot(vR,volG,'g');

return;

function [arcL] = arclength(fc)
arcL = sum(sqrt(sum((fc(:,2:end) - fc(:,1:end-1)).^2,1)));
return;

function vol = volBundle(fibers,radius)
lenFibers=0;
for ff=1:length(fibers)
    lenFibers = lenFibers + arclength(fibers{ff});
end
vol = lenFibers*pi*radius^2;
return;


% eeg_miclust() - Calculate mutual information matrix between independent component
%                 activations and cluster the components using it. Opens several figures:
%                -  A figure with sorted silhouette values of each cluster
%                -  A figure showing clusters in 3-D, with brightness of
%                     each point proportional to its silhouette value
%                -  Multiple 2-D plots, each for one cluster. Background
%                     color shows interpolated silhouette value. A bright
%                     background means the component fits well in the cluster;
%                     a dark background means it belongs near equally
%                     to another cluster or to no cluster.
%                -  A dendrogram of componets if 'linkage' method is used
% Usage:
%              >> EEG = eeg_miclust(EEG);
%              >> EEG = eeg_miclust(EEG,N,components, clusterMethod);
% Inputs:
%
%   EEG        - EEG data structure
%   N          - number of clusters to produce {default: 5}
%   components - a vector containing component indices (rows of sim matrix)
%                to cluster. For example [1:10] uses only first 10 components
%                {Default: [1:120] or all}
%
% Optional Inputs:
%
%   clusterMethod - which clustering method to use, options are 'linkage'
%                   and 'kmeans' {defgault = 'linkage'}
% Output:
%
%   EEG        - input EEG structure containing mutual information
%                between specified components in field EEG.etc.miclust.mutual_info and indices of
%                these components in field EEG.etc.miclust.allcomponents Cluster information
%                is placed in EEG.etc.miclust field.
% Example:
%
%   % Cluster components 1 to 30 into 4 clusters using mutual information.
%   >> EEG = eeg_miclust(EEG,4,1:30);
%
% See also: %   getmiclusts(), showmiclusts(), mi_pairs()
%
% Author: Nima Bigdely Shamlo, SCCN/INC/UCSD, 2006

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

% Edit history:
%
% 5/26/06 added default for N, test for <120 comps, extended help msg, formatted -sm
% 5/30/06 use only first 120 comps -nb

function EEGout = eeg_miclust(EEG,n,comps, clusterMethod)

if nargin<2
   n = 5; % default to 5 clusters
end
if nargin<3
    if size(EEG.icaact,1) >= 120
      comps = 120;
    else
      comps = 1:size(EEG.icaact,1);
    end;
end;

if nargin<4
   clusterMethod = 'linkage' ;
else
    if ~strcmp(clusterMethod, 'kmeans') && ~strcmp(clusterMethod, 'linkage')
        fprintf('Clustering method not found.');
        return;
    end;
end

if ~(isfield(EEG,'etc') && isfield(EEG.etc,'miclust') && isfield(EEG.etc.miclust,'mutual_info')  ) || isempty(EEG.etc.miclust.allcomponents) || ~isfield(EEG.etc.miclust,'allcomponents') || length(EEG.etc.miclust.allcomponents) ~= length(comps) || sum(EEG.etc.miclust.allcomponents ~= comps) > 0
    fprintf('Calculating mutual information matrix (time consuming)...');
    mutual_info = mi_pairs(EEG.icaact(comps,:),'full',false);
else
    mutual_info = EEG.etc.miclust.mutual_info;
end

EEG.etc.miclust = getmiclusts(mutual_info,n, comps, clusterMethod);

EEG.etc.miclust.mutual_info = mutual_info;

plotTopo = @(x) topoplot(EEG.icawinv(:,x), EEG.chanlocs,'electrodes','off');

showmiclusts(EEG.etc.miclust,plotTopo);

if nargout>0
    EEGout = EEG;
end;


function show(I,M)
%show - show content aware image resizing
%
% show(I,M) opens a figure window to allow interactive resizing of I
% in one direction based on the seam removal map M.
%
% See also removalMap.

% figure out if seams are horizontal or vertical
[h,w] = size(M);
horiz = all(sum(M,1)==sum(1:h))
vert = all(sum(M,2)==sum(1:w))
if ~xor(horiz,vert), error('seams not horizontal or vertical?!'); end

fh = figure; clf;
set(fh,'DoubleBuffer','on');
set(fh,'WindowButtonDownFcn',@mouseDown);
set(fh,'WindowButtonMotionFcn',@mouseMove);
set(fh,'WindowButtonUpFcn',@mouseUp);
set(fh,'WindowScrollWheelFcn',@mouseWheel);
% set(fh,'Interruptible','off');
% set(fh,'BusyAction','queue');
if horiz,
    ih = imshow(cat(1,I,I));
    title('horizontal seams');
else
    ih = imshow(cat(2,I,I));
    title('vertical seams');
end
set(ih,'CData',I);

if ~horiz,
    I = permute(I,[2,1,3]);
    M = M';
end

[h,w] = size(M);

n = 0; % how many seams we are removing

    % remove an additional dn seams
    function move(dn)
        n = n + dn;
        if n >= 0, % shrink image
            if horiz,
                set(ih,'CData',shrink(I,M,n));
            else
                set(ih,'CData',permute(shrink(I,M,n),[2,1,3]));
            end
        else % grow image
            if horiz,
                set(ih,'CData',expand(I,M,-n));
            else
                set(ih,'CData',permute(expand(I,M,-n),[2,1,3]));
            end
        end
        drawnow;
    end

p0 = []; % location of mouse down event
drag = false; % is the mouse being dragged?
moving = false; % are we moving the image?

    function mouseDown(src,evt)
        p0 = get(fh,'CurrentPoint');
        drag = true;
    end

    function mouseMove(src,evt)
        if ~drag, return; end
        if moving, return; end % serialize execution of this function
        moving = true;
        p = get(fh,'CurrentPoint');
        if horiz,
            move( p(2) - p0(2) );
        else
            move( p0(1) - p(1) );
        end
        p0 = p;
        moving = false;
    end

    function mouseWheel(src,evt)
        move( -evt.VerticalScrollCount * evt.VerticalScrollAmount );
    end

    function mouseUp(src,evt)
        drag = false;
    end

end


function grad = B_mixture_mse(input_layers, CostLayer)
predicted{1} = input_layers{1}.a;
predicted{2} = input_layers{2}.a;
ref{1} = input_layers{3}.a;
ref{2} = input_layers{4}.a;

[D,T,N] = size(ref{1});

ref_idx = CostLayer.ref_idx;

precision = class(gather(predicted{1}(1)));
if IsInGPU(predicted{1}(1))
    for i=1:length(predicted)
        grad{i} = gpuArray.zeros(size(predicted{1}), precision);
    end
else
    for i=1:length(predicted)
        grad{i} = zeros(size(predicted{1}), precision);
    end
end

for i=1:N
    idx = ref_idx(:,i);
    for j=1:length(idx)
        diff = predicted{j}(:,:,i) - ref{idx(j)}(:,:,i);
        grad{j}(:,:,i) = diff / T / N;
    end
end

end


function drawbox(varargin)
% function drawbox(width,height, param, properties)
%                 ([width,height], param, properties)
%
%   param, properties are optional
%

%% Copyright (C) Jongwoo Lim and David Ross.
%% All rights reserved.

%----------------------------------------------------------
% Process the input.
%----------------------------------------------------------
if (length(varargin{1}) == 2)
  w = varargin{1}(1);
  h = varargin{1}(2);
  varargin(1) = [];
else
  [w,h] = deal(varargin{1:2});
  varargin(1:2) = [];
end

if (length(varargin) < 1 || any(length(varargin{1}) ~= 6))
  M = [0,1,0; 0,0,1];
else
  p = varargin{1};
  if (length(varargin) > 1 && strcmp(varargin{2},'geom'))
    p = affparam2mat(p);
    varargin(1:2) = [];
  else
    varargin(1) = [];
  end
  M = [p(1) p(3) p(4); p(2) p(5) p(6)];
end

%----------------------------------------------------------
% Draw the box.
%----------------------------------------------------------

%corners = [ 1,0,0; 1,w,0; 1,w,h; 1,0,h; 1,0,0 ]';
corners = [ 1,-w/2,-h/2; 1,w/2,-h/2; 1,w/2,h/2; 1,-w/2,h/2; 1,-w/2,-h/2 ]';
corners = M * corners;
global result_corners;
global result_rect;
frame_num = 1;
result_corners(:,:,frame_num) = round(corners(:,1:4));
x=result_corners(1,1,frame_num);
y=result_corners(2,1,frame_num);
w=result_corners(1,3,frame_num)-result_corners(1,1,frame_num)+1;
h=result_corners(2,3,frame_num)-result_corners(2,1,frame_num)+1;
result_rect(:,frame_num) = [x y w h];

line(corners(1,:), corners(2,:), varargin{:});
% patch(corners(1,:), corners(2,:), 'y', ...
%       'FaceAlpha',0, 'LineWidth',1.5, varargin{:});

center = mean(corners(:,1:4),2);
hold_was_on = ishold; hold on;
% plot(center(1),center(2),varargin{:});
if (~hold_was_on) hold off; end


function h = addAsterisks(Y,E,sel,varargin);
% h = addAsterisks(Y,E,sel,[plot options]);
%
% Creates an errorbar plot, with asterisks over selected
% points. Y is a matrix of column vectors containing the
% data to be plotted, E is a matrix the same size as Y containing
% error lengths for each point. (In this respect, it is the same
% as calling ERRORBAR(Y,E).) sel can either be:
%      1) a matrix the same size as Y and E, with a 1 at each
%         location which will get an asterisk and a 0 otherwise, or
%
%      2) a vector equal to the # of rows or columns, selecting entire
%         rows or columns for asterisks.
%
% Any fourth or later input arguments are passed on to the ERRORBAR command
% (e.g., formatting arguments -- see HELP PLOT for more details.), except
% for the following:
%
% 'X',[vals]: provide the values to plot on the X-axis as the next arg.
%
% 'leg',{'labels'}: add a legend using the labels in the next argument
% (as a cell-of-strings)

% 03/03 ras
% 01/04 ras: fixed bug in which asterisks were never centered
if ~exist('sel','var')  | isempty(sel)
    sel = zeros(size(Y));
end

% figure out if entire rows/colums are selected for asterisks
if length(sel)==size(Y,1) & isequal(unique(sel),[0 1])
    sel = repmat(sel,size(Y,2),1)';
elseif length(sel)==size(Y,2) & isequal(unique(sel),[0 1])
    sel = repmat(sel,size(Y,1),1);
end

%%%%% defaults
X = repmat(1:size(Y,1),size(Y,2),1)';
plotOptions = {};

%%%%% parse the option flags
for i = 1:length(varargin)
    if ischar(varargin{i})
        switch lower(varargin{i})
            case {'leg','legend'}
                leg = varargin{i+1};
            case 'x',
                X = varargin{i+1};
                if size(X,1)==1
                    X = repmat(X,size(Y,2),1)';
                end
            otherwise,
                plotOptions{end+1} = varargin{i};
        end
    end
end

if isempty(plotOptions)
    h = errorbar(X,Y,E);
else
    cmd = ['h = errorbar(X,Y,E'];
    for i = 1:length(plotOptions)
        cmd = [cmd ',''' plotOptions{i} ''''];
    end
    cmd = [cmd ');'];
    eval(cmd);
end
AX = axis;
xSz = AX(2) - AX(1);
ySz = AX(4) - AX(3);

whichPoints = find(sel);

for i = 1:length(whichPoints)
    pt = whichPoints(i);
    xLoc = X(pt);
    yLoc = Y(pt) + E(pt) + 0.05*ySz;
    h2 = text(xLoc,yLoc,'*','FontSize',24,'HorizontalAlignment','center');
    %set(h2,'HorizontalAlignment','center');
    %set(h2,'Fontsize',20);
end

return


function y = prtRvUtilDiscretePdf(X,values,probabilities)
%y = discretepdf(X,values,probabilities);
%	Return the PDF of a discrete distribution with values and
%	probabilities.

y = zeros(size(X));
for i = 1:size(X(:),1);
    cI = find(X(i) == values);
    if isempty(cI)
        y(i) = 0;
    else
        y(i) = probabilities(cI);
    end
end


clear all

dt = 0.2;
t  = 0:dt:10;

Nsamples = length(t);

Avgsaved = zeros(Nsamples, 1);
Xmsaved  = zeros(Nsamples, 1);

for k=1:Nsamples
  xm  = GetVolt();
  avg = AvgFilter(xm);

  Avgsaved(k) = avg;
  Xmsaved(k)  = xm;
end

figure
plot(t, Xmsaved, 'r:*')
hold on
plot(t, Avgsaved, 'o-')


filename='Cantilever_hexahedra_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'SLERP';
incrementFactor = 1;
designVariable = 'LevelSet';
filterType = 'P1';

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

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


%install;

load('..\mat\LPnetlib\lp_agg.mat')

A = Problem.A;
p = colamd(A');
A = A(p,:);
w = (0.5+rand(size(A,2),1)).^5;
R = chol((A*(w.*A')));
x = rand(size(A,1),1);
z = R\(R'\x);
d = full(diag(R));
w = diag(sparse(w));

o = AdaptiveChol(A, 1e-8);
acc = o.factorize(w)
z2 = o.solve(ddouble(x), ddouble(w));
z3 = o.solve(ddouble(x), ddouble(w), 2);
z4 = o.solve(ddouble(x), ddouble(w), 3);
z5 = o.solve(ddouble(x), ddouble(w), 4);
d2 = o.diagonal();

norm(z2-z)
norm(z2-z3)
norm(z3-z4)
norm(z4-z5)

%norm(d2-d)
ls = o.leverageScore(100);
max(ls)


%
% Computes manipulability of poses given in qs
%
function manips = compute_manip(robot, qs)
manips = [];
for i=1:size(qs,2)
    %compute current manip
    J = manipulator_jacobian(robot, qs(:,i));
    manip = sqrt(det(J*J'));
    manips = [manips manip];
end


%% PASCAL3D evaluation
% This script runs evaluation on PASCAL3D.
% We assume that the heatmaps (keypoint localizations)
% are already computed and located in the folder 'predpath'.

clear
startup

% path for annotations
datapath = 'pose-hg/pose-hg-demo/data/pascal3d/annot/';
% path for network output
predpath = 'pose-hg/pose-hg-demo/exp/pascal3d/';
% path where the results are stored
savepath = 'result/cad/';
mkdir(savepath);
annotfile = sprintf('%s/valid.mat',datapath);
load(annotfile);

% determines shape model used for pose optimization
% if set to 1 then we use the particular cad model instance
% if set to 0 then we use the deformable shape model
cadSpecific = 1;

% visualization flag
vis = 0;

% evaluate only on the clean set
testlist = find(~annot.occluded & ~annot.truncated);

for ID = testlist'

    % input
    imgname = annot.imgname{ID};
    center = annot.center(ID,:);
    scale = annot.scale(ID);
    class = annot.class{ID};
    indices = annot.indices{ID};
    cadID = annot.cad_index(ID);

    cad = load(sprintf('cad/%s.mat',class));
    cad = cad.(class);
    cad = cad(cadID);

    savefile = sprintf('%s/valid_%d.mat',savepath,ID);

    if cadSpecific
        dict = getPascalTemplate(cad);
    else
        dict = load(sprintf('dict/pca-%s.mat',class));
    end
    % read heatmaps and detect maximum responses
    heatmap = h5read(sprintf('%s/valid_%d.h5',predpath,ID),'/heatmaps');
    heatmap = permute(heatmap(:,:,indices(dict.kpt_id)),[2,1,3]);
    [W_hp,score] = findWmax(heatmap);

    % pose optimization - weak perspective
    output_wp = PoseFromKpts_WP(W_hp,dict,'weight',score,'verb',false,'lam',1);

    % visualization
    if vis
        img = imread(sprintf('%s/../images/%s.jpg',datapath,imgname));
        vis_wp(img,output_wp,heatmap,center,scale,cad,dict);
        pause
        close all
    end

    % save output
    save(savefile,'output_wp');
end

% results
pascal3d_res(datapath, savepath);


function       [nDCnlX,blk_arr,DC,par] = Image2PGs_II( im, par)
% record the non-local patch set and the index of each patch in
% of seed patches in image
% Pad noisy image to avoid Borader Issues
Paddedim = padarray(im,[par.win,par.win],'symmetric','both');
X = zeros(par.ps2ch, par.maxrcp, 'double');
k = 0;
for l = 1:par.ch
    for i = 1:par.ps
        for j = 1:par.ps
            k = k+1;
            blk = Paddedim(i:end-par.ps+i,j:end-par.ps+j,l);
            X(k,:) = blk(:)';
        end
    end
end
% index of each patch in Pad image
Index = (1:par.maxrcp);
Index = reshape(Index,par.maxrp,par.maxcp);
% record the indexs of patches similar to the seed patch
blk_arr = zeros(1, par.lenrc*par.nlsp,'double');
% Patch Group Means
DC = zeros(par.ps2ch,par.lenrc*par.nlsp,'double');
% non-local patch groups
nDCnlX = zeros(par.ps2ch,par.lenrc *par.nlsp,'double');
% record the distance of compared patches to the reference patches
Vdis_rp = zeros((2*par.win+1)^2, par.lenrc);
% record the index of compared patches to the reference patches
Vidx_rp = Vdis_rp;
% Compute the Integral Image
v = Paddedim(1:par.h+par.win,1:par.w+par.win);
k = 0;
for dy = -par.win:par.win
    for dx = -par.win:par.win
        k = k + 1;
        % initial distance matrix in each iteration
        Mdis_rp = Inf*ones(par.lenr, par.lenc, 'single' );
        % Decide shift type, tx = vx+dx; ty = vy+dy
        t = zeros(size(v));
        if dx == 0 && dy == 0
            Midx_rp = Index(par.r+par.win,par.c+par.win);
            Vidx_rp(k,:) = Midx_rp(:);
            continue;
        elseif dx <= 0 && dy <= 0
            t(-dx+1:end,-dy+1:end) = v(1:end+dx,1:end+dy);
            a = 1-floor(dx/par.step);
            b = par.lenr;
            c = 1-floor(dy/par.step);
            d = par.lenc;
        elseif dx <= 0 && dy > 0
            t(-dx+1:end,1:end-dy) = v(1:end+dx,dy+1:end);
            ddy = (dy>1)*(2+ceil((dy-2)/par.step)) + (0<dy && dy<=1)*dy;
            a = 1 - floor(dx/par.step);
            b = par.lenr;
            c = 1;
            d = par.lenc - ddy;
        elseif dx > 0 && dy <= 0
            t(1:end-dx,-dy+1:end) = v(dx+1:end,1:end+dy);
            ddx = (dx>1)*(2+ceil((dx-2)/par.step)) + (0<dx && dx<=1)*dx;
            a = 1;
            b = par.lenr - ddx;
            c = 1-floor(dy/par.step);
            d = par.lenc;
        elseif dx > 0 && dy > 0
            t(1:end-dx,1:end-dy) = v(dx+1:end,dy+1:end);
            ddx = (dx>1)*(2+ceil((dx-2)/par.step)) + (0<dx && dx<=1)*dx;
            ddy = (dy>1)*(2+ceil((dy-2)/par.step)) + (0<dy && dy<=1)*dy;
            a = 1;
            b = par.lenr - ddx;
            c = 1;
            d = par.lenc - ddy;
        end
        % Create sqaured difference image
        diff = (v-t).^2;
        % Construct integral image along rows
        Sd = cumsum(diff,1);
        % Construct integral image along columns
        Sd = cumsum(Sd,2);
        % Obtaine the Square difference for each reference patches
        SqDist = Sd(par.x,par.y) + Sd(par.x0,par.y0) - Sd(par.x,par.y0) - Sd(par.x0,par.y);
        % Obtain the real value Square difference for suitable reference patches
        Mdis_rp( a:b,c:d ) = SqDist(a:b,c:d);
        % corresponding distance to reference patches
        Vdis_rp(k,:) = Mdis_rp(:);
        % corresponding index of compared patch to reference patches
        Midx_rp = Index(par.r+par.win+dx,par.c+par.win+dy);
        Vidx_rp(k,:) = Midx_rp(:);
    end
end
[~,Ind] = sort(Vdis_rp);
for i = 1:size(Vdis_rp,2)
    indc = Vidx_rp( Ind( 1:par.nlsp,i ),i );
    blk_arr(1,(i-1)*par.nlsp+1:i*par.nlsp) = indc;
    temp = X( : , indc );
    DC(:,(i-1)*par.nlsp+1:i*par.nlsp) = repmat(mean(temp,2),[1 par.nlsp]);
    nDCnlX(:,(i-1)*par.nlsp+1:i*par.nlsp) = temp - DC(:,(i-1)*par.nlsp+1:i*par.nlsp);
end
blk_arr = par.maxr*(floor(blk_arr/par.maxrp)-par.win) + mod(blk_arr,par.maxrp) - par.win;


function dt6 = dtiTalNormalize(dt6file,destinationDir)
% dt6 = dtiTalNormalize(dt6file,destinationDir)
%
% Talirach Normalization:
% Takes in a dt6 filename and scales according to the talScale factors
% stored within.
%
% Outputs new file of form originalFileName_tal.mat in destination
% directory
%
% dtiTalNormalize uses mrAnatResliceSpm
%

load(dt6file);

% Get a bounding box in talairach space that is the same size as the input
% image.
origin  = anat.xformToAcPc\[0 0 0 1]';
origin  = origin(1:3)';
bb = [-anat.mmPerVox .* (origin-1) ; anat.mmPerVox .*(size(anat.img)-origin)]';

ts = anat.talScale;
ts.talScaleDir = 'tal2acpc';
% warp T1 to tal space
ts.outMat = inv(anat.xformToAcPc);
[anat.img,newT1Xform] = mrAnatResliceSpm(anat.img, ts, bb', [1 1 1], [7 7 7 0 0 0]);

ts.outMat = inv(anat.xformToAcPc*xformToAnat);
[b0,newB0Xform]  = mrAnatResliceSpm(b0, ts, bb', mmPerVox, [7 7 7 0 0 0]);
[dt6, newDt6Xform, def] = mrAnatResliceSpm(dt6, ts, bb', mmPerVox, [1 1 1 0 0 0]);
dt6(isnan(dt6)) = 0;

%PPD Correction
dt6 = dtiXformTensorsPPD(dt6,def,1);

anat.xformToAcPc = newT1Xform;
xformToAcPc = anat.xformToAcPc*xformToAnat;

b0(isnan(b0)) = 0;
b0 = int16(b0);
[subDir subFile x xx] = fileparts(dt6file);
save([destinationDir,subFile,'_tal'],'b0','xformToAcPc','xformToAnat','anat','notes','mmPerVox','dt6');
if(nargout==0)
    clear dt6;
end
return


%% Lucas-Kanade Sparse Optical Flow
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/tapi/pyrlk_optical_flow.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/gpu/pyrlk_optical_flow.cpp>
%

function pyrlk_optical_flow_demo()
    % Prepare video source
    if true
        vid = 0;
    elseif true
        vid = fullfile(mexopencv.root(), 'test', '768x576.avi');
    elseif mexopencv.require('vision')
        vid = fullfile(toolboxdir('vision'), 'visiondata', 'visiontraffic.avi');
    end
    cap = cv.VideoCapture(vid);
    assert(cap.isOpened(), 'Failed to initialize capturing');

    % Grab first frame
    frame = cap.read();
    assert(~isempty(frame), 'Failed to read frame');
    gray0 = cv.cvtColor(frame, 'RGB2GRAY');

    % Plot
    hImg = imshow(frame);
    title('PyrLK [Sparse]')

    % Main loop
    while ishghandle(hImg)
        % Grab next frame
        frame = cap.read();
        if isempty(frame), break; end
        gray1 = cv.cvtColor(frame, 'RGB2GRAY');

        % Detect corners in previous frame
        pts0 = cv.goodFeaturesToTrack(gray0, ...
            'MaxCorners',100, 'QualityLevel',0.01, 'MinDistance',0);
        pts0 = cat(1, pts0{:});
        if isempty(pts0), continue; end

        % Compute sparse optical flow (track points from previous to current frame)
        [pts1, status] = cv.calcOpticalFlowPyrLK(gray0, gray1, pts0);
        pts1 = cat(1, pts1{:});
        status = logical(status);

        % Draw sparse flow (only good points for which flow was found)
        if any(status)
            frame = drawArrows(frame, pts0(status,:), pts1(status,:));
        end

        % Display result
        set(hImg, 'CData',frame);
        drawnow;

        % Next iteration
        gray0 = gray1;
        pts0 = pts1;
    end
    cap.release();
end

%% Helper function

function img = drawArrows(img, pts0, pts1)
    %DRAWARROWS  Draw sparse optical flow
    %
    % See also: quiver
    %

    % compute angle and hypotenuse
    uv = pts0 - pts1;
    ang = atan2(uv(:,2), uv(:,1));
    mag = hypot(uv(:,2), uv(:,1));

    % skip short ones
    idx = (mag < 1.0);
    if all(idx), return; end
    pts0(idx,:) = [];
    pts1(idx,:) = [];
    ang(idx,:) = [];
    mag(idx,:) = [];

    % Options for line drawing
    props = {'Color',[255 0 0], 'Thickness',1};

    % Here we lengthen the arrow by a factor of three,
    % and draw the main line of the arrow
    pts1 = pts0 - 3 * bsxfun(@times, mag, [cos(ang) sin(ang)]);
    img = cv.line(img, pts0, pts1, props{:});

    % Now we draw the tips of the arrow. We do some scaling so that the
    % tips look proportional to the main line of the arrow
    pts0 = pts1 + 9 * [cos(ang + pi/4) sin(ang + pi/4)];
    img = cv.line(img, pts0, pts1, props{:});
    pts0 = pts1 + 9 * [cos(ang - pi/4) sin(ang - pi/4)];
    img = cv.line(img, pts0, pts1, props{:});
end


%filename = 'CantileverSquareSYmmetricMesh';
%filename = 'CantileverSquareMedium';
%'CantileverSquareSYmmetricMesh';
%filename = 'CantileverSquare';
filename = 'CantileverSquareSmall';
%filename = 'CantileverSquareNew';
%'CantileverSquareNewFine';
%filename = 'Cantilever_quad_coarse';
%filename = 'LshapeTriFine';
%filename = 'LshapeTri';
%filename = 'LshapeTriSmall';
%filename = 'Lshape';
%filename = 'LshapeFine';
%filename = 'ArchTriFine';
%'Arch_quad_coarse';
%'Bridge_quad_coarse';
%'BridgeCool_Quadrilateral_Bilinear_Structured_Coarse';
%filename = 'Bridge';

ptype = 'MACRO';
initial_case = 'given';
m1 = 0.0101;
m2 = 0.0101;
cost = {'compliance'};
%cost = {'stressNorm'};
%cost = {'stressNorm','compliance'};
%cost = {'stressNorm','compliance'};
%weights = [1,10];
weights = 1;
constraint = {'volumeConstraint'};
filterType = 'PDE';
%filterType = 'P1';
constraint_case = 'EQUALITY';

Vfrac_initial = 0.3;
optimality_initial = 1e-5;
constr_initial = 1e-5;

Vfrac_final = 0.3;
optimality_final = 1e-5;
constr_final = 1e-5;

stressNormExponent_initial = 2;
stressNormExponent_final = 16;
%
optimizer = 'DualNestedInPrimal';
%optimizer = 'AlternatingPrimalDual';
optimizerUnconstrained = 'PROJECTED GRADIENT';

%optimizer = 'MMA';
%optimizer = 'IPOPT';

designVariable = 'MicroParams';
ub = 0.989;
lb = 0.011;
homegenizedVariablesComputer = 'ByVademecum';
%
%vademecumFileName = 'SuperEllipseQMax';
%vademecumFileName = 'SuperEllipseQ2';
vademecumFileName = 'SuperEllipseQOptAnalytic';

% designVariable = 'LevelSet';
% homegenizedVariablesComputer = 'ByInterpolation';
% method = 'SIMPALL';
% materialType = 'ISOTROPIC';
% initial_case = 'full';% optimizerUnconstrained = 'SLERP';

% designVariable = 'Density';
% homegenizedVariablesComputer = 'ByInterpolation';
% method = 'SIMPALL';
% materialType = 'ISOTROPIC';
% initial_case = 'full';
% rho0 = 0.3;

line_search_initiator = 'INCREASING LAST STEP';
incrementFactor = 1.05;
%

%kfrac = 2;
nsteps = 1;%17;

plotting = true;
printing = true;
monitoring = true;
monitoring_interval = 1;
maxiter = 800;

%
% % %
% isDirichletPartX = @(x) x > -1e-12 & x < 0.1;
% isDirichletPart1 = @(y) y > 0.20 & y < 0.30;
% isDirichletPart2 = @(y) y > 0.70 & y < 0.80;
% isDirichletPartY = @(y) isDirichletPart1(y) | isDirichletPart2(y);
% isDirichletPart = @(x,y) isDirichletPartX(x) & isDirichletPartY(y);
%  isNeumannPartX = @(x) x > (0.96) & x < (1+1e-12);
%  isNeumannPartY = @(y) y > 0.35 & y < 0.5;
%
%  isCornerX = @(x) x > (0.4 - 0.05) & x < (0.4 +0.05);
%  isCornerY = @(y) y > (0.4 - 0.05) & y < (0.4 +0.05);
%  isCornerPart = @(x,y) isCornerX(x) & isCornerY(y);
% %
%  isNeumannPart = @(x,y) isNeumannPartX(x) & isNeumannPartY(y);
% % iNotOptimizable = @(coord) isDirichletPart(coord(:,1),coord(:,2)) & ~isNeumannPart(coord(:,1),coord(:,2));
%
% iNotOptimizable = @(coord) isCornerPart(coord(:,1),coord(:,2)) | isNeumannPart(coord(:,1),coord(:,2));
%
% costDomainNotOptimizable       = {iNotOptimizable};
% constraintDomainNotOptimizable = {[]};
%
% isDesignVariableFixed.nodes  = iNotOptimizable;
% isDesignVariableFixed.values = @(x) [m1*ones(size(x,1),1);m2*ones(size(x,1),1)];


%%***********************************************************************
%% nzlist: find the combined list of non-zero elements
%%         of Aj, j = 1:k, for each k,
%%         assuming that the Aj's are permuted such that
%%         A1 has the fewest nonzero elements, followed by A2, and so on.
%%
%% [isspA,nzlistA,nzlistAsum,isspAy,nzlistAy] = nzlist(blk,At,par)
%%
%%  isspA(p,k) = 1 if Apk is sparse, 0 if it is dense.
%%  nzlistA = px2 cell array.
%%            nzlistA{p,1}(k) is the starting row index (in C convention)
%%            in the 2-column matrix nzlistA{p,2} that
%%            stores the row and column index of the nonzero elements
%%            of Apk.
%%            nzlistA{p,1}(k) = inf if nnz(Apk) exceeds given threshold.
%%  nzlistAsum = px2 cell array.
%%            nzlistA{p,1}(k) is the starting row index (in C convention)
%%            in the 2-column matrix nzlistA{p,2} that
%%            stores the row and column index of the nonzero elements
%%            of Apk that are not already present
%%            in the combined list from Ap1+...Ap,k-1.
%%  nzlistAy = px1 cell array.
%%            nzlistAy{p} is a 2-column matrix that stores the
%%            row and column index of the nonzero elements of
%%            Ap,1+.... Ap,m.
%%            nzlistAy{p} = inf  if the number of nonzero elements
%%            exceeds a given threshold.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

  function [isspA,nzlistA,nzlistAsum,isspAy,nzlistAy] = nzlist(blk,At,par)

  spdensity   = par.spdensity;
  smallblkdim = par.smallblkdim;
  m = par.numcolAt;
%%
  numblk = size(blk,1);
  isspA = zeros(numblk,m);
  nzlistA = cell(numblk,2);  nzlistAsum = cell(numblk,2);
  isspAy = zeros(numblk,1);  nzlistAy = cell(numblk,1);
%%
  for p = 1:size(blk,1)
     pblk = blk(p,:);
     if strcmp(pblk{1},'s') & ((max(pblk{2}) > smallblkdim) | (length(pblk{2}) <= 10))
        numblk = length(pblk{2});
        n = sum(pblk{2});
        n2 = sum(pblk{2}.*pblk{2});
        if (numblk == 1)
           nztol = spdensity*n;
           nztol2 = spdensity*n2/2;
        else
           nztol = spdensity*n/2;
           nztol2 = spdensity*n2/4;
        end
        nzlist1 = zeros(1,m+1); nzlist2 = [];
        nzlist3 = zeros(1,m+1); nzlist4 = []; breakyes = zeros(1,2);
        Asum = sparse(n,n);
%%
        m1 = size(At{p,1},2);
        for k = 1:m1
           Ak = mexsmat(blk,At,1,p,k);
           nnzAk = nnz(Ak);
           isspA(p,k) = (nnzAk < spdensity*n2) | (numblk > 1);
           if ~all(breakyes)
              [I,J] = find(abs(Ak) > 0);
              %%
              %% nonzero elements of Ak.
              %%
   	      if (breakyes(1) == 0);
                 if (nnzAk <= nztol)
                    idx = find(I<=J);
                    nzlist1(k+1) = nzlist1(k)+length(idx);
                    nzlist2 = [nzlist2; [I(idx), J(idx)] ];
                 else
                    nzlist1(k+1:m+1) = inf*ones(1,m-k+1);
                    breakyes(1) = 1;
                 end
              end
              %%
	      %% nonzero elements of |A1|+...+|Ak|.
              %%
	      if (breakyes(2) == 0)
                 nztmp = zeros(length(I),1);
                 for t = 1:length(I);
                    i=I(t); j=J(t); nztmp(t)=Asum(i,j);
                 end
                 %% find new nonzero positions when Ak is added to Asum.
                 idx = find(nztmp == 0);
                 nzlist3(k+1) = nzlist3(k) + length(idx);
                 if (nzlist3(k+1) < nztol2);
                    nzlist4 = [ nzlist4; [I(idx), J(idx)] ];
                 else
                    nzlist3(k+1:m+1) = inf*ones(1,m-k+1);
                    breakyes(2) = 1;
                 end
                 Asum = Asum+abs(Ak);
              end
           end
        end
	if (numblk == 1)
   	   isspAy(p,1) = (nzlist1(m+1) < inf) | (nzlist3(m+1) < inf);
        else
   	   isspAy(p,1) = 1;
        end
	nzlistA{p,1} = nzlist1;    nzlistA{p,2} = nzlist2;
    	nzlistAsum{p,1} = nzlist3; nzlistAsum{p,2} = nzlist4;
        %%
        %% nonzero elements of (A1*y1+...Am*ym).
        %%
        if (nzlist3(m+1) < inf);
           if (length(pblk) > 2)
              m2 = length(pblk{3});
              len = sum(pblk{3});
              DD = spconvert([At{p,3}(:,2:4); len, len, 0]);
              Asum = Asum + abs(At{p,2}*DD*At{p,2}');
           end
           [I,J] = find(Asum > 0);
           if (length(I) < nztol2)
              nzlistAy{p} = [I, J];
           else
              nzlistAy{p} = inf;
           end
        else
           nzlistAy{p} = inf;
        end
     end
  end
%%***********************************************************************


 function [T, reuse] = Gdsft_gram(ob, W, reuse)
%function [T, reuse] = Gdsft_gram(A, W, reuse)
%|
%| Construct Toeplitz gram matrix object T = A'WA for a Gdsft object.
%| This object performs T*x rapidly using an over-sampled FFT.
%|
%| in
%|	A	[M np]		Gdsft object (fatrix2)
%|	W	[M M]		W = Gdiag() for fatrix2 (often simply "1" or [])
%|				assume W=diag(wi) with real wi so T is Hermitian
%|	reuse	struct		stuff from the last call that can be reused
%|
%| out
%|	T	[np np]		fatrix2 object
%|	reuse	struct		stuff that can be reused on future calls
%|
%| Copyright 2012-06-05, Jeff Fessler, University of Michigan

if nargin == 1 && streq(ob, 'test'), Gdsft_gram_test, return, end
if nargin == 1 && streq(ob, 'test1'), Gdsft_gram_test1, return, end
if nargin == 1 && streq(ob, 'test2'), Gdsft_gram_test2, return, end
if nargin == 1 && streq(ob, 'test3'), Gdsft_gram_test3, return, end
if nargin < 2 || nargin > 3, ir_usage, end

if ~isvar('reuse'), reuse = []; end

if ~isa(ob, 'fatrix2')
	fail('A wrong class: %s', class(ob))
end

[nd np] = size(ob);
forw = @(arg, x) Gdsft_gram_mult(x, arg.fftkern, arg.mask);

% extract wi
if isnumeric(W) % includes case where W is empty
	wi = W;
elseif isa(W, 'fatrix2') && streq(W.caller, 'Gdiag')
	wi = W.arg.diag;
else
	error 'W must be diag_sp or Gdiag or wi array'
end
clear W

if isempty(wi)
	wi = ones(nd, 1); % the usual unweighted case
end

if isscalar(wi)
	wi = wi * ones(nd, 1);
end

if isreal(wi)
	back = forw; % trick: because Hermitian!
else
	fail('not implemented for complex wi')
end

arg = ob.arg;
arg.mask = ob.imask_array;
[arg.fftkern arg.reuse] = Gdsft_gram_init(arg, wi, reuse, ob.idim);

if any(~arg.mask(:))
	omask = arg.mask;
else
	omask = []; % trick: omask not needed in full case
end
T = fatrix2('arg', arg', ...
	'idim', ob.idim, 'imask', arg.mask, ...
	'odim', ob.idim, 'omask', omask, ...
	'forw', forw, 'back', back);

% Gdsft_gram_init()
% construct kernel of circulant matrix into which T is embedded
% and take its DFT to prepare for multiplication.
% out
%	fftkern [[2Nd]]		FFT of kernel of circulant matrix
%
function [fftkern, reuse] = Gdsft_gram_init(arg, wi, reuse, Nd)

switch numel(Nd)
case 1
	[fftkern reuse] = Gdsft_gram_init1(arg, wi, reuse, Nd, arg.show);
case 2
	[fftkern reuse] = Gdsft_gram_init2(arg, wi, reuse, Nd, arg.show);
case 3
	[fftkern reuse] = Gdsft_gram_init3(arg, wi, reuse, Nd, arg.show);
otherwise
	fail('dim %d not done', numel(Nd))
end

% Gdsft_gram_init1()
% 1d filter for circulant matrix
% note: only toeplitz kernel values from -(N-1) to (N-1) are relevant
% so the value at +/- N does not matter so we set it to zero.
function [fftkern, reuse] = Gdsft_gram_init1(arg, wi, reuse, N1, show)

if isempty(reuse)
	reuse.G1 = Gdsft(arg.om_t', arg.Nd); % with n_shift = 0
end

block1 = reuse.G1' * real(wi); % kludge

% kernel of Toeplitz matrix from -N to N-1 but with fftshift
% This is inherently Hermitian symmetric except for the middle [0] value.
% Use 0 for the irrelevant value at -N.
err1 = abs(imag(block1(1))) / abs(block1(1));
tol = 0;
if err1 > tol
	printm('removing imaginary h[0] part of relative size %g', err1)
	block1(1) = real(block1(1));
end
kern = [block1; 0; flipud(conj(block1(2:N1)))]; % [2*N1]
if show
	kern = dsft_gram_hermitify(kern, show); % need not due to block1(1) fix
end
fftkern = fft(kern); % [2*N1]

% Gdsft_gram_init2()
function [fftkern, reuse] = Gdsft_gram_init2(arg, wi, reuse, Nd, show)

if isempty(reuse)
	[reuse.G1 reuse.G2] = Gdsft_gram_setup2(arg);
end

N1 = Nd(1);
N2 = Nd(2);

block1 = reshape(reuse.G1' * real(wi), Nd); % kludge
block2 = reshape(reuse.G2' * real(wi), Nd);

z1 = zeros(N1,1);
z2 = zeros(N1-1,1);
kern = [
	[block1 z1 conj(fliplr([block1(1,2:N2); block2(2:N1,2:N2)]))];
	zeros(1,2*N2);
	[flipud(block2(2:N1,:)) z2 fliplr(flipud(conj(block1(2:N1, 2:N2))))]
]; % [(2Nd)]
kern = dsft_gram_hermitify(kern, show); % force Hermitian symmetry
fftkern = fftn_fast(kern);

% Gdsft_gram_init3()
function [fftkern reuse] = Gdsft_gram_init3(arg, wi, reuse, Nd, show)
if isempty(reuse)
	[reuse.G1 reuse.G2 reuse.G3 reuse.G4 ] = build_G1_G2_G3_G4(arg);
end
N1 = Nd(1);
N2 = Nd(2);
N3 = Nd(3);

filtblk1 = reshape(reuse.G1' * real(wi), Nd);
filtblk2 = reshape(reuse.G2' * real(wi), Nd);
filtblk3 = reshape(reuse.G3' * real(wi), Nd);
filtblk4 = reshape(reuse.G4' * real(wi), Nd);

tblk1 = filtblk1;
tblk2 = filtblk2(2:N1,:,:);	% remove the duplicated part with filtblk1
tblk3 = filtblk3(2:N1,2:N2,:);	% remove the duplicated part with block 1, 2, 4
tblk4 = filtblk4(:,2:N2,:);	% remove the duplicated part with block 1

z1 = zeros(N1,1,N3);
z2 = zeros(N1-1,1,N3);

% top half of the 3D filter
kern_top = [
	tblk1 z1 flipdim(tblk4,2);	% upper block
	zeros(1,2*N2,N3);		% zero padding in the middle
	flipdim(tblk2,1) z2 flipdim(flipdim(tblk3,1),2) % lower block
];

% construct the bottom half now
bblk1 = flipdim(conj(filtblk3(:,:,2:N3)),3);
bblk2 = flipdim(conj(filtblk4(:,:,2:N3)),3);
bblk2 = bblk2(2:N1,:,:);
bblk3 = flipdim(conj(filtblk1(:,:,2:N3)),3);
bblk3 = bblk3(2:N1,2:N2,:);
bblk4 = flipdim(conj(filtblk2(:,:,2:N3)),3);
bblk4 = bblk4(:,2:N2,:);

z4 = zeros(N1,1,N3-1);
z5 = zeros(N1-1,1,N3-1);
kern_bottom = [
	bblk1 z4 flipdim(bblk4,2);
	zeros(1,2*N2,N3-1);
	flipdim(bblk2,1) z5 flipdim(flipdim(bblk3,1),2)
];

kern = cat(3, kern_top, zeros(2*N1,2*N2,1));
kern = cat(3, kern, kern_bottom);

kern = dsft_gram_hermitify(kern, show); % force Hermitian symmetry
fftkern = fftn_fast(kern);

% Gdsft_gram_setup2()
% modified versions of A for 2D
% this routine is quite tricky.  nothing new is really rebuilt here,
% but some structure values are changed.
function [G1, G2] = Gdsft_gram_setup2(arg)
om = arg.om_t';
Nd = arg.Nd;
G1 = Gdsft(om, Nd);
om(:,1) = -om(:,1);
G2 = Gdsft(om, Nd);

% build_G1_G2_G3_G4()
% Created by Daehyun Yoon for 3D case
% build more Gnufft objects, with negative om.
function [G1, G2, G3, G4] = build_G1_G2_G3_G4(arg)
om = arg.om_t';
Nd = arg.Nd;
G1 = Gdsft(om, Nd);
om(:,1) = -om(:,1);
G2 = Gdsft(om, Nd);
om(:,2) = -om(:,2);
G3 = Gdsft(om, Nd);
om(:,1) = -om(:,1);
G4 = Gdsft(om, Nd);

% Gdsft_gram_mult()
% multiply an image x by a toeplitz matrix
% by embedding it into a circulant matrix of twice the size and using FFT.
% in
%	x	[(Nd)]
%	fftkern	[[2Nd]]
%	mask	[(Nd)]
% out
%	y	[(Nd)]
function y = Gdsft_gram_mult(x, fftkern, mask)

N2 = size(fftkern);
if numel(N2) == 2 && N2(2) == 1
	N2 = N2(1);
end
Nd = N2 / 2;

LL = size(x,1+numel(N2));
y = zeros([Nd LL]);

switch numel(N2)
case 1
	tmp = fft(x, N2); % [N2 L]
	tmp = tmp .* repmat(fftkern, [1 ncol(tmp)]);
	y = ifft(tmp);
	y = y(1:Nd,:); % [Nd L]
	y = y .* repmat(mask, [1 ncol(y)]); % fatrix2 requires mask

case 2
	for ll=1:LL
		tmp = ifftn_fast(fftkern .* fftn_fast(x(:,:,ll), N2));
		tmp = tmp(1:Nd(1), 1:Nd(2));
		tmp = tmp .* mask; % fatrix2 requires mask
		y(:,:,ll) = tmp;
	end

case 3
	for ll=1:LL
		tmp = ifftn_fast(fftkern .* fftn_fast(x(:,:,:,ll), N2));
		tmp = tmp(1:Nd(1), 1:Nd(2), 1:Nd(3));
		tmp = tmp .* mask; % fatrix2 requires mask
		y(:,:,:,ll) = tmp;
	end

otherwise
	fail('dim %d not done', numel(N2))
end

% Gdsft_gram_test1()
% test 1d version
function Gdsft_gram_test1
N = 16;
M = 21;
omega = 2*pi*rand(M,1);
wi = [1:size(omega,1)]';
mask = true(N,1); mask(end-3) = false;
A = Gdsft(omega, N, 'mask', mask, 'n_shift', N/2, 'show', 1);
T = build_gram(A, wi);
test_adjoint(T, 'complex', 1, 'tolre', 1e-7);
fatrix2_tests(A, 'complex', 1, 'tol_gram', 2e-7);

% Gdsft_gram_test2()
function Gdsft_gram_test2
N = [16 14];
fov = N;
ktype = 'spiral0';
[kspace omega wi] = mri_trajectory(ktype, {}, N, fov, {'voronoi'});
ig = image_geom('nx', N(1), 'ny', N(2), 'dx', 1);
mask = ellipse_im(ig, [0 0 14 15 0 1], 'oversample', 3) > 0;
A = Gdsft(omega, N, 'n_shift', N/2, 'show', 1);
fatrix2_tests(A, 'complex', 1, 'tol_gram', 2e-7);

T = build_gram(A, wi);
fatrix2_tests(T, 'complex', 1, 'tol_gram', 1e-7);
test_adjoint(T, 'complex', 1, 'tolre', 1e-7); % matches!

if 1
	x = ellipse_im(ig, 'shepplogan-emis');
	x1 = A' * (wi .* (A * x));
	x1 = embed(x1, mask);
	x2 = T * x;
	equivs(x1, x2, 'thresh', 2e-7)
end

tic
for ii=1:50, b1 = embed(A' * (wi .* (A * x(mask))), mask); end
t1 = toc;
tic
for ii=1:50, b2 = embed(T * x(mask), mask); end
t2 = toc;
equivs(b1, b2)
printm('time: A''Ax = %.3f, Tx=%.3f', t1, t2)

% Gdsft_gram_test3()
function Gdsft_gram_test3
N = [8 4 2];
rng(0)
omega = rand(21,3) * 2 * pi;
A = Gdsft(omega, N, 'n_shift', N/2, 'show', 1);
fatrix2_tests(A, 'complex', 1, 'tol_gram', 2e-7);
test_adjoint(A, 'complex', 1, 'big', 1);

wi = [1:size(omega,1)]';
T = build_gram(A, wi);
fatrix2_tests(T, 'complex', 1, 'tol_gram', 1e-7);
test_adjoint(T, 'complex', 1, 'tolre', 1e-7);

if 1
	x = cumsum(ones(N), 2);
	x1 = A' * (wi .* (A * x));
	x1 = iembed(A, x1);
	x2 = T * x;
	equivs(x1, x2)
end

% Gdsft_gram_test()
function Gdsft_gram_test
Gdsft_gram_test1
Gdsft_gram_test2
Gdsft_gram_test3


function gpsdata_all=loadAllGPSData(gpsfile, gpsSE, coordinateStyle)
% load rtklib output of gps solutions, input gpsfile name, gps start and
% end time of gps TOW, gpspostype are as described in readgpsheader
% output: Nx9 gps entries in the session gpsSE, each entry is
% GPS TOW, ECEF XYZ, Q and no of satels and sdx sdy sdz

% if coordinateStyle == lla, the first four columns will be
% GPS TOW, lat lon ellipsoid height.
if nargin < 3
    coordinateStyle = 'ecef';
end

gpsdata_all=zeros( ceil((gpsSE(2)- gpsSE(1)+100)*5), 12);
num_entries=0;
[fgps, gpsdata, gpspostype]=readgpsheader(gpsfile, gpsSE(1));
num_entries=num_entries+1;
gpsdata_all(num_entries, :) = gpsdata';
while(1)
    [fgps, gpsdata]=grabnextgpsdata(fgps, gpspostype);
    if(gpsdata(1)==inf || gpsdata(1)> gpsSE(2))
        break;
    else
        num_entries=num_entries+1;
        gpsdata_all(num_entries, :) = gpsdata';
    end
end
fclose(fgps);
gpsdata_all= gpsdata_all(1:num_entries, :);
if strcmp(coordinateStyle, 'lla')
    gpsdata_all(:, 2:4) = ecef2lla(gpsdata_all(:, 2:4));
    gpsdata_all(:, 2:3) = gpsdata_all(:, 2:3) * pi / 180;
end
end


function [hhist,hax,hfig,hleg,hxlabel,hylabel,frac,frac_outside,edges,centers_plot] = ...
  HistogramPerFrameFeature(data,perframefn,varargin)

[hax,hfig,...
  minprctile,maxprctile,minv,maxv,...
  edges,nbins,...
  binmode,...
  labelcolors,unknowncolor,...
  frac,...
  frac_outside,...
  centers_plot] = ...
  myparse(varargin,'axes',[],'figure',[],...
  'minprctile',[],'maxprctile',[],...
  'minv',[],'maxv',[],...
  'edges',[],...
  'nbins',100,...
  'binmode','linear',...
  'labelcolors',[],...
  'unknowncolor',[1,1,1],...
  'frac',[],...
  'frac_outside',[],...
  'centers_plot',[]);

nbehaviors = data.nbehaviors;

if isempty(hax),
  if isempty(hfig),
    hfig = figure;
  end
  hax = gca;
end

holdstate = ishold(hax);
hold(hax,'off');

docompute = isempty(frac) || isempty(frac_outside) || ...
  isempty(edges) || isempty(centers_plot);

if docompute,

  perframeidx = find(strcmpi(perframefn,data.allperframefns),1);
  if isempty(perframeidx),
    error('Unknown perframe field %s',perframefn);
  end

  % use current fly to choose hist edges
  if isempty(edges),
    x = data.perframedata{perframeidx};
    x = x(~isnan(x));
    if isempty(minv),
      if isempty(minprctile),
        minv = min(x);
      else
        minv = prctile(x,minprctile);
      end
    end
    if isempty(maxv),
      if isempty(minprctile),
        maxv = max(x);
      else
        maxv = prctile(x,maxprctile);
      end
    end
    [edges,centers] = SelectHistEdges(nbins,[minv,maxv],binmode);
  else
    centers = (edges(1:end-1)+edges(2:end))/2;
    nbins = numel(centers);
  end

  counts = zeros(nbehaviors+1,nbins);
  counts_outside = zeros(nbehaviors+1,2);
  n = zeros(nbehaviors+1,1);

  for expi = 1:data.nexps,

    % load per-frame data for this experiment
    perframedir = data.GetFile('perframedir',expi);
    file = fullfile(perframedir,[perframefn,'.mat']);
    if ~exist(file,'file'),
      warning('Per-frame data file %s does not exist',file);
      continue
    end
    perframedata = load(file);

    % TODO: extend to multiple flies
    for fly = 1:data.nflies_per_exp(expi),

      x = perframedata.data{fly}(:)';

      % load labels for this fly and experiment
      if expi == data.expi && all(fly == data.flies),
        % use current labelidx
        labelidx = data.labelidx.vals;
      else
        labelidx = data.GetLabelIdx(expi,fly);
        labelidx = labelidx.vals;
      end
      noff = numel(labelidx) - numel(x);
      noff_start = floor(noff/2);
      noff_end = noff - noff_start;
      labelidx = labelidx(1+noff_start:end-noff_end);

      % histogram
      for i = 0:nbehaviors,
        idx = ~isnan(x) & labelidx == i;
        if ~any(idx), continue; end
        n(i+1) = n(i+1) + nnz(idx);
        counts_curr = histc(x(idx),edges);
        counts(i+1,:) = counts(i+1,:) + counts_curr(1:end-1);
        counts_outside(i+1,1) = counts_outside(i+1,1) + nnz(x(idx)<edges(1));
        counts_outside(i+1,2) = counts_outside(i+1,1) + nnz(x(idx)>edges(end));
      end
    end
  end

  frac = bsxfun(@rdivide,counts,n);
  frac_outside = bsxfun(@rdivide,counts_outside,n);

  centers_plot = [2*centers(1)-centers(2),centers,2*centers(end)-centers(end-1)];

end

hhist = zeros(1,nbehaviors+1);

if size(labelcolors,1) < nbehaviors,
  labelcolors(size(labelcolors,1)+1:nbehaviors,:) = jet(nbehaviors-size(labelcolors,1));
end

for i = 1:nbehaviors+1,
  hhist(i) = plot(hax,centers_plot,...
    [frac_outside(i,1),frac(i,:),frac_outside(i,2)],...
    '-','linewidth',2);
  if i == 1,
    set(hhist(i),'Color',unknowncolor);
    hold(hax,'on');
  else
    set(hhist(i),'Color',labelcolors(i-1,:));
  end
end

axisalmosttight(1/20,hax);
xtick = get(hax,'XTick');
xticklabel = get(hax,'XTickLabel');
xticklabel = cellstr(xticklabel);
i = find(xtick >= centers_plot(5),1);
if ~isempty(i),
  xtick = xtick(i:end);
  xticklabel = xticklabel(i:end);
end
i = find(xtick <= centers_plot(end-4),1,'last');
if ~isempty(i),
  xtick = xtick(1:i);
  xticklabel = xticklabel(1:i);
end
xtick = xtick(xtick>centers_plot(1));
xtick = xtick(xtick<centers_plot(end));
if centers_plot(1)==centers_plot(end)
  xtick = [centers_plot(1)];
else
  xtick = [centers_plot(1),xtick,centers_plot(end)];
end
xticklabel = [{sprintf('<%.1f',edges(1))};xticklabel;{sprintf('>=%.1f',edges(end))}];
set(hax,'XTick',xtick,'XTickLabel',xticklabel);

s = [{'Unknown'},data.labelnames];
hleg = legend(hax,hhist(n>0),s(n>0));
hxlabel = xlabel(hax,perframefn,'Interpreter','none');
hylabel = ylabel(hax,'Fraction of frames');

if ~holdstate,
  hold(hax,'off');
end


%--- help for ts/aggregate ---
%
%  Convert time series to a lower frequency
%
%  ::
%
%     this=aggregate(this,newfreq);
%     this=aggregate(this,newfreq,method);
%
%  Args:
%     this (ts object): time series object
%     newfreq (char): frequency to convert the data to. It must be a lower than the frequency of the data in **this**. Available frequencies are
%
%        - 'Q': quarterly
%        - 'M': monthly
%        - 'H': semiannual
%        - 'W': weekly
%        - 'D': daily
%
%     method ('interpolation' , 'distribution'): method of conversion (default: 'distribution')
%
%        - 'interpolation': Use the value corresponding to the first date of the coarse frequency
%        - 'distribution': Use the average of all observations in the sampling frequency
%
%  Returns:
%     :
%
%     - **this** (ts object): time series object
%
%