% OP_GRADSYMU_V_OTIMES_N: assemble the matrix A = [A(i,j)], A(i,j) = (epsilon (gradsym u)_j, (v \otimes n)_i ).
%
%   A = op_gradsymu_v_otimes_n (spu, spv, msh, coeff);
%
% INPUT:
%
%   spu:   structure representing the space of trial functions (see sp_vector/sp_eval_boundary_side)
%   spv:   structure representing the space of test functions (see sp_vector/sp_eval_boundary_side)
%   msh:   structure containing the domain partition and the quadrature rule for the boundary,
%           since it must contain the normal vector (see msh_cartesian/msh_eval_boundary_side)
%   coeff: vector-valued function f, evaluated at the quadrature points
%
% OUTPUT:
%
%   A: assembled matrix
%
% Copyright (C) 2015, 2017, 2020 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function varargout = op_gradsymu_v_otimes_n (spu, spv, msh, coeff)

  gradu = reshape (spu.shape_function_gradients, spu.ncomp, [], ...
		   msh.nqn, spu.nsh_max, msh.nel);

  shpv = reshape (spv.shape_functions, spv.ncomp, msh.nqn, spv.nsh_max, msh.nel);

  ncomp = size (gradu, 1);
  ndir = size (gradu, 2);

  rows = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  cols = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  values = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);

  jacdet_weights = msh.jacdet .* msh.quad_weights .* coeff;

  ncounter = 0;
  for iel = 1:msh.nel
    if (all (msh.jacdet(:, iel)))
      gradu_iel = reshape (gradu(:,:,:,:,iel), spu.ncomp, ndir, msh.nqn, spu.nsh_max);
      gradu_iel = 0.5 * (gradu_iel + permute(gradu_iel, [2 1 3 4]));
      gradu_iel = reshape (gradu_iel, spu.ncomp*ndir, msh.nqn, 1, spu.nsh_max);
      shpv_iel = reshape (shpv(:, :, :, iel), spv.ncomp, msh.nqn, spv.nsh_max);

      v_otimes_n_iel = zeros (ncomp, ndir, msh.nqn, spv.nsh_max);
      normal_iel = msh.normal (:, :, iel);
      for ii = 1:spv.ncomp
        for jj = 1:ndir
          v_otimes_n_iel(ii,jj,:,:) = bsxfun (@times, shpv_iel(ii,:,:), normal_iel(jj,:));
        end
      end
      v_otimes_n_iel = reshape (v_otimes_n_iel, ncomp*ndir, msh.nqn, spv.nsh_max, 1);

      jacdet_iel = reshape (jacdet_weights(:,iel), [1, msh.nqn, 1]);
      v_oxn_times_jw = bsxfun (@times, jacdet_iel, v_otimes_n_iel);
      tmp1 = sum (bsxfun (@times, v_oxn_times_jw, gradu_iel), 1);
      elementary_values = reshape (sum (tmp1, 2), spv.nsh_max, spu.nsh_max);

      [rows_loc, cols_loc] = ndgrid (spv.connectivity(:,iel), spu.connectivity(:,iel));
      indices = rows_loc & cols_loc;
      rows(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = rows_loc(indices);
      cols(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = cols_loc(indices);
      values(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = elementary_values(indices);
      ncounter = ncounter + spu.nsh(iel)*spv.nsh(iel);

    else
      warning ('geopdes:jacdet_zero_at_quad_node', 'op_gradsymu_v_otimes_n: singular map in element number %d', iel)
    end
  end

  if (nargout == 1 || nargout == 0)
    varargout{1} = sparse (rows(1:ncounter), cols(1:ncounter), ...
                           values(1:ncounter), spv.ndof, spu.ndof);
  elseif (nargout == 3)
    varargout{1} = rows(1:ncounter);
    varargout{2} = cols(1:ncounter);
    varargout{3} = values(1:ncounter);
  else
    error ('op_gradsymu_v_otimes_n: wrong number of output arguments')
  end

end


function a = gproc(hyper)

%=============================================================================
% GPROC Gaussian-Process object
%=============================================================================
%
% A=GPROC(hyper) returns a GPROC object initialized with hyperparameters hyper.
% minimize log-likelihood for length runs
%
%
%  Hyperparameters, and their defaults
%
%
%   child=kernel;        -- the kernel is stored as a member called "child"
%   length=50            -- default maximum iteration of line searches.
%
%  Model
%  H			-- covariance hyperparameters
%
%  Methods:
%  d=gen(toyreg({'o=1','n=2','l=200'}))
%
%  [r,a]=train(gproc,get(d,1:100))
%  r=test(a,get(d,101:200))
%  % Note that r.X has 2 an extra output columns!
%
%   train, test
%=============================================================================
% Reference : Introduction to gaussian processes
% Author    : David J.C. MacKay
% Link      : http://www.inference.phy.cam.ac.uk/mackay/gpB.ps.gz
%=============================================================================

  % hyperparams
  a.input= [];
  a.target = [];
  a.length=50;
  a.H=[];
  a.pred_var=0;
  % model

  p=algorithm('gproc');
  a= class(a,'gproc',p);
  a.algorithm.use_signed_output=0;

  if nargin==1
    eval_hyper;
  end;


function y = norms( x, p, dim )

%NORMS   Internal cvx version.

%
% Check second argument
%

error( nargchk( 1, 3, nargin ) );
if nargin < 2 || isempty( p ),
    p = 2;
elseif ~isnumeric( p ) || numel( p ) ~= 1 || ~isreal( p ),
    error( 'Second argument must be a real number.' );
elseif p < 1 || isnan( p ),
    error( 'Second argument must be between 1 and +Inf, inclusive.' );
end

%
% Check third argument
%

sx = size( x );
if nargin < 3 || isempty( dim ),
    dim = cvx_default_dimension( sx );
elseif ~cvx_check_dimension( dim, false ),
    error( 'Third argument must be a valid dimension.' );
end
if isempty( x ) || length( sx ) < dim || sx( dim ) == 1,
    p = 1;
end

%
% Type check
%

persistent remap1 remap2
if isempty( remap2 ),
    remap1 = cvx_remap( 'constant', 'log-convex' );
    remap2 = cvx_remap( 'affine', 'log-convex' );
end
xc = reshape( cvx_classify( x ), sx );
if ~all( remap2( xc( : ) ) ),
    error( 'Disciplined convex programming error:\n   Invalid computation: norms( {%s}, ... )', cvx_class( x, true, true ) );
end

%
% Compute norms
%

switch p,
    case 1,
        y = sum( abs(x), dim );
    case Inf,
        y = max( abs(x), [], dim );
    otherwise,
        nd = length( sx );
        nx = sx( dim );
        sy = sx;
        sy( dim ) = 1;
        tt = all( remap1( xc ), dim );
        if all( tt( : ) ),
            y = sum( abs(x) .^ p, dim ) .^ (1/p);
        elseif any( tt( : ) ),
            if dim ~= 1,
                perm = [ dim, 1:dim-1, dim+1:length(sx) ];
                x  = permute( x,  perm );
                tt = permute( tt, perm );
                sy = sy( perm );
            else
                perm = [];
            end
            nv = prod( sy );
            x  = reshape( x, nx, nv );
            y  = cvx( [ 1, nv ], [] );
            xt = cvx_subsref( x, ':', tt );
            y  = cvx_subsasgn( y, tt, sum( (abs(xt)).^p, 1 ) .^ (1/p) );
            tt = ~tt;
            xt = cvx_subsref( x, ':', tt );
            y  = cvx_subsasgn( y, tt, norms( xt, p, 1 ) );
            y  = reshape( y, sy );
            if ~isempty( perm ),
                y = ipermute( y, perm );
            end
        elseif p == 2,
            cvx_begin
                epigraph variable y( sy )
                { cvx_accept_convex(x), y } == lorentz( sx, dim, ~isreal( x ) ); %#ok
            cvx_end
        else
            sw = sx;
            sw(nd+1) = 2;
            cvx_begin
                variable z( sx )
                epigraph variable y( sy )
                if isreal(x), cmode = 'abs'; else cmode = 'cabs'; end
                { cat( nd+1, z, cvx_expand_dim( y, dim, nx ) ), cvx_accept_convex(x) } ...
                    == geo_mean_cone( sw, nd+1, [1/p,1-1/p], cmode ); %#ok
                sum( z, dim ) == y;
            cvx_end
        end
end

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function circle_arc_grid_test ( )

%*****************************************************************************80
%
%% CIRCLE_ARC_GRID_TEST tests the CIRCLE_ARC_GRID library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 November 2011
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_ARC_GRID_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test CIRCLE_ARC_GRID.\n' );

  circle_arc_grid_test01 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_ARC_GRID_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function res = bf_inverse_lcmv(BF, S)
% Computes LCMV filters
% Copyright (C) 2012 Wellcome Trust Centre for Neuroimaging

% Vladimir Litvak
% $Id: bf_inverse_lcmv.m 7703 2019-11-22 12:06:29Z guillaume $

%--------------------------------------------------------------------------
if nargin == 0
    orient        = cfg_menu;
    orient.tag    = 'orient';
    orient.name   = 'Orient to maximum power';
    orient.labels = {'yes', 'no'};
    orient.values = {true, false};
    orient.val    = {true};

    keeplf        = cfg_menu;
    keeplf.tag    = 'keeplf';
    keeplf.name   = 'Keep oriented leadfields';
    keeplf.labels = {'yes', 'no'};
    keeplf.values = {true, false};
    keeplf.val    = {false};

    lcmv      = cfg_branch;
    lcmv.tag  = 'lcmv';
    lcmv.name = 'LCMV';
    lcmv.val  = {orient, keeplf};
    res = lcmv;

    return
elseif nargin < 2
    error('Two input arguments are required');
end

invCy =  BF.features.(S.modality).Cinv;
U     = BF.features.(S.modality).U;

reduce_rank = BF.sources.reduce_rank.(S.modality(1:3));

L = S.L;
W = cell(size(L));

nvert = numel(W);

spm('Pointer', 'Watch');drawnow;
spm_progress_bar('Init', nvert, ['Computing ' S.modality ' filters']); drawnow;
if nvert > 100, Ibar = floor(linspace(1, nvert,100));
else Ibar = 1:nvert; end

for i = 1:nvert
    if ~isnan(L{i})
        lf    = U'*L{i};

        if S.orient
            % Robert's code
            [u, dum] = svd(real(pinv_plus(lf' * invCy *lf, reduce_rank, 0)),'econ');
            eta = u(:,1);
            lf  = lf * eta;
        end

        % construct the spatial filter
        W{i} = pinv_plus(lf' * invCy * lf, reduce_rank, 0)*lf'*invCy;

        if S.keeplf
            L{i} = lf;
        end
    else
        W{i} = NaN;
    end

    if ismember(i, Ibar)
        spm_progress_bar('Set', i); drawnow;
    end
end

spm_progress_bar('Clear');

res.W = W;
res.L = L;


clear all;

%%%%%%%%%%%%%This example will need to be reorganized

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%provide parameters and inputs below
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

addpath('../')

addpath(genpath('../CoreModules'));

addpath('./lm_data');

n_epoch=20;%20 %%training epochs
dataset_name='char'; % dataset name
network_name='qrnn';%'gru';'rnn','lstm'
use_gpu=0; %%use gpu or not
opts.use_nntoolbox=0;

PrepareDataFunc=@PrepareData_Char_RNN; %%function handler to prepare your data

if strcmp(network_name,'lstm')
    NetInit=@net_init_char_lstm;%_bn  %% function to initialize the network
end

if strcmp(network_name,'gru')
    NetInit=@net_init_char_gru;  %% function to initialize the network
end

if strcmp(network_name,'rnn')
    NetInit=@net_init_char_rnn;  %% function to initialize the network
    opts.parameters.Id_w=1;%vanilla rnn:0, rnn with skip links: 1
end

if strcmp(network_name,'qrnn')
    NetInit=@net_init_char_qrnn;  %% function to initialize the network
end

use_selective_sgd=0; %automatically select learning rates
%%selective-sgd parameters
%ssgd_search_freq=10; %select new coarse-scale learning rates every n epochs

learning_method=@adam; %training method: @rmsprop;

%sgd parameter (unnecessary if selective-sgd is used)
sgd_lr=1e-2;

opts.parameters.batch_size=100;
opts.parameters.n_hidden_nodes=30;
opts.parameters.n_cell_nodes=30;
opts.parameters.n_input_nodes=67;
opts.parameters.n_output_nodes=67;
if strcmp(network_name,'lstm')
    opts.parameters.n_gates=3;
end
if strcmp(network_name,'gru')
    opts.parameters.n_gates=2;
end

if strcmp(network_name,'qrnn')
    opts.parameters.n_gates=1;
    use_gpu=(gpuDeviceCount>0);
    opts.use_nntoolbox=license('test','neural_network_toolbox');
end

opts.parameters.n_frames=64;%%%%max sentence length

opts.parameters.lr =sgd_lr;
opts.parameters.mom =0.9;
opts.parameters.learning_method=learning_method;
opts.parameters.selective_sgd=use_selective_sgd;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%provide parameters and inputs above
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%stupid settings below (so please ignore)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

opts.n_epoch=n_epoch; %training epochs
opts.dataset_name=dataset_name; %dataset name
opts.network_name=network_name; %network name
opts.use_gpu=use_gpu; %use gpu or not

opts.results=[];
opts.results.TrainEpochError=[];
opts.results.TestEpochError=[];
opts.results.TrainEpochLoss=[];
opts.results.TestEpochLoss=[];
opts.RecordStats=1;
opts.results.TrainLoss=[];
opts.results.TrainError=[];

opts.plot=1;

opts.dataDir=['./',opts.dataset_name,'/'];
opts=PrepareDataFunc(opts);

net=NetInit(opts);

opts=generate_output_filename(opts);

if(opts.use_gpu)
    for i=1:length(net)
        net{i}=SwitchProcessor(net{i},'gpu');
    end
else
    for i=1:length(net)
        net{i}=SwitchProcessor(net{i},'cpu');
    end
end

opts.n_batch=floor(opts.n_train/opts.parameters.batch_size);
opts.n_test_batch=floor(opts.n_test/opts.parameters.batch_size);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%stupid settings above
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%training goes below
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

opts.parameters.current_ep=1;

start_ep=opts.parameters.current_ep;
if opts.plot
    figure1=figure;
end
for ep=start_ep:opts.n_epoch

    [net,opts]=train_rnn(net,opts);
    [opts]=test_rnn(net,opts);
    opts.parameters.current_ep=opts.parameters.current_ep+1;
    disp(['Epoch ',num2str(ep),' testing error: ',num2str(opts.results.TestEpochError(end)), ' testing loss: ',num2str(opts.results.TestEpochLoss(end))])

    %
    if opts.plot
        subplot(1,2,1); plot(opts.results.TrainEpochError);hold on;plot(opts.results.TestEpochError);hold off;title('Error Rate per Epoch')
        subplot(1,2,2);plot(opts.results.TrainEpochLoss);hold on;plot(opts.results.TestEpochLoss);hold off;title('Loss per Epoch')
        drawnow;
    end
    %}
    parameters=opts.parameters;
    results=opts.results;
    save([fullfile(opts.output_dir,[opts.output_name,num2str(ep),'.mat'])],'net','parameters','results');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%training goes above
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function tf=in_range_exclusive(value, R)
    % in_range_exclusive returns true where value in interval (A B), where  R = [A B];
    %
    % example:
    %  >> in_range_exclusive( 1:5 , [2 4])  % returns logical aray [0 0 1 0 0]
    %
    % see also in_range, in_range_inclusive
    if numel(R)==2
        tf = R(1) < value & value < R(2);
    else
        error('range should be a 1x2 vector of [minval maxval]');
    end
end


function [err, bf] = spm_bf (TR)
%
%   [err,] = spm_bf ()
%
%Purpose:
%
%
%
%Input Parameters:
%
%
%
%Output Parameters:
%   err : 0 No Problem
%       : 1  Problems
%
%
%
%Key Terms:
%
%More Info :
%
%
%
%
%     Author : Gang Chen
%     Date : Mon Oct 27 17:13:26 EST 2003
%     SSCC/NIMH/ National Institutes of Health, Bethesda MD 20892

%Define the function name for easy referencing
FuncName = 'spm_bf.m';

%Debug Flag
DBG = 1;

%initailize return variables
err = 1;

%	p(1) - delay of response (relative to onset)	   6        -gamd  in waver -GAM
%	p(2) - delay of undershoot (relative to onset)    16
%	p(3) - dispersion of response			   1
%	p(4) - dispersion of undershoot			   1   X
%	p(5) - ratio of response to undershoot		   6   X
%	p(6) - onset (seconds)				   0     -gamd
%	p(7) - length of kernel (seconds)		  32

      p      = [6 16 1 1 6 0 32];   %default parameters
	   [bf1 p]         = spm_hrf(TR,p);      %get gamma hrf from SPM function spm_hrf
	   dp     = 1;
	   p(6)   = p(6) + dp;    %for calculating time derivative
	   bf2    = (bf1 - spm_hrf(TR,p))/dp;
%		D      = (bf(:,1) - spm_hrf(TR,p))/dp;   %time derivative
		bf     = [bf1 bf2];     %combine the two

err = 0;
return;


function [X,T] = read_spm_file(spm_file,mat_file)
% Load an (continuous data) SPM file with filename specified in the spm_file
% input parameter, and put the data into X in matrix format ready to be used by
% the HMM. If mat_file is specified, X is saved in the file mat_file
% Importantly, if there are bad time points in the time series, it removes
% them and returns a vector of segment lengths (T) for the HMM to take
% into account that the time series is not seamlessly continuous anymore.
% In this case, although the hmmmar function can receive SPM files
% directly, it is strongly recommended to run this function for each
% subject before running the HMM, concatenate T across subjects,
% and then pass the mat_file's to the hmmmar function.
%
% Author: Diego Vidaurre, OHBA, University of Oxford (2018)

D = spm_eeg_load(spm_file);
X = D(:,:,:);
X = reshape(X,[D.nchannels,D.nsamples*D.ntrials]);

% select only good data
goodsamples = good_samples(D,[],[],[],0);
goodsamples = reshape(goodsamples,1,D.nsamples*D.ntrials);
X = X(:,goodsamples)';
options = struct();
T = cell2mat(getStateLifeTimes (goodsamples',length(goodsamples),options,1));

if min(T)<20
    warning('There are time segments with less than 20 time points')
end

if nargin > 1
   save(mat_file,'X','T')
end

end


function res = bf_inverse_champagne(BF, S)
% Computes Champagne filters
% See Owen et al. Performance evaluation of the Champagne source
% reconstruction algorithm on simulated and real M/EEG data. Neuroimage. 2012 Mar;60(1):305-23
% Code contributed by Sri Nagarajan
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Vladimir Litvak
% $Id: bf_inverse_champagne.m 7703 2019-11-22 12:06:29Z guillaume $

%--------------------------------------------------------------------------
if nargin == 0
    nem = cfg_entry;
    nem.tag = 'nem';
    nem.name = 'Number of EM iterations';
    nem.strtype = 'n';
    nem.num = [1 1];
    nem.val = {100};

    vcs         = cfg_menu;
    vcs.tag     = 'vcs';
    vcs.name    = 'Voxel covariance structure';
    vcs.labels  = {
        'scalar'
        'diagonal'
        'general'
        }';
    vcs.values  = {0, 1, 2};
    vcs.val = {2};

    nupd         = cfg_menu;
    nupd.tag     = 'nupd';
    nupd.name    = 'Noise covariance';
    nupd.labels  = {
        'use provided'
        'learn scalar'
        'learn diagonal'
        }';
    nupd.values  = {0, 1, 2};
    nupd.val = {0};

    champagne      = cfg_branch;
    champagne.tag  = 'champagne';
    champagne.name = 'Champagne';
    champagne.val  = {nem, vcs, nupd};
    res = champagne;

    return
elseif nargin < 2
    error('Two input arguments are required');
end

C     =  BF.features.(S.modality).C;
Y     =  BF.features.(S.modality).Y;

L = S.L;

W = cell(size(L));
nvert = numel(W);
nd = size(L{1}, 2);

LL = zeros(size(L{1}, 1), nvert*nd);
ind = 1;

spm('Pointer', 'Watch');drawnow;
spm_progress_bar('Init', nvert, ['Preparing ' S.modality ' leadfields']); drawnow;
if nvert > 100, Ibar = floor(linspace(1, nvert,100));
else Ibar = 1:nvert; end

for i = 1:nvert

    for j = 1:nd
        lf = L{i}(:, j);
        LL(:, ind) = lf./norm(lf);
        ind = ind+1;
    end
    if ismember(i, Ibar)
        spm_progress_bar('Set', i); drawnow;
    end
end

Fgraph  = spm_figure('GetWin', 'Champagne'); figure(Fgraph); clf

%****************************************************************************************
[gamma,x,w,sigu,like]=champagne_aug2015(Y, LL, C, S.nem, nd, S.vcs, S.nupd, [], 0, Fgraph);
%****************************************************************************************

spm_progress_bar('Init', nvert, ['Preparing ' S.modality ' filters']); drawnow;
if nvert > 100, Ibar = floor(linspace(1, nvert,100));
else Ibar = 1:nvert; end

for i = 1:nvert
    W{i} = w((i-1)*nd+(1:3), :);

    if ismember(i, Ibar)
        spm_progress_bar('Set', i); drawnow;
    end
end

spm_progress_bar('Clear');

spm('Pointer', 'Arrow');

res.W = W;


%MDL_SIMPLE6 A minimalistic 6DOF robot arm
%
% MDL_SIMPLE6 is a script creates the workspace variable s6 which describes
% the kinematic characteristics of a simple arm manipulator with a
% spherical wrist and no shoulder offset, using standard DH conventions.
%
% Also define the workspace vectors:
%   qz         zero joint angle configuration
%
% Notes::
% - Unlike most other mdl_xxx scripts this one is actually a function that
%   behaves like a script and writes to the global workspace.
%
% See also mdl_offset6, mdl_puma560, SerialLink.

% MODEL: generic, 6DOF, standard_DH

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function r = mdl_simple6()

    % robot length values (metres)
    L1 = 1;
    L2 = 1;

    % and build a serial link manipulator

    robot = SerialLink([
        Revolute('alpha', pi/2, 'a', 0,  'd', 0)
        Revolute('alpha', 0,     'a', L1, 'd', 0)
        Revolute('alpha', -pi/2,  'a', L2, 'd', 0)
        Revolute('alpha', -pi/2, 'a', 0,  'd', 0)
        Revolute('alpha', pi/2,  'a', 0,  'd', 0)
        Revolute('alpha', 0,     'a', 0,  'd', 0)
        ], ...
        'name', 'Simple6');

    % place the variables into the global workspace
    if nargout == 1
        r = robot;
    elseif nargout == 0
        assignin('caller', 's6', robot);
        assignin('caller', 'qz', [0 0 0 0 0 0]); % zero angles, arm up
    end
end


function g = whiteblockKernGradient(kern, x, x2, covGrad)

% WHITEBLOCKKERNGRADIENT Gradient of WHITEBLOCK kernel's parameters.
% FORMAT
% DESC computes the gradient of functions with respect to the
% white noise block kernel's parameters. As well as the kernel structure
% and the input positions, the user provides a matrix PARTIAL which gives
% the partial derivatives of the function with respect to the
% relevant elements of the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x : the input locations for which the gradients are being
% computed.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The argument takes
% the form of a square matrix of dimension  numData, where numData is
% the number of rows in X.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters. The ordering of the vector should match
% that provided by the function kernExtractParam.
%
% FORMAT
% DESC computes the derivatives as above, but input locations are
% now provided in two matrices associated with rows and columns of
% the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x1 : the input locations associated with the rows of the
% kernel matrix.
% ARG x2 : the input locations associated with the columns of the
% kernel matrix.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The matrix should
% have the same number of rows as X1 and the same number of columns
% as X2 has rows.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters.
%
% SEEALSO whiteblockKernParamInit, whiteblockKernDiagGradient
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if nargin < 4
    covGrad = x2;
end

g = zeros(1, kern.nout);
if nargin < 4
    startOne = 1;
    endOne = 0;
    for i=1:kern.nout
        if iscell(x)
            endOne = endOne + size(x{i},1);
        else
            endOne = endOne + size(x,1);
        end
        g(1, i) = trace(covGrad(startOne:endOne, startOne:endOne));
        startOne = endOne + 1;
    end
end


function L = symbchol()
%                                                          L = symbchol(X)
%
% SYMBCHOL Symbolic block sparse Cholesky factorization.
%   L = symbchol(X) returns a structure L that can be used
%   by the efficient block sparse Cholesky solver SPARCHOL.
%   The fields in L have the following meaning:
%
%   L.perm   - Multiple minimum degree ordering.
%
%   L.L      -  Sparse lower triangular matrix, has sparsity structure
%     of Cholesky factor of X(L.perm,L.perm).
%
%   L.xsuper - Supernode partition. Supernode jsup consists of
%     the nodes   L.xsuper(jsup) : L.xsuper(jsup)-1.
%
%   L.split  - Splitting of supernodes. Recommends to split supernode
%     in blocks of sizes   L.split(xsuper(jsup):L.xsuper(jsup)-1).
%
%   L.tmpsiz - Quantity used by SPARCHOL, to allocated enough working
%     storage.
%
%   L = symbchol(X,cachsz) optimizes L.split for a computer cache
%     of size CACHSZ * 1024 byte. Default cachsz = 512.
%
% See also sparchol, sparfwslv, sparbwslv, symbfact, symmmd, chol.

% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% 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
%

% ----------------------------------------
% Enter here the cache-size in KB, for shaping
% optimal dense blocks of floats.
% ----------------------------------------
global ADA_sedumi_
if ~issparse(ADA_sedumi_)
    error('X should be a sparse symmetric matrix')
end
cachsz = 512;
% ----------------------------------------
% Compute multiple minimum degree ordering.
% If the matrix is actually dense we don't bother.
% ----------------------------------------
if spars(ADA_sedumi_)<1
    perm = ordmmdmex(ADA_sedumi_);
    L = symfctmex(ADA_sedumi_,perm);
else
    L.perm=(1:size(ADA_sedumi_,1))';
    L.L=sparse(tril(ones(size(ADA_sedumi_))));
    L.xsuper=[1;size(ADA_sedumi_,1)+1];
end
% ----------------------------------------
% Symbolic Cholesky factorization structures, stored in L.
% ----------------------------------------
L.tmpsiz = choltmpsiz(L);
L.split = cholsplit(L,cachsz);


function [l, L_rf, L_sf, L_k, L_seg, L_n] = ...
    retroProjIdpLinFromPinHoleOnRob(Rf, Sf, k, seg, n)

% RETROPROJIDPLINFROMPINHOLEONROB retroprj Idp Line from pinhole on robot.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

if nargout == 1

    ls = invPinHoleIdpLin(k, seg, n) ;
    lr = fromFrameIdpLin(Sf, ls);
    l  = fromFrameIdpLin(Rf, lr);

else % Jacobians requested

    [ls, LS_seg, LS_n, LS_k] = invPinHoleIdpLin(seg, n, k) ;
    [lr, LR_sf, LR_ls]       = fromFrameIdpLin(Sf, ls);
    [l, L_rf, L_lr]          = fromFrameIdpLin(Rf, lr);

    L_sf  = L_lr*LR_sf;
    L_ls  = L_lr*LR_ls;
    L_k   = L_ls*LS_k;
    L_seg = L_ls*LS_seg;
    L_n   = L_ls*LS_n;

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


% Color code for each image:

if ~exist('n_ima')|~exist('fc'),
    fprintf(1,'No calibration data available.\n');
    return;
end;

check_active_images;

if n_ima ~=0,
if ~exist(['ex_' num2str(ind_active(1)) ]),
    fprintf(1,'Need to calibrate before analysing reprojection error. Maybe need to load Calib_Results.mat file.\n');
    return;
end;
end;

%if ~exist('no_grid'),
no_grid = 0;
%end;

colors = 'brgkcm';

figure(5);

for kk = 1:n_ima,
    if exist(['y_' num2str(kk)]),
        if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']),

            if ~no_grid,
                eval(['XX_kk = X_' num2str(kk) ';']);
                N_kk = size(XX_kk,2);

                if ~exist(['n_sq_x_' num2str(kk)]),
                    no_grid = 1;
                end;

                if ~no_grid,
                    eval(['n_sq_x = n_sq_x_' num2str(kk) ';']);
                    eval(['n_sq_y = n_sq_y_' num2str(kk) ';']);
                    if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))),
                        no_grid = 1;
                    end;
                end;
            end;

            eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']);

            hold on;
        end;
    end;
end;

hold off;
axis('equal');
if 1, %~no_grid,
    title('Reprojection error (in pixel) - To exit: right button');
else
    title('Reprojection error (in pixel)');
end;
xlabel('x');
ylabel('y');

set(5,'color',[1 1 1]);
set(5,'Name','error','NumberTitle','off');

if n_ima == 0,

        text(.5,.5,'No image data available','fontsize',24,'horizontalalignment' ,'center');

else

err_std = std(ex')';

fprintf(1,'Pixel error:          err = [ %3.5f   %3.5f] (all active images)\n\n',err_std);

b = 1;

while b==1,

    [xp,yp,b] = ginput3(1);

    if b==1,
        ddd = (ex(1,:)-xp).^2 + (ex(2,:)-yp).^2;

        [mind,indmin] = min(ddd);

        done = 0;
        kk_ima = 1;
        while (~done)&(kk_ima<=n_ima),
            %fprintf(1,'%d...',kk_ima);
            eval(['ex_kk = ex_' num2str(kk_ima) ';']);
            sol_kk = find((ex_kk(1,:) == ex(1,indmin))&(ex_kk(2,:) == ex(2,indmin)));
            if isempty(sol_kk),
                kk_ima = kk_ima + 1;
            else
                done = 1;
            end;
        end;

        eval(['x_kk = x_' num2str(kk_ima) ';']);
        xpt = x_kk(:,sol_kk);

        if ~no_grid,

            eval(['n_sq_x = n_sq_x_' num2str(kk_ima) ';']);
            eval(['n_sq_y = n_sq_y_' num2str(kk_ima) ';']);

            Nx = n_sq_x+1;
            Ny = n_sq_y+1;

            y1 = floor((sol_kk-1)./Nx);
            x1 = sol_kk - 1 - Nx*y1; %rem(sol_kk-1,Nx);

            y1 = (n_sq_y+1) - y1;
            x1 = x1 + 1;

            fprintf(1,'\n');
            fprintf(1,'Selected image: %d\n',kk_ima);
            fprintf(1,'Selected point index: %d\n',sol_kk);
            fprintf(1,'Pattern coordinates (in units of (dX,dY)): (X,Y)=(%d,%d)\n',[x1-1 y1-1]);
            fprintf(1,'Image coordinates (in pixel): (%3.2f,%3.2f)\n',[xpt']);
            fprintf(1,'Pixel error = (%3.5f,%3.5f)\n',[ex(1,indmin) ex(2,indmin)]);

        else

            fprintf(1,'\n');
            fprintf(1,'Selected image: %d\n',kk_ima);
            fprintf(1,'Selected point index: %d\n',sol_kk);
            fprintf(1,'Image coordinates (in pixel): (%3.2f,%3.2f)\n',[xpt']);
            fprintf(1,'Pixel error = (%3.5f,%3.5f)\n',[ex(1,indmin) ex(2,indmin)]);

        end;

        if exist(['wintx_' num2str(kk_ima)]),

            eval(['wintx = wintx_' num2str(kk_ima) ';']);
            eval(['winty = winty_' num2str(kk_ima) ';']);

            fprintf(1,'Window size: (wintx,winty) = (%d,%d)\n',[wintx winty]);
        end;

    end;

end;

disp('done');

end;


 function [sig, kernel, kernel_ft] = nufft_best_gauss(J, K_N, sn_type)
%function [sig, kernel, kernel_ft] = nufft_best_gauss(J, K_N, sn_type)
%|
%| Return "sigma" of best (truncated) gaussian for NUFFT
%| with previously numerically-optimized width
%|
%| in
%|	K_N		K/N
%|	J		# of neighbors used per frequency location
%|	sn_type		'zn' or 'ft' (latter recommended)
%|
%| out
%|	sig		best sigma
%|	kernel		string for inline kernel function, args (k,J)
%|	kernel_ft	string for Fourier transform function, arg: (t)
%|
%| Copyright 2002-4-11, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end

if nargin == 1 && streq(J, 'test')
	[sig, kernel, kernel_ft] = nufft_best_gauss(6)
	clear sig kernel kernel_ft
return
end

if nargin < 2, K_N = 2; end
if nargin < 3, sn_type = 'ft'; end

if K_N ~= 2, error 'only K/N=2 done', end

%if ir_is_octave % avoid annoying load warning
%	warning('off', 'Octave:load-file-in-path', 'local')
%end

nufft_dir = path_find_dir('nufft');
s = load([nufft_dir filesep 'param-data/nufft_gauss2.mat']);

ii = find(J == s.Jgauss2);
if length(ii) ~= 1
	disp(s.Jgauss2(:)')
	error 'only above J values done'
end
if streq(sn_type, 'ft')
	sig = s.Sgauss2.ft(find(J == s.Jgauss2));
elseif streq(sn_type, 'zn')
	sig = s.Sgauss2.zn(find(J == s.Jgauss2));
else
	error 'bad sn_type'
end
[kernel, kernel_ft] = nufft_gauss('string', J, sig);


function z = svec( x, nrm )

if nargin < 2 || isempty( nrm ) || isequal( nrm, 'fro' ),
    nrm = 2;
elseif ~isnumeric( nrm ) || length( nrm ) ~= 1 || nrm < 1,
    error( 'Second argument must be a number between 1 and Inf, or ''fro''.' );
end

if ~isreal( x ) && nrm ~= 2,
    z = vec( x );
    return
else
    [ xR, y ] = bcompress( x );
    if isempty( y ),
        z = cvx( 0 );
    else
        z = y .* norms( xR, nrm, 2 );
    end
end

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function gray = image_threshold3 ( gray, a, b )

%*****************************************************************************80
%
%% IMAGE_THRESHOLD3 resets gray pixels to white based on a threshold.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, uint8 GRAY(:,:), the original data.
%
%    Input, uint8 A, B, the threshold values.  Pixels of value less
%    than A or greater than B are reset to white.  0 <= A <= B <= 255
%
%    Output, uint8 GRAY(:,:), the thresholded image data.
%
  i = find ( gray < a );
  j = find ( b < gray );

  gray(i) = 255;
  gray(j) = 255;

  return
end


function trajectory_planning()
%%
close all
clear
%% requirements
% given obstacle field O, start configuration z_s with centroid s and destination g for the formation's centroid.
import iris.inflate_region.*
import iris.drawing.*
import iris.thirdParty.polytopes.*
% constants
lcx = 0.6; lcy = 0.6; lcz = 0.2; % size of object
lmx = 0.6; lmy = 0.4; lmz = 0.3; lm_b = 0.15; % size of platform
% obstacles
xo1 = [3.75,4.75,0.5,0,0,0];
xo2 = [8.25,3,0.5,0,0,0];
obst(1) = pkgMechanics.RigidCuboid(1,xo1,[1.5,3.5,1]);
obst(2) = pkgMechanics.RigidCuboid(1,xo2,[1.5,1,1]);
% obstacles cell
obstcell = createobstcell(obst);
% range
range.lb = [0;0];
range.ub = [10;10];
%% initialize an empty graph
nodez = []; % z vector in row
nodepoly = []; % polytope struct
nodezinpoly = {}; % z index
% initialize edges
edgez = []; % z index
%% configuration
a0 = 0.7; w0 = 0.5;
config = [a0,w0];
%% generate start and goal
start = [1.5,1.5];     % start position
goal = [8.5,8.5];     % goal position
[nodez, nodepoly, nodezinpoly]= startxgoal(nodez,nodepoly,nodezinpoly,[start;goal],config,obstcell,range,[lcx,lcy,lcz],[lmx,lmy,lmz]);
%% detemine z
z0 = [start,config];
[xr0,xc0,xm0]= z2x(z0,[lcx,lcy,lcz],[lmx,lmy,lmz]);
%% build object
cub = pkgMechanics.RigidCuboid(1,xc0,[lcx,lcy,lcz]);
%% build platforms
for i=1:4
    plat(i) = pkgMechanics.RigidCuboid(1,xm0(i,:),[lmx,lmy,lmz]);
end
%% build robot arms
Tc2e = lc2Tce([lcx,lcy,lcz]); % => constants
Tm2b = transl([lm_b,0,lmz/2]); % => constants
mdl_puma560
for i=1:4
    rob(i) = SerialLink('name','robot');
    copy(rob(i),p560); rob(i).name = ['robot',num2str(i)];
    rob(i).base = x2T(xm0(i,:))*Tm2b;
    q0(i,:) = rob(i).ikine6s(x2T(xc0)*Tc2e{i},'ru');% right hand elbow up
end
%% visualize map
figure;
draw_region_2d(nodepoly,obstcell.vert,range);
drawplat3d(nodez,[lcx,lcy,lcz],[lmx,lmy,lmz])
%% add goal node to graph
[nodez,nodezinpoly,nodegrid,edgez]=creategraph(nodez,nodepoly,nodezinpoly,edgez,1,config,range,[lcx,lcy,lcz],[lmx,lmy,lmz]);
[noderoute,polyroute] = shortestpath(nodez,nodezinpoly,nodegrid,1,2);
%% generate random nodes
counter = 0;
while isempty(noderoute)
    nodenum = 1;
    [nodez,nodepoly,nodezinpoly] = randnode(nodez,nodepoly,nodezinpoly,nodenum,config,obstcell,range,[lcx,lcy,lcz],[lmx,lmy,lmz]);
    % create undirected graph and its edges
    [nodez,nodezinpoly,nodegrid,edgez]=creategraph(nodez,nodepoly,nodezinpoly,edgez,nodenum,config,range,[lcx,lcy,lcz],[lmx,lmy,lmz]);
    [noderoute,polyroute] = shortestpath(nodez,nodezinpoly,nodegrid,1,2);
    counter = counter + 1;
end
draw_region_2d(nodepoly(polyroute),[],range);
drawplat3d(nodez(noderoute,:),[lcx,lcy,lcz],[lmx,lmy,lmz]);
hold on
x1=nodez(noderoute,1);
y1=nodez(noderoute,2);
plot(x1,y1,'r-');
hold off
if ~exist('shortestpath.mat', 'file')
    save('shortestpath.mat','nodez','nodepoly','edgez','noderoute','polyroute');
end
%%%%%%%%% the above is the whole global planning part %%%%%%%%%%%

%%%%%%%%% the following is the animation %%%%%%%%%%
load('shortestpath2.mat','nodez','nodepoly','edgez','noderoute','polyroute');
%% save data
% trajectory
dt = 0.5; tacc = 1;
[zarray, dzarray, tarray] = calctraj(nodez(noderoute,:),dt,tacc);
%% visualize map
figure;
xlim([range.lb(1) range.ub(1)]);
ylim([range.lb(2) range.ub(2)]);
hold on
% % obstacles
% draw_region_2d([],obstcell.vert,range);
% fig.obst = patch('Vertices',obst.verticesStates.position','Faces',obst.faces,'FaceColor','g','FaceAlpha',0.5);
% pause(0.2)
% % object
% fig.cub = patch('Vertices',cub.verticesStates.position','Faces',cub.faces,'FaceColor','m','FaceAlpha',0.5);
% % robot arm and base
zlim([0 8]); plotaxis = gca;
plotopt = {'noname', 'noraise', 'nobase', 'noshadow', 'notiles', 'nowrist', 'nojaxes', 'delay', 0, 'workspace',[plotaxis.XLim,plotaxis.YLim,plotaxis.ZLim]};
% for i=1:4
%     fig.plat(i) = patch('Vertices',plat(i).vertw,'Faces',plat(i).face,'FaceColor','y','FaceAlpha',0.5);
%     rob(i).plot(q0(i,:),plotopt{:});
% end
% pause(0.2)
% % polytope
% for i=1:length(polyroute)
%     draw_region_2d(nodepoly(polyroute(i)),[],range);
%     pause(0.2)
% end
% % graph
% for kk=1:2:size(edgez,1)
%     x1=[nodez(edgez(kk,1),1);nodez(edgez(kk,2),1)];
%     y1=[nodez(edgez(kk,1),2);nodez(edgez(kk,2),2)];
%     line(x1,y1);
% end
% pause(0.2)
% % path
% plot(nodez(noderoute(end),1),nodez(noderoute(end),2),'kd', 'LineWidth', 2)
% plot(nodez(noderoute,1),nodez(noderoute,2),'k-', 'LineWidth', 2)
% pause(0.2)
% hold off
% % obstacles
% cla
% hold on
draw_region_2d([],obstcell.vert,range);
for i=1:length(obst)
    fig.obst = patch('Vertices',obst(i).verticesStates.position','Faces',obst(i).faces,'FaceColor','g','FaceAlpha',0.5);
end
% object
fig.cub = patch('Vertices',cub.verticesStates.position','Faces',cub.faces,'FaceColor','m','FaceAlpha',0.5);
% robot arm and base
for i=1:4
    fig.plat(i) = patch('Vertices',plat(i).verticesStates.position','Faces',plat(i).faces,'FaceColor','b','FaceAlpha',0.5);
    rob(i).plot(q0(i,:),plotopt{:});
end
% path
plot(nodez(noderoute(end),1),nodez(noderoute(end),2),'rd', 'LineWidth', 2)
plot(nodez(noderoute,1),nodez(noderoute,2),'r-', 'LineWidth', 2)
pause(0.2)
view(3)
hold off
%% animation loop
tic;
playspeed = 1;
while toc<tarray(end)/playspeed
    tnow = toc*playspeed;
    % choose via point in time 'tnow'
    [z,dz] = getviatnow(zarray, dzarray, tarray, tnow);
    [xr,xc,xm] = z2x(z,[lcx,lcy,lcz],[lmx,lmy,lmz]);
    % update object
    cub.updateStates(xc',zeros(6,1));
    % set robot arm state
    for i=1:4
        rob(i).base = x2T(xm(i,:))*Tm2b;
        q(i,:) = rob(i).ikine6s(x2T(xc)*Tc2e{i});
        plat(i).updateStates(xm(i,:)',zeros(6,1));
    end
    % animation
    set(fig.cub,'Vertices',cub.verticesStates.position','Faces',cub.faces);
    for i=1:4
        set(fig.plat(i),'Vertices',plat(i).verticesStates.position','Faces',plat(i).faces);
        rob(i).animate(q(i,:));
    end
    drawnow
end
end

function obstacle = createobstcell(obstcub)
n_obs=length(obstcub);
for i=1:n_obs
    obstacle_pts(:,:,i) = obstcub(i).verticesStates.position(1:2,1:4);
end
% reset obstacles vertices in its convexhull (iris requires obstacles to be convex)
obsidx = cell(1,size(obstacle_pts,3));
obstacle.vert = cell(1,size(obstacle_pts,3));
for j = 1:n_obs
    obs = obstacle_pts(:,:,j);
    if size(obs, 2) > 1
        if size(obs, 2) > 2
            obsidx{j} = convhull(obs(1,:), obs(2,:));
        else
            obsidx{j} = [1,2,1];
        end
    end
    obstacle.vert{j} = obs(:,obsidx{j})';
    obstacle.calc{j} = obs(:,obsidx{j});
end
% obstacles cell
obstacle.poly = obstacle.vert{1}';
for i=2:size(obstacle.vert,1)
    obstacle.poly = [obstacle.poly,NaN(2,1),obstacle.vert{i}'];
end
end

function [nodez, nodepoly, nodezinpoly]= startxgoal(nodez,nodepoly,nodezinpoly,randnode,nodeconfig,obstacle,range,lc,lm)
% set nodez to all zero
znum = size(nodez,1);
polynum = length(nodepoly);
% generate nodes outside obstacles
randnum = size(randnode,1);
for i=1:randnum
    % generate ploytope
    randpoly = polytope(obstacle.calc,randnode(i,:)',range);
    % check whether formation exists in a polytope
    [randz,fg] = formationP2z(randpoly,[randnode(i,:),nodeconfig],range,lc,lm);
    if fg==1
        % add this location to nodelocation list
        nodez = [nodez;randz];
        nodepoly = [nodepoly,randpoly];
        nodezinpoly{polynum+i} = znum+i;
        %%%%%
%         hold on
%         plot(randlocation(1),randlocation(2),'go');
%         plot(randz(1),randz(2),'r*');
%         draw_region_2d(randpoly,[],range);
%         drawplat3d(randz);
%         hold off
        %%%%%
    end
end
end

function [nodez, nodepoly, nodezinpoly]= randnode(nodez,nodepoly,nodezinpoly,nodenum,nodeconfig,obstacle,range,lc,lm)
% set nodez to all zero
znum = size(nodez,1);
polynum = length(nodepoly);
% generate nodes outside obstacles
ctr=1; % counter
while (ctr<=nodenum)
    % generate random two number in range of map's border
    randlocation = [rand* (range.ub(1)-range.lb(1)) + range.lb(1);
        rand* (range.ub(2)-range.lb(2)) + range.lb(2)];
    % if this node is not inside any obstacle
    if ~inpolygon(randlocation(1),randlocation(2),obstacle.poly(1,:),obstacle.poly(2,:))
        % chech if inside a existing polytope
        if ~innodepoly(randlocation,nodepoly)
            % generate ploytope
            randpoly = polytope(obstacle.calc,randlocation,range);
            % check whether formation exists in a polytope
            [randz,fg] = formationP2z(randpoly,[randlocation',nodeconfig],range,lc,lm);
            if fg==1
                % add this location to nodelocation list
                nodez = [nodez;randz];
                nodepoly = [nodepoly,randpoly];
                nodezinpoly{polynum+ctr} = znum+ctr;
                %%%%%
%                 hold on
%                 plot(randlocation(1),randlocation(2),'go');
%                 plot(randz(1),randz(2),'r*');
%                 draw_region_2d(randpoly,[],range);
%                 drawplat3d(randz);
%                 hold off
                %%%%%
                ctr=ctr+1;
            end
        end
    end
end
end

function flag = innodepoly(randlocation,nodepoly)
polynum = length(nodepoly);
count = 0;
for i=1:polynum
    if nodepoly(i).A*randlocation-nodepoly(i).b<=0
        count = count+1;
    end
end
if count==0
    flag = false;
else
    flag = true;
end
end


function [c,b,c1,g,sn,sp] = constrained_foopsi(y,b,c1,g,sn,options)
% spike inference using a constrained deconvolution approach:
%      min      sum(sp)
%    c,sp,b,c1
%      subject to: sp >= 0
%                   b >= 0
%                  G*c = sp
%                   c1 >= 0
%           ||y-b-c - c_in|| <= sn*sqrt(T)

%   Variables:
%   y:      raw fluorescence data (vector of length(T))
%   c:      denoised calcium concentration (Tx1 vector)
%   b:      baseline concentration (scalar)
%  c1:      initial concentration (scalar)
%   g:      discrete time constant(s) (scalar or 2x1 vector)
%  sn:      noise standard deviation (scalar)
%  sp:      spike vector (Tx1 vector)

%   USAGE:
%   [c,b,c1,g,sn,sp] = constrained_foopsi(y,b,c1,g,sn,OPTIONS)
%   The parameters b,cin,g,sn can be given or else are estimated from the data

%   OPTIONS: (stuct for specifying options)
%         p: order for AR model, used when g is not given (default 2)
%    method: methods for performing spike inference
%   available methods: 'dual' uses dual ascent
%                       'cvx' uses the cvx package available from cvxr.com (default)
%                      'lars' uses the least regression algorithm
%                     'spgl1' uses the spgl1 package available from
%                     math.ucdavis.edu/~mpf/spgl1/  (can be faster)
%   bas_nonneg:   flag for setting the baseline lower bound. if 1, then b >= 0 else b >= min(y)
%   noise_range:  frequency range over which the noise power is estimated. Default [Fs/4,Fs/2]
%   noise_method: method to average the PSD in order to obtain a robust noise level estimate
%   lags:         number of extra autocovariance lags to be considered when estimating the time constants
%   resparse:     number of times that the solution is resparsened (default 0). Currently available only with methods 'cvx', 'spgl'
%   fudge_factor: scaling constant to reduce bias in the time constant estimation (default 1 - no scaling)

% Written by:
% Eftychios A. Pnevmatikakis, Simons Foundation, 2015

defoptions.p = 2;
defoptions.method = 'cvx';
defoptions.bas_nonneg = 1;              % nonnegativity option for baseline estimation
defoptions.noise_range = [0.25,0.5];    % frequency range over which to estimate the noise
defoptions.noise_method = 'logmexp';    % method for which to estimate the noise level
defoptions.lags = 5;                    % number of extra lags when computing the AR coefficients
defoptions.resparse = 0;                % number of times to re-sparse solution
defoptions.fudge_factor = 1;            % fudge factor for time constants

if nargin < 6
    options = defoptions;
    if nargin < 5
        sn = [];
        if nargin < 4
            g = [];
            if nargin < 3
                c1 = [];
                if nargin < 2
                    b = [];
                end
            end
        end
    end
end

if ~isfield(options,'p');  options.p = defoptions.p;  end
if ~isfield(options,'method'); options.method = defoptions.method; end
if ~isfield(options,'bas_nonneg'); options.bas_nonneg = defoptions.bas_nonneg; end
if ~isfield(options,'noise_range'); options.noise_range = defoptions.noise_range; end
if ~isfield(options,'noise_method'); options.noise_method = defoptions.noise_method; end
if ~isfield(options,'lags'); options.lags = defoptions.lags; end
if ~isfield(options,'resparse'); options.resparse = defoptions.resparse; end
if ~isfield(options,'fudge_factor'); options.fudge_factor = defoptions.fudge_factor; end

method = options.method;
if isempty(b);
    bas_est = 1;
else
    bas_est = 0;
end
if isempty(c1)
    c1_est = 1;
else
    c1_est = 0;
end

y = y(:);
T = length(y);
y_full = y;
mis_data = isnan(y);
E = speye(T);
E(mis_data,:) = [];

if any(mis_data)
    y_full(mis_data) = interp1(find(~mis_data),y(~mis_data),find(mis_data));
end

if isempty(sn)
    sn = GetSn(y_full,options.noise_range,options.noise_method);
end
if isempty(g)
    g = estimate_time_constants(y_full,options.p,sn,options.lags);
    while max(abs(roots([1,-g(:)']))>1) && options.p < 5
        warning('No stable AR(%i) model found. Checking for AR(%i) model \n',options.p,options.p+1);
        options.p = options.p + 1;
        g = estimate_time_constants(y,options.p,sn,options.lags);
    end
    if options.p == 5
        g = 0;
    end
    %fprintf('Stable AR(%i) model found \n',options.p);
    % re-adjust time constant values
    rg = roots([1;-g(:)]);
    if ~isreal(rg); rg = real(rg) + .001*randn(size(rg)); end
    rg(rg>1) = 0.95 + 0.001*randn(size(rg(rg>1)));
    rg(rg<0) = 0.15 + 0.001*randn(size(rg(rg<0)));
    pg = poly(options.fudge_factor*rg);
    g = -pg(2:end);
end
if options.bas_nonneg  % lower bound for baseline
    b_lb = 0;
else
    b_lb = min(y);
end

if strcmpi(method,'dual'); method = 'dual';
elseif strcmpi(method,'cvx'); method = 'cvx';
elseif strcmpi(method,'lars'); method = 'lars';
elseif strcmpi(method,'spgl1'); method = 'spgl1';
else fprintf('Invalid choice of method. Using CVX \n'); method = 'cvx';
end

if strcmpi(method,'dual') && any(mis_data)
    warning('Dual method does not support missing data. Switching to CVX');
    method = 'cvx';
end

if options.resparse > 0 && (strcmpi(method,'dual') || strcmpi(method,'lars'))
    warning('Resparsening is not supported with chosen method. Switching to CVX');
    method = 'cvx';
end

pathCell = regexp(path, pathsep, 'split');
g = g(:);
G = spdiags(ones(T,1)*[-g(end:-1:1)',1],-length(g):0,T,T);
gd = max(roots([1,-g']));  % decay time constant for initial concentration
gd_vec = gd.^((0:T-1)');

switch method
    case 'dual'
         v = G'*ones(T,1);
        thr = sn*sqrt(T-sum(mis_data));
        if bas_est; b = 0; end
        if c1_est; c1 = 0; end
        myfun = @(Ald) lagrangian_temporal_gradient(Ald,thr^2,y(~mis_data)-b-c1*gd_vec(~mis_data),bas_est,c1_est);
        c = [G\max(G*y,0);zeros(bas_est);zeros(c1_est)];
        options_dual = optimset('GradObj','On','Display','Off','Algorithm','interior-point','TolX',1e-8);
        ld_in = 10;
        [ld,~,flag] = fmincon(myfun,ld_in,[],[],[],[],0,[],[],options_dual);
        if (flag == -2) || (flag == -3)
            warning('Problem seems unbounded or infeasible. Try a different method.');
        end
        if bas_est; b = c(T+bas_est); end
        if c1_est; c1 = c(end); end
        c = c(1:T);
        sp = G*c;
    case 'cvx'
        onPath = ~isempty(which('cvx_begin'));
        if onPath
            c = zeros(T,1+options.resparse);
            sp = zeros(T,1+options.resparse);
            bas = zeros(1+options.resparse,1);
            cin = zeros(1+options.resparse,1);
            w_ = ones(T,1);
            for rep = 1:options.resparse+1
                [c(:,rep),bas(rep),cin(rep)] = cvx_foopsi(y,b,c1,sn,b_lb,g,w_,~mis_data);
                sp(:,rep) = G*c(:,rep);
                w_ = 1./(max(sp(:,rep),0) + 1e-8);
            end
            sp(sp<1e-6) = 0;
            c = G\sp;
            b = bas;
            c1 = cin;
        else
            error('CVX does not appear to be on the MATLAB path. It can be downloaded from cvxr.com \n');
        end
    case 'lars'
         Ginv = E*[full(G\speye(T)),ones(T,bas_est),gd_vec*ones(1,c1_est)];
         if bas_est; b = 0; end
         if c1_est; c1 = 0; end
         [~, ~, spikes, ~, ~] = lars_regression_noise(y(~mis_data)-b_lb*bas_est - b - c1*gd_vec(~mis_data), Ginv, 1, sn^2*(T-sum(mis_data)));
         sp = spikes(1:T);
         b = (spikes(T+bas_est)+b_lb)*bas_est + b*(1-bas_est);
         c1 = spikes(end)*c1_est + c1*(1-c1_est);
         c = G\sp;
    case 'spgl1'
        onPath = ~isempty(which('spgl1'));
        if onPath
            Gx = @(x,mode) G_inv_mat(x,mode,T,g,gd_vec,bas_est,c1_est,E);
            c = zeros(T,1+options.resparse);
            sp = zeros(T,1+options.resparse);
            bas = zeros(1+options.resparse,1);
            cin = zeros(1+options.resparse,1);
            w_ = ones(T,1);
            for rep = 1:options.resparse+1
                if bas_est; b = 0; w_ = [w_;1e-10]; end
                if c1_est; c1 = 0; w_ = [w_;1e-10]; end
                options_spgl = spgSetParms('project',@NormL1NN_project ,'primal_norm', @NormL1NN_primal,'dual_norm',@NormL1NN_dual,'verbosity',0,'weights',w_);
                [spikes,r,~,~] = spg_bpdn( Gx, y(~mis_data)-b_lb*bas_est - (1-bas_est)*b-(1-c1_est)*c1*gd_vec(~mis_data), sn*sqrt(T-sum(mis_data)),options_spgl);
                c(:,rep) = G\spikes(1:T); %Gx([spikes(1:T);0],1);                                  %% calcium signal
                bas(rep) = b*(1-bas_est) + bas_est*spikes(T+bas_est)+b_lb*bas_est;       %% baseline
                cin(rep) = c1*(1-c1_est) + c1_est*spikes(end);
                sp(:,rep) = spikes(1:T);                                           %% spiking signal
                w_ = 1./(spikes(1:T)+1e-8);
            end
            b = bas;
            c1 = cin;
            %sn = norm(r)/sqrt(T);
        else
            error('SPGL1 does not appear to be on the MATLAB path. It can be downloaded from math.ucdavis.edu/~mpf/spgl1 \n');
        end
end

    function sn = GetSn(Y,range_ff,method)
        % estimate noise level with a power spectral density method
        L=length(Y);
%         if ~isempty(which('pmtm'))
%             [psd_Y,ff] = pmtm(Y,5/2,1000,1);
%         end
        if ~isempty(which('pwelch'));
            [psd_Y,ff]=pwelch(Y,round(L/8),[],1000,1);
        else
            xdft = fft(Y);
            xdft = xdft(1:round(L/2)+1);
            psd_Y = (1/L) * abs(xdft).^2;
            ff = 0:1/L:1/2;
            psd_Y(2:end-1) = 2*psd_Y(2:end-1);
        end
        ind=ff>range_ff(1);
        ind(ff>range_ff(2))=0;
        switch method
            case 'mean'
                sn=sqrt(mean(psd_Y(ind)/2));
            case 'median'
                sn=sqrt(median(psd_Y(ind)/2));
            case 'logmexp'
                sn = sqrt(exp(mean(log(psd_Y(ind)/2))));
        end
    end

    function g = estimate_time_constants(y,p,sn,lags)
        % estimate time constants from the sample autocovariance function

        lags = lags + p;
        if ~isempty(which('xcov')) %signal processing toolbox
            xc = xcov(y,lags,'biased');
        else
            ynormed = (y - mean(y));
            xc = nan(lags + 1, 1);
            for k = 0:lags
                xc(k + 1) = ynormed(1 + k:end)' * ynormed(1:end - k);
            end
            xc = [flipud(xc(2:end)); xc] / numel(y);
        end
        xc = xc(:);
        A = toeplitz(xc(lags+(1:lags)),xc(lags+(1:p))) - sn^2*eye(lags,p);
        try
            g = pinv(A)*xc(lags+2:end);
        catch
            g = 0;
        end
    end

    function [f,grad] = lagrangian_temporal_gradient(Al,thr,y_raw,bas_flag,c1_flag)
        options_qp = optimset('Display','Off','Algorithm','interior-point-convex');
        H = [speye(T),ones(T,bas_flag),gd_vec*ones(1,c1_flag);...
            ones(bas_flag,T),T*ones(bas_flag),(1-gd^T)/(1-gd)*ones(c1_flag,bas_flag);...
            (gd_vec*ones(1,c1_flag))',(1-gd^T)/(1-gd)*ones(c1_flag,bas_flag),(1-gd^(2*T))/(1-gd^2)*ones(c1_flag,c1_flag)];
        Ay = [y_raw;sum(y_raw)*ones(bas_flag);gd_vec'*y_raw*ones(c1_flag)];
        c = quadprog(2*Al(1)*H,[v;zeros(bas_flag+c1_flag,1)]-2*Al(1)*Ay,[-G,sparse(T,bas_flag+c1_flag);sparse(bas_flag+c1_flag,T),-speye(bas_flag+c1_flag)]...
            ,[sparse(T,1);-b_lb*ones(bas_flag);zeros(c1_flag)],[],[],[],[],c,options_qp);
        f = v'*c(1:T);
        grad = [sum((c(1:T)-y_raw + c(T+bas_flag)*bas_flag + c(end)*gd_vec*c1_flag).^2)-thr];
        f = f + Al(:)'*grad;
    end

    function b = G_inv_mat(x,mode,NT,gs,gd_vec,bas_flag,c1_flag,Emat)
        if mode == 1
            b = filter(1,[1;-gs(:)],x(1:NT)) + bas_flag*x(NT+bas_flag) + c1_flag*gd_vec*x(end);
            b = Emat*b;
           %b = G\x(1:NT) + x(NT+bas_flag)*bas_flag + x(end)*c1_flag;
        elseif mode == 2
            x = Emat'*x;
            b = [flipud(filter(1,[1;-gs(:)],flipud(x)));ones(bas_flag,1)*sum(x);ones(c1_flag,1)*(gd_vec'*x)];
           %b = [G'\x;ones(bas_flag,1)*sum(x);ones(c1_flag,1)*(gd_vec'*x)] ;
        end
    end

end


function x = r8vec_scale ( s, n, x )

%*****************************************************************************80
%
%% R8VEC_SCALE multiplies an R8VEC by a scale factor.
%
%  Discussion:
%
%    An R8VEC is a vector of R8 values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 October 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer S, the scale factor.
%
%    Input, integer N, the length of the vector.
%
%    Input/output, real X(N), the vector to be scaled.
%
  x(1:n) = s * x(1:n);

  return
end


function hail_test ( )

%*****************************************************************************80
%
%% HAIL_TEST tests HAIL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HAIL_TEST\n' );
  fprintf ( 1, '  HAIL(I) computes the length of the hail sequence\n' );
  fprintf ( 1, '  for I, also known as the 3*N+1 sequence.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  I,  HAIL(I)\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 20
    fprintf ( 1, '%2d  %6d\n', i, hail(i) );
  end

  return
end


function [priormeans,posteriormeans,covmatrix] = gpnddisimPredictRNAOnly(model,predtimes);

% GPASIMPREDICT Compute predictions (means and a covariance matrix)
% of RNA values for the GPASIM model, conditional on the existing
% RNA values in the model but not on any existing POL2 values.
%
% FORMAT
%---------------------------------
% DESC computes predictions for the asynchronous Gaussian
% process single input motif model.
%
% ARG model : the model for which the gradient is computed.
%
% ARG pol2times : the time points where predictions for POL2 are needed
%
% ARG rnatime : the time points where predictions for RNA are needed
%
% RETURN means : the predicted mean values, first the POL2
% predictions and then the RNA predictions.
%
% RETURN covmatrix : the covariance matrix between the
% predictions; for example, the diagonal values are the variances
% of each prediction.
%---------------------------------
%
% SEEALSO : gpasimCreate
%
% COPYRIGHT : Jaakko Peltonen, 2011

% GPASIMPREDICT

numGenes=model.numGenes;
if numGenes==0,
  error('gpnddisimPredictRNAOnly requires a model that has parameters for RNA modeling\n');
end;

% compute prior means
%pol2priormeans=[1:size(predtimes,1)]'*0;
pol2priormeans=ones(size(predtimes,1),1)*model.simMean;

% Mean for the mRNA is nonconstant over time and depends on the
% B,D,S parameters and on the POL2 mean
Bj=model.B(1);
Dj=model.D(1);
Sj=model.S(1);
if model.use_disimstartmean==1,
  disimStartMean=model.disimStartMean(1);
end;

% compute the RNA mean curve

rnapriormeans=[];
tempind1=1;
for k=1:numGenes,
  nt=length(predtimes);
  rnapriormeans=[rnapriormeans;nan*ones(nt,1)];
  if (model.use_disimstartmean==1),
    tempt=predtimes-model.delay(k);
    I=find(tempt<0);
    tempt(I)=0;
    rnapriormeans(tempind1:tempind1+nt-1)=...
        model.disimStartMean(k)*exp(model.D(k)*(-predtimes)) ...
        +(model.B(k)/model.D(k))*(1-exp(-model.D(k)*predtimes)) ...
        +(model.simMean*model.S(k)/model.D(k))*(1-exp(-model.D(k)*tempt));
  else
    tempt=predtimes-model.delay(k);
    I=find(tempt<0);
    tempt(I)=0;
    rnapriormeans(tempind1:tempind1+nt-1)=...
        ((model.B(k)+model.simMean*model.S(k))/model.D(k))*exp(model.D(k)*(-predtimes))...
        +((model.B(k)+model.simMean*model.S(k))/model.D(k))*(1-exp(-model.D(k)*tempt));
  end;
  tempind1=tempind1+nt;
end;

%if model.use_disimstartmean==1,
%  rnapriormeans=(Bj+model.simMean*Sj)/Dj+(disimStartMean-(Bj+model.simMean*Sj)/Dj)*exp(Dj*(-predtimes));
%else
%  rnapriormeans=(Bj+model.simMean*Sj)/Dj*ones(size(predtimes));
%end;

if 1,
K_new=kernCompute(model.kern, predtimes);
K_new=K_new(length(predtimes)+1:2*length(predtimes),length(predtimes)+1:2*length(predtimes));

K_new_old=kernCompute(model.kern, predtimes, model.t);
K_new_old=K_new_old(length(predtimes)+1:2*length(predtimes),length(model.t)+1:2*length(model.t));

K_old=model.K(length(model.t)+1:2*length(model.t),length(model.t)+1:2*length(model.t));
K_old_new=K_new_old';
end;

tempm=model.m(length(model.t)+1:2*length(model.t));

priormeans=rnapriormeans;
size(tempm)
size(K_new_old)
size(K_old)
size(priormeans)
posteriormeans=priormeans+K_new_old*(K_old\tempm);
%covmatrix=K_new-K_new_old*inv(K_old)*K_old_new;

covmatrix=K_new-K_new_old*(K_old\K_old_new);

%posteriormeans=real(posteriormeans);
%covmatrix=real(covmatrix);


function pred_value = xval_featureselect_nmdscluster(X, Y, pthreshold, holdout_method, xyzlist)
%
% pred_value = xval_featureselect_nmdscluster(X, Y, p-value selection for univariate feature selection, holdout_method)
%
% pred_value: cross-validated predictions of outcome data
% X: n x variables matrix of predictors
% Y: n x 1 vector of outcomes
%
% Tor Wager, June 2010
%
% Go to any LASSO output directory and run this:
%
% maskInfo = iimg_read_img(fullfile(pwd, 'mask.img'), 2);
% dat{1} =  iimg_get_data(maskInfo, anticimages);
% pred_value = xval_simple_ols_loo(X, Y)

[N, k] = size(X);
pred_value = zeros(N, 1);

holdout_set = nested_select_holdout_set;

create_figure('test', 1, 2);

fprintf('Fold: %03d', 0);
for wh_fold = 1:length(holdout_set)

    fprintf('\b\b\b%3.0f ', wh_fold);

    % select training data
    Xi = X;
    Yi = Y;

    Yi(holdout_set{wh_fold}) = [];
    Xi(holdout_set{wh_fold}, :) = [];              % leave out the missing observation(s)

    Xtest = X(holdout_set{wh_fold}, :);
    Ytest = Y(holdout_set{wh_fold}, :);          % only used later when we test prediction

    ntrain = size(Xi, 1);
    nholdout = size(Xtest, 1);

    % select features based on univariate correlations
    [r, p, Tstat] = correlation_fast_series(Xi, Yi);
    wh_features = p <= pthreshold;
    nfeatures(wh_fold) = sum(wh_features);
    if nfeatures(wh_fold) == 0
        disp('Warning: no features pass threshold');
        wh_features = p <= prctile(p, 10);
    end

    Xi = Xi(:, wh_features);

    c = []; c.outcome = Yi; c.covs_nointerest = [];
    % get average data from contiguous blobs (??)
    xyz = xyzlist(wh_features, :);
    contig = spm_clusters(xyzlist(wh_features, :)');
    for ii = 1:max(contig)
        if sum(contig == ii) < 3, contig(contig == ii) = 0; end
    end
    u = unique(contig);    u(u == 0) = [];
    for ii = 1:length(u)
        c.dat(:, ii) = nanmean(Xi(:, contig == u(ii))')';
    end
    c = nmdsfig_tools('get_correlations',c);
    c.GroupSpace = mdscale(c.D, 8, 'Replicates', 10,'Options',statset('MaxIter',500),'Criterion','sstress');
    c.ClusterSolution.classes = clusterdata(c.GroupSpace, 'maxclust', 5, 'linkage', 'average');

    c.APPLY_CLUSTER = apply_cluster_solution(c.ClusterSolution.classes,...
        c.r,...
        'names',[],'bcov',c.outcome, 'n', size(c.dat, 1), 'dointeractive', 0);
    class_avg_dat = [];
     for i = 1:max(c.ClusterSolution.classes)
        wh = find(c.ClusterSolution.classes == i);
        if length(wh) > 1
            class_avg_dat = [class_avg_dat nanmean(c.dat(:,wh)')'];
        else
            class_avg_dat = [class_avg_dat c.dat(:,wh)];
        end
     end
    c.class_avg_dat = class_avg_dat;

    c.class_STEPWISE = stepwise_tor(c.class_avg_dat,c.outcome,c.APPLY_CLUSTER.class_names);

    Xi = [ones(ntrain, 1) c.class_avg_dat(:, c.class_STEPWISE.inmodel)]; % add intercept

    % apply to Xtest
    c.testdat = zeros(size(Xtest, 1), size(c.dat, 2));
    for ii = 1:length(u)
        c.testdat(:, ii) = nanmean(Xtest(:, contig == u(ii))')';
    end
     class_avg_dat = [];
     for i = 1:max(c.ClusterSolution.classes)
        wh = find(c.ClusterSolution.classes == i);
        if length(wh) > 1
            class_avg_dat = [class_avg_dat nanmean(c.testdat(:,wh)')'];
        else
            class_avg_dat = [class_avg_dat c.testdat(:,wh)];
        end
     end
    c.class_avg_testdat = class_avg_dat;
    Xtest = c.class_avg_testdat(:, c.class_STEPWISE.inmodel);

    % Make prediction
    b = pinv(Xi) * Yi;
    pred_value(holdout_set{wh_fold}, 1) = [ones(nholdout, 1) Xtest] * b;

    create_figure('test', 1, 2, 1); subplot(1, 2, 1); plot(Xi*b, Yi, 'ko');
    plot(pred_value(holdout_set{wh_fold}, 1), Ytest, 'ro', 'MarkerFaceColor', 'r');
    subplot(1, 2, 2);
   title('Black circles = training set; Red = holdout obs');
    drawnow;

end

cm = colormap(jet(N));
figure; hold on;
for i = 1:N
    plot(pred_value(i), Y(i), 'ko', 'MarkerFaceColor', cm(i, :));
end
xlabel('Predicted outcome (xval)'); ylabel('Outcome');
title('Color = order in data series');

function holdout_set = nested_select_holdout_set
        % purpose: return holdout_set variable

        nobs_s = length(Y);

        switch lower(holdout_method)
            case 'loo'
                holdout_set = cell(1, nobs_s);
                for i = 1:nobs_s, holdout_set{i} = i; end

            case 'l4o_covbalanced'
                disp('Selecting holdout sets: Balancing on covariates and outcome, and also trying to ensure that each obs is selected equally often.');
                holdout_proportion = 4 ./ nobs_s; % prop for leave-4-out
                nfolds = 40;
                if isempty(cov_val)
                    wh_holdout = xval_select_holdout_set(Y, [], nfolds, holdout_proportion, verbose);
                else
                    wh_holdout = xval_select_holdout_set(Y, cov_val, nfolds, holdout_proportion, verbose);
                end
                holdout_set = cell(1, nfolds);
                for k = 1:nfolds
                    holdout_set{k} = wh_holdout(:, k);
                end

            case 'categorical_covs'

                holdout_set = xval_select_holdout_set_categoricalcovs(cov_val);

            case 'balanced4'
                nfolds = 4;
                holdout_set = cell(1, nfolds);
                [ys, indx] = sort(Y);
                for k = 1:nfolds

                    holdout_set{k} = indx(k:nfolds:end);
                    if isempty(holdout_set{k}), error('Holdout set construction error: Dataset too small?'); end

                end

            otherwise error('Unknown holdout method.  See help.');
        end
end

end % main function


function hh = quiver(varargin)
%QUIVER Quiver plot.
%   QUIVER(X,Y,U,V) plots velocity vectors as arrows with components (u,v)
%   at the points (x,y).  The matrices X,Y,U,V must all be the same size
%   and contain corresponding position and velocity components (X and Y
%   can also be vectors to specify a uniform grid).  QUIVER automatically
%   scales the arrows to fit within the grid.
%
%   QUIVER(U,V) plots velocity vectors at equally spaced points in
%   the x-y plane.
%
%   QUIVER(U,V,S) or QUIVER(X,Y,U,V,S) automatically scales the
%   arrows to fit within the grid and then stretches them by S.  Use
%   S=0 to plot the arrows without the automatic scaling.
%
%   QUIVER(...,LINESPEC) uses the plot linestyle specified for
%   the velocity vectors.  Any marker in LINESPEC is drawn at the base
%   instead of an arrow on the tip.  Use a marker of '.' to specify
%   no marker at all.  See PLOT for other possibilities.
%
%   QUIVER(...,'filled') fills any markers specified.
%
%   H = QUIVER(...) returns a vector of line handles.
%
%   Example:
%      [x,y] = meshgrid(-2:.2:2,-1:.15:1);
%      z = x .* exp(-x.^2 - y.^2); [px,py] = gradient(z,.2,.15);
%      contour(x,y,z), hold on
%      quiver(x,y,px,py), hold off, axis image
%
%   See also FEATHER, QUIVER3, PLOT.

%   Clay M. Thompson 3-3-94
%   Copyright 1984-2002 The MathWorks, Inc.
%   $Revision: 2.1 $  $Date: 2005/05/23 16:20:12 $

% Arrow head parameters
alpha = 0.33; % Size of arrow head relative to the length of the vector
beta = 0.33;  % Width of the base of the arrow head relative to the length
autoscale = 1; % Autoscale if ~= 0 then scale by this.
plotarrows = 1; % Plot arrows
sym = '';

filled = 0;
ls = '-';
ms = '';
col = '';

nin = nargin;
% Parse the string inputs
while isstr(varargin{nin}),
  vv = varargin{nin};
  if ~isempty(vv) & strcmp(lower(vv(1)),'f')
    filled = 1;
    nin = nin-1;
  else
    [l,c,m,msg] = colstyle(vv);
    if ~isempty(msg),
      error(sprintf('Unknown option "%s".',vv));
    end
    if ~isempty(l), ls = l; end
    if ~isempty(c), col = c; end
    if ~isempty(m), ms = m; plotarrows = 0; end
    if isequal(m,'.'), ms = ''; end % Don't plot '.'
    nin = nin-1;
  end
end

error(nargchk(2,5,nin));

% Check numeric input arguments
if nin<4, % quiver(u,v) or quiver(u,v,s)
  [msg,x,y,u,v] = xyzchk(varargin{1:2});
else
  [msg,x,y,u,v] = xyzchk(varargin{1:4});
end
if ~isempty(msg), error(msg); end

if nin==3 | nin==5, % quiver(u,v,s) or quiver(x,y,u,v,s)
  autoscale = varargin{nin};
end

% Scalar expand u,v
if prod(size(u))==1, u = u(ones(size(x))); end
if prod(size(v))==1, v = v(ones(size(u))); end

if autoscale,
  % Base autoscale value on average spacing in the x and y
  % directions.  Estimate number of points in each direction as
  % either the size of the input arrays or the effective square
  % spacing if x and y are vectors.
  if min(size(x))==1, n=sqrt(prod(size(x))); m=n; else [m,n]=size(x); end
  delx = diff([min(x(:)) max(x(:))])/n;
  dely = diff([min(y(:)) max(y(:))])/m;
  del = delx.^2 + dely.^2;
  if del>0
    len = sqrt((u.^2 + v.^2)/del);
    maxlen = max(len(:));
  else
    maxlen = 0;
  end

  if maxlen>0
    autoscale = autoscale*0.9 / maxlen;
  else
    autoscale = autoscale*0.9;
  end
  u = u*autoscale; v = v*autoscale;
end

ax = newplot;
next = lower(get(ax,'NextPlot'));
hold_state = ishold;

% Make velocity vectors
x = x(:).'; y = y(:).';
u = u(:).'; v = v(:).';
uu = [x;x+u;repmat(NaN,size(u))];
vv = [y;y+v;repmat(NaN,size(u))];

h1 = plot(uu(:),vv(:),[col ls]);

if plotarrows,
  % Make arrow heads and plot them
  hu = [x+u-alpha*(u+beta*(v+eps));x+u; ...
        x+u-alpha*(u-beta*(v+eps));repmat(NaN,size(u))];
  hv = [y+v-alpha*(v-beta*(u+eps));y+v; ...
        y+v-alpha*(v+beta*(u+eps));repmat(NaN,size(v))];
  hold on
  h2 = plot(hu(:),hv(:),[col ls]);
else
  h2 = [];
end

if ~isempty(ms), % Plot marker on base
  hu = x; hv = y;
  hold on
  h3 = plot(hu(:),hv(:),[col ms]);
  if filled, set(h3,'markerfacecolor',get(h1,'color')); end
else
  h3 = [];
end

if ~hold_state, hold off, view(2); set(ax,'NextPlot',next); end

if nargout>0, hh = [h1;h2;h3]; end


function ta = scatter3_2(X,str)
% SCATTER3_2 - scatter plot 3 dimensional data
% (slight modification of MATLAB's scatter3.m)
%
% First argument - input is an nx3 matrix. output is a scatter plot graph, where each point
% comes from one of the rows of the input matrix. The matrix must have 3
% columns.
%
% Optional second argument - string to determine plotting style, using same
% options as in MATLAB's plot function
%
% EXAMPLE:  >> A = rand(20,3);
%           >> scatter3_2(A);
%

% Motivation: MATLAB's scatter3 requires 3 column vectors, eg, scatter3(x,y,z), where
% each 'row' of (x,y,z) coordinates is plotted. We would instead like to be
% able to just input one nx3 matrix. It will have n points to plot, and
% each point is found in each row. The columns do not need to be separated
% into three different arguments.

if nargin<2
    str='b';
end

scatter3(X(:,1),X(:,2),X(:,3),str);


% lw=load('wandell_fixed_19991208_V3A-V7_dti_leftOccFG+Splenium_wedge_dat');
% lr=load('wandell_fixed_19991208_V3A-V7_dti_leftOccFG+Splenium_ring_dat');
% rw=load('wandell_fixed_19991208_V3A-V7_dti_rightOccFG+Splenium_wedge_dat');
% rr=load('wandell_fixed_19991208_V3A-V7_dti_rightOccFG+Splenium_ring_dat');
mkdir(tempdir, 'matlabFigs');
baseDir = fullfile(tempdir, 'matlabFigs');
disp(['figures will be saved in ' baseDir]);
scanNames = {'ring','wedge','ring'};
scanNum = 2;

coThresh = 0.2;
fontName = 'Helvetica'; %'Comic Sans MS';
fontSize = 18;
leftPhaseLabels  = ['R-HM'; ' UVM'; 'L-HM'; ' LVM'; 'R-HM'];
rightPhaseLabels = ['L-HM'; ' LVM'; 'R-HM'; ' UVM'; 'L-HM'];

volView = getSelectedVolume;
d.roiCoords = getCurROIcoords(volView);
% We get the last coord, which is the left-right axis (lower = left)
% The following will swap coordinate pairs so that the left-most coord
% is always first.
fiberEndptCoordX = [d.roiCoords(3,1:2:end); d.roiCoords(3,2:2:end)];
swapThese = diff(fiberEndptCoordX)<0;
tmp = fiberEndptCoordX(1,swapThese);
fiberEndptCoordX(1,swapThese) = fiberEndptCoordX(2,swapThese);
fiberEndptCoordX(2,swapThese) = tmp;
subName = mrSESSION.subject;
[junk,sessionCode] = fileparts(pwd);
roiName = volView.ROIs(viewGet(volView, 'selectedROI')).name;

fname = fullfile(baseDir, [subName '_' sessionCode '_' roiName '_' scanNames{scanNum}]);
d.co = getCurDataROI(volView,'co',scanNum,d.roiCoords);
d.ph = getCurDataROI(volView,'ph',scanNum,d.roiCoords);

fiberEndptPh = [d.ph(1:2:end); d.ph(2:2:end)];
tmp = fiberEndptPh(1,swapThese);
fiberEndptPh(1,swapThese) = fiberEndptPh(2,swapThese);
fiberEndptPh(2,swapThese) = tmp;
goodVals = isfinite(fiberEndptPh(1,:)) & isfinite(fiberEndptPh(2,:)) & d.co(1:2:end)>coThresh & d.co(2:2:end)>coThresh;
fiberEndptPh = fiberEndptPh(:,goodVals);
% figure;
% subplot(1,2,1); scatter(fiberEndptPh(1,:), fiberEndptPh(2,:));
% title(roiName);
% subplot(1,2,2); hist(fiberEndptPh');

% Get data from entire occ lobe (eg. create a 60mm radius disk ROI
% centered in the calcarine- call the left 'allLeft' and the right
% 'allRight'.)

%Process left hemisphere
volView = getSelectedVolume;
roiNum = strmatch('allleft',lower({volView.ROIs(:).name}));
l.roiCoords = volView.ROIs(roiNum).coords;
l.co = getCurDataROI(volView,'co',scanNum,l.roiCoords);
l.ph = getCurDataROI(volView,'ph',scanNum,l.roiCoords);
allLeftPh = l.ph;
goodVals = isfinite(allLeftPh) & l.co>coThresh;
allLeftPh = allLeftPh(goodVals);
%figure; hist(allLeftPh',20);
cxPhLeft = exp(sqrt(-1)*allLeftPh);
centerPh = mean(cxPhLeft);
mnLeftPh = complexPh2PositiveRad(mean(cxPhLeft));
allLeftPhCentered = angle(cxPhLeft/centerPh);
fiberLeftPhCentered = angle(exp(sqrt(-1)*fiberEndptPh(1,:))/centerPh);

[p,t,df] = statTest(allLeftPhCentered, fiberLeftPhCentered, 't');
figure; set(gcf,'Position', [7, 50, 500, 700]);
subplot(2,1,1); hist(allLeftPhCentered,20);
set(gca, 'fontName', fontName, 'fontSize', fontSize, 'XLim', [-pi, pi], 'XTickLabel','');
pos = get(gca,'YLim');
text(-pi*.9,pos(2)*.9,['Mean phase = ' num2str(mnLeftPh,'%0.1f rad')]);
title(sprintf('t=%0.2f (p=%0.4f, df=%d)', t, p, df));
subplot(2,1,2); hist(fiberLeftPhCentered,20);
set(gca, 'fontName', fontName, 'fontSize', fontSize, 'XLim', [-pi, pi], 'XTickLabel','');
set(gca, 'XLim', [-pi, pi], 'XTick', [-pi, -pi/2, 0, pi/2, pi], 'XTickLabel', leftPhaseLabels);
set(gcf, 'PaperPositionMode', 'auto');
print(gcf, '-dpng', '-r120', [fname '_left.png']);

% Process the right
volView = getSelectedVolume;
roiNum = strmatch('allright',lower({volView.ROIs(:).name}));
r.roiCoords = volView.ROIs(roiNum).coords;
r.co = getCurDataROI(volView,'co',scanNum, r.roiCoords);
r.ph = getCurDataROI(volView,'ph',scanNum, r.roiCoords);
allRightPh = r.ph;
goodVals = isfinite(allRightPh) & r.co>coThresh;
allRightPh = allRightPh(goodVals);
%figure; hist(allRightPh',20);
cxPhRight = exp(sqrt(-1)*allRightPh);
centerPh = mean(cxPhRight);
mnRightPh = complexPh2PositiveRad(mean(cxPhRight));
allRightPhCentered = angle(cxPhRight/centerPh);
fiberRightPhCentered = angle(exp(sqrt(-1)*fiberEndptPh(2,:))/centerPh);

[p,t,df] = statTest(allRightPhCentered, fiberRightPhCentered, 't');
figure; set(gcf,'Position', [7, 50, 500, 700]);
subplot(2,1,1); hist(allRightPhCentered,20);
set(gca, 'fontName', fontName, 'fontSize', fontSize, 'XLim', [-pi, pi], 'XTickLabel','');
pos = get(gca,'YLim');
text(-pi*.9,pos(2)*.9,['Mean phase = ' num2str(mnRightPh,'%0.1f rad')]);
title(sprintf('t=%0.2f (p=%0.4f, df=%d)', t, p, df));
subplot(2,1,2); hist(fiberRightPhCentered,20);
set(gca, 'fontName', fontName, 'fontSize', fontSize, 'XLim', [-pi, pi], 'XTickLabel','');
set(gca, 'XLim', [-pi, pi], 'XTick', [-pi, -pi/2, 0, pi/2, pi], 'XTickLabel', rightPhaseLabels);
set(gcf, 'PaperPositionMode', 'auto');
print(gcf, '-dpng', '-r120', [fname '_right.png']);

save([fname '.mat'], 'd', 'l', 'r', 'coThresh');

% set(gcf, 'PaperPositionMode', 'auto');
% print(gcf, '-dpng', '-r120', '/tmp/bw_right_wedge.png']);
%print(gcf, '-depsc', '-tiff', [fileName '_scatter.eps']);


function h = p13_fh ( p, varargin )

%*****************************************************************************80
%
%% P13_FH returns a mesh size function for problem 13.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 February 2006
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, real P(NP,ND), the point coordinates.
%
%    Input, VARARGIN, room for extra arguments.
%
%    Output, real H(NP,1), the mesh size function.
%
  np = size ( p, 1 );
  h = ones ( np, 1 );

  return
end


function a = r8col_permute ( m, n, a, p )

%*****************************************************************************80
%
%% R8COL_PERMUTE permutes an R8COL in place.
%
%  Discussion:
%
%    An R8COL is an M by N array of R8's, regarded
%    as an array of N columns of length M.
%
%    This routine permutes an array of real "objects", but the same
%    logic can be used to permute an array of objects of any arithmetic
%    type, or an array of objects of any complexity.  The only temporary
%    storage required is enough to store a single object.  The number
%    of data movements made is N + the number of cycles of order 2 or more,
%    which is never more than N + N/2.
%
%  Example:
%
%    Input:
%
%      M = 2
%      N = 5
%      P = (   2,    4,    5,    1,    3 )
%      A = ( 1.0,  2.0,  3.0,  4.0,  5.0 )
%          (11.0, 22.0, 33.0, 44.0, 55.0 )
%
%    Output:
%
%      A    = (  2.0,  4.0,  5.0,  1.0,  3.0 )
%             ( 22.0, 44.0, 55.0, 11.0, 33.0 ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the size of the objects.
%
%    Input, integer N, the number of objects.
%
%    Input, real A(M,N), the array to be permuted.
%
%    Input, integer P(N), the permutation.  P(I) = J means
%    that the I-th element of the output array should be the J-th
%    element of the input array.  P must be a legal permutation
%    of the integers from 1 to N, otherwise the algorithm will
%    fail catastrophically.
%
%    Output, real A(M,N), the permuted array.
%

%
%  Search for the next element of the permutation that has not been used.
%
  for istart = 1 : n

    if ( p(istart) < 0 )

      continue

    elseif ( p(istart) == istart )

      p(istart) = - p(istart);
      continue

    else

      a_temp(1:m) = a(1:m,istart);
      iget = istart;
%
%  Copy the new value into the vacated entry.
%
      while ( 1 )

        iput = iget;
        iget = p(iget);

        p(iput) = - p(iput);

        if ( iget < 1 || n < iget )
          fprintf ( 1, '\n' );
          fprintf ( 1, 'R8COL_PERMUTE - Fatal error!\n' );
          fprintf ( 1, '  A permutation index is out of range.\n' );
          fprintf ( 1, '  P(%d) = %d\n', iput, iget );
          error ( 'R8COL_PERMUTE - Fatal error!' );
        end

        if ( iget == istart )
          a(1:m,iput) = a_temp(1:m)';
          break
        end

        a(1:m,iput) = a(1:m,iget);

      end

    end

  end
%
%  Restore the signs of the entries.
%
% p(1:n) = -p(1:n);

  return
end


function [interpImg, interpImg3D] = rxInterpSlice(rx, rxSlice, ori);
%
% [interpImg, interpImg3D] = rxInterpSlice(rx, rxSlice, [ori=3]);
%
% Given the transform matrix and slice specified by
% the mrRx GUI, compute an interpolated slice.
%
% The optional third argument, ori, specifies the orientation of the slice
% to take relative to the prescription:
%	3 [default] - prescribed slice (corresponds to the slices shown in the
%					Rx window)
%	2 - slice corresponds to a column of the prescription. (e.g., if the
%					prescription is sagittal, this is coronal.)
%	1 - slice corresponds to a row of the prescription. (e.g., if the
%					prescription is sagittal, this is axial.)
%
%
% Returns two images: interpImg is a 2D matrix
% with the raw interpolated values; interpImg3D
% is a 3D TrueColor image, with the brightness
% and contrast settings of the GUI taken into
% account.
%
%
% ras 03/05.
% ras, 02/08/08: added orientation flag.
if ~exist('rx', 'var') | isempty(rx),
	rx = get(findobj('Tag','rxControlFig'), 'UserData');
end
if ~exist('ori', 'var') | isempty(ori), ori = 3;				 end
if ~exist('rxSlice', 'var') | isempty(rxSlice)
    rxSlice = get(rx.ui.rxSlice.sliderHandle, 'Value');
end

ysz = rx.rxDims(1);
xsz = rx.rxDims(2);
zsz = rx.rxDims(3);

%% get the sampling range for each dimension
switch ori
	case 1,	 % row
		yRange = rxSlice;
		xRange = 1:xsz;
		zRange = 1:zsz;
		imgSize = [xsz zsz];

	case 2,  % column
		yRange = 1:ysz;
		xRange = rxSlice;
		zRange = 1:zsz;
		imgSize = [ysz zsz];

	case 3,  % slice
		yRange = 1:ysz;
		xRange = 1:xsz;
		zRange = rxSlice;
		imgSize = [ysz xsz];

	otherwise, error('Invalid orientation flag.')
end

% build up coords for interp vol slice
% here we flip: x->cols, y->rows
[X Y Z] = meshgrid(xRange, yRange, zRange);
coords = [X(:) Y(:) Z(:) ones(size(Z(:)))]';
coords = double(coords);

% get new coords for the interp slice
newCoords = rx.xform * coords;

% interpolate
interpVals = myCinterp3(rx.vol, rx.volDims(1:2), rx.volDims(3), ...
						newCoords(1:3,:)', 0);
interpImg = reshape(interpVals, imgSize);

% convert to truecolor if requested
if nargout >= 2
	if ishandle(rx.ui.interpBright.sliderHandle)
		brightness = rx.ui.interpBright;
	else
		brightness = 0.5;
	end

	if ishandle(rx.ui.interpContrast.sliderHandle)
		contrast = rx.ui.interpContrast;
	else
		contrast = .7;
	end

	if ishandle(rx.ui.interpHistoThresh)
		histoThresh = rx.ui.interpHistoThresh;
	else
		histoThresh = 1;
	end

    interpImg3D = rxClip(interpImg, [], brightness, contrast, histoThresh);
end

return


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Released under the MIT License.
% If you use this code, please cite the following paper:
% Mahmoud Afifi, Abdelrahman Abdelhamed, Abdullah Abuolaim, Abhijith
% Punnappurath, and Michael S Brown. CIE XYZ Net: Unprocessing Images for
% Low-Level Computer Vision Tasks. arXiv preprint, 2020.
%
% Author: Mahmoud Afifi | Email: mafifi@eecs.yorku.ca, m.3afifi@gmail.com
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function lossLayer = addLossLayer(name, loss, scaleFactor)

switch lower(loss)
    case 'mse'
        lossLayer = mseLossLayer(name, scaleFactor);
    case 'mae'
        lossLayer = maeLossLayer(name, scaleFactor);
    case 'mse + cosine'
        lossLayer = mse_cos_LossLayer(name, scaleFactor);
    case 'mae + cosine'
        lossLayer = mae_cos_LossLayer(name, scaleFactor);
    otherwise
        error('Wrong value in inSpace argument');
end


% twa_demo.m, The following script demonstrates how to use the TWA code in
% the MVtoolbox to estimate TWAs using the modified moving average (MMA)
% method coupled with a non parametric surrogate statistical reshuffling
% method to reduce false positive TWA detections. The algorithm is
% referenced as follows,
%
%   Nemati S, Abdala O, Monasterio V, Yim-Yeh S, Malhotra A, Clifford GD.
%   A nonparametric surrogate-based test of significance for T-wave alternans detection.
%   IEEE Trans Biomed Eng. 2011;58(5):1356–1364. doi:10.1109/TBME.2010.2047859
%
% The main function is ComputeTWA. The output is stored in the TWAResult
% structure with the following fields with the expected output listed as follows,
%
%   Field                   Expected values
%   TWAResult.HR            [80.32,80.21,79.79,80.65,80.65,80.43,80,80,80.21,80.32,80,79.47]
%   TWAResult.VAlt          [38.84,40.10,37.81,37.38,37.70,38.14,39.32,39.74,38.31,40.39,39.07,42.20]
%   TWAResult.VAlt_Sig      [38.84,40.10,37.81,37.38,37.70,38.14,39.32,39.74,38.31,40.39,39.07,42.20]
%   TWAResult.Noise_Median  [4.28,3.92,4.40,3.75,3.68,4.14,3.00,3.21,4.15,3.95,3.78,7.20]
%   TWAResult.Noise_95      [11.19,10.75,9.61,12.67,11.69,9.72,9.15,9.84,10.29,11.03,9.89,19.40]
%   TWAResult.VAltPt        [132,116,115,125,116,122,130,117,91,111,101,159]
%   TWAResult.successful    1
%
% REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Written by Ismail Sadiq
%	COPYRIGHT (C) 2019
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information. The license may be found in
%       the Documents folder of the Physionet-Cardiovascular-Signal-Toolbox.

% add dependencies within cardiovascular toolbox
addpath(genpath('../Tools/'));
addpath(genpath('../../../../../PhysioNet-Cardiovascular-Signal-Toolbox-master/'));
% load data and annotations
sig = load('./testdata/twa/sample_ecg_twam');
siginfo = readheader('./testdata/twa/sample_ecg_twam.hea');
Fs = siginfo.freq;
ecg = (sig.val - siginfo.adczero)./siginfo.gain; % Adjust signal according to gain and dc offset
clear sig siginfo
if (size(ecg,2) > size(ecg,1))
   ecg = ecg';
end

% load annotations
ann.QRSon = read_ann('./testdata/twa/sample_ecg_twa','qrson')'; ann.Q = read_ann('./testdata/twa/sample_ecg_twa','q')'; ann.R = read_ann('./testdata/twa/sample_ecg_twa','r')'; ann.S = read_ann('./testdata/twa/sample_ecg_twa','s')'; ann.QRSoff = read_ann('./testdata/twa/sample_ecg_twa','qrsoff')'; ann.Toff = read_ann('./testdata/twa/sample_ecg_twa','toff')';

% TWA evaluation
TWAResult = ComputeTWA(ecg,ann,Fs);


function C = interleave_rows(A,B)
  % Takes two nxm matrices and creates a 2nxm matrix such that even rows are
  % from the first matrix and odd rows are from the second.
  %
  % C = interleave_rows(A,B)
  %
  % Input:
  %   A  First nxm matrix, top row of output will be same as top of this matrix
  %      along with every other row
  %   B  Second nxm matrix, bottom row of output will be same as bottom of this
  %      matrix along with every other row
  %
  % Output:
  %   C  2nxm matrix, alternating rows
  %
  %
  if(size(A)~=size(B))
    error('Input matrices must be same shape');
  end
  C = reshape([A(:) B(:)]',size(A,1)*2, size(A,2));
end


function prediction = NIN(varargin)
%NIN Returns a Network-in-Network model for CIFAR10
%   M = models.NIN() returns the model proposed in:
%
%     Lin et al, "Network in network", arXiv technical report 2013.
%
%   models.NIN(..., 'option', value, ...) accepts the following options:
%
%   `input`:: default input
%     Specifies an input (images) layer for the network. If unspecified, a
%     new one is created.
%
%   `numClasses`:: 10
%     Number of output classes.
%
%   `batchNorm`:: true
%     Whether to use batch normalization.
%
%   Any other options will be passed to models.ConvBlock(), and can be used
%   to change the activation function, weight initialization, etc.
%
%   Suggested SGD training options are also returned in the struct M.meta.

% Copyright (C) 2018 Joao F. Henriques, Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

  % parse options. unknown arguments will be passed to ConvBlock (e.g.
  % batchNorm).
  opts.input = Input('name', 'images', 'gpu', true) ;  % default input layer
  opts.numClasses = 10 ;  % number of predicted classes
  opts.batchNorm = true ;  % whether to use batch normalization
  [opts, convBlockArgs] = vl_argparse(opts, varargin, 'nonrecursive') ;

  % build network
  images = opts.input ;

  % first NIN block
  channels = [192 160 96] ;  % number of outputs channels per conv layer
  ker = [5 5] ;  % conv kernel
  poolKer = [3 3] ;  % pooling kernel
  poolMethod = 'max' ;  % pooling method
  pad = 2 ;  % input padding
  m1 = ninBlock(images, 3, channels, ker, pad, ...
    poolKer, poolMethod, convBlockArgs, opts.batchNorm, false) ;
  outChannels = channels(3) ;  % output channels of the NIN block

  % second NIN block
  channels = [192 192 192] ;
  ker = [5 5] ;
  poolKer = [3 3] ;
  poolMethod = 'avg' ;
  pad = 2 ;
  m2 = ninBlock(m1, outChannels, channels, ker, pad, ...
    poolKer, poolMethod, convBlockArgs, opts.batchNorm, false) ;
  outChannels = channels(3) ;

  % third NIN block
  channels = [192 192 opts.numClasses] ;
  ker = [3 3] ;
  poolKer = [7 7] ;
  poolMethod = 'avg' ;
  pad = 1 ;
  prediction = ninBlock(m2, outChannels, channels, ker, pad, ...
    poolKer, poolMethod, convBlockArgs, opts.batchNorm, true) ;

  % default training options for this network
  defaults.batchSize = 100 ;
  defaults.weightDecay = 0.0005 ;
  defaults.imageSize = [32, 32, 3] ;
  % the default learning rate schedule
  if ~opts.batchNorm
    defaults.learningRate = [0.002, 0.01, 0.02, 0.04 * ones(1,80), 0.004 * ones(1,10), 0.0004 * ones(1,10)] ;
    defaults.numEpochs = numel(defaults.learningRate) ;
  else
    defaults.learningRate = 0.01 ;
    defaults.numEpochs = 40 ;
  end
  prediction.meta = defaults ;

end

function block = ninBlock(in, inChannels, outChannels, ...
  ker, pad, poolKer, poolMethod, convBlockArgs, bnorm, final)

  % get conv block generator with the given options.
  % default activation is ReLU, with post-activation batch normalization.
  conv = models.ConvBlock('batchNorm', bnorm, convBlockArgs{:}) ;

  % if it's the final layer (prediction), the last conv block has no
  % activation or batch-norm
  finalArgs = {} ;
  if final
    finalArgs = {'activation', 'none', 'batchNorm', false} ;
  end

  % 3 conv blocks
  c1 = conv(in, 'size', [ker(1:2), inChannels, outChannels(1)], 'pad', pad) ;
  c2 = conv(c1, 'size', [1, 1, outChannels(1), outChannels(2)]) ;
  c3 = conv(c2, 'size', [1, 1, outChannels(2), outChannels(3)], finalArgs{:}) ;

  % pooling
  p1 = vl_nnpool(c3, poolKer, 'method', poolMethod, 'stride', 2) ;

  % dropout, skipped if it's the final layer (prediction)
  if ~final
    block = vl_nndropout(p1, 'rate', 0.5) ;
  else
    block = p1 ;
  end
end


function tc = tc_sparklineWholeTc(tc);
%
% tc = tc_sparklineWholeTc(tc);
%
% An attempt to visualize a whole time course using
% Edward Tufte's sparkline method.
%
% ras, 01/2007.
if notDefined('tc'),    tc = get(gcf, 'UserData');      end

%% clear previous objects in figure
delete( findobj('Parent', tc.ui.plot) );

%% get time vector t in seconds
nFrames = length(tc.wholeTc);
t = [0:nFrames-1] .* tc.params.framePeriod;

%% plot time course in small axes
axes('Parent', tc.ui.plot, 'Units', 'norm', 'Position', [.1 .6 .65 .3]);
plot(t, tc.wholeTc, 'k');
hold on, axis off;
AX = axis;
w = AX(2) - AX(1);  % width
h = AX(4) - AX(3);  % height

%% add time point annotation underneath the time course
% mark only a few salient time points
nPoints = 8;
pts = [t(1) t(mod(t,100)==0) t(end)];
pts = pts( round( linspace(1, length(pts), nPoints) ) );
pts = unique(pts);

for p = pts
    f = find(t==p);
    plot(p, tc.wholeTc(f), 'r.');
    if p==pts(1)
        txt = sprintf('t = %i s', p);
    else
        txt = num2str(p);
    end
    text(p, -0.33*h, txt, 'FontName', 'Helvetica', ...
         'FontSize', 9, 'HorizontalAlignment', 'center');
end

%% add color bars indicating condition underneath the text annotation
cond = er_resample(tc.trials.onsetSecs, tc.trials.cond, t);
for i = 1:length(tc.trials.condColors)
    cmap(i,:) = colorLookup(tc.trials.condColors{i});
end
cmap(1,:) = [1 1 1];
condImg = ind2rgb(repmat(cond(:)'+1, 2, 1), cmap);
image(t, min(tc.wholeTc)-.2*h, condImg);
% axis([AX(1:2) AX(3)-h AX(4)])

% % add a scrollbar
% scrollbar(gca, tc.wholeTc);

return

% The # of samples we show per row depends on the current
% aspect ratio of the figure. (Although the user can
% resize it, we want it to look nice in the figure as given.)
pos = get(gcf, 'Position');
rFig = pos(3) / pos(4);  % xSize / ySize


function [Q,T] = forward_kinematics(C,BE,P,dQ)
  % Compute absolute transformations from FK hierarchy and relative bone
  % rotations.
  %
  % Inputs:
  %   C  #C by dim list of joint positions
  %   BE  #BE by 2 list of bone edge indices
  %   P  #BE list of parent indices into BE
  %   dQ  #BE by 4 list of relative rotations
  % Outputs:
  %   Q  #BE by 4 list of absolute rotations
  %   T  #BE by 2 list of absolute translations
  %
  % Example:
  %   [Q,T] = forward_kinematics(C,BE,P,mpose);
  %   % stacked transposed 3x4 Affine matrices
  %   A = reshape(cat(2,permute(quat2mat(Q),[2 1 3]),permute(T,[2 3 1])),3,[])';
  %   n = size(V,1);
  %   M = reshape(bsxfun(@times,[V ones(n,1)],permute(W,[1 3 2])),n,[]);
  %   % Linear blend skinning:
  %   U = M*A

  function fk_helper(b)
    %function w = quatrotate(q,v)
    %  vv = [v 0];
    %  w = quatmultiply(quatmultiply(q,v),quatconj(q));
    %  w = w(1:3);
    %end
    if ~computed(b)
      p = P(b);
      if p < 1
        % base case for roots
        Q(b,:) = dQ(b,:);
        r = C(BE(b,1),:);
        T(b,:) = r - quatrotate(dQ(b,:),r);
      else
        % otherwise first compute parent's
        fk_helper(p);
        Q(b,:) = quatmultiply(Q(p,:),dQ(b,:));
        r = C(BE(b,1),:);
        T(b,:) = T(p,:) - quatrotate(Q(b,:),r) + ...
          quatrotate(Q(p,:),r);
      end
      computed(b) = true;
    end
  end

  % number of bones
  m = size(BE,1);
  assert(m == size(dQ,1));
  assert(m == size(P,1));

  % Whether we've already computed FK for this bone: dynamic programming
  computed = false(m,1);
  Q = zeros(m,4);
  T = zeros(m,3);
  for b = 1:m
    fk_helper(b);
  end

end


function test_ft_artifact_threshold

% WALLTIME 00:10:00
% MEM 1gb
% DEPENDENCY

% this script serves to test the asymetric onset/offset and the peak-detection

%%

data = [];
data.label = {'ramp'};
data.fsample = 1000;
data.time{1} = (1:1000)/1000;
data.trial{1} = zeros(1,1000);
data.sampleinfo = [1 1000];

% ramp up and down
data.trial{1}(  1: 500) = 1:500;
data.trial{1}(501:1000) = 500:-1:1;

%%

cfg = [];
cfg.artfctdef.threshold.bpfilter = 'no';
cfg.artfctdef.threshold.max = 400;
[cfg1, artifact1] = ft_artifact_threshold(cfg, data);

cfg = [];
cfg.trl = artifact1;
data1 = ft_redefinetrial(cfg, data);

cfg = [];
timelock1 = ft_timelockanalysis(cfg, data1);

figure
plot(timelock1.time, timelock1.avg, '.-');

% the peak should be at t=0
[val, indx] = max(timelock1.avg);
assert(isalmostequal(timelock1.time(indx), 0));

%%

cfg = [];
cfg.artfctdef.threshold.bpfilter = 'no';
cfg.artfctdef.threshold.onset  = 400;
cfg.artfctdef.threshold.offset = 400;
[cfg2, artifact2] = ft_artifact_threshold(cfg, data);

assert(isequal(artifact2, artifact1));

%%

cfg = [];
cfg.artfctdef.threshold.bpfilter = 'no';
cfg.artfctdef.threshold.onset  = 400;
cfg.artfctdef.threshold.offset = 300;
[cfg3, artifact3] = ft_artifact_threshold(cfg, data);

assert(~isequal(artifact3, artifact1));

cfg = [];
cfg.trl = artifact3;
data3 = ft_redefinetrial(cfg, data);

cfg = [];
timelock3 = ft_timelockanalysis(cfg, data3);

figure
plot(timelock3.time, timelock3.avg, '.-'); % it should be asymetric

% the peak should be at t=0
[val, indx] = max(timelock3.avg);
assert(isalmostequal(timelock3.time(indx), 0, 'abstol', 100*eps));

%%

cfg = [];
cfg.artfctdef.threshold.bpfilter = 'no';
cfg.artfctdef.threshold.onset  = 1;
cfg.artfctdef.threshold.offset = 1;
[cfg4, artifact4] = ft_artifact_threshold(cfg, data);

assert(isequal(artifact4, [1 1000 -499]));

%%
% invert the data and invert the thresholds

data.trial{1} = -data.trial{1};

cfg = [];
cfg.artfctdef.threshold.bpfilter = 'no';
cfg.artfctdef.threshold.onset  = -400;
cfg.artfctdef.threshold.offset = -300;
[cfg5, artifact5] = ft_artifact_threshold(cfg, data);

assert(isequal(artifact5, artifact3));